【移动机器人技术】cartographer算法流程分析

0 scan数据订阅与处理

在src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc文件的void SensorBridge::HandleLaserScanMessage()函数中，顺序调用了ToPointCloudWithIntensities(),HandleLaserScan()两个函数。

激光雷达 LaserScan 消息类型转换为 point_cloud

std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
在这个函数中，主要进行了下面的操作：

主要运算语句的注解，省略了一些。
float angle = msg.angle_min; // 第1束激光的角度
...
const float first_echo = GetFirstEcho(echoes); // 第i束激光的测障距离；
...
if (msg.range_min <= first_echo && first_echo <= msg.range_max) { // 根据距离判断该束激光的有效性；
...
point << rotation * (first_echo * Eigen::Vector3f::UnitX()), // 激光极坐标变笛卡尔坐标，x轴旋转至xy轴；
            i * msg.time_increment;  // 时间增量 dt = i * msg.time_increment;
...
angle += msg.angle_increment; // 角度随着激光束累加
...
::cartographer::common::Time timestamp = FromRos(msg.header.stamp); // 起始时间戳
  if (!point_cloud.points.empty()) {
    const double duration = point_cloud.points.back()[3];   // 尾部时间增量
    timestamp += cartographer::common::FromSeconds(duration); // 时间戳以尾部为基准
    for (Eigen::Vector4f& point : point_cloud.points) {
      point[3] -= duration; // 时间前移，以最后一束激光时间为基准
    }
  }

处理新得到的 point_cloud 消息

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
将点云细分成若干份，逐份处理，降低畸变程度。

  // 切分一帧激光点云，逐份处理.
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
  ...
    // 切分后的子集点云帧    
   carto::sensor::TimedPointCloud subdivision(
       points.points.begin() + start_index, points.points.begin() + end_index);
...
// 子点云帧的时间戳
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end); 
...
if (it != sensor_to_previous_subdivision_time_.end() &&
        it->second >= subdivision_time) {...}
...
// 处理子集点云帧
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);        

关于void SensorBridge::HandleRangefinder()函数

该函数实现点云封装，并调用了轨迹创建函数模块，与后面的轨迹创建连接在一起。

封装数据接口，调用轨迹创建模块的函数；
if (sensor_to_tracking != nullptr) {
    // 添加传感器数据
    trajectory_builder_->AddSensorData(
        sensor_id, 
        carto::sensor::TimedPointCloudData{
              time, 
              // 雷达的基坐标；
              sensor_to_tracking->translation().cast<float>(),
              // 点云相对于基坐标的值；
              carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())
              }
        );
  }

1、全局轨迹中添加激光雷达点云

在global_trajectory_builder.cc文件中：

void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data)

1.1 局部轨迹构建中添加激光雷达点云

向 local_trajectory_builder_ 中添加点云数据，执行函数AddRangeData：

std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
       matching_result = local_trajectory_builder_->AddRangeData(
           sensor_id, timed_point_cloud_data);

函数中的详细流程如下：

1.1.1 激光点云数据的预处理

激光点云数据的同步处理

将unsynchronized_data 转变 为synchronized_data；

更新激光点云数据序列赋值位姿

位姿根据时间，从ExtrapolatePose中查找（插补）；
时间time_point = 基准时间time + 点云时间range.point_time；

 range_data_poses.push_back(
 			extrapolator_->ExtrapolatePose(time_point).cast<float>());

激光点云数据的范围处理

小于min_range ，丢弃；
介于min_range与max_range之间，accumulated_range_data_.returns.push_back(hit_in_local);
大于max_range，accumulated_range_data_.misses.push_back(坐标处理至missing_data_ray_length);

以下是做了去畸变的处理吗？
// 根据每个点的时间戳，计算其原点坐标；
    // kaikai.gao：两个相乘是什么含义？
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

1.1.2 添加点云数据

点云数据累积到一定数量后，构成一个最小单位，满足添加条件，执行函数：
AddAccumulatedRangeData

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

1.2.1.1 CSM匹配

确定初始位姿

从extrapolator_中提取预测位姿，并在校准重力；

CSM-ScanMatch

pose_estimate_2d =
      ScanMatch(time, pose_prediction, gravity_aligned_range_data);

• 取活跃状态的第一个子地图作为匹配对象；
• 自适应体素滤波器 adaptive_voxel_filter 滤波处理；
• 选择性的启动 online_correlative_scan_matching 功能：性能高，但消耗大；
• CSM匹配算法：基于指定的初始位姿进行匹配：
  构建优化问题；
  算法优化的误差项有3个：点云与栅格地图的匹配误差、位姿平移预测的误差、位姿旋转预测的误差；
  求解优化问题；
  将优化结果转化成位姿估计pose_estimate_2d；
• 成功匹配后，做什么处理？向 kCeresScanMatcherCostMetric，kScanMatcherResidualDistanceMetric， kScanMatcherResidualAngleMetric 中添加误差项；这些有什么用？

位姿维护更新

向extrapolator_设定位姿；
插入点云前匹配位姿，用匹配结果更新位姿维护；即使由于低速移动过滤掉点云，此点云的定位结果也已经被应用；

插入活跃的子地图中

• 运动滤波-滤除静止下的数据；
  motion_filter_.IsSimilar(time, pose_estimate)
• 向活跃的子地图中插入一帧匹配好位姿的点云数据；
  active_submaps_.InsertRangeData(range_data_in_local);
