在上一篇文章对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,子图的插入数据的工具函数等。我们依然按照传感器数据从输入到输出的流程来阅读代码，其中也会对其分支进行阅读。当我们按照这一个流程走过一遍，代码也就可以读懂了。