KM匹配算法的C++实现 - Apollo 6.0前景匹配算法理解

相关链接
上篇链接	Apollo 6.0前景匹配逻辑分析（hm_data_association文件夹）
下篇链接	-

Apollo6.0前景匹配中用到了KM匹配方法，本博客博主参考KM算法用C++写了一个KM，并写了单元测试。其中，单元测试的例子参考了另一篇KM算法 。

1. 代码

新建myKMMatcher.cpp，将下方的代码复制并保存，然后打开终端输入

$ g++ myKMMatcher.cpp -o testKM
$ ./testKM

博主写的单元测试代码如下，代码比较简陋，对主要的变量和步骤进行了注释帮助您理解:

#include <iostream>
#include <vector>

#define INF 10000;

using namespace std;

class KMMatcher {
 public:
  KMMatcher() {}
  ~KMMatcher() {}

  void km() {
    initKM();
    for (int i{0}; i < nx; i++) {
      while (true) {
        cout << "i = " << i << endl;
        // 初始化标志位（思考为什么要在此重置标志位）
        visx.assign(nx, false);
        visy.assign(ny, false);
        minz = INF;
        // 如果顶点i成功匹配，那么跳出循环
        if (findDfs(i)) {
          break;
        }
        // 如果匹配失败,则更新顶标，扩大相等子图
        for (int j{0}; j < nx; j++) {
          if (visx[j]) {
            wx[j] -= minz;
          }
        }
        for (int k{0}; k < ny; k++) {
          if (visy[k]) {
            wy[k] += minz;
          }
        }
        printVecwithStr(wx, "wx");
        printVecwithStr(wy, "wy");
        cout << "minz: " << minz << endl;
      }
    }
  }

  bool setDisMat(vector<vector<float>> inMat) { disMat = inMat; }

  bool setSize(int x, int y) {
    nx = x;
    ny = y;
  }

  vector<int> getResult() { return linkx; }

  void printVecwithStr(vector<float> vec, string s) {
    cout << s << ": ";
    for (int i{0}; i < vec.size(); i++) {
      cout << vec[i] << " ";
    }
    cout << endl;
  }

  void printVecwithStr(vector<int> vec, string s) {
    cout << s << ": ";
    for (int i{0}; i < vec.size(); i++) {
      cout << vec[i] << " ";
    }
    cout << endl;
  }

 private:
  int nx, ny;                    // 二分图的顶点个数
  vector<float> wx, wy;          // 二分图的顶标
  vector<vector<float>> disMat;  // 匹配矩阵
  vector<bool> visx, visy;       // 标志位,用于更新顶标
  vector<int> linkx, linky;      // 二分图顶点匹配位置记录
  float minz;  // 最小的顶标与匹配矩阵的差，以扩大相等子图

  void initKM() {
    minz = INF;
    wx.resize(nx);
    wy.resize(ny);
    int maxx = -INF;
    for (int i{0}; i < disMat.size(); i++) {
      maxx = -INF;
      for (int j{0}; j < disMat[i].size(); j++) {
        if (disMat[i][j] > maxx) {
          maxx = disMat[i][j];
        }
        wx[i] = maxx;
      }
    }
    visx.resize(nx);
    visy.resize(ny);
    visx.assign(nx, false);
    visy.assign(ny, false);
    linkx.resize(nx);
    linky.resize(ny);
    linkx.assign(nx, -1);
    linky.assign(ny, -1);
    cout << "-------Init-------" << endl;
    printVecwithStr(wx, "wx");
    printVecwithStr(wy, "wy");
    printVecwithStr(linkx, "linkx");
    printVecwithStr(linky, "linky");
    cout << "-------Init-------" << endl;
  }

  // 第k个顶点能否成功匹配
  bool findDfs(int i) {
    visx[i] = true;
    for (int j{0}; j < ny; j++) {
      if (!visy[j]) {
        float t = wx[i] + wy[j] - disMat[i][j];
        // 如果在相等子图内
        if (t <= 0.00001 && t >= -0.00001) {
          // 标记j位置占用
          visy[j] = true;
          if (linky[j] == -1 || findDfs(linky[j])) {
            //匹配成功
            linkx[i] = j;
            linky[j] = i;
            printVecwithStr(linkx, "linkx");
            printVecwithStr(linky, "linky");
            return true;
          }
        } else if (t > 0) {
          //匹配失败，只能更新minz以扩大相等子图
          if (t < minz) {
            minz = t;
          }
        }
      }
    }
    return false;
  }
};

int main() {
  KMMatcher kmMatcher;
  int nx = 3;
  int ny = 3;
  vector<vector<float>> disMat = {{3, 0, 4}, {2, 1, 3}, {0, 0, 5}};
  vector<int> res;
  kmMatcher.setSize(nx, ny);
  kmMatcher.setDisMat(disMat);
  kmMatcher.km();
  res = kmMatcher.getResult();
  kmMatcher.printVecwithStr(res, "Match Result");
  cout << "Match Successful!" << endl;
  return 0;
}

2. 代码运行结果

结果如下所示，可以结合另一篇KM算法一起看。

$ g++ myKMMatcher.cpp -o testKM

$ ./testKM

-------Init------- // 初始化值
wx: 4 3 5
wy: 0 0 0
linkx: -1 -1 -1
linky: -1 -1 -1
-------Init-------
i = 0 // 第一个顶点匹配
linkx: 2 -1 -1 // 匹配成功，修改匹配结果
linky: -1 -1 0
i = 1 // 第二个顶点第一次匹配
wx: 3 2 5
wy: 0 0 1
minz: 1 // 匹配失败，更新顶标，更新幅度为1
i = 1 // 第二个顶点第二次匹配
linkx: 2 0 -1 //匹配成功，修改匹配结果
linky: 1 -1 0
i = 2 // 第三个顶点第一次匹配，失败
wx: 3 2 4
wy: 0 0 1
minz: 1
i = 2 // 第三个顶点第二次匹配，又失败
wx: 2 1 3
wy: 1 0 2
minz: 1
i = 2 // 第三个顶点第三次匹配，成功
linkx: 2 1 -1 // 更新结果
linky: 1 1 0
linkx: 0 1 -1
linky: 0 1 0
linkx: 0 1 2
linky: 0 1 2
Match Result: 0 1 2 // 最终匹配结果linkx
Match Successful!

该测试用例的结果符合预期。