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

一、前言(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$ 個點云，其每個點云對應的方格記為 $cel{l}_i$，$cel{l}_i$ 對應被占用的概率為 $p_i$，那么每次匹配的得分可以記為 $s_x=({\sum }_{i=1}^n{p}_i)/n$。然后認為所有匹配(84次)得分中最高的 $s_{max}$ 為最優匹配。也就是圖一的理想匹配。

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

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

二、框架梳理1→AddAccumulatedRangeData()

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

1、獲得點云time時刻機器人位姿

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

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

2、機器人位姿重力校正

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

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

3、重力對齊水平點云

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

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 這里記為 ${Robo{t}^{\prime }}_{gravity}^{tracking}$。
?

5、機器人位姿恢復到 local 系

// 將二維坐標旋轉回之前的姿態const transform::Rigid3d pose_estimate =transform::Embed3D(*pose_estimate_2d) * gravity_alignment;// 校準位姿估計器extrapolator_->AddPose(time, pose_estimate);

優化過的位姿 ${Robo{t}^{\prime }}_{gravity}^{tracking}$ 是基于 gravity 系的，我們需要把其恢復到 local 系下，恢復之后的位姿記為 ${Robo{t}^{\prime }}_{local}^{tracking}$，那么對應的數學公式如下所示：$${\mathbf{Robo}{\mathbf{t}}^{\prime }}_{local}^{tracking}={\mathbf{Robo}{\mathbf{t}}^{\prime }}_{gravity}^{tracking}?{\mathbf{GravityAlignment}}_{local}^{gravity}\text{\hspace{1em}(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 系下的點云數據記為 $point{s}^{local}$，那么還需要把 gravity_aligned_range_data= $point{s}^{gravity}$，也就是 gravity 系下的點云數據，變換到 local 系下，那么對應的數學公式如下：
$$point{s}^{local}={\mathbf{Robot}}_{gravity}^{local}?point{s}^{gravity}\text{\hspace{1em}(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)$ 從活躍的子圖 active_submaps_ 中獲得第一個子圖，用于后續的掃描匹配。這里需要會議一下前面的內容，active_submaps_ 中最多保存兩個子圖。

$(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)$ 使用ceres進行掃描匹配，該部分十分的重要，后續會進行詳細介紹。

$(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_ ，相關性掃描匹配，其原理就是進行遍歷的暴力匹配。

?
?
?

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。