1. 從“看懂”到“改懂”為什么我們需要深入R3LIVE的代碼細(xì)節(jié)如果你已經(jīng)跟著教程跑通了R3LIVE的Demo看著點(diǎn)云地圖一點(diǎn)點(diǎn)構(gòu)建出來感覺挺酷那恭喜你入門了。但接下來你可能會(huì)遇到一堆問題為什么我的設(shè)備跑起來幀率這么低這個(gè)參數(shù)調(diào)了好像沒用我想加個(gè)新的傳感器該怎么接想自己改點(diǎn)東西從哪下手這些問題官方文檔和論文往往不會(huì)告訴你答案都藏在代碼里?！按a詳解”系列目的就是幫你跨過“能用”到“能改”這道坎。上一篇我們搭建了環(huán)境理清了數(shù)據(jù)流的主干。這一篇我們要真正扎進(jìn)核心的算法模塊里去看看那些炫酷的實(shí)時(shí)建圖與定位功能到底是怎么一行行代碼實(shí)現(xiàn)的。我會(huì)帶你像讀一個(gè)復(fù)雜工程的源碼一樣不是浮光掠影而是抓住幾個(gè)最關(guān)鍵、最常被問到的“牛鼻子”圖像數(shù)據(jù)的處理流水線、激光雷達(dá)點(diǎn)云的畸變補(bǔ)償與特征提取以及最核心的緊耦合優(yōu)化器。我會(huì)用大量的代碼片段和流程圖文字描述來解釋并分享我在調(diào)試和修改這些模塊時(shí)踩過的坑和總結(jié)的經(jīng)驗(yàn)。2. 視覺前端從RAW圖到光流追蹤的完整流水線R3LIVE的視覺部分承擔(dān)著提供高頻姿態(tài)更新和深度估計(jì)的重任。它的輸入是原始圖像輸出則是用于后續(xù)優(yōu)化的視覺特征點(diǎn)。這個(gè)過程遠(yuǎn)比調(diào)用一個(gè)cv::calcOpticalFlowPyrLK函數(shù)復(fù)雜。2.1 圖像預(yù)處理與金字塔構(gòu)建原始圖像進(jìn)來第一步絕不是直接丟給光流。在R3LIVE的image_process函數(shù)中通常位于某個(gè)visual_processing.cpp或類似文件中你會(huì)發(fā)現(xiàn)它做了以下幾件事去畸變根據(jù)事先標(biāo)定好的相機(jī)內(nèi)參和畸變系數(shù)對(duì)圖像進(jìn)行矯正。這里用的是OpenCV的cv::undistort函數(shù)。一個(gè)常見的坑是標(biāo)定參數(shù)的單位和格式必須與代碼中的cv::Mat矩陣完全匹配否則去畸變后圖像會(huì)變得很奇怪。我曾經(jīng)因?yàn)闃?biāo)定文件里k1 k2 p1 p2 k3的順序和代碼里讀取的順序不一致導(dǎo)致后續(xù)光流追蹤完全失敗查了半天才定位到這里。// 偽代碼示意 cv::Mat intrinsic_mat (cv::Mat_double(3 3) fx 0 cx 0 fy cy 0 0 1); cv::Mat dist_coeffs (cv::Mat_double(1 5) k1 k2 p1 p2 k3); cv::undistort(raw_image undistorted_image intrinsic_mat dist_coeffs);圖像金字塔生成光流追蹤需要在不同尺度上尋找匹配點(diǎn)以提高對(duì)大運(yùn)動(dòng)的魯棒性和計(jì)算效率。R3LIVE會(huì)構(gòu)建一個(gè)高斯金字塔比如4層。底層是原圖或下采樣后的基礎(chǔ)層上層是逐步降采樣的模糊圖像。這里的關(guān)鍵參數(shù)是金字塔層數(shù)(pyramid_levels)和每層的縮放系數(shù)(scale_factor通常為0.5)。層數(shù)越多能處理的幀間位移越大但計(jì)算量也越大。對(duì)于車載或無人機(jī)高速場(chǎng)景我通常會(huì)把層數(shù)設(shè)為4甚至5對(duì)于室內(nèi)機(jī)器人低速場(chǎng)景3層就夠了可以節(jié)省計(jì)算資源。2.2 光流追蹤的核心策略與關(guān)鍵代碼預(yù)處理后的圖像連同上一幀追蹤到的特征點(diǎn)被送入光流模塊。R3LIVE通常采用LK光流法。但直接調(diào)用OpenCV函數(shù)是不夠的需要一套策略來管理特征點(diǎn)的“生死”。在代碼中你會(huì)找到一個(gè)FeatureTracker類或類似的結(jié)構(gòu)體。它的trackImage函數(shù)是核心。我拆解一下它的典型步驟前向光流用上一幀的特征點(diǎn)prev_pts在當(dāng)前幀金字塔上追蹤得到curr_pts。反向光流校驗(yàn)這是一個(gè)非常有效的去外點(diǎn)手段。再用當(dāng)前幀追蹤到的點(diǎn)curr_pts反向到上一幀金字塔進(jìn)行追蹤得到back_pts。然后計(jì)算prev_pts和back_pts的距離如果大于某個(gè)閾值比如0.5個(gè)像素則認(rèn)為這個(gè)追蹤不可靠剔除。F矩陣校驗(yàn)對(duì)于前后幀的點(diǎn)對(duì)計(jì)算基礎(chǔ)矩陣F利用RANSAC剔除不符合極線約束的匹配外點(diǎn)。這一步能過濾掉由于遮擋、重復(fù)紋理等造成的錯(cuò)誤匹配。均勻化與補(bǔ)點(diǎn)經(jīng)過剔除特征點(diǎn)可能變得稀疏或不均勻。代碼里會(huì)有一個(gè)rejectWithF和uniformDistribution函數(shù)。它會(huì)將圖像劃分成網(wǎng)格如10x10的格子確保每個(gè)格子里有一定數(shù)量的特征點(diǎn)。如果某個(gè)格子點(diǎn)太少就會(huì)在這個(gè)格子內(nèi)用cv::goodFeaturesToTrack檢測(cè)新的角點(diǎn)進(jìn)行補(bǔ)充。這里有一個(gè)重要的實(shí)操細(xì)節(jié)特征點(diǎn)ID的管理。每個(gè)被追蹤的特征點(diǎn)都有一個(gè)唯一ID。在補(bǔ)點(diǎn)時(shí)新點(diǎn)會(huì)被賦予新的ID追蹤成功的點(diǎn)保留原有ID。這個(gè)ID會(huì)一直傳遞到后端優(yōu)化用于構(gòu)建重投影誤差。如果ID管理混亂后端的數(shù)據(jù)關(guān)聯(lián)就會(huì)出錯(cuò)。在閱讀代碼時(shí)務(wù)必理清ids這個(gè)向量是如何隨著光流追蹤而更新和傳遞的。2.3 視覺深度估計(jì)與點(diǎn)云生成追蹤到的2D像素點(diǎn)需要轉(zhuǎn)換成3D空間點(diǎn)才能用于建圖。這就是視覺深度估計(jì)模塊的工作。在R3LIVE中深度主要來源于兩種方式激光雷達(dá)深度關(guān)聯(lián)這是R3LIVE的核心創(chuàng)新之一。對(duì)于當(dāng)前幀的每個(gè)視覺特征點(diǎn)代碼會(huì)去查找同一時(shí)刻或時(shí)間戳對(duì)齊后的激光雷達(dá)點(diǎn)云。利用當(dāng)前的姿態(tài)估計(jì)來自IMU預(yù)測(cè)或上一輪優(yōu)化將視覺點(diǎn)反向投影到3D空間然后在投影線附近的一定半徑內(nèi)搜索最近鄰的激光點(diǎn)。如果找到則該激光點(diǎn)的深度就作為這個(gè)視覺特征點(diǎn)的深度。這個(gè)過程在代碼中體現(xiàn)為一個(gè)searchPointCloudForDepth的函數(shù)或循環(huán)。注意這里的“半徑”參數(shù)非常關(guān)鍵。設(shè)大了容易關(guān)聯(lián)到錯(cuò)誤的、非對(duì)應(yīng)的物體上的點(diǎn)引入噪聲設(shè)小了很多點(diǎn)會(huì)因找不到對(duì)應(yīng)深度而被丟棄導(dǎo)致特征利用率低。我的經(jīng)驗(yàn)是這個(gè)半徑需要根據(jù)激光雷達(dá)的精度和點(diǎn)云密度來調(diào)整。對(duì)于16線雷達(dá)在5-10米內(nèi)0.1-0.2米是個(gè)不錯(cuò)的起點(diǎn)對(duì)于固態(tài)激光雷達(dá)或高線數(shù)雷達(dá)可以更小一些。三角化對(duì)于無法從激光雷達(dá)獲得深度的點(diǎn)比如遠(yuǎn)處或雷達(dá)盲區(qū)如果這個(gè)特征點(diǎn)被連續(xù)多幀追蹤到則可以通過多視圖幾何三角化的方式估計(jì)其深度。代碼中會(huì)調(diào)用cv::triangulatePoints函數(shù)。經(jīng)過深度估計(jì)后具有有效深度的視覺特征點(diǎn)就被轉(zhuǎn)換為3D點(diǎn)并和激光點(diǎn)云一起送入后續(xù)的融合建圖模塊。一個(gè)常見的性能瓶頸就在這里對(duì)每一個(gè)視覺點(diǎn)進(jìn)行最近鄰搜索尤其是暴力搜索是非常耗時(shí)的。在代碼優(yōu)化時(shí)可以考慮對(duì)當(dāng)前幀的激光點(diǎn)云建立KD-Tree能大幅加速搜索過程。3. 激光雷達(dá)前端點(diǎn)云去畸變與特征提取激光雷達(dá)在旋轉(zhuǎn)掃描過程中載體本身也在運(yùn)動(dòng)這會(huì)導(dǎo)致點(diǎn)云發(fā)生“運(yùn)動(dòng)畸變”。R3LIVE作為緊耦合系統(tǒng)必須首先糾正這個(gè)畸變?nèi)缓筇崛∮杏玫奶卣鳌?.1 運(yùn)動(dòng)畸變補(bǔ)償原理與插值策略假設(shè)一幀激光點(diǎn)云覆蓋了100ms的時(shí)間在這100ms里機(jī)器人的位置和姿態(tài)發(fā)生了變化。掃描開始時(shí)打到的點(diǎn)和掃描結(jié)束時(shí)打到的點(diǎn)其對(duì)應(yīng)的機(jī)器人位姿是不同的?；冄a(bǔ)償?shù)哪康木褪菍⑦@一幀內(nèi)所有的點(diǎn)都統(tǒng)一補(bǔ)償?shù)酵粋€(gè)時(shí)刻通常是這幀點(diǎn)云的結(jié)束時(shí)間戳或稱為scan_end_time。在代碼中這個(gè)功能通常由一個(gè)LidarProcessor類的distortionCorrection函數(shù)完成。它的輸入是原始點(diǎn)云和一系列IMU數(shù)據(jù)提供了高頻的姿態(tài)變化。其核心步驟如下時(shí)間對(duì)齊為點(diǎn)云中的每個(gè)點(diǎn)point_i計(jì)算其相對(duì)于本幀結(jié)束時(shí)間scan_end_time的時(shí)間差delta_t_i。這個(gè)時(shí)間差通常由雷達(dá)驅(qū)動(dòng)提供如timestamp字段或者是根據(jù)點(diǎn)的掃描角度和雷達(dá)轉(zhuǎn)速推算的。位姿插值我們需要知道在scan_end_time - delta_t_i這個(gè)時(shí)刻機(jī)器人的位姿是什么。由于IMU頻率高如200Hz我們可以利用IMU預(yù)積分或直接插值的方式獲取。R3LIVE通常維護(hù)一個(gè)姿態(tài)緩沖區(qū)state_buffer里面按時(shí)間戳存儲(chǔ)了歷史時(shí)刻的機(jī)器人狀態(tài)位置、姿態(tài)、速度等。通過查找這個(gè)緩沖區(qū)對(duì)兩個(gè)最近時(shí)刻的狀態(tài)進(jìn)行線性插值或球面線性插值對(duì)于旋轉(zhuǎn)得到每個(gè)點(diǎn)對(duì)應(yīng)時(shí)刻的位姿T_i。坐標(biāo)變換設(shè)scan_end_time時(shí)刻的位姿為T_end。那么將點(diǎn)point_i從T_i坐標(biāo)系變換到T_end坐標(biāo)系的公式為point_i_corrected T_end.inverse() * T_i * point_i。這樣所有點(diǎn)就被校正到了同一時(shí)刻的坐標(biāo)系下。這里有一個(gè)極易出錯(cuò)的細(xì)節(jié)姿態(tài)的存儲(chǔ)和插值方式。如果代碼中使用的是四元數(shù)表示旋轉(zhuǎn)插值時(shí)必須使用球面線性插值slerp而不是對(duì)四元數(shù)的四個(gè)分量進(jìn)行線性插值后者會(huì)破壞四元數(shù)的單位約束導(dǎo)致結(jié)果錯(cuò)誤。檢查代碼中是否有Eigen::Quaterniond::slerp或類似函數(shù)的調(diào)用。3.2 特征提取平面點(diǎn)與邊緣點(diǎn)的判別去畸變后的點(diǎn)云接下來要被分類為“平面點(diǎn)”和“邊緣點(diǎn)”。這兩類特征在后端優(yōu)化中分別用于構(gòu)建點(diǎn)到面的距離誤差和點(diǎn)到線的距離誤差。特征提取的算法通常借鑒LOAM系列。在代碼里你會(huì)找到一個(gè)extractFeatures函數(shù)。它的大致邏輯是計(jì)算曲率對(duì)于點(diǎn)云中的每個(gè)點(diǎn)i取其前后各5個(gè)點(diǎn)在同一掃描線上計(jì)算這些鄰近點(diǎn)到點(diǎn)i擬合的直線的距離或到擬合的平面的距離這個(gè)距離的某種統(tǒng)計(jì)量如方差被定義為曲率。曲率大的點(diǎn)意味著其局部幾何變化劇烈可能是邊緣曲率小的點(diǎn)局部平坦可能是平面。分類篩選對(duì)每一條掃描線ring上的點(diǎn)根據(jù)曲率排序。選取曲率最大的幾個(gè)點(diǎn)作為候選邊緣點(diǎn)曲率最小的幾個(gè)點(diǎn)作為候選平面點(diǎn)。同時(shí)要避免特征點(diǎn)聚集所以會(huì)施加一個(gè)距離閾值確保選出的特征點(diǎn)在空間上是分散的。遮擋判斷這是一個(gè)非常重要的邏輯。如下圖所示如果點(diǎn)B被選中但點(diǎn)A和點(diǎn)C與點(diǎn)B的夾角很小且距離激光雷達(dá)更近那么點(diǎn)B可能是一個(gè)被遮擋的邊緣比如墻的側(cè)面這種點(diǎn)不穩(wěn)定應(yīng)當(dāng)剔除。代碼中會(huì)計(jì)算點(diǎn)B與其前后點(diǎn)的向量夾角和距離比來進(jìn)行判斷。激光雷達(dá) / \ / \ A C \ / \ / B (可能被遮擋的邊緣點(diǎn))調(diào)參經(jīng)驗(yàn)特征提取環(huán)節(jié)有幾個(gè)關(guān)鍵參數(shù)curvature_threshold曲率閾值直接影響特征點(diǎn)的數(shù)量。閾值設(shè)低特征點(diǎn)多優(yōu)化約束強(qiáng)但可能引入噪聲閾值設(shè)高特征點(diǎn)少可能約束不足。需要根據(jù)場(chǎng)景調(diào)整。neighbor_count計(jì)算曲率時(shí)考慮的鄰近點(diǎn)數(shù)。通常5-10是個(gè)合理范圍。selected_feature_num_per_scan每條掃描線選取的特征點(diǎn)數(shù)量。這需要平衡計(jì)算量和精度。對(duì)于16線雷達(dá)每條線選2-4個(gè)邊緣點(diǎn)和4-6個(gè)平面點(diǎn)可能是個(gè)起點(diǎn)。提取出的特征點(diǎn)會(huì)被存儲(chǔ)起來并和視覺特征點(diǎn)一起等待后端優(yōu)化器的“召喚”。4. 緊耦合后端優(yōu)化誤差構(gòu)建與求解的魔鬼細(xì)節(jié)這是R3LIVE最核心、也是最復(fù)雜的部分。它接收來自視覺前端和激光前端預(yù)處理好的特征數(shù)據(jù)以及IMU的預(yù)積分量構(gòu)建一個(gè)統(tǒng)一的非線性最小二乘問題然后求解出最優(yōu)的機(jī)器人狀態(tài)。4.1 狀態(tài)向量與IMU預(yù)積分約束優(yōu)化器如Estimator類維護(hù)著一個(gè)滑窗內(nèi)的狀態(tài)向量。狀態(tài)x通常包括位置p(3維)姿態(tài)四元數(shù)q或旋轉(zhuǎn)矩陣R(4維或3維SO3)速度v(3維)陀螺儀零偏b_g(3維)加速度計(jì)零偏b_a(3維)可能還有視覺特征的逆深度λ。IMU預(yù)積分約束連接了滑窗中相鄰兩個(gè)關(guān)鍵幀i和j之間的狀態(tài)。它提供了相對(duì)位置、速度和旋轉(zhuǎn)的變化量Δp Δv Δq這些變化量只與i和j時(shí)刻之間的IMU測(cè)量以及零偏有關(guān)與全局狀態(tài)無關(guān)。在優(yōu)化中IMU約束構(gòu)建的誤差項(xiàng)是e_imu [Δp - pred_Δp Δv - pred_Δv 2*[q_i^{-1} ? q_j ? pred_Δq^{-1}]_xyz]其中pred_Δp pred_Δv pred_Δq是由IMU預(yù)積分理論計(jì)算出的預(yù)測(cè)值。這個(gè)誤差項(xiàng)將i和j時(shí)刻的狀態(tài)緊密耦合在一起。代碼閱讀重點(diǎn)找到IMU預(yù)積分類如IntegrationBase。關(guān)注它如何接收IMU數(shù)據(jù)、進(jìn)行中值積分或歐拉積分、更新預(yù)積分量delta_pdelta_vdelta_q和協(xié)方差矩陣。更重要的是找到它如何提供對(duì)零偏b_ab_g的雅可比矩陣因?yàn)閮?yōu)化過程中需要這些雅可比來更新預(yù)積分量當(dāng)零偏估計(jì)發(fā)生變化時(shí)。4.2 激光雷達(dá)約束點(diǎn)到面與點(diǎn)到線對(duì)于提取的一個(gè)激光平面點(diǎn)p_l我們?cè)跉v史地圖中尋找其對(duì)應(yīng)的平面。具體做法是將p_l根據(jù)當(dāng)前估計(jì)的位姿T變換到世界坐標(biāo)系然后在世界坐標(biāo)系下的全局地圖KD-Tree中尋找p_l最近的5個(gè)點(diǎn)用這5個(gè)點(diǎn)擬合一個(gè)平面π (n d)其中n是單位法向量d是原點(diǎn)到平面的距離。那么點(diǎn)到面的距離誤差就是e_lidar_plane n^T * (T * p_l) d這個(gè)誤差的物理意義是當(dāng)前觀測(cè)到的點(diǎn)p_l應(yīng)該落在它對(duì)應(yīng)的歷史地圖平面π上。同理對(duì)于一個(gè)激光邊緣點(diǎn)我們?cè)跉v史地圖中尋找最近的5個(gè)點(diǎn)用主成分分析PCA判斷這5個(gè)點(diǎn)是否構(gòu)成一條線第二大特征值遠(yuǎn)大于第三大。如果是則用這5個(gè)點(diǎn)擬合一條直線。點(diǎn)到線的距離誤差就是點(diǎn)到這條直線距離的平方。在代碼中這部分體現(xiàn)在LidarMapFactor或LidarPlaneNormFactor這樣的自定義因子類中。你需要關(guān)注它如何重載Evaluate函數(shù)來計(jì)算誤差和雅可比矩陣。雅可比的計(jì)算是重中之重它決定了優(yōu)化收斂的速度和正確性。雅可比矩陣描述了誤差e相對(duì)于狀態(tài)x如位姿T的變化率即de/dx。通常需要用到李代數(shù)擾動(dòng)模型來推導(dǎo)。4.3 視覺約束重投影誤差對(duì)于一個(gè)具有深度λ或3D坐標(biāo)P_w的視覺特征點(diǎn)其誤差構(gòu)建更為直觀。假設(shè)這個(gè)點(diǎn)在第i幀圖像中被觀測(cè)到像素坐標(biāo)為(u_i v_i)。將這個(gè)3D點(diǎn)P_w用當(dāng)前估計(jì)的第i幀相機(jī)位姿T_cam_i變換到相機(jī)坐標(biāo)系P_c T_cam_i * P_w。將P_c投影到歸一化平面p_n (P_c.x / P_c.z P_c.y / P_c.z)。再經(jīng)過相機(jī)內(nèi)參矩陣K投影到像素平面得到預(yù)測(cè)的像素坐標(biāo)(u_pred v_pred)。重投影誤差就是觀測(cè)值與預(yù)測(cè)值的差e_vision [u_i - u_pred v_i - v_pred]^T。在代碼中這對(duì)應(yīng)著ReprojectionFactor或ProjectionFactor類。同樣需要關(guān)注其Evaluate函數(shù)中誤差和雅可比的計(jì)算。視覺誤差的雅可比涉及到對(duì)3D點(diǎn)坐標(biāo)的求導(dǎo)如果狀態(tài)中包含逆深度和對(duì)相機(jī)位姿的求導(dǎo)。4.4 優(yōu)化求解與邊緣化所有的誤差項(xiàng)IMU、激光、視覺構(gòu)建好后就形成了一個(gè)大規(guī)模的非線性最小二乘問題min Σ ||e_i||^2。R3LIVE通常使用Ceres Solver或g2o來求解。求解前需要設(shè)置參數(shù)塊狀態(tài)變量和殘差塊誤差項(xiàng)。一個(gè)關(guān)鍵技巧是參數(shù)塊的參數(shù)化。對(duì)于旋轉(zhuǎn)四元數(shù)必須使用EigenQuaternionParameterization或Ceres的QuaternionParameterization以確保優(yōu)化過程中四元數(shù)保持單位長(zhǎng)度。另一個(gè)核心概念是邊緣化。由于滑窗大小固定當(dāng)新的關(guān)鍵幀到來時(shí)舊的關(guān)鍵幀需要被移出窗口。但不能直接扔掉因?yàn)榕f幀攜帶的信息尤其是IMU預(yù)積分約束著滑窗內(nèi)的狀態(tài)。直接扔掉會(huì)導(dǎo)致約束丟失系統(tǒng)變得不客觀。邊緣化就是將舊幀的狀態(tài)從優(yōu)化變量中消除但將其攜帶的約束信息轉(zhuǎn)化為一個(gè)先驗(yàn)約束作為一個(gè)新的誤差項(xiàng)加到后續(xù)的優(yōu)化問題中。這個(gè)過程在代碼中體現(xiàn)為維護(hù)一個(gè)MarginalizationInfo對(duì)象它會(huì)構(gòu)造一個(gè)舒爾補(bǔ)將待邊緣化的狀態(tài)消元并更新剩余狀態(tài)的Hessian矩陣和b向量。調(diào)試心得優(yōu)化部分出問題最難查。我的方法是打印誤差值在每次迭代開始或結(jié)束時(shí)打印出各類誤差e_imue_lidare_vision的范數(shù)。正常情況下它們應(yīng)該隨著迭代逐漸減小。如果某一項(xiàng)誤差突然變得巨大說明對(duì)應(yīng)的數(shù)據(jù)關(guān)聯(lián)或雅可比計(jì)算可能出錯(cuò)了。檢查雅可比Ceres Solver提供了數(shù)值求導(dǎo)選項(xiàng)。對(duì)于自定義因子可以先使用數(shù)值求導(dǎo)NumericDiffCostFunction來驗(yàn)證你解析推導(dǎo)的雅可比是否正確。如果兩者結(jié)果一致再用解析求導(dǎo)以提升效率?？梢暬虚g結(jié)果將優(yōu)化前的激光匹配關(guān)系、視覺重投影關(guān)系用RVIZ或Pangolin畫出來。肉眼觀察匹配是否正確能快速定位是前端特征關(guān)聯(lián)的問題還是后端優(yōu)化本身的問題。5. 地圖管理體素濾波與增量更新優(yōu)化得到的新幀位姿后需要將當(dāng)前幀處理好的點(diǎn)云包括激光特征點(diǎn)和帶深度的視覺點(diǎn)融合到全局地圖中。R3LIVE通常使用體素網(wǎng)格Voxel Grid來管理地圖以防止地圖無限膨脹并保持實(shí)時(shí)性。5.1 體素濾波融合代碼中會(huì)有一個(gè)VoxelMap或PointCloudMap類。其addPoints函數(shù)負(fù)責(zé)融合新點(diǎn)云。流程如下坐標(biāo)變換將當(dāng)前幀點(diǎn)云根據(jù)優(yōu)化后的位姿T變換到世界坐標(biāo)系。體素化為世界坐標(biāo)系下的每個(gè)點(diǎn)計(jì)算其所屬的體素網(wǎng)格的索引。例如體素分辨率設(shè)為0.1米那么點(diǎn)(1.05 2.13 0.08)就屬于體素(10 21 0)每個(gè)坐標(biāo)除以分辨率后取整。點(diǎn)云融合對(duì)于同一個(gè)體素內(nèi)的所有點(diǎn)包括新來的點(diǎn)和地圖中已有的點(diǎn)不是簡(jiǎn)單地將新點(diǎn)添加進(jìn)去而是進(jìn)行降采樣或取平均。常見策略有體素中心化用該體素內(nèi)所有點(diǎn)的坐標(biāo)平均值作為這個(gè)體素的唯一代表點(diǎn)。隨機(jī)采樣在該體素內(nèi)隨機(jī)保留一個(gè)點(diǎn)。最近點(diǎn)保留保留離體素中心最近的點(diǎn)。第一種方法取平均能有效平滑噪聲是更常用的方法。在代碼中這通常通過一個(gè)std::unordered_map來實(shí)現(xiàn)鍵是體素索引值是一個(gè)結(jié)構(gòu)體存儲(chǔ)該體素內(nèi)點(diǎn)的累加和、數(shù)量以便快速計(jì)算平均值。5.2 地圖的保存與加載一個(gè)實(shí)用的SLAM系統(tǒng)必須支持地圖的保存與重載。R3LIVE的地圖保存通常很簡(jiǎn)單將全局地圖點(diǎn)云經(jīng)過體素濾波后的寫入一個(gè).pcd或.ply文件。同時(shí)可能需要保存一些關(guān)鍵幀的位姿軌跡TUM格式或KITTI格式。更高級(jí)的功能是增量保存與加載。這意味著系統(tǒng)在運(yùn)行時(shí)可以定期將新增的地圖點(diǎn)云追加到磁盤文件而不是每次都保存整個(gè)地圖。加載時(shí)可以快速讀入已有的地圖點(diǎn)云并重建KD-Tree。在相關(guān)代碼文件中你會(huì)看到saveMap和loadMap函數(shù)里面主要是pcl::io::savePCDFile和pcl::io::loadPCDFile的調(diào)用。一個(gè)性能陷阱頻繁地將點(diǎn)云添加到全局地圖并立即重建KD-Tree用于后續(xù)的激光匹配是非常耗時(shí)的。常見的優(yōu)化是設(shè)置一個(gè)緩沖機(jī)制累積一定數(shù)量的點(diǎn)比如一幀或若干幀后再一次性添加到地圖并重建KD-Tree。在代碼中可能會(huì)有一個(gè)map_cloud_buff點(diǎn)云緩沖區(qū)和相關(guān)的計(jì)數(shù)器。6. 實(shí)戰(zhàn)調(diào)試從理論到跑通的最后一步讀懂了代碼最終還是要落到修改和調(diào)試上?；谖叶啻我浦埠驼{(diào)試R3LIVE及其衍生項(xiàng)目的經(jīng)驗(yàn)分享幾個(gè)最具實(shí)操性的建議。6.1 如何高效地修改與驗(yàn)證代碼不要一上來就改核心算法。遵循以下步驟先理解數(shù)據(jù)流在關(guān)鍵函數(shù)的入口和出口添加打印語句或ROS_INFO輸出數(shù)據(jù)的尺寸、時(shí)間戳、前幾個(gè)數(shù)值。確保數(shù)據(jù)如你預(yù)期的那樣流動(dòng)。例如在featureTracker.trackImage()前后打印追蹤到的特征點(diǎn)數(shù)量在lidarProcessor.distortionCorrection()前后打印點(diǎn)云大小。模塊化測(cè)試如果可能將視覺前端、激光前端等模塊單獨(dú)剝離出來編寫簡(jiǎn)單的測(cè)試程序輸入模擬數(shù)據(jù)或錄制好的ROS Bag看輸出是否符合預(yù)期。這能幫你快速定位問題是出在模塊內(nèi)部還是模塊間的接口上。善用調(diào)試工具GDB對(duì)于運(yùn)行時(shí)崩潰段錯(cuò)誤GDB是定位神器。編譯時(shí)加上-g選項(xiàng)運(yùn)行程序崩潰后用bt命令查看調(diào)用棧。Valgrind檢查內(nèi)存泄漏。SLAM系統(tǒng)長(zhǎng)時(shí)間運(yùn)行微小的內(nèi)存泄漏也會(huì)導(dǎo)致崩潰。用valgrind --leak-checkfull來運(yùn)行你的程序。ROS工具rqt_graph查看節(jié)點(diǎn)連接rqt_console查看日志rosbag play -r 0.5慢速播放數(shù)據(jù)包以便觀察。6.2 參數(shù)調(diào)優(yōu)一個(gè)系統(tǒng)性的思路參數(shù)眾多不要盲目亂調(diào)。建立一個(gè)系統(tǒng)性的調(diào)優(yōu)順序運(yùn)動(dòng)畸變補(bǔ)償相關(guān)首先確保這部分正確。如果畸變補(bǔ)償沒做好特征提取和后續(xù)匹配全是錯(cuò)的。檢查IMU和激光雷達(dá)的時(shí)間戳同步是否精確到毫秒級(jí)?？梢試L試讓系統(tǒng)靜止不動(dòng)然后查看去畸變后的點(diǎn)云是否還是一個(gè)“干凈”的靜止場(chǎng)景點(diǎn)云如果仍有拖影說明補(bǔ)償有問題。特征提取參數(shù)在靜止和低速場(chǎng)景下調(diào)整曲率閾值、每線提取特征數(shù)等確保提取的特征點(diǎn)既不過于稀疏導(dǎo)致約束不足也不過于密集引入噪聲且增加計(jì)算量?？梢酝ㄟ^RVIZ將提取的邊緣點(diǎn)紅色和平面點(diǎn)綠色可視化出來直觀判斷。匹配搜索參數(shù)主要是激光匹配時(shí)的搜索半徑kdtree_search_radius和視覺深度關(guān)聯(lián)時(shí)的搜索半徑。從小值開始慢慢調(diào)大觀察地圖質(zhì)量是否出現(xiàn)很多漂浮的離群點(diǎn)和跟蹤穩(wěn)定性。優(yōu)化器參數(shù)Ceres Solver的迭代次數(shù)、線性求解器類型SPARSE_NORMAL_CHOLESKY常用、信任域策略的初始半徑等。對(duì)于大多數(shù)問題保持默認(rèn)值通常可以工作。如果優(yōu)化不收斂首先應(yīng)懷疑是前端數(shù)據(jù)關(guān)聯(lián)或雅可比計(jì)算錯(cuò)誤而不是盲目調(diào)整優(yōu)化器參數(shù)。6.3 遇到“跑著跑著就飛了”怎么辦這是SLAM調(diào)試中最常見也最頭疼的問題。提供一個(gè)排查清單檢查時(shí)間戳這是首要嫌疑犯。確保相機(jī)、激光雷達(dá)、IMU三者的時(shí)間戳在同一個(gè)時(shí)間源最好是硬件同步下并且在代碼中正確對(duì)齊。打印關(guān)鍵回調(diào)函數(shù)收到消息的時(shí)間戳看差值是否穩(wěn)定。檢查傳感器外參T_lidar_to_imu和T_cam_to_imu這兩個(gè)變換矩陣標(biāo)定不準(zhǔn)是致命的。可以嘗試讓設(shè)備做純旋轉(zhuǎn)運(yùn)動(dòng)如果軌跡在旋轉(zhuǎn)中心嚴(yán)重漂移很可能是外參有問題。檢查特征關(guān)聯(lián)增加可視化。將當(dāng)前幀的激光特征點(diǎn)變換到世界坐標(biāo)系和歷史地圖中為其找到的對(duì)應(yīng)平面/線同時(shí)顯示出來。肉眼觀察匹配是否正確。大量錯(cuò)誤的匹配會(huì)導(dǎo)致優(yōu)化器“被帶偏”。檢查IMU預(yù)積分在靜止初始化階段IMU預(yù)積分應(yīng)該能較好地估計(jì)出重力和零偏。如果初始化后重力方向估計(jì)錯(cuò)誤導(dǎo)致地圖傾斜或者零偏估計(jì)值異常大可能是IMU數(shù)據(jù)有問題或預(yù)積分代碼有bug。逐步簡(jiǎn)化問題如果問題復(fù)雜嘗試先關(guān)閉視覺只運(yùn)行激光SLAM部分類似LIO。如果激光部分穩(wěn)定再加入視覺。或者在簡(jiǎn)單、紋理豐富的環(huán)境中先測(cè)試排除環(huán)境因素。閱讀R3LIVE這類大型開源項(xiàng)目的代碼就像在解一個(gè)精密的機(jī)械手表。每個(gè)齒輪模塊都必須嚴(yán)絲合縫地配合。本篇詳解的幾個(gè)核心模塊是保證這塊“手表”走時(shí)準(zhǔn)確的關(guān)鍵齒輪。希望這些帶著代碼細(xì)節(jié)和實(shí)踐經(jīng)驗(yàn)的拆解能讓你在動(dòng)手修改和調(diào)試時(shí)不再茫然而是能有條不紊地定位問題甚至實(shí)現(xiàn)自己的創(chuàng)新想法。代碼的世界里沒有黑魔法每一行背后都是確定的數(shù)學(xué)和邏輯理解它們你就能真正掌控它。