cartographer_learn_20子图匹配器与分支定界算法初涉

续接上篇

上一篇我们讨论到了说cartographer在做局部窗口匹配还是做全局窗口匹配前做的一些准备和计算。本篇我们先来主要讨论做局部窗口匹配的情况。在这里，作者建议各位读者先去读一篇论文——《Real-Time Loop Closure in 2D LIDAR SLAM》，对照着论文看接下来的源码部分会更佳。想想上一篇的更新已经是十几天前了，作者跑去看了一会会多视图几何所以才拖了这么久。

构建子图的匹配器

我们直接进入去看看局部窗口匹配的函数：

void ConstraintBuilder2D::MaybeAddConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose) {
  // 节点和子图原点距离太远就不会计算约束
  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance()) {
    return;
  }
  //采样器控制相似约束的数量，提高实时性
  if (!per_submap_sampler_
           .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id),
                    std::forward_as_tuple(options_.sampling_ratio()))
           .first->second.Pulse()) {
    return;
  }
  absl::MutexLock locker(&mutex_);
  // 当when_done_正在处理任务时调用本函数, 报个警告
  if (when_done_) {
    LOG(WARNING)
        << "MaybeAddConstraint was called while WhenDone was scheduled.";
  }
  //为匹配结果准备空间
  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());
  auto* const constraint = &constraints_.back();
  
  // 获取匹配器指针，每个submap都有属于它自己的匹配器
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());
  ......//一堆其他操作
}

可以看到cartographer使用采样器去控制约束产生的数量，这种操作使得作者想起了视觉slam中的关键帧。由于采样频率是很高的，无论是激光和视觉都能达到10HZ以上的频率，想必很多连续帧的数据是相似的，它们为整个最小二乘问题带来的约束也是相似的。为了提高实时性，我们并不是把它们都加入最小二乘问题，而是选择它们中的一些代表加入。这样既可以减少计算量，保证实时性，同时对精度的影响也不大。上面贴出的函数片段中，重点是获取匹配器指针的那个函数，我们后续看看，在此之前我们先去看看ConstraintBuilder2D这个类，因为后端正是通过它实现回环检测的。

class ConstraintBuilder2D {
 public:
  using Constraint = PoseGraphInterface::Constraint;
  using Result = std::vector<Constraint>;

......//一堆类方法
 private:
......//又是一堆类方法
   //线程池的指针，第17篇提到过
  common::ThreadPoolInterface* thread_pool_;
  absl::Mutex mutex_;
  //接下来是三个任务，目前作者也没明白这三个任务的成员变量的含义
  // 'callback' set by WhenDone().
  std::unique_ptr<std::function<void(const Result&)>> when_done_
      GUARDED_BY(mutex_);

  // TODO(gaschler): Use atomics instead of mutex to access these counters.
  // Number of the node in reaction to which computations are currently
  // added. This is always the number of nodes seen so far, even when older
  // nodes are matched against a new submap.
  int num_started_nodes_ GUARDED_BY(mutex_) = 0;

  int num_finished_nodes_ GUARDED_BY(mutex_) = 0;

  std::unique_ptr<common::Task> finish_node_task_ GUARDED_BY(mutex_);

  std::unique_ptr<common::Task> when_done_task_ GUARDED_BY(mutex_);

  // Constraints currently being computed in the background. A deque is used to
  // keep pointers valid when adding more entries. Constraint search results
  // with below-threshold scores are also 'nullptr'.
  //匹配的结果
  std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);

  // 每个submap都有自己的匹配器
  std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
      GUARDED_BY(mutex_);
  //上面提到过的采样器，同样的每个submap都有
  std::map<SubmapId, common::FixedRatioSampler> per_submap_sampler_;
  //cere的匹配器
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
  // Histogram of scan matcher scores.
  common::Histogram score_histogram_ GUARDED_BY(mutex_);
};