• 体素滤波器
  adaptive_voxel_filter.Filter()
• 返回插入结果

	return common::make_unique<InsertionResult>(InsertionResult{
      std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate}),
      std::move(insertion_submaps)});

延时

auto duration = std::chrono::steady_clock::now() - accumulation_started_;
  kLocalSlamLatencyMetric->Set(
      std::chrono::duration_cast<std::chrono::seconds>(duration).count());

返回结果

return common::make_unique<MatchingResult>(
      MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                     std::move(insertion_result)});

1.2 在pose_graph中增加node

如果1.1步骤返回结果有效，则继续运行1.2，否则返回；
kLocalSlamMatchingResults累计；

1.2.1 点云插入有效时增加node

条件matching_result->insertion_result != nullptr，执行：
kLocalSlamInsertionResults累计；
增加Node:

// 子地图中每插入一组数据帧，都需要增加一个Node吗？
// 答：每一组点云数据固定在一个node上面；
// Node又与trajectory_id_是什么关系？
// 一个trajectory_id下有很多submap，一个submap下有很多node；
auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);
      CHECK_EQ(node_id.trajectory_id, trajectory_id_);

制作node插入结果：

std::unique_ptr<InsertionResult> insertion_result;
insertion_result = common::make_unique<InsertionResult>(InsertionResult{
          node_id, matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});

执行回调函数

if (local_slam_result_callback_) {
      local_slam_result_callback_(
          trajectory_id_, matching_result->time, matching_result->local_pose,
          std::move(matching_result->range_data_in_local),
          std::move(insertion_result));
    }

回调函数执行了如下函数，在map_builder_bridge.cc中：

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::
                              TrajectoryBuilderInterface::InsertionResult>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

1.2.2 AddNode详细

在pose_graph_2d.cc文件中定义了函数NodeId PoseGraph2D::AddNode，实现如下：

• 计算global_pose；
• trajectory_nodes_.Append()
  // 包含数据：1、隶属轨迹；
  // 2、约束数据：time, g, point_cloud, pose_in_local；
  // 3、node位姿: pose_in_global；
• submap_data_.Append()
  // 当尾部地图发生新增时（指针不同），将其映射到 submap_data_ 中；
• 为新增node增加约束

1.2.2.1 为新增node增加约束

函数调用，在文件pose_graph_2d.cc中，

ComputeConstraintsForNode(node_id, insertion_submaps,
                              newly_finished_submap)；

函数定义，在文件pose_graph_2d.cc中，

InitializeGlobalSubmapPoses函数

• 新建轨迹时，创建初始位姿，增加第一个submap；
• 新增submap时，在轨迹优化中新增submap；
• 没有新增submap时，维持当前数据；

获取第一个活动的submap(倒数第2个活动的submap)；

计算local_pose_2d、global_pose_2d

向优化问题中增加节点

对于活跃的地图，新增node_id，并在约束中加入该node的约束；

• 对于所有活跃状态(正在插入点云)的submap执行以下操作：
  在submap的node列表尾部添加一个node_id；

submap_data_.at(submap_id).node_ids.emplace(node_id);

增加内部约束：

constraints_.push_back(Constraint{submap_id,
                                      node_id,
                                      {transform::Embed3D(constraint_transform),
                                       options_.matcher_translation_weight(),
                                       options_.matcher_rotation_weight()},
                                      Constraint::INTRA_SUBMAP}); // 内部约束

问题：下面的约束物理含义是什么？
constraints::ComputeSubmapPose(*insertion_submaps[i])含义，submap在local map中的位姿；
local_pose_2d含义，node在local SLAM frame中的位姿；
根据两者，计算出了node在submap中的位姿；

constraint_transform =
        constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
        local_pose_2d;

对于所有已经完成的submaps执行以下操作：

循环所有处于完成状态的submap与当前node匹配
ComputeConstraint(node_id, submap_id_data.id);
函数内部实现：
常规约束添加条件：如果 node_id 与 submap_id 同属一个 trajectory_id；或，延时足够长，需要回环约束；
初始相对位姿：node在指定submap坐标系的相对位姿， initial_relative_pose；

• 局部匹配，仅匹配 node 与周围的 submap, 如果指定 node 与 指定 submap 距离大于设定阈值，返回；
• 计数没达到采样率，返回；部分node参与约束计算；
• 将计算约束任务添加到任务队列

constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {    
    ComputeConstraint(submap_id, submap, node_id, false, /* don't match_full_submap */
                      constant_data, initial_relative_pose, *scan_matcher,
                      constraint);

新完成一个submap做如下处理

循环所有优化的node与新完成的submap匹配
ComputeConstraintsForOldNodes(finished_submap_id);

• 循环所有优化的node与新完成的submap匹配；
• constraint_builder_.MaybeAddConstraint();

达到一定数量的新增nodes后，进行一次图优化

DispatchOptimization();

如果没有创建work_queue_，则先创建队列，再执行回环检测的任务；
此处仅进入一次，后面就依靠run_loop_closure_标志，在HandleWorkQueue()函数中循环执行；
进入PoseGraph2D::HandleWorkQueue()
进入RunOptimization();
进入optimization_problem_->Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
增加冻结状态frozen的submap_data_信息；
增加冻结状态frozen的node_data_信息；
增加内部与之间的约束constraints；

附：重要数据

在点云插入子图生成node后的约束数据

constant_data
{
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;

    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };