講解關(guān)于slam一系列文章匯總鏈接:史上最全slam從零開始，針對于本欄目講解(02)Cartographer源碼無死角解析-鏈接如下:
(02)Cartographer源碼無死角解析- (00)目錄_最新無死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?
$文末正下方中心提供了本人聯(lián)系方式，點(diǎn)擊本人照片即可顯示WX\to 官方認(rèn)證$
?

一、前言(Scan match原理)

通過前面你的一系列博客，已經(jīng)知道 Cartographer 中的概率柵格圖是如何建立的了。不過需要注意的一點(diǎn)是該地圖并不僅僅是保存下來給來看的，其還會被點(diǎn)云掃描匹配使用到，點(diǎn)云掃描匹配目的是估算位姿(該部分內(nèi)容后面會詳細(xì)講解)。在 slam 中分為 Scan match 與 Point cloud match，通常情況下前者指的就是2D掃描匹配，后者指的是3D點(diǎn)云匹配。為了方便后續(xù)代碼的理解，這里簡單介紹一下掃描匹配的原理，先來看下圖(理想位姿)：

圖一：理想位姿

1、方格 → 所有的柵格組成。 2、黑色方格 → 障礙物，光子會被阻礙，形成點(diǎn)云數(shù)據(jù)。 3、白色方格 → 空閑區(qū)域，表示沒有障礙物，光子可以直接穿過。 4、紫色多邊形 → 代表機(jī)器人，當(dāng)然也可以理解為雷達(dá)傳感器原點(diǎn)，或者點(diǎn)云數(shù)據(jù)的原點(diǎn)。 5、黃色圓形 → 點(diǎn)云數(shù)據(jù)。

從上圖來看，可見所有的點(diǎn)云數(shù)據(jù)都與障礙物重疊了，說明此時估算出來的位姿十分正確，這里稱為理想位置。但是系統(tǒng)在運(yùn)行的過程中并非是這樣的。這里假設(shè)我們由傳感器數(shù)據(jù)(Imu、GPS、里程計) 等估算出來的位姿如下所示：

圖二：初始位姿 容易看出上圖估算出來的位姿是存在偏差的，假設(shè)傳感器估算出來的初始位姿，是不太準(zhǔn)確的，那么問題就來了，如何根據(jù)初始位姿求解出理想位姿呢？首先，通常來說理想位姿需要在初始位姿的一定范圍之內(nèi)(也就是說，傳感器的誤差不能太大了)，基于這個前提，再來分析上圖。

根據(jù)圖示可以看出初始位姿與理想位姿在位置與姿態(tài)上都存在差異，也就是說由初始位姿變換到理想位姿，其平移與旋轉(zhuǎn)可能都需要發(fā)生改變。基于這個原理，最簡單的一種掃描方式就是暴力匹配，也就是在位移與角度上維度上進(jìn)行遍歷，流程如下：

1、設(shè)定初始位姿 2、位移掃描匹配遍歷 3、角度掃描匹配遍歷。 4、評分標(biāo)準(zhǔn)

初始位姿可以直接由傳感器估算出來，先來看下圖，

圖三：位姿遍歷

綠色區(qū)域 1-6 表示需要遍歷的區(qū)域，也就是理想位姿的平移處于該區(qū)域內(nèi)，也就是位移掃描匹配需要遍歷綠色區(qū)域1-6。藍(lán)色的箭頭表示角度遍歷的方向。該箭頭的起點(diǎn)就是角度開始遍歷的起點(diǎn)，終點(diǎn)就是角度遍歷的終點(diǎn)。

如上圖所示，假設(shè)從從1號方格開始第一次遍歷，Robot角度為-35°(假設(shè)平行y軸為0°)，記為 1_(-35)。需要主意的是，Robot 與點(diǎn)云需要一直保持相對禁止，Robot位姿如何變換，點(diǎn)云則需做同樣的變換。在 1_(-35) 的位置上，我們希望所有的點(diǎn)云都能夠與黑色方格(障礙物)完全匹配，但是顯然是不行的，所以改變一下角度，這里假設(shè)角分辨率為5°，那么下次利用 1_(-30) 的 Robot 與點(diǎn)云位姿與黑色方格進(jìn)行匹配，依此一下，進(jìn)行位姿和角度上的遍歷，如下：

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 與點(diǎn)云位姿與黑色方格進(jìn)行匹配之后，我們都需要進(jìn)行一次評分，在這個例子中會匹配6x(70/5)=6x14=84次，也就是說這里需要進(jìn)行84次評分。那么問題來了，如何進(jìn)行評分，才能體現(xiàn)出匹配結(jié)果的優(yōu)劣呢？

