Cartographer源码阅读03-local_trajectory_builder.cc(2)-前端匹配

在上一篇文章对local_trajectory_builder的部分代码进行了阅读，接下来，将对ScanMatch及InserttoSubmap进行代码的阅读。代码位于Cartographer/mapping/internal/2d/scan_matching/local_trajectory_builder_2d.cc

1.扫描匹配，ScanMatch

在扫描匹配方面，Cartographer采用的是CSM（相关性扫描匹配）＋基于梯度优化的方法（ceres）来实现，CSM可以简单地理解为暴力搜索，即每一个激光数据与子图里的每一个位姿进行匹配，直到找到最优位姿，进行三层for循环，不过谷歌采用多分辨率搜索的方式，来降低计算量，在后续也会进行解读。基于优化的方法是谷歌的ceres库来实现，原理就是构造误差函数，使其对最小化，其精度高，但是对初始值敏感。因此需要结合两种方法

//进行scanMatch，CSM+Ceres，
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
        const common::Time time,
        const transform::Rigid2d& pose_prediction,
        const sensor::PointCloud& filtered_gravity_aligned_point_cloud)
{
    if (active_submaps_.submaps().empty())
    {
        return absl::make_unique<transform::Rigid2d>(pose_prediction);
    }

    //进行匹配的子图，采用激光数据与子图进行匹配的方法，因此肯定要有子图
    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.
    // 初始的位姿，由PoseExtrapolator预测的初始位姿pose_prediction
    transform::Rigid2d initial_ceres_pose = pose_prediction;

    //是否进行real-time csm的匹配，默认是fasle但建议使用
	//采用CSM（在线相关扫描匹配器将）对Ceres扫描匹配器的初始估计进行细化。
	////返回一个更好的值initial_ceres_poseif (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);
		//返回一个更好的值initial_ceres_pose,传给ceres进行优化
    }

    //进行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);

    // 统计误差，如果扫描获得位姿，则更新当前时刻的状态，
    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);
    }
    return pose_observation;//返回扫描匹配后的位姿
}

阅读代码可以知道，CSM将姿态外推器插值得到的位姿进行细分优化，然后传给ceres进行扫描匹配，来解决基于优化的方法对初始值敏感的缺点。

2.InsertToSubmap

ScanMatch获得了最优的匹配位姿，接下来InsertToSubmap将按照获得的最优位姿，把激光数据插入子图。

//插入到局部子图中．
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)
{

  //如果没有被MotionFilter滤掉
    if (motion_filter_.IsSimilar(time, pose_estimate))
    {
        return nullptr;
    }

    //把当前帧数据,插入到地图中.
	//调用的插入函数是子图的工具函数
    // 把active_submaps_中维护的Submap列表放到insertion_submaps这个向量中
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
            active_submaps_.InsertRangeData(range_data_in_local);
	//返回匹配结果
    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)});
}

将激光束插入子图实际是调用的子图的插入工具函数，后面我们也将进行阅读。

3.AddImuData加入IMU数据

//加入IMU数据,用来初始化和更新PoseExtrapolator
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);
}

4.AddOdometryData

//加入里程计数据,用来初始化更新PoseExtrapolator
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);
}

5.初始化PoseExtrapolator.

/初始化PoseExtrapolator.
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time)
{
    if (extrapolator_ != nullptr)
    {
        return;
    }

    // We derive velocities from poses which are at least 1 ms apart for numerical
    // stability. Usually poses known to the extrapolator will be further apart
    // in time and thus the last two are used.
    // 用相差至少1ms的两个位姿来计算速度.
    constexpr double kExtrapolationEstimationTimeSec = 0.001;

    // TODO(gaschler): Consider using InitializeWithImu as 3D does.
    extrapolator_ = absl::make_unique<PoseExtrapolator>(
                ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
                options_.imu_gravity_time_constant());

    extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}

小节

到目前为止，local_trajectory_builder的核心代码阅读完成，从总体上对前端匹配有了了解，具体的实现还有待进一步探索，不如说位姿外推器，csm相关性扫描匹配，ceres,子图的插入数据的工具函数等。我们依然按照传感器数据从输入到输出的流程来阅读代码，其中也会对其分支进行阅读。当我们按照这一个流程走过一遍，代码也就可以读懂了。