cartographer 代码思想解读（10）- slam前端LocalTrajectoryBuilder2D类（主流程）

前面9节描述和分析了cartographer在slam前端使用的算法、原理以及具体实现。每个知识点都是任何slam中几乎必不可少的，也是slam技术的关键点。而slam则是将每个算法串联起来，构建slam的前端处理流程，从而实现slam（暂时缺少后端闭环处理）。而LocalTrajectoryBuilder2D类即为cartographer的前端核心流程和调用接口，即local slam。完成了传感器预处理、scanmatch、位置估计、submap构建及更新调用。其代码目录：
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

local_trajectory_builder_2d处理流程

引入官方的总体流程图

其中红色框圈起来的则是local_trajectory_builder_2d所处理的所有内容和流程，其输入仅为激光数据（点云）、odometry，Imu DATA。而经过一系列处理后输出结果为cartographer定义的InsertionResult结果类，包含了时间戳、当前机器人位置、submap及在世界坐标的位置、激光点云结合等。即经过前端处理可以得到一系列的submap，如此后续其他模块可将submap进行后端处理拼接形成整个地图。
整体处理流程如下：
1.激光原始数据预处理融合转换成cartographer使用的点云格式；
2.激光点云数据经过两次滤波后进行真正使用；
3.odometry、imu及其匹配后的精确位置构建位置估计器实时估计下刻位置；
4.根据预估位置和点云数据进行scan match获取优化后更精确的位置；
5.经过运动滤波器（目的是降采样）后的位置及其对应的点云进行维护和更新submap；
6.当submap满足一定数量的激光帧时，输出MatchingResult类型结果；

注意：
1.其中Fixed Frame Pose在2DSLAM中暂未找到传入接口。
2.输出的结果类型应该是MatchingResult，而不是图中的InsertionResult

local_trajectory_builder_2d类定义

类含有变量

  const proto::LocalTrajectoryBuilderOptions2D options_;    // 轨迹跟踪器的配置信息
  ActiveSubmaps2D active_submaps_;                          // 维护正在使用的submap,已分析过，包括栅格地图类型，插入，更新

  MotionFilter motion_filter_;                              // 运动滤波器，用来对运动pose进行降采样
  scan_matching::RealTimeCorrelativeScanMatcher2D           // 相关匹配器，已分析过
      real_time_correlative_scan_matcher_;
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;    // 优化匹配器，已分析过

  std::unique_ptr<PoseExtrapolator> extrapolator_;          // 位置估计器，可根据历史轨迹及其传感器估计下时刻位置

  int num_accumulated_ = 0;                                 // 累计激光点云个数
  sensor::RangeData accumulated_range_data_;                // 预处理后激光点云，也包含激光原点origin坐标，即经过融合，矫正等操作
                                                            // 也是核心算法使用的数据结构，用于map 更新和匹配
  absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
  absl::optional<double> last_thread_cpu_time_seconds_;
  absl::optional<common::Time> last_sensor_time_;

  RangeDataCollator range_data_collator_;                   // 激光数据收集器，几种激光按照时间戳融合

从类成员变量可看出包括
1.submap封装维护类ActiveSubmaps2D
2.相关scan-match类RealTimeCorrelativeScanMatcher2D
3.ceres库匹配类CeresScanMatcher2D
4.位置估计器类PoseExtrapolator
5.激光点云预处理后结果类sensor::RangeData
6.多激光数据融合同步类RangeDataCollator
7.运动滤波器类MotionFilter（较为简单，分析流程会插入分析）
以上除第7项外，均已分析。

输出结果MatchingResult结构体

  // 用于存储submap列表数据的结构 
  // 用于存储submap列表数据的结构 
  struct InsertionResult {
    std::shared_ptr<const TrajectoryNode::Data> constant_data;       // 匹配后的点云结果
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;  // 更新后的submap列表（仅最新的submap）
  };
  // 用于前端匹配的数据结构
  struct MatchingResult {
    common::Time time;                                               // 时间戳
    transform::Rigid3d local_pose;                                   // 在submap中位置
    sensor::RangeData range_data_in_local;                           // 本帧的点云
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;         // 匹配后的submap结果
  };

其中rajectoryNode::Data是后续用于记录和拼接的rajectoryNode定义的结构体，主要记录的是submap对应的属性信息包括位置，重力加速度方向，修正后的水平点云数据等；为2d和3dSLAM公共结构体类型。其代码目录在
cartographer/mapping/trajectory_node.h

