cartographer_learn_14点云写入地图

续接上篇

第13篇，点云匹配
回忆一下我们上一篇得到了什么结果，上一篇讨论到了使用ceres求解点云与地图匹配的问题，得到的结果是tracking坐标系再local坐标系下的位姿乘以一个旋转矩阵（imu数据计算的tracking坐标系的姿态）的逆。想必它还要恢复原本tracking坐标系的位姿，然后将点云转换到local坐标系下，最后把点云写入地图。我们来具体看看操作。

写入地图前的剩余操作

回到上一篇“匹配前剩余的一些操作”这一节中的LocalTrajectoryBuilder2D::AddAccumulatedRangeData这个方法，我们继续来看这个函数再匹配过后都做了一些什么其他的操作。

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {
    .......//预测tracking坐标系的位姿
    .......//给出匹配的初值
    .......//自适应体素滤波
    .......//点云的匹配
    if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }
  //恢复tracking坐标系真正的位姿
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  //第12篇“点云的处理（匹配前含位姿推断器）”这一节中提到的功能6
  extrapolator_->AddPose(time, pose_estimate);
  //将点云转换到local坐标系下，为点云写入地图做准备
  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));
  ......//一些其他操作
}

如我们所想，确实是先恢复tracking坐标系的位姿，把tracking坐标系的位姿加入位姿推断器中为下一次的匹配过程中的点云处理和预测tracking的位姿做准备。同时还把点云转换到了local坐标系下。同时提一嘴，之前我们说misses没有用，那是在匹配过程中，作者今天看源码貌似发现它在将点云写入地图时貌似是有用的，我们讨论到那是再提提。但是在此之前如果读者还不太理解测距仪的模型，即什么是return，什么是miss，还是建议去学习一下《概率机器人》中的第六章。

栅格地图的更新中理论和实现的差别

相信如果读者学习《概率机器人》的第9章就知道，根据观测结果更新每个栅格的值只需要执行一次加法（而且加的那个数是固定的），但是代码中并不是这样实现的，代码中选择使用空间换时间的策略。如果任意某个栅格其初值一定，且知道它在本次更新过程中它对应的究竟是return还是miss，那在我们就已经知道了它更新后的值是多少了。换句话说我们可以分别对应return和miss这两种情况做两个表，索引对应的是栅格的初值，值对应的是更新后的值。这就是cartographer中实现的思路。但是可能的谷歌的工程师觉得这样做还是有点浪费空间，能不能不记录初值呢？能，其实栅格地图中记录的不是0-1的概率值，大神们把0.1到0.9分成32768份，用0-32768去记录对应的0.1-0.9的概率值。发现了，初值（索引）居然是整数，那天然含有整数索引的是哪种STL容器？没错就是vector，vector中记录的内容是对应要更新的值。
所以总结一下大神们是怎么做的，建立两个表，一个叫returntable，一个叫misstable（程序中不是真的是这个名字。。。），如果某个栅格的初值a（整数），且本次测量发现它对应return这种情况，那么该栅格更新的值就是returntalbe[a]。
好了接下来我们具体去看代码。

点云写入地图

回到上一节中的LocalTrajectoryBuilder2D::AddAccumulatedRangeData这个方法：

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {
    .......//预测tracking坐标系的位姿
    .......//给出匹配的初值
    .......//自适应体素滤波
    .......//点云的匹配
    .......//恢复tracking坐标系的位姿，并把它加入位姿推断器，为下次点云处理和匹配做准备
    .......//将点云转换到local坐标系下
    // 点云写入submap
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());
  .......//一些其他操作
}

我们接着进入InsertIntoSubmap里面看看：

std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& range_data_in_local,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment) {
  // 很早之前提过的运动滤波器，时间间隔太小，或者运动幅度不够大拒绝把数据写入submap
  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }
  // 将点云数据写入到submap中
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
   .......//一些其他操作
}

为了集中我们注意里讨论如何使用点云更新submap，我们这里跳过运动滤波器，后面有时间作者再详细介绍一下它。代码把我们带到了ActiveSubmaps2D::InsertRangeData中，我们去瞧瞧：

std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
 //检查要不要加入新的submap
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 将点云写入两个submap中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  //和本函数第一句操作联合在一起理解，看是不是旧的submap已经完成了更新
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

这里先不讨论如何将数据写入submap中，不知道读者是否还记得作者曾经提过再一个ActiveSubmaps2D中一般都会存储两个submap这件事情（第13篇）。那如何判断一个submap是否更新完成的呢？每次往submap中插入数据时，submap会自己纪律已经插入了多少帧数据，当这个帧数达到一定的数量（即代码中的2 * options_.num_range_data()）就认为完成了更新。第一个if判断第一个条件容易理解，没有submap当然要添加submap，第二个条件是，当第二个submap（新）数量达到options_.num_range_data()就意味着，第一个submap插入的数量已经达到了2 * options_.num_range_data()，这个时候当然要加入新的submap了。
好了我们继续跟踪将点云写入submap中的代码往前走，即Submap2D::InsertRangeData。于此同时我们看到它的第二个参数的类型又是一个指针指向一个叫***Interface的对象，想必这又是我们前面多次提到的基类指向派生的把戏，作者去瞧了瞧，由于我们之前介绍匹配时介绍的时PROBABILITY_GRID这种地图格式，为了保证前后的一致性，介绍插入者这个类时也介绍与这种地图格式对应的类型——ProbabilityGridRangeDataInserter2D

