Apollo perception源码阅读 | fusion之匈牙利算法

Apollo perception源码阅读 | fusion之匈牙利算法

本文为Apollo感知融合源码阅读笔记，建议参照Apollo6.0源码阅读本文，水平有限，有错误的地方希望大佬多加指正!
个人代码注释链接： leox24 / perception_learn

fusion-匈牙利匹配算法

注意：Apollo融合里用的是Munkres算法（匈牙利算法）修改版本

• hungarian_test
  单独把Apollo perception6.0中的匈牙利匹配的代码拿出来了，换了下数据结构，可以简单调试用，test文件夹中cmake一下就行。

• hungarian-algorithm-cpp
  这个里面也是匈牙利的实现，将二维矩阵转成了一维数组，用指针数组来实现的，可供参考。

• 匈牙利算法原理
  关于匈牙利算法原理可以参考这篇博客，用Python和二分图实现，其实原理就是尽可能挪位置，腾地方，保证都匹配上，慢慢看是能看懂的

• 趣写算法系列之–匈牙利算法
  这篇也挺有意思的，讲的很清楚，“腾”

• 匈牙利算法(Kuhn-Munkres)算法
  比较详细的PPT讲解

• Munkres’ Assignment Algorithm
  原始 Munkres 分配算法（有时称为匈牙利算法）的修改形式，和Apollo里用的相同。

• 多目标跟踪之数据关联（匈牙利算法&KM算法）

• 带你入门多目标跟踪（三）匈牙利算法&KM算法
  匈牙利和KM算法的区别：KM算法解决的是带权二分图的最优匹配问题，引入了权重之后，匹配成功率大大提高。匈牙利算法只有将置信度较高的边进行匹配，这样才能得到较好的结果。

• Apollo_3d_obstacle_perception_cn.md
  Apollo官方有关于匈牙利算法简单阐述

进入正文(apollo代码规范是谷歌代码规范,所以文章看起来会比较长)

1 入口函数

modules\perception\common\graph\gated_hungarian_bigraph_matcher.h

• GatedHungarianMatcher::Match()
  将关联矩阵costs转换成二分图，计算连通子图，对子图计算匈牙利匹配

template <typename T>
void GatedHungarianMatcher<T>::Match(
    T cost_thresh, T bound_value, OptimizeFlag opt_flag,
    std::vector<std::pair<size_t, size_t>>* assignments,
    std::vector<size_t>* unassigned_rows,
    std::vector<size_t>* unassigned_cols) {
  CHECK_NOTNULL(assignments);
  CHECK_NOTNULL(unassigned_rows);
  CHECK_NOTNULL(unassigned_cols);

  /* initialize matcher */
  cost_thresh_ = cost_thresh; //=4
  opt_flag_ = opt_flag; //OPTMIN
  bound_value_ = bound_value; //=100
  assignments_ptr_ = assignments;

  //初始化is_valid_cost_函数，object和tracker的欧氏距离小于4米有效
  MatchInit(); 

  /* compute components */
  std::vector<std::vector<size_t>> row_components;
  std::vector<std::vector<size_t>> col_components;
  
  //计算二分图的连通子图，即匈牙利的增广路.row放入对应的col，col放入对应的row
  //row/col_components的size都是一样的，
  this->ComputeConnectedComponents(&row_components, &col_components);
  CHECK_EQ(row_components.size(), col_components.size());

  /* compute assignments */
  assignments_ptr_->clear();
  assignments_ptr_->reserve(std::max(rows_num_, cols_num_));
  //对连通子图分别进行匈牙利匹配 最小距离
  for (size_t i = 0; i < row_components.size(); ++i) {
    this->OptimizeConnectedComponent(row_components[i], col_components[i]);
  }

  //生成未分配的
  this->GenerateUnassignedData(unassigned_rows, unassigned_cols);
}

2 主函数

modules\perception\common\graph\hungarian_optimizer.h

由HungarianOptimizer类来实现匹配算法，注意类中的关联矩阵由接口更新为最新的关联矩阵，并且为等边矩阵。算最小值，最主要的就是DoMunkres()函数和几个step函数。

使用迭代来计算出最优匹配，每次step函数后fn_state_绑定新的step，其中关联值会打标记，三种enum class Mark { NONE, PRIME, STAR }，使用☆表示star，※表示prime

最后有具体示例表示计算过程，按我的理解prime就是最后的结果。

谢谢@Lyy_Zzz补充:
star代表的是现有匹配结果，而prime的含义是可行的匹配但还没匹配的，每次通过增广操作将prime更改为star，匹配的数目就会增加1，不断迭代得到最终匹配结果，如果想更详细了解KM算法可以参考这个网站Munkres’ Assignment Algorithm，里面有详细的图解和c#的代码示例，同时也给出的相关的参考文献。

