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

一、前言

上一篇博客中對類 SearchParameters 進行了詳細的介紹，同時對 src/cartographer/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc 文件中的 RealTimeCorrelativeScanMatcher2D::Match() 進行了大致的講解。下面對 RealTimeCorrelativeScanMatcher2D 的各個函數進行一個具體的分析。

其給構造函數十分簡單，就不單獨講解，主要就是獲取如下配置參數然后賦值給成員變量 options_：

-- 是否使用 real_time_correlative_scan_matcher 為ceres提供先驗信息-- 計算復雜度高 , 但是很魯棒 , 在odom或者imu不準時依然能達到很好的效果use_online_correlative_scan_matching = false,real_time_correlative_scan_matcher = {linear_search_window = 0.1, -- 線性搜索窗口的大小angular_search_window = math.rad(20.), -- 角度搜索窗口的大小translation_delta_cost_weight = 1e-1, -- 用于計算各部分score的權重rotation_delta_cost_weight = 1e-1,},

不過再分析 RealTimeCorrelativeScanMatcher2D 的各個函數之前，需要先把 correlative_scan_matcher_2d.cc 文件中的如下兩個函數進行講解：

// 生成按照不同角度旋轉后的點云集合 std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud& point_cloud,const SearchParameters& search_parameters) {......}// 將旋轉后的點云集合按照預測出的平移量進行平移, 獲取平移后的點在地圖中的索引 std::vector<DiscreteScan2D> DiscretizeScans(const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,const Eigen::Translation2f& initial_translation) {......}

為了方便后續的講解，這里把上一篇博客的圖示粘貼一下：

圖1

?

二、GenerateRotatedScans()

該函數需要傳遞兩個參數：①point_cloud→點云數據；②search_parameters→已經計算好的搜索配置參數。該函數的邏輯比較簡單，就是對點云數據做多次旋轉，每次旋轉度數都是在上一次旋轉的基礎上再增加角分辨率度數。總的旋轉次數為 search_parameters.num_scans，該參數在上一篇博客中提到過，如下：

// 范圍除以分辨率得到個數num_angular_perturbations =std::ceil(angular_search_window / angular_perturbation_step_size);// num_scans是要生成旋轉點云的個數, 將 num_angular_perturbations 擴大了2倍num_scans = 2 * num_angular_perturbations + 1;

需要注意的是，這里的 point_cloud 點云數據是相對于機器人的，且已經進行過重力矯正，所以對該點云的旋轉只需要繞z軸即可，記 scan_index=i 次旋轉之后的點云為 $point{s}_{scan\_i}^{tracking}$，初始點云 $point\_cloud=point{s}_{init}^{tracking}$，那么使用數學公式表示如下：
$$point{s}_{scan\_i}^{tracking}={\mathbf{R}}_{init}^{scan\_i}?point{s}_{init}^{tracking}\text{\hspace{1em}(01)}$$
其上的 ${\mathbf{R}}_{init}^{scan\_i}$ 等價于源碼中的 transform::Rigid3f::Rotation(Eigen::AngleAxisf(delta_theta, Eigen::Vector3f::UnitZ()))。每次變換之后的結果都存儲于 rotated_scans 中，遍歷完成之后返回該變量，源碼注釋如下：

// 生成按照不同角度旋轉后的點云集合 std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud& point_cloud,const SearchParameters& search_parameters) {std::vector<sensor::PointCloud> rotated_scans;// 生成 num_scans 個旋轉后的點云rotated_scans.reserve(search_parameters.num_scans);// 起始角度double delta_theta = -search_parameters.num_angular_perturbations *search_parameters.angular_perturbation_step_size;// 進行遍歷，生成旋轉不同角度后的點云集合for (int scan_index = 0; scan_index < search_parameters.num_scans;++scan_index,delta_theta += search_parameters.angular_perturbation_step_size) {// 將 point_cloud 繞Z軸旋轉了delta_thetarotated_scans.push_back(sensor::TransformPointCloud(point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(delta_theta, Eigen::Vector3f::UnitZ()))));}return rotated_scans; }

?

三、DiscretizeScans()