void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
  CHECK(grid_);
  CHECK(!insertion_finished());
  // 将点云到栅格地图中
  range_data_inserter->Insert(range_data, grid_.get());
  // 前面提过的插入的帧数加1
  set_num_range_data(num_range_data() + 1);
}

又把我们带到了ProbabilityGridRangeDataInserter2D::Insert中，去看看。同时注意第二个参数。

void ProbabilityGridRangeDataInserter2D::Insert(
    const sensor::RangeData& range_data, GridInterface* const grid) const {
  //恢复这个指针指向的类型
  ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
  CHECK(probability_grid != nullptr);
  // By not finishing the update after hits are inserted, we give hits priority
  // (i.e. no hits will be ignored because of a miss in the same cell).
  //正真做事的函数
  CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
           probability_grid);
  probability_grid->FinishUpdate();
}

去看看CastRays这个函数，注意看看第二第三个参数，它们就是我们之前说过的returntable和misstable

void CastRays(const sensor::RangeData& range_data,
              const std::vector<uint16>& hit_table,
              const std::vector<uint16>& miss_table,
              const bool insert_free_space, ProbabilityGrid* probability_grid) {
  //这个函数的作用就是找出点云中两个最边缘的点，然后我们上一篇“地图介绍”这一节提过的Grid2D::GrowLimits
  //作用的拓展地图
  GrowAsNeeded(range_data, probability_grid);
  
  const MapLimits& limits = probability_grid->limits();
  //计算高分辨率地图的信息，注意这里只是信息而已，并没有生成高分辨率地图
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  //在高分辨率地图中计算点云起点对应高分辨率地图中的哪个栅格
  const Eigen::Array2i begin =
      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
  // Compute and add the end points.
  std::vector<Eigen::Array2i> ends;
  ends.reserve(range_data.returns.size());
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
  	//在高分辨率地图中计算该点对应的栅格，并把记录下高分辨率下的地图，下面有用
    ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
    //将高分辨的栅格恢复到低分辨率的地图的栅格中，并更新栅格地图
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
  }

  if (!insert_free_space) {
    return;
  }

  // Now add the misses，和return类似思路
  for (const Eigen::Array2i& end : ends) {
  //原点和return之间的栅格对应着misses状态，把它们都是哪些栅格都计算出来
    std::vector<Eigen::Array2i> ray =
        RayToPixelMask(begin, end, kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }

  // Finally, compute and add empty rays based on misses in the range data.
  for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
    std::vector<Eigen::Array2i> ray = RayToPixelMask(
        begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
        kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }
}

这个函数的功能就是要把哪些栅格对应着return的状态，哪些栅格对应着misses状态统统都给它们计算下来（若不明白什么时return和misses可学习《概率机器人》第6章），并把它们做对应的处理。注意这里有两种种类不一样的misses，一种是原点和return之间的栅格，还有一种就是原点到之前超过最大测量距离的测量点之间的栅格。还有需要特别指出的一点是这个函数——RayToPixelMask，点A和点B之间的点和容易求，但是我们是栅格不是数学意义上的点，那怎么求栅格A和栅格B之间的栅格呢？就是这个函数实现的，据说使用的是breshman算法，大家可以百度，作者也没学习。想必为什么要搞一个高分辨率的地图，也是因为这个算法吧（猜的）。好了下一个重点要看的函数时ApplyLookupTable：

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  //将栅格坐标转换成vector中对应的下标
  const int flat_index = ToFlatIndex(cell_index);
  //取出它的值
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  //这里下面解释，kUpdateMarker=32768
  if (*cell >= kUpdateMarker) {
    return false;
  }
  //记录这个栅格我们已经知道了
  mutable_update_indices()->push_back(flat_index);
  //更新
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  mutable_known_cells_box()->extend(cell_index.matrix());
  return true;
}

这里请读者结合着本篇的“栅格地图的更新中理论和实现的差别”这一节看。还记得32768这个数字吗？它是2的15次方而已，但是我们看到每个栅格的值都是uint16这种类型，那为什么不使用2的16次方65536呢？作者的通过看它初始化时候的代码（想不起来那一块了。。。）理解是这样的，它总是把一个1往右移15位之后再加上栅格本身自己的值，这就使得栅格中记录的值很大，如果栅格更新过一次后，栅格的值就会大于kUpdateMarker了，这样栅格在一次数据的更新中就不会被更新第二次。说到这，作者想提醒读者回头查看一下上面ProbabilityGridRangeDataInserter2D::Insert这个函数，我们看到它在更新完之后会执行一个probability_grid->FinishUpdate()的操作，其实就是减掉这个很大的值。
基本上二维slam的前端就是这样了，接下来作者会先去看前端怎么把自己的结果传递给后端的，讨论完这个后再去看看后端。当然如果有时间的话会补上运动滤波器和breshman