前沿
本文将研究一下Cartographer中前端实现的过程,其主要流程为:将激光雷达数据和位姿推理器(Pose Extrapolator)估计的位姿输入到扫描匹配器(Scan Matching),并使用Ceres库完成优化计算,得到机器人位姿的观测值。观测值一方面反馈给位姿推理器用于修正其估计值,另一方面提供给运动滤波器用于判断机器人是否运动了。如果运动了,就将此刻的点云数据插入到当前正在维护的子图中。同时输出插入的结果,包含了位姿、子图、激光扫描数据和时间信息。
Cartographer有关其他分析文章:回到目录
有关文件在:cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
接下来探讨一下每个函数的具体实现过程:
代码解析
AddRangeData函数分析
/**
* @brief 激光雷达扫描数据处理
* @param sensor_id 激光雷达的索引
* @param unsynchronized_data 未经时间同步的扫描数据
* @return 扫描匹配的结果
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
// 把传入的索引和扫描数据记录在数据收集器中range_data_collator_,同时获得一个做了时间同步的扫描数据
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;
}
// 将同步时间记录到临时对象time中
const common::Time& time = synchronized_data.time;
// 如果配置中设置不使用IMU估计位姿,则在此初始化位姿推理器;
// 如果设置使用IMU,将在AddImuData()函数中初始化位姿推理器
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;
}
CHECK(!synchronized_data.ranges.empty());
// 确保扫描数据中最后一个激光束对应的时间戳大于等于0
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
// 获取第一个激光束的时间戳
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
// 与位姿推理器的时间比较判断位姿推理器是否完成位姿更新
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
// 如果没有累计数据,就用成员变量记录当前时间
if (num_accumulated_ == 0) {
accumulation_started_ = std::chrono::steady_clock::now();
}
// 创建临时容器range_data_poses,用于记录各个激光束对应的机器人位姿
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[3]);
// 判断当前激光束对应时间戳比位姿推理器最新更新时间滞后,说明位姿推理器有最新的位姿信息。
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>());
}
// 如果程序执行到此还没有累计扫描数据,就重置对象accumulated_range_data_
if (num_accumulated_ == 0) {
accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
}
// 进行hit点和miss点的记录
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
// 通过左乘机器人位姿,将机器人坐标系下的传感器位姿转换到局部子图的坐标系下
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
// 通过左乘机器人位姿,将机器人坐标系下的激光束转换到局部子图的坐标系下
const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
// 激光束的测量距离(原始扫描数据就带有距离信息,这里来回计算一遍有点多余)
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
const float range = delta.norm();
// 根据距离判断是hit还是miss。
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
accumulated_range_data_.misses.push_back(
origin_in_local +
options_.missing_data_ray_length() / range * delta);
}
}
}
++num_accumulated_;
// 如果累计的数据足够,则进行匹配。
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
// 计数器清零
num_accumulated_ = 0;
// 获取重力向量。
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
// 更新累计扫描数据的原点信息
accumulated_range_data_.origin = range_data_poses.back().translation();
// TransformToGravityAlignedFrameAndFilter()函数实现了以重力方向为参考修正累计扫描数据,同时完成了体素滤波
return AddAccumulatedRangeData(
time,
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment);
}
return nullptr;
}
AddAccumulatedRangeData函数分析
/**
* @brief 激光雷达扫描数据处理
* @param time 当前同步时间
* @param gravity_aligned_range_data 经过重力修正后的扫描数据
* @param gravity_alignment 重力方向
* @return 扫描匹配的结果
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment) {
if (gravity_aligned_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr;
}
// 从位姿推理器获得6自由度的位姿
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
// 把上面的6自由度位姿投影成3自由度的位姿。作为scan match 的初值。
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
// 进行ScanMatch匹配。
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, gravity_aligned_range_data);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
// 位姿重新转换成6自由度。
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
// 更新位姿推理器
extrapolator_->AddPose(time, pose_estimate);
// 将扫描数据转换到新的位姿下,即Submap坐标系。
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
// 将转换后的扫描数据插入子图中,返回插入结果。
std::unique_ptr<InsertionResult> insertion_result =
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
pose_estimate, gravity_alignment.rotation());
// 记录累积时间
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)});
}
ScanMatche函数分析
/**
* @brief 进行扫描匹配
* @param time 记录了当前时间
* @param pose_prediction 位姿推理器预测的位姿
* @param gravity_aligned_range_data 重力修正过后的扫描数据
* @return 扫描匹配修正后的位姿
*/
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data) {
// 获得一张最新的子图
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// 临时变量记录初始位姿。
transform::Rigid2d initial_ceres_pose = pose_prediction;
// 构建一个自适应体素滤波器,同时对扫描数据进行滤波处理
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
// 判断是否进行real-time CSM匹配
if (options_.use_online_correlative_scan_matching()) {
CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
proto::GridOptions2D_GridType_PROBABILITY_GRID);
double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
*static_cast<const ProbabilityGrid*>(matching_submap->grid()),
&initial_ceres_pose);
kFastCorrelativeScanMatcherScoreMetric->Observe(score);
}
auto pose_observation = common::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
// 通过对象ceres_scan_matcher_把扫描匹配问题描述成一个最小二乘问题,使用优化库Ceres库寻优,
// 得到一个使得观测数据出现概率最大化的位姿估计,位姿估计值记录在pose_observatio中
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
// 如果成功求得扫描匹配修正后位姿,就通过kCeresScanMatcherCostMetric,kScanMatcherResidualDistanceMetric
// 和kScanMatcherResidualAgnleMetric检查一下计算结果
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
double residual_angle = std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
// 返回修正后的位姿
return pose_observation;
}
InsertIntoSubmap()函数分析
/**
* @brief 扫描数据插入当前正在维护的子图
* @param time 当前同步时间
* @param range_data_in_local 记录了优化后的位姿估计下的扫描数据
* @param gravity_aligned_range_data 记录了优化前在重力修正的扫描数据
* @param pose_estimate 记录了优化后的位姿估计
* @param gravity_alignment 重力方向
* @return 扫描匹配的结果
*/
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
const 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) {
// 通过运动滤波器判断机器人是否发生明显运动,若未发生则直接返回,这样可以减少计算量
if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr;
}
// 将对象active_submaps_中维护的子图存到临时容器insertion_submaps中
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
for (const std::shared_ptr<Submap2D>& submap : active_submaps_.submaps()) {
insertion_submaps.push_back(submap);
}
// 把当前扫描数据插入到active_submaps_中
active_submaps_.InsertRangeData(range_data_in_local);
// 构建一个自适应体素滤波器对优化前重力修正的扫描数据进行滤波,目的是减少闭环检测的计算量
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.loop_closure_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
// 构建InsertionResult对象并返回
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)});
}
小结
函数AddRangeData()和它调用的函数AddAccummulatedRangeData()基本控制Local SLAM实现过程。AddRangeData()主要是筛选激光点云并将它们划分为hit点和miss点,AddAccumulatedRangeData()则调用函数ScanMatch()和InsertIntoSubmap()完成扫描匹配和子图更新的操作。