AddAccumulatedRangeData

现在进入AddAccumulatedRangeData函数，返回类型为 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>

所用参数为

• const common::Time time     当前同步时间
• const sensor::RangeData& gravity_aligned_range_data     经过重力修正后的点云returns和miss
• const transform::Rigid3d& gravity_alignment,     重力向量
• const absl::optional<common::Duration>& sensor_duration

先看开头，我交换了部分代码顺序，并不影响。这里实际是获得scanMatch的两个参数

if (gravity_aligned_range_data.returns.empty())
{
  LOG(WARNING) << "Dropped empty horizontal range data.";
  return nullptr;
}
const sensor::PointCloud&  filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);

if (filtered_gravity_aligned_point_cloud.empty())
    return nullptr;

这一段就是流程图的Adaptive Voxel Filter环节，解释参考(九) Voxel Filter和Adaptive Voxel Filter

然后是估计位姿部分

const transform::Rigid3d  non_gravity_aligned_pose_prediction =
  extrapolator_->ExtrapolatePose(time);
  // 经过重力方向计算投影后的2d位置
const transform::Rigid2d   pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse() );

返回的pose_prediction和上面的filtered_gravity_aligned_point_cloud一起进入 Scan Mathching 环节，这个在流程图上很明显。

表面上只是重力对齐以后再处理，其实这里面还为后续的匹配与查找减小了查找范围，因为* gravity_alignment.inverse()这一乘使得与上一帧数据的角度变化缩小了（基本是同一角度），所以 为后面的ScanMatch减小了角度的查找范围。

在后面再把角度乘回来：

pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

transform::Rigid3d pose_estimate =
     transform::Embed3D(*pose_estimate_2d) * gravity_alignment;

这样就实现了角度的估计，gravity_alignment上下是同一数值，只是为了减少计算量，点云数据也按这个gravity_alignment做了处理

scan match

// local map frame <--- gravity-aligned frame
// 求 Scan 插入Submap的最优 Pose
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
    ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr)
{
  LOG(WARNING) << "Scan matching failed.";
  return nullptr;
}

这里的ScanMatch包含了real time correlative scan matcher 和 ceres scan match两部分，前者为后者提供好的初值

// 输出是对该帧传感器数据的最优pose
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time,  const transform::Rigid2d&  pose_prediction,
    const sensor::PointCloud&  filtered_gravity_aligned_point_cloud)
{
  // 如果还没有子图，返回 PoseExtrapolator 的预测值
  if (active_submaps_.submaps().empty() )
    return absl::make_unique<transform::Rigid2d>(pose_prediction);

  // 永远是头部的子图做匹配   SCAN-to-MAP
  std::shared_ptr<const Submap2D> matching_submap =  
      active_submaps_.submaps().front();

  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 函数最终更新 initial_ceres_pose，返回匹配的最高分数
  if (options_.use_online_correlative_scan_matching())
  {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction,   filtered_gravity_aligned_point_cloud,
        // Grid2D类型
        *matching_submap->grid(),  &initial_ceres_pose );
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }
  // ceres scan matcher 部分
}

Real Time Correlative Scan Matcher

这里借用了karto的做法，即本质是暴力匹配，而hector是用了高斯牛顿法和多分辨率地图。但是karto还是用了多分辨率地图，而cartographer这里没有。

它使用类似于在回环检测中激光与子地图匹配的方式，然后最好的匹配用作Ceres Scan Matcher的先验。这个scan matcher非常消耗资源，但它在特征丰富的环境中是很鲁棒。

我们要找到机器人位姿的后验概率分布$p(x_i|x_{i-1}\ ,u,m,z)$，使用贝叶斯公式得到

$$p(x_i\ |x_{i-1}\ ,u,m,z) \propto p(x_i\ |x_{i-1}\ , u)* p(z\ |x_i,m)$$

运动模型是已知的多元高斯分布， 算法只研究观测模型。观测模型难以计算，一般具有多极值。 雷达观测数据 $z_t$ 里包含了很多激光点数据，假设他们是完全独立的