class Histogram {
 public:
  void Add(float value);
  std::string ToString(int buckets) const;

 private:
  std::vector<float> values_;
};

在看完ConstraintBuilder2D的类成员后，我们继续看看DispatchScanMatcherConstruction，与此同时既然我们是去看匹配器是如果产生的我们也有必要去看看匹配器是什么：

//匹配器的结构
  struct SubmapScanMatcher {
  	//子图的地图数据
    const Grid2D* grid = nullptr;
    //计算存储《Real-Time Loop Closure in 2D LIDAR SLAM》论文中的“Precomputed grids”的类，这里作者称之为“预先计算的栅格”
    std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
        fast_correlative_scan_matcher;
    //后面线程池使用，这个任务是另外某个任务的依赖
    std::weak_ptr<common::Task> creation_task_handle;
  };


//匹配器的生成
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                     const Grid2D* const grid) {
  CHECK(grid);
  // 如果匹配器里已经存在, 则直接返回对应id的匹配器指针
  if (submap_scan_matchers_.count(submap_id) != 0) {
    return &submap_scan_matchers_.at(submap_id);
  }
  // 如果没有为该子图生成默认的匹配器，后续在处理这个默认的匹配器
  auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
  // 给匹配器的grid赋值
  submap_scan_matcher.grid = grid;

  auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
  auto scan_matcher_task = absl::make_unique<common::Task>();
  // 生成一个为该匹配器计算其fast_correlative_scan_matcher的任务
  scan_matcher_task->SetWorkItem(
      [&submap_scan_matcher, &scan_matcher_options]() {
        // 进行匹配器的初始化
        submap_scan_matcher.fast_correlative_scan_matcher =
            absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
                *submap_scan_matcher.grid, scan_matcher_options);
      });
  // 将该任务放入线程池中, 并且将任务的智能指针保存起来
  submap_scan_matcher.creation_task_handle =
      thread_pool_->Schedule(std::move(scan_matcher_task));

  return &submap_scan_matchers_.at(submap_id);
}

可见这个函数的重点的放入线程池中的那个任务，即为该子图计算论文中所提到的预先计算的栅格（Precomputed grids），我们去瞧瞧。同样的，既然是看看如何计算FastCorrelativeScanMatcher2D这个类，我们有必要先去看看这个类：

//FastCorrelativeScanMatcher2D
class FastCorrelativeScanMatcher2D {
public：
//构造函数，可以看到被放到线程池中的任务就是这个构造函数
FastCorrelativeScanMatcher2D(
      const Grid2D& grid,
      const proto::FastCorrelativeScanMatcherOptions2D& options);
......//一堆类方法
private:
......//又是一堆类方法
//cartograoher常见的有关于这个类的选项
  const proto::FastCorrelativeScanMatcherOptions2D options_;
//地图信息，包括栅格数量，分辨率，最大点等
  MapLimits limits_;
//存储预先计算的栅格（Precomputed grids）的地方
  std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
};

//FastCorrelativeScanMatcher2D的构造函数
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options)
    : options_(options),
      limits_(grid.limits()),
      //计算预先计算的栅格（Precomputed grids）
      precomputation_grid_stack_(
          absl::make_unique<PrecomputationGridStack2D>(grid, options)) {}

这里有涉及到一个新的类PrecomputationGridStack2D，按照惯例我们先去看看这个类是什么，在看看它的构造函数：

class PrecomputationGridStack2D {
 public:
  PrecomputationGridStack2D(
      const Grid2D& grid,
      const proto::FastCorrelativeScanMatcherOptions2D& options);

  const PrecomputationGrid2D& Get(int index) {
    return precomputation_grids_[index];
  }
  int max_depth() const { return precomputation_grids_.size() - 1; }
 private:
 //真正存储预先计算的栅格（Precomputed grids）的地方，因为分支定界算法涉及到在不同的层级的搜索树上计算某个区域可行解的上界，
 //这里vector中的每个元素是对应某个层级（即论文中的h）计算过程中的预先计算的栅格（Precomputed grids）
  std::vector<PrecomputationGrid2D> precomputation_grids_;
};