// 轨迹节点结构体
struct TrajectoryNode {
  // 轨迹节点中传感器数据结构体
  struct 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.
    // 水平方向上投影后的对应点云，若无imu可认为一直在水平面
    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.
    // 节点在local submap中的位置
    transform::Rigid3d local_pose;
  };

  common::Time time() const { return constant_data->time; }

  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;

  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;
};

结构体MatchingResult记录了扫描匹配发生的时间(time)、在局部地图坐标系下的位姿(local_pose)、局部的扫描数据(range_data_in_local)、以及子图插入结果(insertion_result)。

Odometry输入接口AddOdometryData

// 推算器添加里程计数据
void LocalTrajectoryBuilder2D::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator we cannot add odometry data.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return;
  }
  extrapolator_->AddOdometryData(odometry_data);
}

显然和结构图一致，仅作为位置估计器的输入，用于位置估计，不直接参与核心算法流程，其插入过程独立；

IMU输入接口AddImuData

// 如果有IMU数据则添加imu时初始化推算器
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
  InitializeExtrapolator(imu_data.time);
  extrapolator_->AddImuData(imu_data);
}

与odometry输入接口一致，仅作为位置估计器的输入。由于每种传感器的频率不一致，因此和主处理流程完全独立。

点云输入接口AddRangeData（核心处理方法）

AddRangeData为激光点云输入接口，也是每帧处理的核心流程，包含了结构图中的local slam虚框中的处理过程，即使没有odometer和imu两个接口，local slam依然可以正常运行。其具体处理步骤如下：

1. 多激光传感器数据基于时间戳同步融合
2. 开启位置估计器
3. 根据位置估计器，针对每个点云point的时间戳进行预测位置进行畸变校准
4. 校准后的点云转换成scanmath和map 更新使用的range格式，并将miss和hit分类
5. 获取校准后的origin pos
6. 获取预测的重力加速度方向
7. 根据重力加速度方向投影点云
8. 降采样滤波，并抛弃设置高度范围外的所有点云
9. 然后调用匹配方法
10. 插入并更新submap
11. 获取匹配后的结果内容，包括时间，轨迹节点位置，节点位置对应点云，submap

//顶层插入新的激光类型的传感器原始数据
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data)

函数接口输入包括传感器类型和原始点云数据，其中点云数据sensor::TimedPointCloudData为cartographer中定义，其本质有ros scanmessage转换成点云，同时为点云获取时间戳和每个点采样偏移时间戳。

  // 添加到多传感器融合集合中,即多种传感器同步后的激光雷达数据
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

多种传感器融合同步处理，synchronized_data为同步后的点云数据。前面已经分析过，其目的可保证后续的匹配，插入等处理认为只有一种类型的整体点云，降低后续的处理难度。

  // 
  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  // 如果没有imu数据，直接初始化推算器
  if (!options_.use_imu_data()) {
    InitializeExtrapolator(time);
  }

  // 等待估计器初始化完成
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return nullptr;
  }

位置估计器初始化，即第一次或者没有imu数据时应该先初始化位置估计器。由于位置估计器提供匹配的预测位置，因此未完成初始化无需后续的匹配等处理；

 // 开辟一个新的vector存储所有当前雷达传感器点云每个点对应的位置信息，其位置信息由估计器预测而来
 std::vector<transform::Rigid3f> range_data_poses;
 range_data_poses.reserve(synchronized_data.ranges.size());
 bool warned = false;
 for (const auto& range : synchronized_data.ranges) {
   common::Time time_point = time + common::FromSeconds(range.point_time.time);
   // 遍历每一个点云点的时间戳，理论上应晚于估计器上刻位置时间戳，否则说明传感器采集时间错误
   if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
     if (!warned) {
       LOG(ERROR)
           << "Timestamp of individual range data point jumps backwards from "
           << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
       warned = true;
     }
     //
     time_point = extrapolator_->GetLastExtrapolatedTime();
   }
   // 根据每个点的时间戳估计点云点对应的位置并进行缓存
   range_data_poses.push_back(
       extrapolator_->ExtrapolatePose(time_point).cast<float>());
 }

 // 初始化
 if (num_accumulated_ == 0) {
   // 'accumulated_range_data_.origin' is uninitialized until the last
   // accumulation.
   accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
 }

 // Drop any returns below the minimum range and convert returns beyond the
 // maximum range into misses.
 for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
   // 提取每一个点云点的pose（包含时间戳）
   const sensor::TimedRangefinderPoint& hit =
       synchronized_data.ranges[i].point_time;
   // 提取此点云对应的原点坐标pose，并进行畸变矫正
   const Eigen::Vector3f origin_in_local =
       range_data_poses[i] *
       synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
   // 对此点进行畸变矫正，并转换为pose，不包含时间戳
   sensor::RangefinderPoint hit_in_local =
       range_data_poses[i] * sensor::ToRangefinderPoint(hit);
   // 计算点到原点距离
   const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
   const float range = delta.norm();
   // 距离满足一定范围内保留，否则丢弃
   if (range >= options_.min_range()) {
     if (range <= options_.max_range()) {
       accumulated_range_data_.returns.push_back(hit_in_local);
     } else {
       // 超出设置最远距离，则放入miss队列中，并且距离全部调整为配置值
       hit_in_local.position =
           origin_in_local +
           options_.missing_data_ray_length() / range * delta;
       accumulated_range_data_.misses.push_back(hit_in_local);
     }
   }
 }