從上圖可以很直觀的看出點(diǎn)云與障礙物重疊度越高，則匹配效果越好。結(jié)合前面的內(nèi)容，這里假設(shè)存在 $n$ 個點(diǎn)云，其每個點(diǎn)云對應(yīng)的方格記為 $cel{l}_i$，$cel{l}_i$ 對應(yīng)被占用的概率為 $p_i$，那么每次匹配的得分可以記為 $s_x=({\sum }_{i=1}^n{p}_i)/n$。然后認(rèn)為所有匹配(84次)得分中最高的 $s_{max}$ 為最優(yōu)匹配。也就是圖一的理想匹配。

$注意$ 這里講解的僅僅是一種十分簡單的方式，實(shí)際工程的實(shí)現(xiàn)并非如此簡單，需要考慮很多的東西，比如通常情況下，點(diǎn)云與黑色方格很難完全匹配，也就是說不存在精確解，那么只能通過迭代的方式求得最優(yōu)解。除此之外，還有涉及到其他的很多技巧，后續(xù)會進(jìn)行部分講解。

Cartographer 中的掃描匹配主要是參考：A Flexible and Scalable SLAM System with Full 3D Motion Estimation 實(shí)現(xiàn)，有興趣的朋友可以好好閱讀一下。
?

二、框架梳理1→AddAccumulatedRangeData()

下面來看看源碼中是如何進(jìn)行掃描匹配的，該篇不可不對細(xì)節(jié)進(jìn)行刨析，了解基本流程即可，下一篇博客再進(jìn)行細(xì)節(jié)上的分析。首先找到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函數(shù)：

1、獲得點(diǎn)云time時刻機(jī)器人位姿

// Computes a gravity aligned pose prediction.// 進(jìn)行位姿的預(yù)測, 先驗(yàn)位姿zconst transform::Rigid3d non_gravity_aligned_pose_prediction =extrapolator_->ExtrapolatePose(time);

該函數(shù)是獲得點(diǎn)云數(shù)據(jù)time時刻機(jī)器人的位姿，其是先對于 local 坐標(biāo)系的，這里記 non_gravity_aligned_pose_prediction 為 ${\mathbf{Robot}}_{local}^{tracking}$。表示 tracking_frame 在 local 系中的位姿。
?

2、機(jī)器人位姿重力校正

