cartographer LocalTrajectoryBuilder2D

1. 所处：

LocalTrajectoryBuilder2D cartographer/mapping/internal/2d/local_trajectory_build_2d.cc /.h中

LocalTrajectoryBuilder2D 类在 map_build.cc 中  AddTrajectoryBuilder 函数中被构造

2. 主要函数：

函数一、AddRangeData

  std::unique_ptr<MatchingResult> AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& range_data);

具体分析：

1.同步不同传感器的点云数据，得到 较好的点云数据   该点云数据的坐标为 tracking

 range_data_collator_.AddRangeData(sensor_id, unsynchronized_data)

2. 不是用IMU时 初始化外推器

3.将点云数据带上时间，同时将点云数据用重力修正。

4.将点云数据按照 配置的范围选取 最终获得 重力对齐范围数据 

5.调用下面函数 AddAccumulatedRangeData函数

函数二、AddAccumulatedRangeData

  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
      common::Time time, const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& gravity_alignment);

1. 姿态预测（transform::Rigid2d ） 非重力对齐姿势预测 * 重力对齐的逆（gravity_alignment.inverse()）

  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

2.  对激光雷达数据进行滤波 & 转换成点云数据  这里的点云数据是在平面机器人坐标系中

  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);

3.调用匹配函数 ScanMatch 函数，激光匹配后得到较准确的 pose_estimate的位姿。（匹配是跟活跃子图匹配）

4.pose_estimate转化为 Rigid3d  反重力对齐。

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

5.将激光数据转化为 local_trajectory 坐标系下

  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));

6.插入子图，InsertIntoSubmap 函数。插入子图的激光数据的坐标系为local_trajectory

  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

7.返回匹配结果，MatchingResult结构体。

函数三、ScanMatch

  std::unique_ptr<transform::Rigid2d> ScanMatch(
      common::Time time, const transform::Rigid2d& pose_prediction,
      const sensor::RangeData& gravity_aligned_range_data);

1.活跃子图为空时，返回当前估计位姿的指针 

  if (active_submaps_.submaps().empty()) {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }

2.匹配的子图为当前活跃子图的第一个子图

3. 配置文件中是否需要用csm来优化ceres-scan-match的初始解

    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *static_cast<const ProbabilityGrid*>(matching_submap->grid()),
        &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);

 通过csm和滤波器过后的2d平面的　激光雷达数据来进行位姿优化传入预测的初始位姿＼激光雷达数据＼栅格地图
 返回一个更好的值initial_ceres_pose

4.  最终通过ceres_scan_match来得到最终的位姿   

  ceres::Solver::Summary summary;
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);

5.如果扫描获得位姿，则跟新当前时刻的状态，

  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }

6.返回 pose_observation

函数四、InsertIntoSubmap

  std::unique_ptr<InsertionResult> InsertIntoSubmap(
      common::Time time, const sensor::RangeData& range_data_in_local,
      const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& pose_estimate,
      const Eigen::Quaterniond& gravity_alignment);

1.如果累积的运动（线性，旋转或时间）高于 threshold(阈值)，返回false。 否则，相对运动被累积并且返回true。

  if (num_total_ > 1 &&
      time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) &&
      (pose.translation() - last_pose_.translation()).norm() <=
          options_.max_distance_meters() &&
      transform::GetAngle(pose.inverse() * last_pose_) <=
          options_.max_angle_radians()) {
    return true;
  }

2.如果是true  return nullptr;

  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }

3.在调用InsertRangeData（range_data_in_local）之前必须在此处查询活动子图，因为查询的值对下次插入有效。

4.过滤重力对齐点云数据。

5.返回  InsertionResult 类指针。

 

3. cartographer 前端  相关性扫描匹配  和  ceres 优化两种

1.相关性扫描匹配：

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate)

1.1 输入 ： 

1. 预计位姿（推算位子）    
2. 点云数据 
3. 概率栅格地图   
4. 估计位姿（引用输出）

返回：   最佳得分

1.2 步骤：

1. // 初始位姿的角度
2. /将激光转到初始位姿的角度  (x,y)0 点云开始为：baselink坐标系
3. //确定查询窗口
4. //得到旋转的点云     查询角度范围 ×2 /角度分辨率    点云 对各个角度旋转
5. //将点云转到当前位姿下  平移
6. /生成平移的候选值， 只记录 x,y的偏移，未进行 实际点云平移
7. //求取得分  遍历候选值，  再遍历激光点，遍历激光点时将xy的偏移量传入
8.  //取得分最大的候选值的 偏移量
9. //输入的初始位子 + 偏移量

2.ceres 优化：

void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                               const transform::Rigid2d& initial_pose_estimate,
                               const sensor::PointCloud& point_cloud,
                               const Grid2D& grid,
                               transform::Rigid2d* const pose_estimate,
                               ceres::Solver::Summary* const summary) 

2.1 输入 ：   

1. 预计位姿（推算位子）   
2. scanMatch匹配后位姿
3. 点云数据 
4. 概率栅格地图
5. 优化后位姿（输出引用）
6. ceres 优化器

2.2 步骤：

1. 构建观测残差
2. 构建平移变化量残差
3. 构建旋转变化量残差

3.输入数据的坐标关系

                     

返回：   最佳得分

预计位姿    点云数据  在重力坐标系下