/* Find an assignment which minimizes the overall costs.
 * Return an array of pairs of integers. Each pair (i, j) corresponds to
 * assigning agent i to task j. */
template <typename T>
void HungarianOptimizer<T>::Minimize(
    std::vector<std::pair<size_t, size_t>>* assignments) {
  // 初始化参数
  OptimizationInit();

  // 匈牙利算法
  DoMunkres();
  
  // 读取star为最终结果（直接读prime不行吗？）
  FindAssignments(assignments);

  OptimizationClear();
}

/* Run the Munkres algorithm */
// 匈牙利（Kuhn-Munkres）算法
template <typename T>
void HungarianOptimizer<T>::DoMunkres() {
  int max_num_iter = 1000;
  int num_iter = 0;
  fn_state_ = std::bind(&HungarianOptimizer::ReduceRows, this);
  while (fn_state_ != nullptr && num_iter < max_num_iter) {
    fn_state_();
    ++num_iter;
  }
  if (num_iter >= max_num_iter) {
    CheckStar();
  }
}

3 算法流程

• Step 1-ReduceRows
  For each row of the matrix, find the smallest element and subtract it from every element in its row. Go to Step 2.

对于矩阵的每一行，找到最小的元素，该行的每个元素减去它。转到步骤 2。

• Step 2-StarZeroes
  Find a zero (Z) in the matrix. If there is no starred zero in its row or column, star Z. Repeat for every element in the matrix. Go to Step 3.
  Note: profiling shows this method to use 9.2% of the CPU - the next slowest step takes 0.6%. It is hard to find a way further speed it up.

在矩阵中找到一个0(Z)。如果其行或列中没有带star的0，则为Z加上star，有star的0的话就跳过该行或者该列。对矩阵中的每个元素重复此操作。转到步骤 3。
注意：分析显示此方法使用 9.2% 的CPU - 下一个最慢的步骤需要 0.6%。很难找到进一步加速它的方法。

• Step 3-CoverStarredZeroes
  Cover each column containing a starred zero. If all columns are covered, the starred zeros describe a complete set of unique assignments. In this case, terminate the algorithm. Otherwise, go to Step 4.

cover包含star0的每一列。如果所有列都被cover，带star的0描述了一组完整的唯一分配。在这种情况下，终止算法。否则，请转到步骤 4。

• Step 4-PrimeZeroes
  Find a noncovered zero and prime it. If there is no starred zero in the row containing this primed zero, Go to Step 5. Otherwise, cover this row and uncover the column containing the starred zero. Continue in this manner until there are no uncovered zeros left, then go to Step 6.

找到一个未cover的0并将其prime。如果包含这个prime0的行中没有star0，则转到步骤 5。否则，cover该行，uncover有star0的列。以这种方式继续，直到没有剩余的0，然后转到步骤 6。

• Step 5-MakeAugmentingPath
  Construct a series of alternating primed and starred zeros as follows. Let Z0 represent the uncovered primed zero found in Step 4. Let Z1 denote the starred zero in the column of Z0 (if any). Let Z2 denote the primed zero in the row of Z1 (there will always be one). Continue until the series terminates at a primed zero that has no starred zero in its column. Unstar each starred zero of the series, star each primed zero of the series, erase all primes and uncover every line in the matrix. Return to Step 3.

构造一系列交替的prime0和star0。让 Z0 代表在步骤 4 中发现的uncover的prime0。让 Z1 表示 Z0 列中的star0（如果有）。让 Z2 表示 Z1 行中的prime0（总会有一个）。继续直到该系列在其列中没有star0处终止。取消系列的每个star0，为系列的每个prime0加上star，删除所有prime0并uncover矩阵中的每一行。返回步骤 3。

• Step 6-AugmentPath
  Add the smallest uncovered value in the matrix to every element of each covered row, and subtract it from every element of each uncovered column. Return to Step 4 without altering any stars, primes, or covered lines.

将矩阵中最小的uncover值添加到每个cover行的每个元素，并从每个uncover列的每个元素中减去它。返回第 4 步。

step1 ReduceRows

每行减去最小值，必定每行存在0，跳转到step2

/* Step 1:每行减去最小值
 * For each row of the matrix, find the smallest element and substract it
 * from every element in its row. Then, go to Step 2. */
