lego-loam代碼分析（3）-激光里程計

• 匹配初始化
• TransformToStart
• TransformToEnd
• 兩次LM匹配
• • 平面匹配
  • • 匹配點查找
    • 目標點到匹配平面的距離
    • LM求解
  • 角點匹配
  • • 匹配點查找
    • 目標點到直線的距離
• odometry 發布
• 發布建圖數據

上節分析了其點云特征提取，目的是用于幀間匹配獲取激光里程計。而像常見的點云匹配如NDT、ICP等匹配算法，均是對整個點云進行匹配。而loam為提高效率和實時性，則采用特征點進行匹配。

匹配初始化

由于激光里程計相臨兩幀點云進行匹配獲取，即當前幀與上一幀進行匹配，故開啟slam收到的第一幀點云數據則需要對cloud_last進行賦值，即對上一幀進行記錄和存儲，不做匹配運算。

pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp; // 第一次last 和 curr 應一樣， 即賦初始狀態cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;kdtreeCornerLast.setInputCloud(laserCloudCornerLast); // 初始化KD TREEkdtreeSurfLast.setInputCloud(laserCloudSurfLast);laserCloudCornerLastNum = laserCloudCornerLast->points.size(); // 獲取兩種特征點云個數laserCloudSurfLastNum = laserCloudSurfLast->points.size();

TransformToStart

激光點云進行運動補償很容易理解，由于雷達掃描一圈是需要時間的，如果激光雷達靜止不動，則一圈所有激光光束測量的距離為基于同一個原點測量所得；而若雷達在運動，顯然每一次掃描中的光束則基于不同原點進行測量，換算成笛卡爾坐標系時，顯然會發現產生了畸變。因此需要進行運動補償，即根據每個點的時刻計算其對應的原點，然后重新計算在笛卡爾坐標系下的坐標。常用的方法即采用imu等方法估計雷達運動速度，根據每條光速的時刻進行積分，可獲取每條光束對應的原點坐標，進行修正。

但是TransformToStart根據他人的解釋為運動補償，即將每幀中點云均轉換為以第一個掃描點為同一坐標系，即為運動補償。但是分析其代碼一直不太理解其具體意義（望理解的人分享一下）。

有人如此分析，
LeGo-LOAM源碼閱讀筆記(三)—featureAssociation.cpp,

// s代表的其實是一個比例，s的計算方法應該如下： // s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD

表示無法理解，因為(pi->intensity - int(pi->intensity))為強度信息中的小數部分，小數部分應該為_scan_period * relTime，可看下面代碼，這是預處理坐標系更改void adjustDistortion()中賦值。

point.intensity =int(segmentedCloud->points[i].intensity) + _scan_period * relTime;

查找多方面資料，都沒有找到詳細解釋，望有高手分享

