文章目录
官方框架图
Local SLAM（前端）
主要功能：输入传感器信息通过scan-match获取帧间相对位姿关系，构建submap并获取laser-scan与submap相对位姿关系。
大致流程：利用里程计(Odometry)和IMU数据进行轨迹推算，给出小车位姿估计值；将位姿估计值作为初值，对雷达数据进行匹配，并更新位姿估计器的值；雷达一帧帧数据经过运动滤波后，进行叠加，形成子图(submap)。
Global SLAM（后端）
主要功能：将位姿推移类传感器和前端结果按时间顺序进行串联起来形成一条轨迹，寻找闭环关系，最终采用优化求解获取位姿总体误差最小时的位姿轨迹即submap位姿。后端优化目的，是将全部子图形成一张完整可用的地图。
代码目录
cartographer源码文件目录
- bazel: 第三方库的构建包括eigen、ceres等。
- cartographer: 源码部分。
- cmake: 包括了cmake模块的FindXXX.make文件以及相关的函数。
- configuration_files: cartographer的lua配置文件。
- docs: cartographer相关的一些指导文档。
- scripts: 一些工具与库的安装脚本。
- cartographer 文件夹
– cloud: grpc进行远程调用，与多机协同有关，具体后续分析。
– common: 通用的函数包括：任务，线程池，lua文件解析，数学库等。
– ground_truth: slam开发过程中评估用的工具。
– io: 输入输出相关的函数。
– mapping: cartographer核心代码区。
– metrics: cartographer过程数据的各种输出格式。
– sensor: 传感器处理相关的函数imu、里程计、点云、压缩等。
– testing: proto文件解释的测试。
– transform: 包含一些时间戳转换、坐标系转换。
– mapping 文件夹
该文件夹位cartographer的核心代码区，里面包含了2d,3d建图的函数map_builder、pose_grapher、constraint、optimization、scan_match等相关类封装。
Mapping中包含的几个对外接口：
— map_builder_interface：地图创建接口；
— pose_extrapolator_interface：位姿估计/推算接口；
— pose_graph_interface：位姿图接口；
— range_data_inserter_interface：范围数据插入器接口；
— trajectory_builder_interface：运动轨迹创建接口。
本文主要是梳理整体思路，不会粘贴很多代码。添加中文注释的代码版本，可参考https://github.com/xiangli0608/cartographer_detailed_comments_ws/
此处不单独列出cartographer_ros。cartographer_ros代码可以从node_main.cc开始看。
代码路径：cartographer_ros\cartographer_ros\node_main.cc
cartographer_ros::Run()大致流程如下：
- 创建tf_buffer，开启监听tf的独立线程
- 将lua配置文件的参数传给node_options和trajectory_options变量中
- 创建map_builder类
- 创建node类
- 开始默认轨迹
- 结束轨迹
- 全局优化
- 保存pbstream
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
// 开启监听tf的独立线程
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
TrajectoryOptions trajectory_options;
// c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
// 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
// MapBuilder类是完整的SLAM算法类
// 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph)
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
// c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象,
// 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..
// 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象,
// 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
// 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 如果加载了pbstream文件, 就执行这个函数
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
// 结束所有处于活动状态的轨迹
node.FinishAllTrajectories();
// 当所有的轨迹结束时, 再执行一次全局优化
node.RunFinalOptimization();
// 如果save_state_filename非空, 就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
传感器的格式转换与类型变换
将ROS传来的里程计数据进行时间和坐标变换的处理，然后转换成Carto的格式传给前端。按数据类型分类：
- IMU数据、里程计数据与Landmark数据的格式转换与坐标变换
- GPS数据的处理
- 雷达数据的格式转换与坐标变换
三维刚体的坐标变换使用Rigid3类，将传感器坐标转到统一到tracking_frame坐标系下。
tracking_frame:由SLAM算法跟踪的坐标系。
代码路径：
cartographer_ros\cartographer_ros\node.h
// The following functions handle adding sensor data to a trajectory.
void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg);
void HandleLandmarkMessage(
int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage(
int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);
cartographer_ros\cartographer_ros\sensor_bridge.cc
void SensorBridge::HandleOdometryMessage(
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<carto::sensor::OdometryData> odometry_data =
ToOdometryData(msg);
if (odometry_data != nullptr) {
if (IgnoreMessage(sensor_id, odometry_data->time)) {
LOG(WARNING) << "Ignored odometry message from sensor " << sensor_id
<< " because sensor time " << odometry_data->time
<< " is not before last odometry message time "
<< latest_sensor_time_[sensor_id];
return;
}
latest_sensor_time_[sensor_id] = odometry_data->time;
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
}
}
...
传感器数据的分发处理
传感器数据分发过程
待补充。
主要代码：
顺序多队列
cartographer\sensor\internal\ordered_multi_queue.h
cartographer\sensor\internal\dispatchable.h
数据预处理
传感器数据的走向
- cartographer::mapping::PoseExtrapolator（位姿推算用）
- map_builder_bridge_.sensor_bridge(建图用，每条轨迹的传感器数据输入)
代码路径：cartographer_ros\cartographer_ros\node.cc
轨迹对应的传感器数据
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
...
激光雷达数据预处理
2D激光雷达数据预处理
- 多个点云数据的时间同步与融合
- 点云数据运动畸变的校正与无效点的处理
- 点云的坐标变换与z方向的过滤
- 对过滤后的点云进行体素滤波
代码路径：cartographer\mapping\internal\2d\local_trajectory_builder_2d.cc
/**
* @brief 处理点云数据, 进行扫描匹配, 将点云写成地图
*
* @param[in] sensor_id 点云数据对应的话题名称
* @param[in] unsynchronized_data 传入的点云数据
* @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 匹配后的结果
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
// Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
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;
}
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;
}
CHECK(!synchronized_data.ranges.empty());
// TODO(gaschler): Check if this can strictly be 0.
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
// 计算第一个点的时间
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
// 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
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();
}
// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
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) {
// 获取在tracking frame 下点的坐标
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
// 将点云的origins坐标转到 local slam 坐标系下
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
// Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
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();
// param: min_range max_range
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
// 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
// Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
hit_in_local.position =
origin_in_local +
// param: missing_data_ray_length, 是个比例, 不是距离
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
} // end for
// 有一帧有效的数据了
++num_accumulated_;
// param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
// 计算2次有效点云数据的的时间差
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,
// 将点云变换到local原点处, 且姿态为0
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
return nullptr;
}
多个点云数据的时间同步与融合
cartographer\mapping\internal\range_data_collator.cc
/**
* @brief 多个雷达数据的时间同步
*
* @param[in] sensor_id 雷达数据的话题
* @param[in] timed_point_cloud_data 雷达数据
* @return sensor::TimedPointCloudOriginData 根据时间处理之后的数据
*/
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id,
sensor::TimedPointCloudData timed_point_cloud_data) { // 第一次拷贝
CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
...
}
点云数据运动畸变的校正与无效点的处理
- 获取现在点云距离的原始数据(变量为hit)
- 将点云的原始距离数据转化为local坐标系下的数据(origin_in_local)
- 真正的去畸变(hit_in_local), 方法是将原始数据转化为local坐标系下的坐标, range_data_poses是通过位姿推测器预测出的点的时间戳
- 计算去畸变点云的距离,并且用配置参数的最大最小距离去进行数据的过滤, 对距离合适的数据放在returns这个数组里, 对于距离不合适的放在misses这个数组里。
代码实现在local_trajectory_builder_2d.cc中的AddRangeData函数中。
点云的坐标变换与z方向的过滤
对过滤后的点云进行体素滤波
代码路径：cartographer\mapping\internal\2d\local_trajectory_builder_2d.cc
/**
* @brief 先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量
*
* @param[in] transform_to_gravity_aligned_frame 将点云变换到原点处, 且姿态为0的坐标变换
* @param[in] range_data 传入的点云
* @return sensor::RangeData 处理后的点云 拷贝
*/
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const {
// Step: 5 将原点位于机器人当前位姿处的点云 转成 原点位于local坐标系原点处的点云, 再进行z轴上的过滤
const sensor::RangeData cropped =
sensor::CropRangeData(sensor::TransformRangeData(
range_data, transform_to_gravity_aligned_frame),
options_.min_z(), options_.max_z()); // param: min_z max_z
// Step: 6 对点云进行体素滤波
return sensor::RangeData{
cropped.origin,
sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}
其他几个相关代码文件：
cartographer\sensor\range_data.cc
cartographer\sensor\internal\voxel_filter.cc
3D激光雷达数据预处理
处理流程待补充。
代码路径：cartographer\mapping\internal\3d\local_trajectory_builder_3d.cc
std::unique_ptr<MatchingResult> AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& range_data);
前端里程计
初始位姿估计
基于IMU与里程计的位姿推测器
- 进行线速度与角速度的预测
- 根据匹配后的位姿进行状态量的更新
- 位置与姿态的预测
相关代码路径：
cartographer\mapping\pose_extrapolator_interface.h
cartographer\mapping\pose_extrapolator.h
cartographer\mapping\imu_tracker.h
PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
double imu_gravity_time_constant)
: pose_queue_duration_(pose_queue_duration),
gravity_time_constant_(imu_gravity_time_constant),
cached_extrapolated_pose_{common::Time::min(),
transform::Rigid3d::Identity()} {}
// 使用imu数据进行PoseExtrapolator的初始化
std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
const common::Duration pose_queue_duration,
const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {
auto extrapolator = absl::make_unique<PoseExtrapolator>(
pose_queue_duration, imu_gravity_time_constant);
extrapolator->AddImuData(imu_data);
extrapolator->imu_tracker_ =
absl::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(
imu_data.linear_acceleration);
extrapolator->imu_tracker_->AddImuAngularVelocityObservation(
imu_data.angular_velocity);
extrapolator->imu_tracker_->Advance(imu_data.time);
extrapolator->AddPose(
imu_data.time,
transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));
return extrapolator;
}
ExtrapolatePose
// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
CHECK_GE(time, newest_timed_pose.time);
// 如果本次预测时间与上次计算时间相同 就不再重复计算
if (cached_extrapolated_pose_.time != time) {
// 预测tracking frame在local坐标系下time时刻的位置
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
// 预测tracking frame在local坐标系下time时刻的姿态
const Eigen::Quaterniond rotation =
newest_timed_pose.pose.rotation() *
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
cached_extrapolated_pose_ =
TimedPose{time, transform::Rigid3d{translation, rotation}};
}
return cached_extrapolated_pose_.pose;
}
相关函数
1.插入IMU数据 AddImuData（）：
由于IMU可直接提供角速度信息，因此无需进行一定预处理，即已获取imu的角速度信息；
2.插入里程计数据 AddOdometryData（）：
仅根据odometer估计当前线速度和角速度信息；
3.UpdateVelocitiesFromPoses（）：
此函数则是根据历史最近的两帧位姿信息进行微分，获取线速度和角速度。
4. 插入历史位置AddPose（）：
插入历史位置进行更新估计为主要和必要方法，即使没有odometer和imu插入数据也可正常工作，因此AddPose（）可为主要函数，即目的按照时间顺序插入历史真实位置（实际上为scan-match后较为准确的位置）。然后进行估计线速度和角速度。通过角速度再次进行积分获取航向变换信息。
5.航向角积分实现AdvanceImuTracker（）：
由于已估计出目前的线速度和角速度，为获取估计姿态则需要对角速度进行积分，然后获取角度旋转矩阵。由于航向积分并非像平移一样线性加减即可，因此cartographer专门提供了一个用于角度积分的ImuTracker类。
6. 获取预测位置ExtrapolatePose(const common::Time time)：
输出预测值接口，在上次真实位置基础上进行积分，即获取平移矩阵和旋转矩阵，进行转换获取最终估计位置和姿态；
预测模式：
预测平移时: 有里程计就用里程计的线速度, 没有里程计就用 pose 计算的线速度进行预测。
预测姿态时: 有 IMU 就用 IMU 的角速度, 没有 IMU 时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用 pose 计算的角速度进行预测。
当前的线速度如果有里程计就用里程计计算出的, 没有里程计就用 pose 计算的角速度进行预测。
栅格地图/子地图
子地图管理
submap2d地图类定义
cartographer\mapping\2d\submap_2d.h
cartographer\mapping\2d\submap_2d.cc
// 将雷达数据写到栅格地图中
void Submap2D::InsertRangeData(
const sensor::RangeData& range_data,
const RangeDataInserterInterface* range_data_inserter) {
CHECK(grid_);
CHECK(!insertion_finished());
// 将雷达数据写到栅格地图中
range_data_inserter->Insert(range_data, grid_.get());
// 插入到地图中的雷达数据的个数加1
set_num_range_data(num_range_data() + 1);
}
submap3d地图类
待补充。
栅格地图更新
2D栅格地图
- 概率栅格地图
栅格更新步骤：
1.GrowAsNeeded实现当前grid map边界的扩展，即由于新的scan加入，可能会导致地图变大；
2.将地图分辨提高kSubpixelScale=1000倍，目的是为后面画直线精度更加精确；
3.获取高分辨率地图下的激光原点range_origin坐标索引;
4.获取高分率地图下所有有效激光点云的坐标索引；
5.获取还原原始地图分辨率坐标cell的value，然后查询hit_table表格进行更新，即z=hit条件下的更新；
6.采用RayToPixelMask画线的方法，获取激光原点到点云之间直线的所有点坐标；
7.通过还原原始地图分辨率获取value，查询miss_table表格进行更新，即z=miss条件下的更新；
代码路径：
cartographer\mapping\2d\probability_grid.cc
cartographer\mapping\2d\probability_grid_range_data_inserter_2d.cc
// 根据点云的bounding box, 看是否需要对地图进行扩张
void GrowAsNeeded(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) {
// 找到点云的bounding_box
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
// Padding around bounding box to avoid numerical issues at cell boundaries.
constexpr float kPadding = 1e-6f;
for (const sensor::RangefinderPoint& hit : range_data.returns) {
bounding_box.extend(hit.position.head<2>());
}
for (const sensor::RangefinderPoint& miss : range_data.misses) {
bounding_box.extend(miss.position.head<2>());
}
// 是否对地图进行扩张
probability_grid->GrowLimits(bounding_box.min() -
kPadding * Eigen::Vector2f::Ones());
probability_grid->GrowLimits(bounding_box.max() +
kPadding * Eigen::Vector2f::Ones());
}
/**
* @brief 根据雷达点对栅格地图进行更新
*
* @param[in] range_data
* @param[in] hit_table 更新占用栅格时的查找表
* @param[in] miss_table 更新空闲栅格时的查找表
* @param[in] insert_free_space
* @param[in] probability_grid 栅格地图
*/
void CastRays(const sensor::RangeData& range_data,
const std::vector<uint16>& hit_table,
const std::vector<uint16>& miss_table,
const bool insert_free_space, ProbabilityGrid* probability_grid) {
// 根据雷达数据调整地图范围
GrowAsNeeded(range_data, probability_grid);
const MapLimits& limits = probability_grid->limits();
...
}
/**
* @brief 将点云写入栅格地图
*
* @param[in] range_data 要写入地图的点云
* @param[in] grid 栅格地图
*/
void ProbabilityGridRangeDataInserter2D::Insert(
const sensor::RangeData& range_data, GridInterface* const grid) const {
ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
CHECK(probability_grid != nullptr);
// By not finishing the update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
// param: insert_free_space
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
probability_grid);
probability_grid->FinishUpdate();
}
- TSDF地图
待补充。
3D栅格地图
- hybridGrid混合概率地图
待补充。
扫描匹配
2D扫描匹配
实时相关性扫描匹配（SCM）
预投影法：先通过角度切片，然后在在xy方向上暴力搜索，为了减少计算量。
处理逻辑：
1.切片:以初始位置（角度）为基础，角度搜索范围内，按照搜索步长进行旋转当前scan。将这一系列的离散化的scan，按照旋转顺序存储起来。
2.搜索：在每个切片内，以初始位置（xy）为基础，xy搜索搜索范围内，按照搜索步长，进行离散，生成候选位置。
3.评分：求得每个候选位置的score，score最高的，就为最佳位置。
在submap一定范围内的暴力匹配。
代码路径：mapping\internal\2d\scan_matching\real_time_correlative_scan_matcher_2d.cc
/**
* @brief 相关性扫描匹配 - 计算量很大
*
* @param[in] initial_pose_estimate 预测出来的先验位姿
* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
* @param[in] grid 用于匹配的栅格地图
* @param[out] pose_estimate 校正后的位姿
* @return double 匹配得分
*/
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate) const {
CHECK(pose_estimate != nullptr);
// Step: 1 将点云旋转到预测的方向上
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
// 根据配置参数初始化 SearchParameters
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, grid.limits().resolution());
// Step: 2 生成按照不同角度旋转后的点云集合
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
// Step: 3 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
// Step: 4 生成所有的候选解
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
// Step: 5 计算所有候选解的加权得分
ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
// Step: 6 获取最优解
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
// Step: 7 将计算出的偏差量加上原始位姿获得校正后的位姿
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return best_candidate.score;
}
Ceres扫描匹配
使用ceres优化位姿
代码路径：mapping\internal\2d\scan_matching\ceres_scan_matcher_2d.cc
/**
* @brief 基于Ceres的扫描匹配
*
* @param[in] target_translation 预测出来的先验位置, 只有xy
* @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta
* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
* @param[in] grid 用于匹配的栅格地图
* @param[out] pose_estimate 优化之后的位姿
* @param[out] summary
*/
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
// 地图部分的残差
CHECK_GT(options_.occupied_space_weight(), 0.);
switch (grid.GetGridType()) {
case GridType::PROBABILITY_GRID:
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
break;
case GridType::TSDF:
problem.AddResidualBlock(
CreateTSDFMatchCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, static_cast<const TSDF2D&>(grid)),
nullptr /* loss function */, ceres_pose_estimate);
break;
}
// 平移的残差
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation), // 平移的目标值, 没有使用校准后的平移
nullptr /* loss function */, ceres_pose_estimate); // 平移的初值
// 旋转的残差, 固定了角度不变
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]), // 角度的目标值
nullptr /* loss function */, ceres_pose_estimate); // 角度的初值
// 根据配置进行求解
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}
3D扫描匹配
待补充。
后端优化
2D后端优化
位姿优化
1.将scannode和submap作为优化节点，加入位姿图；
2.从前端匹配结果中获取相邻scannode节点间约束和submap内部的submap与scannode约束；
3.创建闭环匹配器进行闭环检测，增加闭环约束；
4.构建位姿图优化器进行优化；
5.使用优化结果对旧值进行更新；（优化后位姿的更新）
后端计算位姿的优化方法涉及：
- 约束残差的构建
- Landmark残差的构建
- 里程计与GPS残差的构建
- 使用Ceres进行全局优化求解
代码路径：
cartographer\mapping\internal\optimization\optimization_problem_2d.cc
// 添加子图位姿
void OptimizationProblem2D::AddSubmap(
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
}
// 向优化问题中加入节点位姿数据
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id];
}
**
* @brief 搭建优化问题并进行求解
*
* @param[in] constraints 所有的约束数据
* @param[in] trajectories_state 轨迹的状态
* @param[in] landmark_nodes landmark数据
*/
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes)
线程池和任务队列
待补充。可参考PoseGraph2D整体处理流程
3D后端优化
待补充。
回环检测
约束计算
代码路径：src\cartographer\cartographer\mapping\internal\2d\pose_graph_2d.cc
ComputeConstraint：子图间约束计算
/**
* @brief 进行子图间约束计算, 也可以说成是回环检测
*
* @param[in] node_id 节点的id
* @param[in] submap_id submap的id
*/
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) ;
ComputeConstraintsForNode：计算子图内约束
/**
* @brief 保存节点, 计算子图内约束, 查找回环
*
* @param[in] node_id 刚加入的节点ID
* @param[in] insertion_submaps active_submaps
* @param[in] newly_finished_submap 是否是新finished的submap
* @return WorkItem::Result 是否需要执行全局优化
*/
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) ;
使用滑动窗口算法生成多分辨率栅格地图
流程/步骤
参考滑窗法
代码路径：mapping\internal\2d\scan_matching\fast_correlative_scan_matcher_2d.cc
// 构造不同分辨率的地图
PrecomputationGrid2D::PrecomputationGrid2D(
const Grid2D& grid, const CellLimits& limits, const int width,
std::vector<float>* reusable_intermediate_grid)
: offset_(-width + 1, -width + 1),
wide_limits_(limits.num_x_cells + width - 1,
limits.num_y_cells + width - 1),
min_score_(1.f - grid.GetMaxCorrespondenceCost()), // 0.1 min_score_
max_score_(1.f - grid.GetMinCorrespondenceCost()), // 0.9 max_score_
cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
CHECK_GE(width, 1);
CHECK_GE(limits.num_x_cells, 1);
CHECK_GE(limits.num_y_cells, 1);
const int stride = wide_limits_.num_x_cells;
// First we compute the maximum probability for each (x0, y) achieved in the
// span defined by x0 <= x < x0 + width.
std::vector<float>& intermediate = *reusable_intermediate_grid;
intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
...
std::vector<PrecomputationGrid2D> precomputation_grids_;
// 构造多分辨率地图
PrecomputationGridStack2D::PrecomputationGridStack2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1);
// param: branch_and_bound_depth 默认为7, 确定 最大的分辨率, 也就是64个栅格合成一个格子
const int max_width = 1 << (options.branch_and_bound_depth() - 1); // 64
precomputation_grids_.reserve(options.branch_and_bound_depth());
// 保存地图值
std::vector<float> reusable_intermediate_grid;
const CellLimits limits = grid.limits().cell_limits();
// 经过滑窗后产生的栅格地图会变宽, x方向最多会比原地图多max_width-1个格子
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
limits.num_y_cells);
// 分辨率逐渐变大, i=0时就是默认分辨率0.05, i=6时, width=64,也就是64个格子合成一个值
for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
const int width = 1 << i;
// 构造不同分辨率的地图 PrecomputationGrid2D
precomputation_grids_.emplace_back(grid, limits, width,
&reusable_intermediate_grid);
}
}
// 构造函数
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options)
: options_(options),
limits_(grid.limits()),
// 多分辨率地图的构建
precomputation_grid_stack_(
absl::make_unique<PrecomputationGridStack2D>(grid, options)) {}
基于多分辨率地图的分支定界粗匹配
原理或流程：
1.先进行角度搜索空间和间隔进行生成所有可能性角度解，假设N个解，则生成N个cloudpoint；
2.对所有角度解的cloudpoint均转换至地图初始位置下。
3.先对最低分辨率的地图进行相关匹配，即搜索空间也与最低分辨率一致；
4.将最低分辨率所有位置及其对应评分放入集合中，同时按照评分从高到低排序。
5.调用分支界定方法求出最佳评分及其对应位置，则为相关匹配最佳值。
还有其他函数，此处不再列举。
/**
* @brief 进行局部搜索窗口的约束计算(对局部子图进行回环检测)
*
* @param[in] initial_pose_estimate 先验位姿
* @param[in] point_cloud 原点位于local坐标系原点处的点云
* @param[in] min_score 最小阈值, 低于这个分数会返回失败
* @param[out] score 匹配后的得分
* @param[out] pose_estimate 匹配后得到的位姿
* @return true 匹配成功, 反之匹配失败
*/
bool FastCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// param: linear_search_window angular_search_window
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score,
pose_estimate);
}
// 进行基于分支定界算法的粗匹配
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate)
{
...
}
分支定界匹配搜索
/**
* @brief 基于多分辨率地图的分支定界搜索算法
*
* @param[in] discrete_scans 多个点云的每个点在地图上的栅格坐标
* @param[in] search_parameters 搜索配置参数
* @param[in] candidates 候选解
* @param[in] candidate_depth 搜索树高度
* @param[in] min_score 候选点最小得分
* @return Candidate2D 最优解
*/
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
// 这个函数是以递归调用的方式求解的
// 首先给出了递归终止的条件, 就是如果到了第0层(到底了), 意味着我们搜索到了一个叶子节点.
// 同时由于每次迭代过程我们都是对新扩展的候选点进行降序排列
// 所以队首的这个叶子节点就是最优解, 直接返回即可
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();
}
// 然后创建一个临时的候选解, 并将得分设置为min_score
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;
// 遍历所有的候选点
for (const Candidate2D& candidate : candidates) {
// Step: 剪枝 低于设置的阈值 或者 低于上一层的可行解的最高分 的可行解不进行继续分枝
// 如果遇到一个候选点的分低于阈值, 那么后边的候选解的得分也会低于阈值,就可以直接跳出循环了
if (candidate.score <= min_score) {
break;
}
// 如果for循环能够继续运行, 说明当前候选点是一个更优的选择, 需要对其进行分枝
std::vector<Candidate2D> higher_resolution_candidates;
// 搜索步长减为上层的一半
const int half_width = 1 << (candidate_depth - 1);
// Step: 分枝 对x、y偏移进行遍历, 求出candidate的四个子节点候选解
for (int x_offset : {0, half_width}) { // 只能取0和half_width
// 如果超过了界限, 就跳过
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {0, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
// 候选者依次推进来, 一共4个,可以看出, 分枝定界方法的分枝是向右下角的四个子节点进行分枝
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
// 对新生成的4个候选解进行打分与排序, 同一个点云, 不同地图
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
// 递归调用BranchAndBound对新生成的higher_resolution_candidates进行搜索
// 先对其分数最高的节点继续进行分支, 直到最底层, 然后再返回倒数第二层再进行迭代
// 如果倒数第二层的最高分没有上一个的最底层（叶子层）的分数高, 则跳过,
// 否则继续向下进行分支与评分
// Step: 定界 best_high_resolution_candidate.score
// 以后通过递归调用发现了更优的解都将通过std::max函数来更新已知的最优解.
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}
可参考链接分支定界快速相关匹配
结果输出
优化后结果的传递与处理
待补充。
结束轨迹时的处理
代码路径：cartographer\mapping\pose_graph.h
调用函数 PoseGraph2D::FinishTrajectory()
调用层次
//Node::FinishTrajectoryUnderLock()
Node::HandleFinishTrajectory()
Node::FinishTrajectoryUnderLock()
MapBuilderBridge::FinishTrajectory()
MapBuilder->FinishTrajectory()
PoseGraph::FinishTrajectory()
PoseGraph2D::FinishTrajectory()
void MapBuilderStub::FinishTrajectory(int trajectory_id) {
proto::FinishTrajectoryRequest request;
request.set_client_id(client_id_);
request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::FinishTrajectorySignature> client(
client_channel_);
::grpc::Status status;
client.Write(request, &status);
if (!status.ok()) {
LOG(ERROR) << "Failed to finish trajectory " << trajectory_id
<< " for client_id " << client_id_ << ": "
<< status.error_message();
return;
}
trajectory_builder_stubs_.erase(trajectory_id);
}
参考 轨迹结束处理
Pbstream文件的保存
Pbstrea读写相关的源码路径：
cartographer\io\proto_stream_interface.h
cartographer\io\proto_stream.h
参考资料：
官方文档：https://google-cartographer.readthedocs.io/en/latest/
源码思想解读：https://blog.csdn.net/jiajiading/article/details/110373696?spm=1001.2014.3001.5502#t2
Cartographer源码解析：https://blog.csdn.net/weixin_43013761?type=blog