讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言(Scan match原理)
通过前面你的一系列博客,已经知道 Cartographer 中的概率栅格图是如何建立的了。不过需要注意的一点是该地图并不仅仅是保存下来给来看的,其还会被点云扫描匹配使用到,点云扫描匹配目的是估算位姿(该部分内容后面会详细讲解)。在 slam 中分为 Scan match 与 Point cloud match,通常情况下前者指的就是2D扫描匹配,后者指的是3D点云匹配。为了方便后续代码的理解,这里简单介绍一下扫描匹配的原理,先来看下图(理想位姿):
1、方格 → 所有的栅格组成。
2、黑色方格 → 障碍物,光子会被阻碍,形成点云数据。
3、白色方格 → 空闲区域,表示没有障碍物,光子可以直接穿过。
4、紫色多边形 → 代表机器人,当然也可以理解为雷达传感器原点,或者点云数据的原点。
5、黄色圆形 → 点云数据。
从上图来看,可见所有的点云数据都与障碍物重叠了,说明此时估算出来的位姿十分正确,这里称为理想位置。但是系统在运行的过程中并非是这样的。这里假设我们由传感器数据(Imu、GPS、里程计) 等估算出来的位姿如下所示:
根据图示可以看出初始位姿与理想位姿在位置与姿态上都存在差异,也就是说由初始位姿变换到理想位姿,其平移与旋转可能都需要发生改变。基于这个原理,最简单的一种扫描方式就是暴力匹配,也就是在位移与角度上维度上进行遍历,流程如下:
1、设定初始位姿
2、位移扫描匹配遍历
3、角度扫描匹配遍历。
4、评分标准
初始位姿可以直接由传感器估算出来,先来看下图,
绿色区域 1-6 表示需要遍历的区域,也就是理想位姿的平移处于该区域内,也就是位移扫描匹配需要遍历绿色区域1-6。蓝色的箭头表示角度遍历的方向。该箭头的起点就是角度开始遍历的起点,终点就是角度遍历的终点。
如上图所示,假设从从1号方格开始第一次遍历,Robot角度为-35°(假设平行y轴为0°),记为 1_(-35)。需要主意的是,Robot 与点云需要一直保持相对禁止,Robot位姿如何变换,点云则需做同样的变换。在 1_(-35) 的位置上,我们希望所有的点云都能够与黑色方格(障碍物)完全匹配,但是显然是不行的,所以改变一下角度,这里假设角分辨率为5°,那么下次利用 1_(-30) 的 Robot 与点云位姿与黑色方格进行匹配,依此一下,进行位姿和角度上的遍历,如下:
1_(-35) 1_(-30) 1_(-25) ...... 1_(30) 1_(35)
2_(-35) 2_(-30) 2_(-25) ...... 2_(30) 2_(35)
......
6_(-35) 6_(-30) 6_(-25) ...... 6_(30) 2_(35)
其可以按行遍历,也可以按列遍历,每一个上述 Robot 与点云位姿与黑色方格进行匹配之后,我们都需要进行一次评分,在这个例子中会匹配6x(70/5)=6x14=84次,也就是说这里需要进行84次评分。那么问题来了,如何进行评分,才能体现出匹配结果的优劣呢?
从上图可以很直观的看出点云与障碍物重叠度越高,则匹配效果越好。结合前面的内容,这里假设存在 n n n 个点云,其每个点云对应的方格记为 c e l l i cell_i celli, c e l l i cell_i celli 对应被占用的概率为 p i p_i pi,那么每次匹配的得分可以记为 s x = ( ∑ i = 1 n p i ) / n s_x=(\sum _{i=1}^np_i)/n sx=(∑i=1npi)/n。然后认为所有匹配(84次)得分中最高的 s m a x s_{max} smax 为最优匹配。也就是图一的理想匹配。
注意 \color{red} 注意 注意 这里讲解的仅仅是一种十分简单的方式,实际工程的实现并非如此简单,需要考虑很多的东西,比如通常情况下,点云与黑色方格很难完全匹配,也就是说不存在精确解,那么只能通过迭代的方式求得最优解。除此之外,还有涉及到其他的很多技巧,后续会进行部分讲解。
Cartographer 中的扫描匹配主要是参考:A Flexible and Scalable SLAM System with Full 3D Motion Estimation 实现,有兴趣的朋友可以好好阅读一下。
二、框架梳理1→AddAccumulatedRangeData()
下面来看看源码中是如何进行扫描匹配的,该篇不可不对细节进行刨析,了解基本流程即可,下一篇博客再进行细节上的分析。首先找到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数:
1、获得点云time时刻机器人位姿
// Computes a gravity aligned pose prediction.
// 进行位姿的预测, 先验位姿z
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
该函数是获得点云数据time时刻机器人的位姿,其是先对于 local 坐标系的,这里记 non_gravity_aligned_pose_prediction 为 R o b o t l o c a l t r a c k i n g \mathbf {Robot}^{tracking}_{local} Robotlocaltracking。表示 tracking_frame 在 local 系中的位姿。
2、机器人位姿重力校正
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
gravity_alignment 在前面详细分析过,这里记为 G r a v i t y A l i g n m e n t l o c a l g r a v i t y \mathbf {GravityAlignment}^{gravity}_{local} GravityAlignmentlocalgravity,该矩阵的逆 [ G r a v i t y A l i g n m e n t l o c a l g r a v i t y ] − 1 = G r a v i t y A l i g n m e n t l o c a l g r a v i t y [\mathbf {GravityAlignment}^{gravity}_{local}]^{-1}=\mathbf {GravityAlignment}^{gravity}_{local} [GravityAlignmentlocalgravity]−1=GravityAlignmentlocalgravity,可以对 local 系下的位姿或者点云数据进行重力校正,也就是把local系的数据变换到gravity系下,gravity坐标系原点与local坐标系重叠,但是gravity坐标系的 Z 轴是与重力方向垂直的,上述的的代码就是对推断器推断出来的位姿进行重力矫正,对应的数学公式如下: R o b o t g r a v i t y t r a c k i n g = R o b o t l o c a l t r a c k i n g ∗ [ G r a v i t y A l i g n m e n t l o c a l g r a v i t y ] − 1 (01) \color{Green} \tag{01} \mathbf {Robot}^{tracking}_{gravity}=\mathbf {Robot}^{tracking}_{local}*[\mathbf {GravityAlignment}^{gravity}_{local}]^{-1} Robotgravitytracking=Robotlocaltracking∗[GravityAlignmentlocalgravity]−1(01)其上的 pose_prediction = R o b o t g r a v i t y t r a c k i n g \mathbf {Robot}^{tracking}_{gravity} Robotgravitytracking 表示重力gravity系下机器人的位姿。
3、重力对齐水平点云
// Step: 7 对 returns点云 进行自适应体素滤波,返回的点云的数据类型是PointCloud
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
options_.adaptive_voxel_filter_options());
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
该处代码的 gravity_aligned_range_data.returns 点云数据已经进行过重力矫正矫正了,所以重新构建的点的filtered_gravity_aligned_point_cloud 同样表示经过重力矫正的点云。这里记为 p o i n t s g r a v i t y points^{gravity} pointsgravity。
4、扫描匹配校准位姿
// local map frame <- gravity-aligned frame
// 扫描匹配, 进行点云与submap的匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
扫描匹配的本质是对先验位姿进行矫正,也就对 pose_prediction 位姿进行优化。优化之后的位姿 pose_estimate_2d 这里记为 R o b o t ′ g r a v i t y t r a c k i n g {Robot'}^{tracking}_{gravity} Robot′gravitytracking。
5、机器人位姿恢复到 local 系
// 将二维坐标旋转回之前的姿态
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
// 校准位姿估计器
extrapolator_->AddPose(time, pose_estimate);
优化过的位姿 R o b o t ′ g r a v i t y t r a c k i n g {Robot'}^{tracking}_{gravity} Robot′gravitytracking 是基于 gravity 系的,我们需要把其恢复到 local 系下,恢复之后的位姿记为 R o b o t ′ l o c a l t r a c k i n g {Robot'}^{tracking}_{local} Robot′localtracking,那么对应的数学公式如下所示: R o b o t ′ l o c a l t r a c k i n g = R o b o t ′ g r a v i t y t r a c k i n g ∗ G r a v i t y A l i g n m e n t l o c a l g r a v i t y (02) \color{Green} \tag{02} {Robot'}^{tracking}_{local}={Robot'}^{tracking}_{gravity}*\mathbf {GravityAlignment}^{gravity}_{local} Robot′localtracking=Robot′gravitytracking∗GravityAlignmentlocalgravity(02)
6、点云数据姿恢复到 local 系
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
range_data_in_local 表示 local 系下的点云数据记为 p o i n t s l o c a l points^{local} pointslocal,那么还需要把 gravity_aligned_range_data= p o i n t s g r a v i t y points^{gravity} pointsgravity,也就是 gravity 系下的点云数据,变换到 local 系下,那么对应的数学公式如下:
p o i n t s l o c a l = R o b o t g r a v i t y l o c a l ∗ p o i n t s g r a v i t y (03) \color{Green} \tag{03} points^{local}={Robot}^{local}_{gravity}*points^{gravity} pointslocal=Robotgravitylocal∗pointsgravity(03)虽然代码中 gravity_aligned_range_data 是放在函数的左边,但是实际在计算的时候是按上面的公式计算的,也就是放在右边的。
7、计算cpu耗时
// 计算cpu耗时
const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
if (last_thread_cpu_time_seconds_.has_value()) {
const double thread_cpu_duration_seconds =
thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
if (sensor_duration.has_value()) {
kLocalSlamCpuRealTimeRatio->Set(
common::ToSeconds(sensor_duration.value()) /
thread_cpu_duration_seconds);
}
}
last_wall_time_ = wall_time;
last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
该部分的代码不涉及到算法,就不再进行讲解了,单纯的逻辑处理。
三、框架梳理2→LocalTrajectoryBuilder2D::ScanMatch()
上面的讲解 4、扫描匹配校准位姿,其调用了函数 LocalTrajectoryBuilder2D::ScanMatch(),该函数有两个形参①pose_prediction→先验位姿(待优化);②filtered_gravity_aligned_point_cloud→经过重力矫正之后的点云数据。该函数的主体流程如下:
( 1 ) \color{blue}(1) (1) 从活跃的子图 active_submaps_ 中获得第一个子图,用于后续的扫描匹配。这里需要会议一下前面的内容,active_submaps_ 中最多保存两个子图。
( 2 ) \color{blue}(2) (2) 判断 use_online_correlative_scan_matching 参数,该参数在:
src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_3d.lua
文件中进行配置。如果配置为 true,则会在进行正式扫描匹配之前,进行一次相关性扫描匹配(correlative scan matching),实际上就是暴力搜索匹配。十分的耗时,不过可以对先验进行初步的优化,令后续扫描匹配的结果更加精准,下篇博客会进行详细的介绍。
( 3 ) \color{blue}(3) (3) 使用ceres进行扫描匹配,该部分十分的重要,后续会进行详细介绍。
( 4 ) \color{blue}(4) (4) 使用 metrics::Histogram 把数据都记录下来,优化后位姿 pose_observation 与 优化前位姿 pose_prediction 的平移差,或者角度差,这些数据都可以保存下来进行度量,或者可视化。
代码注释如下所示:
/**
* @brief 进行扫描匹配
*
* @param[in] time 点云的时间
* @param[in] pose_prediction 先验位姿
* @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云
* @return std::unique_ptr<transform::Rigid2d> 匹配后的二维位姿
*/
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);
}
// 使用active_submaps_的第一个子图进行匹配
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;
// 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
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);
}
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
// 使用ceres进行扫描匹配
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);
}
// 返回ceres计算后的位姿
return pose_observation;
}
四、结语
通过该篇博客,知道了扫描匹配的总体调用流程,当然,还没有对其进行细致的讲解。根据上面的分析,可以知道,扫描匹配主调部分都集中在 LocalTrajectoryBuilder2D::ScanMatch() 函数中,其中有如下两个部分是十分重要的:
// 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
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进行扫描匹配
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
下一篇博客,我们来讲解一下 RealTimeCorrelativeScanMatcher2D real_time_correlative_scan_matcher_ ,相关性扫描匹配,其原理就是进行遍历的暴力匹配。