void FeatureAssociation::TransformToStart(PointType const *const pi,PointType *const po) {// 插值系數，為相對與第一個點的時間差// 其中10為系數，可調整，感覺像假設10是個速度？？？？？，但是速度如何來的呢float s = 10 * (pi->intensity - int(pi->intensity)); // 由于強度為thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; // 強度根據不同層則不一樣，可用于顯示區分// 而在特征提取進行了修改 intensity = int(segmentedCloud->points[i].intensity) + _scan_period * relTime; // 如此可看出， s應該為每掃描行中點云與第一個點云的時間差即10*_scan_period * relTime// 線性插值， 如果靜止，顯然無需插值，則每個點相對激光原點（或者相對于上刻位置），全部一致// 由于勻速運動，因此認為點云中的每個點相對于上刻的位置有一定的偏移，其偏移量為s// 如果s不是速度，像是個比例系數，即每個原點都在如此的比例系數下進行平移和旋轉？？？？？// 不太理解，s針對一圈中固定索引的點云基本相當于一個常數// 獲取了每個點的原點坐標，然后校準新的笛卡爾坐標float ry = s * transformCur[1];float rx = s * transformCur[0]; float rz = s * transformCur[2];float tx = s * transformCur[3];float ty = s * transformCur[4];float tz = s * transformCur[5];// 將該點根據自己的畸變進行旋轉和平移float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);float z1 = (pi->z - tz);float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;po->x = cos(ry) * x2 - sin(ry) * z2;po->y = y2;po->z = sin(ry) * x2 + cos(ry) * z2;po->intensity = pi->intensity; }

TransformToEnd

與TransformToStart方法一致，應該是將所有點統一到掃描的最后一點時刻的坐標系，其方法是先采用TransformToStart方法轉換到第一個點，然后再轉換到最后一點。

兩次LM匹配

由于已經獲取前后兩幀的角點和平面點特征點云，因此進行幀間點云匹配，則可獲取相鄰兩幀平移旋轉變換，由初始位姿進行累計，從而獲得激光里程計信息。
lego-loam采用將兩次即兩種特征點分別匹配，平面匹配和2維位姿匹配。
其中LM優化的流程為構建約束方程 -> 約束方程求偏導構建Jaccobian矩陣 -> L-M求解。

平面匹配

匹配點查找

采用KDtree在上一幀特征點云中搜索對應當前點云一個點最近點，然后在找到前后兩個掃描線中最近的兩個點。如下圖所示。其中i為當前時刻中一個點云，而m，j，l為上一幀中對應的三個點，此三個點可構成一個平面。

// 代碼略，較為簡單，容易理解

目標點到匹配平面的距離

匹配采用的方法為點到平面匹配，即需求點到平面的距離，具體公式截圖論文。

公式不容易理解，簡單解釋就是求出三個點構成的平面的方向量并求出平面方程。
具體可參考點、平面和法線關系一文中有具體推導。其中法線計算為平面上兩個不平行的向量叉乘。
代碼中A、B、C三個點為對應的平面上的點；

求出兩個向量AB和AC

計算AB和AC的叉乘，即獲取方向量

帶回平面方程，計算出平面方程常量D，獲取平面方程；

求出方向量的模，獲取單位方向量；

計算目標點到達法向量的投影，即為目標點到達平面的距離；

// 獲取三個點，構成一個平面的三角形PointType tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; // A點PointType tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; // B點PointType tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; // C點// 平面向量// AB (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)// AC (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)// 向量叉乘獲得平面法向量AB × AC// 法向量三個分量(可套公式獲得)float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) -(tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) -(tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) -(tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);// 平面方程 Ax+By+Cz+D = 0 // 獲取平面方程的常數Dfloat pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);// 平面法向量的模float ps = sqrt(pa * pa + pb * pb + pc * pc);// 換算成單位向量pa /= ps;pb /= ps;pc /= ps;pd /= ps;// 計算目標點到達平面的距離， 即目標點在方向量上的投影float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 考慮權重，距離越小，權重越大，應該是經驗值??????float s = 1;if (iterCount >= 5) {s = 1 -1.8 * fabs(pd2) /sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y +pointSel.z * pointSel.z));}// 記錄平面參數和點到平面的距離,權重過小則忽略if (s > 0.1 && pd2 != 0) {PointType coeff;coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;laserCloudOri->push_back(surfPointsFlat->points[i]); // 存放原始點云coeffSel->push_back(coeff); // 存放平面信息和點到平面距離}

LM求解

獲取矩陣方程A*X=B，即求出矩陣A和B。其中A為一階偏導數矩陣，而B即為對應點到面的距離矩陣。考慮到矩陣A不一定滿秩，無法直接QR分解求解，因此矩陣方程左右兩側均左乘以A的轉置。即$A^T?A?x=A^T?B$。然后再進行QR分解求值。

for (int i = 0; i < pointSelNum; i++) {PointType pointOri = laserCloudOri->points[i];PointType coeff = coeffSel->points[i];// 偏導數float arx =(-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x +(a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y +(a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z;float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x +(c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y +(c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z;float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;// 距離float d2 = coeff.intensity;// A 矩陣matA(i, 0) = arx;matA(i, 1) = arz;matA(i, 2) = aty;// B 矩陣matB(i, 0) = -0.05 * d2;}// A的轉置matAt = matA.transpose();// A和B 同左乘A的轉置， 目的是希望左側矩陣滿秩matAtA = matAt * matA;matAtB = matAt * matB;// matAtA * X = matAt * matB// QR分解求解matX = matAtA.colPivHouseholderQr().solve(matAtB);// ????不是太明白，矩陣退化判斷if (iterCount == 0) {Eigen::Matrix<float,1,3> matE;Eigen::Matrix<float,3,3> matV;Eigen::Matrix<float,3,3> matV2;Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float,3,3> > esolver(matAtA);matE = esolver.eigenvalues().real();matV = esolver.eigenvectors().real();matV2 = matV;isDegenerate = false;float eignThre[3] = {10, 10, 10};for (int i = 2; i >= 0; i--) {if (matE(0, i) < eignThre[i]) {for (int j = 0; j < 3; j++) {matV2(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inverse() * matV2;}//if (isDegenerate) {Eigen::Matrix<float,3,1> matX2;matX2 = matX;matX = matP * matX2;}// 疊加每次迭代的旋轉和平移量transformCur[0] += matX(0, 0);transformCur[2] += matX(1, 0);// y 軸平移 （原點云的z的平移）transformCur[4] += matX(2, 0);// 無效值就恢復0for (int i = 0; i < 6; i++) {if (std::isnan(transformCur[i])) transformCur[i] = 0;}// 計算旋轉平移誤差矩陣float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) +pow(RAD2DEG * (matX(1, 0)), 2));float deltaT = sqrt(pow(matX(2, 0) * 100, 2));// 旋轉平移誤差小于一定閾值，則停止迭代if (deltaR < 0.1 && deltaT < 0.1) {return false;}

注：1.沒有認真進行偏導數的推導；2.矩陣退化沒有完全理解；3.作者完全手寫推導出來的，效果估計會現成的優化庫快

角點匹配

角點的整個匹配與平面匹配方法完全一致，其具體實現和代碼注釋不再詳述。

匹配點查找

采用KDtree在上一幀特征點云中搜索對應當前點云一個點最近點，然后在找到前后兩個掃描線中最近的一個點。如下圖所示。 其中i為當前時刻中一個點云，而j，l為上一幀中對應的兩個點，此兩點可構成一條直線。

目標點到直線的距離

匹配采用的是目標點到達參考直線的距離，方法如下，已知A和O兩個點，求出Q到達OA直線的距離。

具體計算可參考點到直線的距離一文中有具體公式推導。

但是從loam代碼中卻看出計算公式如下，即求出i到直線j，l的最短距離，公式卻是
$$d=\frac{|ij\times il|}{|jl|}$$

按照平面法向量的思想可知，其結果一致。

odometry 發布

經過兩次LM匹配，即可認為是經過幀間匹配可獲取相臨幀的位姿轉移矩陣，通過里程計思想進行累加，則可獲取當前幀的全局位姿，即獲取激光里程計。

// 矩陣轉換，根據變換的矩陣， 更新全局位姿 // 先旋轉， 在平移 // 已知初始全局位置和相臨兩幀的位姿轉移矩陣， // 累加獲取當前幀全局位置 void FeatureAssociation::integrateTransformation() {float rx, ry, rz, tx, ty, tz;// 旋轉累加AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],-transformCur[0], -transformCur[1], -transformCur[2], rx,ry, rz);float x1 = cos(rz) * (transformCur[3] ) -sin(rz) * (transformCur[4] );float y1 = sin(rz) * (transformCur[3] ) +cos(rz) * (transformCur[4] );float z1 = transformCur[5];float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;// 平移累加tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2); ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);// 獲取當前幀全局位置，即構建里程計transformSum[0] = rx;transformSum[1] = ry; // transformSum[2] = rz; // 繞z軸方向的yaw 航向角transformSum[3] = tx; // x 坐標transformSum[4] = ty; // y 坐標transformSum[5] = tz; // z 坐標 }

發布建圖數據

整個特征點提取其目的包括兩個：用于獲取高頻率的激光里程計和用于低頻率的建圖匹配。而獲取的激光里程計頻率較高，可為后期建圖匹配提供預測值。可以說前3節所有處理均可認為是slam中傳感器的預處理，將預處理后的數據輸入slam主流程。
其預處理后的數據包括：

平面點云；

角點點云；

非特征點云；

激光里程計；

其中1,2,3三種點云的集合則為原始點云。而激光里程計也可以有其他里程計進行替換，如編碼器，imu等。

其整個特征提取以及里程計處理主流程代碼如下：

void FeatureAssociation::runFeatureAssociation() {while (ros::ok()) {ProjectionOut projection;_input_channel.receive(projection); // 接收三類數據if( !ros::ok() ) break;//-------------- outlierCloud = projection.outlier_cloud; //分別為未被分類的孤立點云簇segmentedCloud = projection.segmented_cloud; //被分類的包含地面的點云segInfo = std::move(projection.seg_msg); //分類的相關信息_scan_msg = std::move(projection.scan_msg);cloudHeader = segInfo.header;timeScanCur = cloudHeader.stamp.toSec();/** 1. Feature Extraction */adjustDistortion(); // 坐標系轉換calculateSmoothness(); // 計算平滑性markOccludedPoints(); // 標記在水平掃描方向上，屏蔽不可靠點extractFeatures(); // 特征提取包括，角點和平坦點publishCloud(); // cloud for visualization// Feature Associationif (!systemInitedLM) { // 僅執行一次，用于初始化checkSystemInitialization();continue;}updateTransformation(); // 計算兩幀激光數據之間轉換矩陣integrateTransformation(); // 迭代更新，將每兩幀之間變換，進行累計變換，構建里程計publishOdometry();publishCloudsLast(); // cloud to mapOptimization// 以上為高頻率的激光里程計獲取//--------------_cycle_count++;// 建圖_mapping_frequency_div倍降頻更新if (_cycle_count == _mapping_frequency_div) {_cycle_count = 0;AssociationOut out;out.cloud_corner_last.reset(new pcl::PointCloud<PointType>());out.cloud_surf_last.reset(new pcl::PointCloud<PointType>());out.cloud_outlier_last.reset(new pcl::PointCloud<PointType>());*out.cloud_corner_last = *laserCloudCornerLast; // 將平面信息 和角點發出給建圖算法*out.cloud_surf_last = *laserCloudSurfLast;*out.cloud_outlier_last = *outlierCloud; // 將非特征點發給建圖算法out.laser_odometry = laserOdometry; // 激光里程計}}

總結

以上是生活随笔為你收集整理的lego-loam代码分析（3）-激光里程计的全部內容，希望文章能夠幫你解決所遇到的問題。

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