cartographer--后端match--减小搜索框策略(ShrinkToFit)

在fast_correlative_scan_matcher.cc文件中，

FastCorrelativeScanMatcher2D::MatchWithSearchParameters 函数真正体现了后端match的策略，即fast相关方法的核心。

在此函数中，在将点云离散化处理并转换到map坐标系之后，进行了搜索框压缩的步骤：

  search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());

因为位姿搜索框较大（在±7m范围内），因此需要根据目前的数据缩小搜索框。

因此运用了ShrinkToFit函数

其定义于correlative_scan_matcher_2d.cc，源代码如下，进行讲解：

// 尽量收缩搜索框，减少计算量
// scans即：被角度搜索框内部的一系列角度旋转后并初始化平移的一系列点云的容器
// 记住，现在点云已经位于map坐标系了，不在以节点自身为参考
// 这个是非常重要的一个信息
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                                   const CellLimits& cell_limits) {
  CHECK_EQ(scans.size(), num_scans);
  CHECK_EQ(linear_bounds.size(), num_scans);
  
  // 搜索框中有多少个角度就是num_scans的大小（注意，搜索范围是设定角度的两倍+1）
  for (int i = 0; i != num_scans; ++i) {
    Eigen::Array2i min_bound = Eigen::Array2i::Zero();
    Eigen::Array2i max_bound = Eigen::Array2i::Zero();
    // 对一个旋转点云的每个栅格化的激光点进行循环
    for (const Eigen::Array2i& xy_index : scans[i]) {
      // Eigen::Array2i类型的比较是按照元素级比较，比如.min()，对应元素相比，哪个小取哪个
      // 这个地方比较难理解，我单独拿一篇blog说一下：
      min_bound = min_bound.min(-xy_index);
      // 得到最大栅格点
      max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
                                               cell_limits.num_y_cells - 1) -
                                xy_index);
      
    }
    linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x());
    linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x());
    linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y());
    linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y());
  }
}

传入参数：

• std::vector<DiscreteScan2D>& scans 一个离散化点云，是经过某个搜索角度初始旋转后，并将坐标映射于map坐标系（即轨迹坐标系或者说local坐标系）的栅格化点云

• CellLimits& cell_limits 当前轨迹建图的map的限制条件

效果：

• 将linear_bounds，即线搜索范围，收紧。

预备

首先要知道linear_bounds的初始值是什么，才能进一步知道怎么将这个初始范围收紧。

由orrelative_scan_matcher_2d.cc的函数：SearchParameters::SearchParameters可知，其初始值为：

  // 按照栅格地图的分辨率来进行长度搜索
  // 看看0.1m以内有多少次长度搜索
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  linear_bounds.reserve(num_scans);
  for (int i = 0; i != num_scans; ++i) {
    // push_back搜索窗口边界，分别为min_x,max_x,min_y,max_y
    // 对于每一个角度搜索，长度搜索范围初步设计成一样的
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations});
  }

xy的方向的初始值均为（-num_linear_perturbations, num_linear_perturbations）

其中num_linear_perturbations是通过搜索长度/栅格地图分辨率得到的搜索次数

之所以正负范围——是因为要根据初始的节点local pose四周进行搜索

ShrinkToFit详解

注意这里：
对一个点云的每个栅格化的激光点进行循环：

    for (const Eigen::Array2i& xy_index : scans[i]) {
      // Eigen::Array2i类型的比较是按照元素级比较，比如.min()，对应元素相比，哪个小取哪个
      min_bound = min_bound.min(-xy_index);
      // 得到最大栅格点
      max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
                                               cell_limits.num_y_cells - 1) -
                                xy_index);
    }

这里体现的是这个节点所携带的点云，对节点长度搜索框 的 限制

首先，从之前carto的代码可以知道，栅格地图的坐标是以左上角为原点，向右为x，向下为y的。

那么我们把这一个旋转点云放入map中：

上图中，点云用一个点云的包络曲线简单代替一下。

回到上面的代码，min_bound = min_bound.min(-xy_index);

通过循环这一语句，可以找到右下角的矩形包络的角点 的 坐标的 相反数

不太好理解？看图：

右下角的矩形包络的角点即为橙色点

它决定了位姿搜索时，往负方向搜索的范围

如何理解呢？

初步设定了往负方向搜索的是7m，即-7m，但是有可能min_bound是-5m，这时候我们只需要朝着负方向搜5m就够了。

原因：

节点的初始位姿在这个点云曲线包络线内部均有可能，在外部就没可能了。所以如果min_bound是-5m，初设是-7m，这就说明初设的搜索框肯定会越界，会超过map的范围（limits）

如下图：

这样就不必搜索到7m了。之所以要用包络矩形的右下角角点，是为了考虑到节点初始位置在点云包络曲线的内部的任意位置都可能出现，可能出现在边界上。

回到上面的代码，

max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
                                               cell_limits.num_y_cells - 1) -
                                xy_index);

通过循环这一语句，可以找到矩形包络的左上角角点

注意其中num_x_cells和.num_y_cells是指map栅格地图的x、y方向的栅格数量

看图：

图中，红色点即为矩形包络的左上角角点，红色线表示
max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
cell_limits.num_y_cells - 1) - xy_index);

即找到向正方向搜索长度。前面说过，节点位姿肯定出现在点云包络曲线内部。而有可能会处于左上角那种极限位置。

这样设置的原因也是，不用搜索到map以外的区域。

总结

减少计算量的一种做法，尽力减小搜索框。如果初始搜索框在map范围内的话，那就按照初始搜索框的搜索。如果初始搜索框在map范围在map范围外的话，在范围外的就不用搜索。

也就是这四行代码的意义：

    linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x());
    linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x());
    linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y());
    linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y());

但是不是一个非常精确的收缩方法，因为你一旦节点位置不在边界处的话，可能也会有计算量的浪费。