//PrecomputationGridStack2D构造函数
PrecomputationGridStack2D::PrecomputationGridStack2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options) {
  CHECK_GE(options.branch_and_bound_depth(), 1);

  // 获取选项中有关dfs搜索的层级数，一般为7
  const int max_width = 1 << (options.branch_and_bound_depth() - 1);
  //根据层级预先分配空间
  precomputation_grids_.reserve(options.branch_and_bound_depth());

  std::vector<float> reusable_intermediate_grid;
  const CellLimits limits = grid.limits().cell_limits();
  reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
                                     limits.num_y_cells);

  // 分辨率逐渐变大, i=0时就是默认分辨率（0.05）, i=6时, width=64,也就是64个格子合成一个值                                   
  for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
    const int width = 1 << i;
    precomputation_grids_.emplace_back(grid, limits, width,
                                       &reusable_intermediate_grid);
  }
}

这里又涉及到一个新的类的构造，依旧是先看看这个类，再去看看它是如何构造：

//PrecomputationGrid2D
class PrecomputationGrid2D {
 public:
  PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width,
                       std::vector<float>* reusable_intermediate_grid);
  ......//一堆类方法
 private:
 ......//一堆类方法
  // Offset of the precomputation grid in relation to the 'grid'
  // including the additional 'width' - 1 cells.
  const Eigen::Array2i offset_;

  // Size of the precomputation grid.
  const CellLimits wide_limits_;

  const float min_score_;     //0.1
  const float max_score_;	  //0.9
  //某一层的预先计算的栅格（Precomputed grids）
  std::vector<uint8> cells_;
};

在介绍PrecomputationGrid2D的构造函数之前有必要先介绍一个类——SlidingWindowMaximum，需要这个类的辅助才能够完成预先计算的栅格（Precomputed grids）的计算。

class SlidingWindowMaximum {
 public:
  // 添加值，添加过程中保证双向数组中元素是降序排列
  void AddValue(const float value) {
    while (!non_ascending_maxima_.empty() &&
           value > non_ascending_maxima_.back()) {
      non_ascending_maxima_.pop_back();
    }
    non_ascending_maxima_.push_back(value);
  }

  // 删除值, 如果第一个值等于要删除的这个值, 则将这个值删掉.
  void RemoveValue(const float value) {
    // DCHECK for performance, since this is done for every value in the
    // precomputation grid.
    DCHECK(!non_ascending_maxima_.empty());
    DCHECK_LE(value, non_ascending_maxima_.front());
    if (value == non_ascending_maxima_.front()) {
      non_ascending_maxima_.pop_front();
    }
  }

  // 因为双向数组中是降序排列，所以第一个数为该数组中的最大值，放回数组的第一个元素，即放回最大值
  float GetMaximum() const {
    // DCHECK for performance, since this is done for every value in the
    // precomputation grid.
    DCHECK_GT(non_ascending_maxima_.size(), 0);
    return non_ascending_maxima_.front();
  }
//检查双向数组是否为空
  void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); }

 private:
  //一个双向数组，这个类主要维护的量
  std::deque<float> non_ascending_maxima_;
};

总结一下这个类的作用，就是给定一组数，通过类函数AddValue一个个的加入到双向数组且保证双向数组内部元素的排序是降序。通过类函数GetMaximum可以得到数组中的第一个元素，即最大值。RemoveValue可以删除掉头元素。接下来我们来看看PrecomputationGrid2D的构造函数函数：

PrecomputationGrid2D::PrecomputationGrid2D(
    const Grid2D& grid, const CellLimits& limits, const int width,
    std::vector<float>* reusable_intermediate_grid)
    : offset_(-width + 1, -width + 1),
      wide_limits_(limits.num_x_cells + width - 1,
                   limits.num_y_cells + width - 1),
      min_score_(1.f - grid.GetMaxCorrespondenceCost()), // 0.1
      max_score_(1.f - grid.GetMinCorrespondenceCost()), // 0.9
      cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
  CHECK_GE(width, 1);
  CHECK_GE(limits.num_x_cells, 1);
  CHECK_GE(limits.num_y_cells, 1);
  const int stride = wide_limits_.num_x_cells;
  // First we compute the maximum probability for each (x0, y) achieved in the
  // span defined by x0 <= x < x0 + width.
  //初始化中间变量
  std::vector<float>& intermediate = *reusable_intermediate_grid;
  intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
  //step1
  for (int y = 0; y != limits.num_y_cells; ++y) {

    SlidingWindowMaximum current_values;
    current_values.AddValue(
        1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
    //step1.1
    for (int x = -width + 1; x != 0; ++x) {
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      if (x + width < limits.num_x_cells) {
        current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
                                          Eigen::Array2i(x + width, y))));
      }
    }
    //step1.2
    for (int x = 0; x < limits.num_x_cells - width; ++x) {
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      current_values.RemoveValue(
          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
      current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
                                        Eigen::Array2i(x + width, y))));
    }
    //step1.3
    for (int x = std::max(limits.num_x_cells - width, 0);
         x != limits.num_x_cells; ++x) {
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      current_values.RemoveValue(
          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
    }
    current_values.CheckIsEmpty();
  }

  // For each (x, y), we compute the maximum probability in the width x width
  // region starting at each (x, y) and precompute the resulting bound on the
  // score.
  //step2 , cell是最终的结果
  for (int x = 0; x != wide_limits_.num_x_cells; ++x) {

    SlidingWindowMaximum current_values;

    current_values.AddValue(intermediate[x]);
    //step2.1
    for (int y = -width + 1; y != 0; ++y) {
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      if (y + width < limits.num_y_cells) {
        current_values.AddValue(intermediate[x + (y + width) * stride]);
      }
    }
    //step2.2
    for (int y = 0; y < limits.num_y_cells - width; ++y) {
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      current_values.RemoveValue(intermediate[x + y * stride]);
      current_values.AddValue(intermediate[x + (y + width) * stride]);
    }
    //step2.3
    for (int y = std::max(limits.num_y_cells - width, 0);
         y != limits.num_y_cells; ++y) {
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      current_values.RemoveValue(intermediate[x + y * stride]);
    }
    current_values.CheckIsEmpty();
  }
}

这个构造函数大体上分成两步，即上述所说的step1和step2。且step1和step2做的事情本质上是一样的，只是操作的对象不一样而已。所以我们将着重讲解step1做了什么事。在此之前先说明一件事情，我们先明确一下我们在讨论一件什么事情。我们其实是在讨论cartograpger为了加快匹配的速度而对原本的栅格地图做了些什么处理，本质上就是在讨论原本的那个表示栅格地图的二维数组做了什么处理。那栅格地图的x，y坐标是怎么表示的呢？这个在第13篇时右介绍，再一个时怎么存储栅格地图的呢？是用一个一维数组去存储的，先存储y=0这一行的所有栅格，再存储y=1这一行的所有栅格以此类推。
接下来我们将讨论step1究竟做了什么事，看下图。假设原始的栅格地图为4*4的栅格地图，假设此时我们要求width=2时的Precomputed grids（即论文中的h=2）。首先我们得想想最终得Precomputed grids应该长什么样子？如下图右边的正方形，此时Precomputed grids应该是一个5 * 5（4+2-1）的栅格地图，外围蓝色的格子均是要拓展的格子。step1.1则是要计算在右图红色框内的栅格的最大值，并把这个最大值填入写着“红”字的栅格中。类似的step1.2是要计算右图黄色框内的栅格的最大值，并把这个最大值填入写着“黄”字的栅格中。但是相比于step1.1，step1.2要把可能的最大值从SlidingWindowMaximum 这个类中删掉，同时因为黄色框要向右平移，所以要加入新的值。step1.3就比较简单了就是要计算右图绿色框内的栅格的最大值，并把这个最大值填入写着“绿”字的栅格中，并且要删除掉可能的最大值。step2和step1是类似的，只不过step2的输入是step1的结果，且于step1中水平的框不同，step2中的框是竖直的。

