xiaoma_bk

cartographer LocalTrajectoryBuilder2D

1. 所处：

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

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

2. 主要函数：

函数一、AddRangeData

  std::unique_ptr 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 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()));

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

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

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

函数三、ScanMatch

  std::unique_ptr 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(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(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 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 步骤：

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

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 输入：

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

2.2 步骤：

构建观测残差
构建平移变化量残差
构建旋转变化量残差

3.输入数据的坐标关系