以上代码即为上节分析过的激光雷达点云数据的畸变矫正过程，其目的是提高点云的准确性。由于估计器可估计出任意时刻的大约位置，而雷达点云第一个采样点到最后一个点必然存在时间间隔，其时间戳已经存储在输入的点云中。故可获取点云中每个点激光原点位置估计值，以此进行修正。
在遍历每个点同时，将点云重新进行封装，即真正意义上点云集合，包括原点坐标和点云坐标。同时为后续的submap更新准备，将点云按照到激光原点的距离分成有效点return和miss两类。实际就是限制map更新激光测距有效距离。
注意：激光雷达原数据本身就是距离值，直接可以划分return和miss两类。在cartographer_ros转换成了点云，然后再反转成距离判断，显然降低了效率。而其他slam算法如gmapping接口输入直接为distance格式，因此可直接判断；
思考和意义：1.由于cartographer算法并非针对某一种传感器、某一类并且个数也不一定，因此cartographer的核心代码采用更加通用的点云格式作为输入。
2.因此只有深入了解cartographer内部技术细节，可针对自己构建的机器人进行优化和修改，更加适配自身的传感器类型

  // 激光点云累积个数
  ++num_accumulated_;

  // 当个数满足条件时，进行处理，
  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    // 最新点云时间戳
    const common::Time current_sensor_time = synchronized_data.time;
    // 获取两次间隔
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value()) {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;
    // 重新清零
    num_accumulated_ = 0;
    // 获取估计的重力加速度方向，格式为旋转向量
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    // 估计的最后一个点的预测位置作为矫正后的点云的原点坐标
    accumulated_range_data_.origin = range_data_poses.back().translation();
    // 进行重力加速度方向上的投影，同时进行降采样滤波
    return AddAccumulatedRangeData(
        time,
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }
  return nullptr;
}

cartographer考虑并非一帧就进行匹配处理，其个数可设置，默认1个，故num_accumulated_可不用考虑。在进入AddAccumulatedRangeData之前获取加速度方向，为后续水平投影准备。其中AddAccumulatedRangeData为匹配等算法调用处理接口。

点云投影和滤波TransformToGravityAlignedFrameAndFilter

// 根据重力加速度方向进行旋转投影，仅保留一定高度，并进行采样滤波
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
  const sensor::RangeData cropped =
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z());
  // 像素法进行降采样（默认0.025m采样）
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
}

由于为2d slam故只需要水平方向的点云，投影后仅保留固定高度范围内的点云，然后采用立体素的方法进行降采样，其基本原理即在一个立方体内仅保留一个点，且为这个立方体内部所有点云的均值坐标，是pcl库中最基本降采样方法；

核心函数AddAccumulatedRangeData

/*
input:  
1.时间戳
2.经水平投影后的点云数据
3.重力加速度旋转向量
4.与上次处理的时间间隔
 */
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)

接口定义可看出，输入预处理后的点云数据。

  // Computes a gravity aligned pose prediction.
  // 采用推算器获取推算出大约位置
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  // 经过重力方向计算投影后的2d位置
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
  // 经过立体像素滤波获取点云
  // 默认size为0.5m， 最小个数200个，最远距离50m
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
          .Filter(gravity_aligned_range_data.returns);