$$p(z_t\ |x_t,m_{t-1}\ ) = \prod \limits_i p(z_t^i\ |x_t,m_{t-1}\ ) \propto \sum\limits_i log\ p(z_t^i\ |x_t, m_{t-1}\ )$$

将一帧观测数据的概率拆分成了当前帧的每一个点的概率，在计算这个概率之前，我们需要对数据帧以及地图进行栅格化。

位姿估计器传来的预测值不适合做初值，真正初值是在它附近。

实时相关性scan match 以预测值为中心，在搜索窗口内枚举出所有候选位姿，并生成每个位姿对应的点云，也就是说按照 $x_i$ 把当前观测 $z_i$ 投影到栅格地图 m ( t 时刻的激光数据与 t-1 时刻的地图匹配， scan to map)，然后分别计算每个点云中hit点在栅格地图的CellIndex，并计算所有hit点的占据概率之和，概率最大值对应的位姿就是我们需要的。代表着在这个位姿下，激光点得映射到占据栅格中的可能性最大，当前观测与已知环境最为一致，前后帧匹配成功。 scan match非常重要，如果结果位姿很差，建图就会失败，后端怎么调整也没用

搜索的精度是栅格地图的分辨率

这个scan matcher可以修改做重定位，不过搜索窗口扩大很多

1. 暴力搜索
   我们要计算 $p(z\ |x_i, m)$，这就需要一个三层for循环 $(x,y,\theta)$ 计算每一个位姿，还有第四个循环，也就是遍历点云中每个点。 计算量巨大，因为每一个位姿都要重新投影，需要计算sin和cos函数

2. 计算2D切片
   把三层循环交换顺序，最外层遍历 $\theta$，这样(x, y)的循环只有加法。

3. 多分辨率搜索
   首先选择一个相对低的分辨率对 map 和 scan 进行栅格化，每个栅格上保留高分辨率情况下的该区域的最高概率，这样在匹配时就不会错过正确的位姿估计
   低分辨率找到最佳匹配后再在该位置进行高分辨率的栅格化，并设定相对较小的 search window 进行计算。

// 更新 pose_estimate， 返回最高的分数
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d&  initial_pose_estimate,
    const sensor::PointCloud&  point_cloud, 
    const Grid2D&  grid,
    transform::Rigid2d*  pose_estimate  ) const
{
  CHECK(pose_estimate != nullptr);
  // 预测位姿的旋转量
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   /*  左乘 initial_rotation， 将当前点云转换为
  初始角度坐标系下点云，这里只有 旋转  */
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), 
          Eigen::Vector3f::UnitZ()  )   )
      );

  const SearchParameters search_parameters(
      options_.linear_search_window(),  options_.angular_search_window(),
      rotated_point_cloud,   grid.limits().resolution()  );
	// 获取搜索窗口下机器人朝向各个方向角时的点云数据

  // 先通过角度切片，然后在xy方向上暴力搜索，为了减少计算量
  // 切片数目 num_scans , 按照搜索角度/步长，以初始值为基础，生成一些列的旋转后的点云
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

     // DiscreteScan2D 就是 std::vector<Eigen::Array2i>
     /* 把切片后的旋转点云坐标，左乘initial_pose_estimate.translation
       转化到地图坐标，这里只有 平移
    最终通过GetCellIndex返回一系列的  激光点return所在的cell index*/
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(),   rotated_scans,
      Eigen::Translation2f( initial_pose_estimate.translation().x(),
      initial_pose_estimate.translation().y()  )
      );
   /*  获得candidates, size是 search_parameters.num_scans * 
  (linear_bounds.max_x - inear_bounds.min_x) * (linear_bounds.max_y - inear_bounds.min_y)*/

    /*  这里是num_scans循环，x和y循环，没有 theta循环了
  成员是 scan_index, x_index_offset(逐步增加), y_index_offset(逐步增加), 
  search_parameters意思其实是 每个激光点附近的矩形区域*/
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

  // 计算当前帧在theta、x、y三层窗口中，每个状态在grid地图中的匹配的评分
  // search_parameters 实际没用
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
  // 取最好的候选
  const Candidate2D&   best_candidate =
      *std::max_element(candidates.begin(), candidates.end());

  // 最后的结果是预测值加候选位姿
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y },
       // 前端观测，所以右乘
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation) );

  return best_candidate.score;
}

1. 获取位姿估计器传来的初值的旋转分量，将激光点云左乘旋转分量，即先做旋转变换
2. 根据搜索窗口x、y、yaw的大小计算 num_scans和LinearBounds， x y搜索的num_linear_perturbations
3. 切片旋转：GenerateRotatedScans函数，其实就是theta循环，将角度进行序列化(初始角度跟search_window的大小相关，按步长增加)，点云依次左乘角度对应的旋转向量(旋转轴为z轴)，即可生成num_scans段点云
4. 把点云中所有点做变换：左乘初值的平移部分，根据变换后的坐标，找出它在栅格地图中的CellIndex并保存
5. 根据搜索范围，生成候选者，数量是25*num_scans

• 根据保存的索引队列，从栅格地图中获取最大置信度，这就用到了ProbabilityGrid类，根据每个点的CellIndex获取占据概率，最终对分数进行加权处理
• 求得每个候选位置的score，score最高的为最佳位置。

候选者是针对scan_index (0~num_scans)所做的x y方向的offset，生成所有候选者的过程其实就是num_scans和x y三个循环

Match 函数的计算量很大 。 试验发现最终的分数，大部分在0.6~0.9

SearchParameters

根据lua参数，确定后面的切片步长angular_perturbation_step_size，切片数num_scans，搜索区域

SearchParameters::SearchParameters(
               const double linear_search_window,
               const double angular_search_window,
               const sensor::PointCloud& point_cloud,
               const double resolution)
    : resolution(resolution)
{
  // We set this value to something on the order of 
  // resolution to make sure that the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }
  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  // 这两者的计算不同
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  // 切片数目，由于resolution不修改，实际取决于 max_scan_range 和 angular_search_window
  num_scans = 2 * num_angular_perturbations + 1;
  /*  Linear search window in pixel offsets; bounds are inclusive.
  struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };
  std::vector<LinearBounds> linear_bounds;   Per rotated scans  */
  linear_bounds.reserve(num_scans);
  // 对同一帧scan, linear_bounds的元素都相同，不同帧就不同了
  for (int i = 0; i != num_scans; ++i) {
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations} );
  }
}

GenerateRotatedScans 切片旋转

生成序列化旋转的多段点云

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud&  point_cloud,
    const SearchParameters&  search_parameters)
{
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);
  // 旋转角度步长
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  //  个数 num_scans
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
       delta_theta += search_parameters.angular_perturbation_step_size)
  {
    // 左乘旋转向量，旋转轴为z轴
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()) ) )  );
  }
  return rotated_scans;
}

ScoreCandidates

void ScoreCandidates(
    const Grid2D& grid,  const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters&  search_parameters,
    std::vector<Candidate2D>*  const candidates) const
{
  for (Candidate2D& candidate : *candidates)
  {
    candidate.score = ComputeCandidateScore(
        static_cast<const ProbabilityGrid&>(grid),
        discrete_scans[candidate.scan_index],
        candidate.x_index_offset,
        candidate.y_index_offset);
    break;
      // 省略 GridType::TSDF
      // 进一步做权重处理
    candidate.score *=
        std::exp(
          -common::Pow2(
            std::hypot(candidate.x, candidate.y) *
            options_.translation_delta_cost_weight() 
                        +
            std::abs(candidate.orientation) *
            options_.rotation_delta_cost_weight()
                       )
          );
  }
}


// 其中用到的函数， 计算每个候选的得分
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
             const DiscreteScan2D&  discrete_scan,
             int x_index_offset,  int y_index_offset)
{
  float candidate_score = 0.f;
  // 这里就用到前面获取的x y 索引队列
  for (const Eigen::Array2i& xy_index : discrete_scan)
  {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset  );
    // Returns probability of the cell with  'cell_index'
    // 这个就是占据概率
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  // 返回的是一帧点云所有激光点得分的平均值
  candidate_score /= static_cast<float>(discrete_scan.size());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

参考： RealTimeCorrelativeScanMatcher2D