該函數主要的功能是對傳入的點云數據做一個平移，實際上調用該函數，是把點云數據變換到 local 坐標系虛下，其需要傳遞三個函數：
①map_limits→用于獲取點云數據
②scans→通常情況下就是上一函數 GenerateRotatedScans() 的返回結果，存儲 num_scans 幀不同角度的點云數據；
③initial_translation→所有點云數據需要平移的數量參數

源碼中會進行兩層遍歷，第一層遍歷for循環，其會獲得每個角度的所有點云數據 scan；第二層遍歷for循環，對單個角度下的每個點云數據進行處理，其處理角較為簡單，就是進行簡單的平移，并且計算出平移之后點云數據在柵格地圖中的二維索引(坐標)，存儲于變量 discrete_scans 中返回。源碼注釋如下：

// 將旋轉后的點云集合按照預測出的平移量進行平移, 獲取平移后的點在地圖中的索引 std::vector<DiscreteScan2D> DiscretizeScans(const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,const Eigen::Translation2f& initial_translation) {// discrete_scans的size 為 旋轉的點云的個數std::vector<DiscreteScan2D> discrete_scans;discrete_scans.reserve(scans.size());for (const sensor::PointCloud& scan : scans) {// discrete_scans中的每一個 DiscreteScan2D 的size設置為這一幀點云中所有點的個數discrete_scans.emplace_back();discrete_scans.back().reserve(scan.size());// 點云中的每一個點進行平移, 獲取平移后的柵格索引for (const sensor::RangefinderPoint& point : scan) {// 對scan中的每個點進行平移const Eigen::Vector2f translated_point =Eigen::Affine2f(initial_translation) * point.position.head<2>();// 將旋轉后的點 對應的柵格的索引放入discrete_scansdiscrete_scans.back().push_back(map_limits.GetCellIndex(translated_point));}}return discrete_scans; }

?

四、GenerateExhaustiveSearchCandidates()

對 correlative_scan_matcher_2d.cc 文件中的兩個函數分析完之后，來看看類 RealTimeCorrelativeScanMatcher2D 中的函數，首先要講解的就是 GenerateExhaustiveSearchCandidates()。從函數命名來看，表示使用窮舉的方式生成候選者。那么，下面就來看看其具體是如何實現的。

$(1)$ 這里把角度遍歷與范圍遍歷組合起來的結果，稱為候選解，最終的目的就是從這些候選解中找到最優者。在這之前，該函數首先計算出候選解的個數，上一篇博客中，提到了 SearchParameters::linear_bounds 成員變量，該變量描述的是需要遍歷的區域，也就是 圖1 中的藍色正方形區域，其以像素(柵格)為單位。遍歷區域的柵格數目 為 num_linear_x_candidates*num_linear_y_candidates，其本質就是以像素(柵格)為單位，求面積。總的候選解還要乘以 search_parameters.num_scans，源碼中使用加法的方式，應該也是一樣的效果。

$(2)$ 創建一個保存候選解的變量 std::vector<Candidate2D> candidates，每個候選解都是 Candidate2D 類型，創建其實例對象需要參數： ①scan_index→角度遍歷的索引；②x_index_offset, y_index_offset→偏移值，這里可以理解為確定搜索區域的原點之后，先對于該原點的偏移值，注意，其以像素(柵格)為單位。

$(3)$ 使用三個for循環進行遍歷，外面的兩個循環用于控制搜索(遍歷)區域，最里面的循環用于控制角度搜索。這里就完成了 圖1 中藍色正方形區域每個位置及其角度的搜尋，角度的范圍由配置文件中的 angular_search_window 參數進行控制。

該函數所有候選解都存儲于 candidates 變量中，讓后返回，源碼如下：

// 生成所有的候選解 std::vector<Candidate2D> RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(const SearchParameters& search_parameters) const {int num_candidates = 0;// 計算候選解的個數for (int scan_index = 0; scan_index != search_parameters.num_scans;++scan_index) {const int num_linear_x_candidates =(search_parameters.linear_bounds[scan_index].max_x -search_parameters.linear_bounds[scan_index].min_x + 1);const int num_linear_y_candidates =(search_parameters.linear_bounds[scan_index].max_y -search_parameters.linear_bounds[scan_index].min_y + 1);num_candidates += num_linear_x_candidates * num_linear_y_candidates;}std::vector<Candidate2D> candidates;candidates.reserve(num_candidates);// 生成候選解, 候選解是由像素坐標的偏差組成的for (int scan_index = 0; scan_index != search_parameters.num_scans;++scan_index) {for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;++x_index_offset) {for (int y_index_offset =search_parameters.linear_bounds[scan_index].min_y;y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;++y_index_offset) {candidates.emplace_back(scan_index, x_index_offset, y_index_offset,search_parameters);}}}CHECK_EQ(candidates.size(), num_candidates);return candidates; }