获取当前帧的预测位置，并且投影到水平方向，如此与点云一致均为2d 水平投影坐标；同时第二次进行降采样。

  // local map frame <- gravity-aligned frame
  // 采用预测位置作为初始位置和滤波后的点云进行相关匹配获得的位置
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

获取预测的机器人位置和滤波后的点云，已经满足scan-match所有输入条件，可以进行scan-match处理获取更加精确的机器人位置（可认为机器人真实位置）。其中ScanMatch并不是直接调用之前分析的三种scan-match方法，而是先调用最基本的相关匹配，然后调用基于ceres库的优化匹配。下面会单独分析。

  // 转换位置类型
  // gravity_alignment 为水平投影系数，假设平地则为1 
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 将此刻匹配后的准确位置加入估计值， 即更新估计器    
  extrapolator_->AddPose(time, pose_estimate);

  // 将点云转换至当前估计位置坐标下
  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));

以上代码较为简单，由于获取真实的匹配位置，从而插入位置估计器进行更新。同时根据真实位置将点云坐标进行更新。

  //点云插入和更新submap获取结果（轨迹节点内容pose，点云和对应更新后的submap）
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());
 return absl::make_unique<MatchingResult>(
      MatchingResult{time, pose_estimate, std::move(range_data_in_local),

根据真实的机器人位置和真实的点云坐标进行submap更新和维护，调用InsertIntoSubmap函数。

点云匹配接口ScanMatch()

  // active_submaps_维持的两个submap前面一个作为匹配地图
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 如果相关匹配使能，则可先进行一次相关匹配，作为优化匹配的初始值
  // 默认未打开
  if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }
  // 采用ceres库进行优化匹配
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  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);
  return pose_observation;

在ActiveSubmaps2D类一节分析过，内部维护两个submap的队列。上面代码可看出队列中前面一个submap用于匹配，预测位置作为匹配的初始位置。真实输出采用的ceres库的优化匹配方法，获取最佳匹配位置。但是可配置在优化匹配前可调用相关匹配correlative_scan_matcher进行一次，得到的匹配值作为优化匹配的初始位置。显然相关匹配的重要性并不高，默认并不开启。
注：在第一节的相关匹配分析中，曾经存在疑问就是cartographer采用相关匹配并没有什么特别之处，这里验证了这一点，仅是过渡作用，可以为优化匹配提供更加准确的初始值，也可不使用。
结论：cartographer采用的前端匹配算法为，基于ceres库的优化匹配

submap插入激光和更新函数InsertIntoSubmap

获得了准确的当前位置，则需要将激光点云插入到维护的submap中，InsertIntoSubmap函数则为调用接口。

//将点云数据根据点云origin位置插入到submap中
/*input:
1.时间戳
2.转换至世界坐标系的点云
3.滤波后的原始点云
4.当前世界坐标
5.重力加速度旋转向量
 */
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) {
    // 运动滤波器，当两帧位置相差过小时，抛弃
  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }
  // 调用submap封装类， 执行插入新的激光数据， 即submap更新
  // 返回的是更新和插入后的submap2d
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
  // 返回结果  （轨迹节点原始数据和对应的submap） 
  return absl::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)});
}

每一帧激光点云均会经过帧间匹配获取准确的机器人位置，则对应的点云可插入submap中，但是基于存储和建图的本身的需求，无需如此多的数据，会产生大量重复点云。因此机器人进行运动滤波，即只有当机器人运行了一定距离或一定角度时，则作为keypose进行存储，即进行后续的插入操作。
其中motion_filter_.IsSimilar(time, pose_estimate)即判断此次与上次之间的位置偏移和角度偏移是否达到一定阈值，否则无需进行插入处理。满足条件后直接调用ActiveSubmaps2D更新函数即可。

总结

LocalTrajectoryBuilder2D实际上就是前端处理的整体流程，通过对所有输入的传感器数据进行预处理。将laser的数据经过一定预处理形成单一的，稀疏的点云，进行帧间匹配获取真实位置，即完成SLAM中的定位。然后将laser点云分成return和miss两类，已知点云和位置进行更新submap。
换言之，LocalTrajectoryBuilder2D是一个没有闭环处理的完整的slam过程，可修改submap的帧的阈值（无限大），则完成了与开源hector slam算法一致的功能。