cartographer的C++1.0的AddRangeData到submap和LBA过程

节点与submap

cartogrpher 在每一次添加节点的时候进行判断是否优化。cartogrpher 的全局优化算法在 sovle( 函数里面，在 PoseGraph2D::RunOptimization() 的判断 逻辑里面，调用堆栈到 AddNode 逻辑。

1. cartogrpher的AddNode与优化过程

2. range数据到submap的嵌入更新过程

cartographer并非使用3d或者视觉slam的经典poseLBA过程，而是通过网格优化来进行计算的。

range 数据添加过程： ros 端的数据添加，对接参考章节 1 的 Ros 端的数据添加过程。

Track 和定位： Track 过程参考章节 2 的 track 过程； Match ： real_time_correlative_scan_matcher _ . Match 和 ceres_scan_matcher _ 过程。

submap 更新过程：使用了 votex 优化 。

时间分析 ： Filte 至多 消耗 1 毫秒之上， 1-2ms ； InsertIntoSubmap 可以消耗 18ms 。

3.优化过程

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment)
{
  if (gravity_aligned_range_data.returns.empty()) {
..............................
  std::unique_ptr<InsertionResult> insertion_result =
      InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
                       pose_estimate, gravity_alignment.rotation());
........................

优化过程：

std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& rang..)
{
..................
 active_submaps_.InsertRangeData(range_data_in_local);
...................
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
....................}