分支定界算法初涉（Branch-and-bound scan matching）

这里我们已经渐渐开始涉及到分支定界算法的代码了，这里作者先从理论上说说作者自己的理解。继续上面的话题，知道了代码的具体操作过程，那么这样操作后的结果是什么呢？还是以上面的width=2（即论文中的h=2）的情况为例，上面经过step1和step2的操作后，在Precomputed grids中的某个栅格（设为A）的值是：在原始栅格地图中以栅格A为左上角画出2*2（width * width）个栅格后，在这（width * width）个栅格中最大的栅格值。

ok，接下来我们简单的讨论分支定界算法，这是作者读论文读来的，英语有点点差，有错误之处该请斧正。
首先论文提出了一个问题，说有一个scan（代码中的节点）和一个submap（子图），且我们通过一些比较粗糙的方法得到了scan和submap的相对位姿关系（粗糙的解），有没有办法能进一步优化这个解使其更加精确呢？有论文中提出了优化的问题

不过这不是重点，重点是怎么解这个问题，或者说通过这个问题去优化原来那个粗糙的解呢？论文又提出了一种传统的方法来解决问题。在了解传统方法前我们先来看看解的形式，解是一个列向量（x,y,theta）。传统的方法认为应该在
的范围内一个个的检索，同时论文中也给出了检索的步长。假设最终x方向上待检索量个数为nx，y方向上的个数为ny，角度上的个数为ntheta，则最终算法的复杂度为o（nx * ny * ntheta），效率不太理想。
论文此时就提出了自己的想法，提出一个复杂度比较低的算法，作者自己认为分支定界算法的复杂度为o（log(nx * ny) * ntheta)不对还请斧正。怎么做的呢？主要是在x，y方向上采用分支的方法。此时假定我们已经通过某种方式找到了最佳的Btheta了，那么接下来我们的任务就是要去寻找最佳的x，y。其本质就是寻找两个整数wx和wy，使得姿态为：

时论文中的BBS那个公式值最大。其中r时检索步长。上面所说的那些主要是想表达分支定界中的所谓分支只是涉及x，y和角度没关系。于是我们的问题就简化了，简化成在原本粗糙的解的基础上将scan的数据在x方向上和在y方向上分别平移多少个r后能使得BBS这个公式最大化。
于是整个可行解的区域可以表示为,其中搜索窗口的大小为2w * r * 2w * r：

假设此时w=4。那么我们怎么先粗略的判断最优解究竟再上图的A，B，C，D的那个区域内呢？论文认为，我们要先把所有scan的数据点转换成对应的栅格地图中的栅格，这里我们假设scan中的数据对应n个栅格（（x1,y1）…(xn,yn)）。分别计算A，B，C，D区域可行解的得分，即定界。对于A区域的得分，先将原始的n个栅格都在x轴平移-w，y轴平移-w个栅格，再计算平移后n个栅格在width=2的Precomputed grids的栅格占有概率之和sum，sum/n即为得分。B区域的的得分是将原始的n个栅格都在x轴平移-w，y轴平移0个栅格，再计算平移后n个栅格在width=2的Precomputed grids的栅格占有概率之和sum，sum/n即为得分。C，D区域的得分计算类似。这样我们就能在o(n)的时间内求任意某个区域的得分。
这里有点绕，希望读者能仔细思考Precomputed grids的作用，至于具体细节，后续随着代码阅读的深入我们还会继续讨论。