template <typename T>
void HungarianOptimizer<T>::ReduceRows() {
  for (size_t row = 0; row < matrix_size_; ++row) {
    T min_cost = costs_(row, 0);
    for (size_t col = 1; col < matrix_size_; ++col) {
      min_cost = std::min(min_cost, costs_(row, col));
    }
    for (size_t col = 0; col < matrix_size_; ++col) {
      costs_(row, col) -= min_cost;
    }
  }
  fn_state_ = std::bind(&HungarianOptimizer::StarZeroes, this);
}

step2 StarZeroes

没有标记cover的行列中，star0的位置，cover对应的行和列。一行一列只能存在一个star0，star是初始的匹配结果，后续进行匈牙利的增广操作，对匹配结果做更改。

/* Step 2:对于未标记的行列，标记有0的行列，并且star☆该位置
 * Find a zero Z in the matrix. If there is no starred zero in its row
 * or column, star Z. Repeat for every element in the matrix. Then, go to
 * Step3. */
template <typename T>
void HungarianOptimizer<T>::StarZeroes() {
  /* since no rows or columns are covered on entry to this step, we use the
   * covers as a quick way of making which rows & columns have stars in them */
  for (size_t row = 0; row < matrix_size_; ++row) {
    if (RowCovered(row)) {
      continue;
    }
    for (size_t col = 0; col < matrix_size_; ++col) {
      if (ColCovered(col)) {
        continue;
      }
      if (costs_(row, col) == 0) {
        Star(row, col);
        CoverRow(row);
        CoverCol(col);
        break;
      }
    }
  }
  ClearCovers();//清除所有的行列标记
  fn_state_ = std::bind(&HungarianOptimizer::CoverStarredZeroes, this);
}

step3 CoverStarredZeroes

标记cover有star0的列，如果列全部cover说明已经匹配完了，得到了一组完整匹配结果。矩阵的列没有全部cover则跳转到step4

/* Step 3:标记有0的列，全标记后结束迭代，未全标记继续下一步step4计算
 * Cover each column containing a starred zero. If all columns are covered,
 * the starred zeros describe a complete set of unique assignments. In this
 * case, terminate the algorithm. Otherwise, go to Step 4. */
template <typename T>
void HungarianOptimizer<T>::CoverStarredZeroes() {
  size_t num_covered = 0;

  for (size_t col = 0; col < matrix_size_; ++col) {
    if (ColContainsStar(col)) {
      CoverCol(col);
      num_covered++;
    }
  }

  if (num_covered >= matrix_size_) {
    fn_state_ = nullptr;
    return;
  }
  fn_state_ = std::bind(&HungarianOptimizer::PrimeZeroes, this);
}

step4 PrimeZeroes

若未cover的行列中没有0，跳到step6增广操作，使得未cover的行列中出现0，跳回step4。
此时未cover的行列中有0，标记prime该位置。
如果prime0该行有star0，标记cover该行，取消标记uncover该列。
如果prime0该行没有star0，计算完毕，跳到step5，生成最后的匹配结果

/* Step 4:找到没有cover的0，prime※该0.没有step6，有step5
 * Find a noncovered zero and prime it. If there is no starred zero in the
 * row containing this primed zero, go to Step 5. Otherwise, cover this row
 * and uncover the column containing the starred zero. Continue in this manner
 * until there are no uncovered zeros left, then go to Step 6. */
template <typename T>
void HungarianOptimizer<T>::PrimeZeroes() {
  // this loop is guaranteed to terminate in at most matrix_size_ iterations,
  // as FindZero() returns a location only if there is at least one uncovered
  // zero in the matrix.  Each iteration, either one row is covered or the
  // loop terminates.  Since there are matrix_size_ rows, after that many
  // iterations there are no uncovered cells and hence no uncovered zeroes,
  // so the loop terminates.
  for (;;) {
    size_t zero_row = 0;
    size_t zero_col = 0;
    //未覆盖的行列中关联值有没有0
    if (!FindZero(&zero_row, &zero_col)) {
      // No uncovered zeroes.
      // 0已经全部cover，没有0，到step6
      fn_state_ = std::bind(&HungarianOptimizer::AugmentPath, this);
      return;
    }

    //有0，标记prime※该位置
    Prime(zero_row, zero_col);
    //找到该行有star☆的0，找到返回列，没找到返回-1。
    int star_col = FindStarInRow(zero_row);

    //该行找到star的0，标记该行，取消标记该列
    if (star_col != kHungarianOptimizerColNotFound) {
      CoverRow(zero_row);
      UncoverCol(star_col);
    } else {
      //该行没找到star的0，基本计算完了，收工标记好配对的关联值，step5
      std::pair<size_t, size_t> first_assignment =
          std::make_pair(zero_row, zero_col);
      assignments_[0] = first_assignment;
      fn_state_ = std::bind(&HungarianOptimizer::MakeAugmentingPath, this);
      return;
    }
  }
}