const transform::Rigid2d pose_prediction = transform::Project2D(non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

gravity_alignment 在前面詳細(xì)分析過，這里記為 ${\mathbf{GravityAlignment}}_{local}^{gravity}$，該矩陣的逆 $[{\mathbf{GravityAlignment}}_{local}^{gravity}{]}^{?1}={\mathbf{GravityAlignment}}_{local}^{gravity}$，可以對 local 系下的位姿或者點(diǎn)云數(shù)據(jù)進(jìn)行重力校正，也就是把local系的數(shù)據(jù)變換到gravity系下，gravity坐標(biāo)系原點(diǎn)與local坐標(biāo)系重疊，但是gravity坐標(biāo)系的 Z 軸是與重力方向垂直的，上述的的代碼就是對推斷器推斷出來的位姿進(jìn)行重力矯正，對應(yīng)的數(shù)學(xué)公式如下：$${\mathbf{Robot}}_{gravity}^{tracking}={\mathbf{Robot}}_{local}^{tracking}?[{\mathbf{GravityAlignment}}_{local}^{gravity}{]}^{?1}\text{\hspace{1em}(01)}$$其上的 pose_prediction = ${\mathbf{Robot}}_{gravity}^{tracking}$ 表示重力gravity系下機(jī)器人的位姿。
?

3、重力對齊水平點(diǎn)云

// Step: 7 對 returns點(diǎn)云 進(jìn)行自適應(yīng)體素濾波，返回的點(diǎn)云的數(shù)據(jù)類型是PointCloudconst 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 點(diǎn)云數(shù)據(jù)已經(jīng)進(jìn)行過重力矯正矯正了，所以重新構(gòu)建的點(diǎn)的filtered_gravity_aligned_point_cloud 同樣表示經(jīng)過重力矯正的點(diǎn)云。這里記為 $point{s}^{gravity}$。
?

4、掃描匹配校準(zhǔn)位姿

// local map frame <- gravity-aligned frame// 掃描匹配, 進(jìn)行點(diǎn)云與submap的匹配std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

掃描匹配的本質(zhì)是對先驗(yàn)位姿進(jìn)行矯正，也就對 pose_prediction 位姿進(jìn)行優(yōu)化。優(yōu)化之后的位姿 pose_estimate_2d 這里記為 ${Robo{t}^{\prime }}_{gravity}^{tracking}$。
?

5、機(jī)器人位姿恢復(fù)到 local 系

// 將二維坐標(biāo)旋轉(zhuǎn)回之前的姿態(tài)const transform::Rigid3d pose_estimate =transform::Embed3D(*pose_estimate_2d) * gravity_alignment;// 校準(zhǔn)位姿估計器extrapolator_->AddPose(time, pose_estimate);

優(yōu)化過的位姿 ${Robo{t}^{\prime }}_{gravity}^{tracking}$ 是基于 gravity 系的，我們需要把其恢復(fù)到 local 系下，恢復(fù)之后的位姿記為 ${Robo{t}^{\prime }}_{local}^{tracking}$，那么對應(yīng)的數(shù)學(xué)公式如下所示：$${\mathbf{Robo}{\mathbf{t}}^{\prime }}_{local}^{tracking}={\mathbf{Robo}{\mathbf{t}}^{\prime }}_{gravity}^{tracking}?{\mathbf{GravityAlignment}}_{local}^{gravity}\text{\hspace{1em}(02)}$$

6、點(diǎn)云數(shù)據(jù)姿恢復(fù)到 local 系

sensor::RangeData range_data_in_local =TransformRangeData(gravity_aligned_range_data,transform::Embed3D(pose_estimate_2d->cast<float>()));

range_data_in_local 表示 local 系下的點(diǎn)云數(shù)據(jù)記為 $point{s}^{local}$，那么還需要把 gravity_aligned_range_data= $point{s}^{gravity}$，也就是 gravity 系下的點(diǎn)云數(shù)據(jù)，變換到 local 系下，那么對應(yīng)的數(shù)學(xué)公式如下：
$$point{s}^{local}={\mathbf{Robot}}_{gravity}^{local}?point{s}^{gravity}\text{\hspace{1em}(03)}$$雖然代碼中 gravity_aligned_range_data 是放在函數(shù)的左邊，但是實(shí)際在計算的時候是按上面的公式計算的，也就是放在右邊的。
?

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;

該部分的代碼不涉及到算法，就不再進(jìn)行講解了，單純的邏輯處理。
?

三、框架梳理2→LocalTrajectoryBuilder2D::ScanMatch()

上面的講解 4、掃描匹配校準(zhǔn)位姿，其調(diào)用了函數(shù) LocalTrajectoryBuilder2D::ScanMatch()，該函數(shù)有兩個形參①pose_prediction→先驗(yàn)位姿(待優(yōu)化)；②filtered_gravity_aligned_point_cloud→經(jīng)過重力矯正之后的點(diǎn)云數(shù)據(jù)。該函數(shù)的主體流程如下：

$(1)$ 從活躍的子圖 active_submaps_ 中獲得第一個子圖，用于后續(xù)的掃描匹配。這里需要會議一下前面的內(nèi)容，active_submaps_ 中最多保存兩個子圖。

$(2)$ 判斷 use_online_correlative_scan_matching 參數(shù)，該參數(shù)在：

src/cartographer/configuration_files/trajectory_builder_2d.lua src/cartographer/configuration_files/trajectory_builder_3d.lua

文件中進(jìn)行配置。如果配置為 true，則會在進(jìn)行正式掃描匹配之前，進(jìn)行一次相關(guān)性掃描匹配(correlative scan matching)，實(shí)際上就是暴力搜索匹配。十分的耗時，不過可以對先驗(yàn)進(jìn)行初步的優(yōu)化，令后續(xù)掃描匹配的結(jié)果更加精準(zhǔn)，下篇博客會進(jìn)行詳細(xì)的介紹。

$(3)$ 使用ceres進(jìn)行掃描匹配，該部分十分的重要，后續(xù)會進(jìn)行詳細(xì)介紹。

$(4)$ 使用 metrics::Histogram 把數(shù)據(jù)都記錄下來，優(yōu)化后位姿 pose_observation 與 優(yōu)化前位姿 pose_prediction 的平移差，或者角度差，這些數(shù)據(jù)都可以保存下來進(jìn)行度量，或者可視化。

代碼注釋如下所示：

/*** @brief 進(jìn)行掃描匹配* * @param[in] time 點(diǎn)云的時間* @param[in] pose_prediction 先驗(yàn)位姿* @param[in] filtered_gravity_aligned_point_cloud 匹配用的點(diǎn)云* @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_的第一個子圖進(jìn)行匹配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;// 根據(jù)參數(shù)決定是否 使用correlative_scan_matching對先驗(yàn)位姿進(jìn)行校準(zhǔn)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進(jìn)行掃描匹配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; }

?

四、結(jié)語

通過該篇博客，知道了掃描匹配的總體調(diào)用流程，當(dāng)然，還沒有對其進(jìn)行細(xì)致的講解。根據(jù)上面的分析，可以知道，掃描匹配主調(diào)部分都集中在 LocalTrajectoryBuilder2D::ScanMatch() 函數(shù)中，其中有如下兩個部分是十分重要的：

// 根據(jù)參數(shù)決定是否 使用correlative_scan_matching對先驗(yàn)位姿進(jìn)行校準(zhǔn)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進(jìn)行掃描匹配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_ ，相關(guān)性掃描匹配，其原理就是進(jìn)行遍歷的暴力匹配。

?
?
?

總結(jié)

以上是生活随笔為你收集整理的(02)Cartographer源码无死角解析-(48) 2D点云扫描匹配→扫描匹配基本原理讲解，代码总体框架梳理AddAccumulatedRangeData()的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。