?

五、ScoreCandidates()

在對該函數進行講解之前，需要提前知道一些東西，那就是 Cartographer 實現相關性暴力匹配匹配的時候，候選解的得分其與距離存在聯系，先來看下圖：

圖2
假設矩形的中心表示通過傳感器或者其他方式初步估算出來的位姿，會在附近進行移動旋轉搜索，嘗試找到最優的候選解，不過從源碼的實現來看，其還是比較信任初步估算出來的位姿，因為在距離初始位姿越遠，則其得分的權重越低，相對于初始位姿旋轉角度越大，其得分的權重也越低(如上圖所示，偏離綠色的角度越大)。首先來看 ScoreCandidates() 函數中的如下代碼：

// 對得分進行加權candidate.score *=std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *options_.translation_delta_cost_weight() +std::abs(candidate.orientation) *options_.rotation_delta_cost_weight()));

其上的 std::hypot(candidate.x, candidate.y) 就是計算與初始位姿的距離長度，candidate.orientation 表示相對于初始位姿的旋轉。options_.translation_delta_cost_weight() 與 options_.rotation_delta_cost_weight() 可在配置文件中配置，默認都是權重都是1。

其上使用的是 std::exp 函數，可以很容易的看出，當距離初始位姿非常近，旋轉角度非常少的時候，該函數的結果趨向于 $e^0=1$，其是符合原理的。另外在 ScoreCandidates() 函數中，根據 grid.GetGridType() 類型調用 ComputeCandidateScore() 函數，本人執行的代碼是如下部分：

candidate.score = ComputeCandidateScore(static_cast<const ProbabilityGrid&>(grid),discrete_scans[candidate.scan_index], candidate.x_index_offset,candidate.y_index_offset);

?

六、ComputeCandidateScore()

該函數同樣實現于 real_time_correlative_scan_matcher_2d.cc 文件中，非成員函數，其存在兩個重載，這里只對第二個進行講解，這里先給出代碼注釋：

// 計算點云在指定像素坐標位置下與ProbabilityGrid地圖匹配的得分 float ComputeCandidateScore(const ProbabilityGrid& probability_grid,const DiscreteScan2D& discrete_scan,int x_index_offset, int y_index_offset) {float candidate_score = 0.f;for (const Eigen::Array2i& xy_index : discrete_scan) {// 對每個點都加上像素坐標的offset, 相當于對點云進行平移const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,xy_index.y() + y_index_offset);// 獲取占用的概率const float probability =probability_grid.GetProbability(proposed_xy_index);// 以概率為得分candidate_score += probability;}// 計算平均得分candidate_score /= static_cast<float>(discrete_scan.size());CHECK_GT(candidate_score, 0.f);return candidate_score; }

其傳入得 discrete_scan 是基于 local 坐標系下一個角度的點云數據像素(柵格)坐標，對所有點云坐標進行遍歷，根據該像素坐標獲取其對應柵格的占用率，進行累加然后取平均值。
?

七、結語

對這些函數都分析完成之后，有必要的朋友可以再回去看一下上一篇博客中講解的 RealTimeCorrelativeScanMatcher2D::Match() 函數，結合起來分析，相信對整體的把握就比較好了。那么，對于 LocalTrajectoryBuilder2D::ScanMatc() 函數中的如下部分可以說是講解完成了：

// 根據參數決定是否 使用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);}

initial_ceres_pose 經過初步矯正后存儲于 pose_prediction 之中，score 表示該最優位置的分值。

?
?
?

總結

以上是生活随笔為你收集整理的(02)Cartographer源码无死角解析-(50) 2D点云扫描匹配→相关性暴力匹配2：RealTimeCorrelativeScanMatcher2D的全部內容，希望文章能夠幫你解決所遇到的問題。

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