step5 MakeAugmentingPath

计算完毕，横竖交替找到0值，放入assignments_数组里
横竖交替找到0值(prime->star->prime->star->…->prime)
其实prime就是最终的结果了。原有star去掉标记，prime标记为star。
跳到step3，判断所有列中都有star0，匹配结束。

/* Step 5:计算完毕，横竖交替找到0值，放入assignments_数组里
 * Construct a series of alternating primed and starred zeros as follows.
 * Let Z0 represent the uncovered primed zero found in Step 4.  Let Z1 denote
 * the starred zero in the column of Z0 (if any). Let Z2 denote the primed
 * zero in the row of Z1 (there will always be one).  Continue until the
 * series terminates at a primed zero that has no starred zero in its column.
 * unstar each starred zero of the series, star each primed zero of the
 * series, erase all primes and uncover every line in the matrix. Return to
 * Step 3. */
template <typename T>
void HungarianOptimizer<T>::MakeAugmentingPath() {
  bool done = false;
  size_t count = 0;
  //横竖交替找到0值(prime->star->prime->star->...->prime)
  //其实prime就是最终的结果了
  while (!done) {
    /* first construct the alternating path... */
    int row = FindStarInCol(assignments_[count].second);

    if (row != kHungarianOptimizerRowNotFound) {
      count++;
      assignments_[count].first = row;
      assignments_[count].second = assignments_[count - 1].second;
    } else {
      done = true;
    }

    if (!done) {
      int col = FindPrimeInRow(assignments_[count].first);
      count++;
      assignments_[count].first = assignments_[count - 1].first;
      assignments_[count].second = col;
    }
  }

  /* then, modify it. */
  //原有star去掉标记，prime标记为star
  for (size_t i = 0; i <= count; ++i) {
    size_t row = assignments_[i].first;
    size_t col = assignments_[i].second;

    if (IsStarred(row, col)) {
      Unstar(row, col);
    } else {
      Star(row, col);
    }
  }

  //清除cover和prime
  ClearCovers();
  ClearPrimes();
  fn_state_ = std::bind(&HungarianOptimizer::CoverStarredZeroes, this);
}

step6 AugmentPath

/* Step 6:0全部cover了，找到未覆盖行列中的最小值，覆盖行加，未覆盖列减，step4
 * Add the smallest uncovered value in the matrix to every element of each
 * covered row, and subtract it from every element of each uncovered column.
 * Return to Step 4 without altering any stars, primes, or covered lines. */
template <typename T>
void HungarianOptimizer<T>::AugmentPath() {
  T minval = FindSmallestUncovered();

  for (size_t row = 0; row < matrix_size_; ++row) {
    if (RowCovered(row)) {
      for (size_t c = 0; c < matrix_size_; ++c) {
        costs_(row, c) += minval;
      }
    }
  }
  for (size_t col = 0; col < matrix_size_; ++col) {
    if (!ColCovered(col)) {
      for (size_t r = 0; r < matrix_size_; ++r) {
        costs_(r, col) -= minval;
      }
    }
  }
  fn_state_ = std::bind(&HungarianOptimizer::PrimeZeroes, this);
}

4 示例计算过程

示例仅仅是一个浅显的例子，实际的关联值应该是比较理想的值，去掉阈值之外的数值，关联的时候会更加准确

5 关于耗时

算法实现代码链接：hungarian_test
其实就是把Apollo里面的代码单独拿出来了，测试用很方便。

使用了随机生成的256*256的矩阵进行测试，如下：

  vector<vector<int>> costs_(256, vector<int>(256, 0));
  for (int i = 0; i < 256; ++i) {
    for (int j = 0; j < 256; ++j) {
      costs_[i][j] = static_cast<int>(rand() % 256 + 1);
    }
  }
  HungarianOptimizer<int> optimizer_;
  optimizer_.costs(costs_);
  
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  std::vector<std::pair<size_t, size_t>> assignments;
  optimizer_.Minimize(&assignments);

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used =
      chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

简单测试了下，对于256*256矩阵的匈牙利匹配，debug编译时间约为0.38s，release编译耗时约为0.05s。

因为平时默认都是debug编译的，运行时间快0.4秒了还是蛮震惊这算法怎么这么耗时，改为release编译后提高了不少效率。
更正：256*256矩阵并没有完全迭代完，详情看博客评论

像示例中的4*4矩阵的话，耗时都到10e-5秒了，基本不怎么耗时。

欢迎指正