transformMaintenance.cpp解析

• 八、位姿估計
• • 1. 雅可比計算
  • 2. 矩陣求解
  • 3. 退化問題分析
  • 4. 姿態更新
  • 5. 坐標轉換

loam源碼地址： https://github.com/cuitaixiang/LOAM_NOTED.
論文學習： LOAM: Lidar Odometry and Mapping in Real-time 論文閱讀.
loam源碼解析匯總：
loam源碼解析1 ： scanRegistration（一）.
loam源碼解析2 ： scanRegistration（二）.
loam源碼解析3 ： laserOdometry（一）.
loam源碼解析4 ： laserOdometry（二）.
loam源碼解析5 ： laserOdometry（三）.
loam源碼解析6 ： laserMapping（一）.
loam源碼解析7 ： laserMapping（二）.
loam源碼解析8 ： transformMaintenance.

八、位姿估計

將特征點匹配以后開始進行位姿估計，在loam中將問題轉換為一個最小二乘的優化問題，使用LM法進行迭代求解，所以優化的重點就是關于雅可比矩陣的求導。

1. 雅可比計算

對于損失函數(loss為特征點計算出的距離，代碼中用$B$矩陣表示)：
$$loss=F({\tilde{X}}_{k+1},T_{k+1})$$
其雅可比矩陣(代碼中用$A$矩陣表示)為：
$$J=\frac{?F}{?T_{k+1}}=\frac{?F}{?{\tilde{X}}_{k+1}}\frac{?{\tilde{X}}_{k+1}}{?T_{k+1}}$$
其中$\frac{?F}{?{\tilde{X}}_{k+1}}$已經完成,若是線特征則為點到直線方向的單位向量，若是線特征則為點到平面方向的單位向量。關于$\frac{?{\tilde{X}}_{k+1}}{?T_{k+1}}$j可分別對平移和旋轉求導，不再贅述。
所以最后問題的求解變成了求解：
$$\sum _{i=0}^{pointSelNum}{H}_i{X}_i=\sum _{i=0}^{pointSelNum}{b}_i$$
其中：
$$H_i=A_i^T{A}_i$$

$$b_i=?A_i^T{B}_i$$
定義各個矩陣：

cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

對每個特征點求解雅可比矩陣A和損失向量B，拼裝成為一個大的求解矩陣。

//計算matA,matB矩陣for (int i = 0; i < pointSelNum; i++) {pointOri = laserCloudOri->points[i];coeff = coeffSel->points[i];float s = 1;float srx = sin(s * transform[0]);float crx = cos(s * transform[0]);float sry = sin(s * transform[1]);float cry = cos(s * transform[1]);float srz = sin(s * transform[2]);float crz = cos(s * transform[2]);float tx = s * transform[3];float ty = s * transform[4];float tz = s * transform[5];float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x+ (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z+ s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y+ (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z+ s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) + s*tz*crx*cry) * coeff.x+ ((s*cry*crz - s*srx*sry*srz)*pointOri.x + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z+ s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y+ tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x+ (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y+ s*ty*crx*srz + s*tx*crx*crz) * coeff.y+ ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y+ tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y - s*(crz*sry + cry*srx*srz) * coeff.z;float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y - s*(sry*srz - cry*crz*srx) * coeff.z;float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;float d2 = coeff.intensity;matA.at<float>(i, 0) = arx;matA.at<float>(i, 1) = ary;matA.at<float>(i, 2) = arz;matA.at<float>(i, 3) = atx;matA.at<float>(i, 4) = aty;matA.at<float>(i, 5) = atz;matB.at<float>(i, 0) = -0.05 * d2;}

2. 矩陣求解

使用OpenCV函數進行求解，利用QR分解加速：

cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;//求解matAtA * matX = matAtBcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

關于OpenCV中的solve函數的一點說明 ，函數原型如下：

bool solve(const Mat & src1, const Mat & src2, Mat & dst, int flags = DECOMP_LU); //src1：輸入矩陣 //src2:線性系統的右部 //dst:輸出矩陣 //flags：求解方法 // DECOMP_LU - Gaussian // DECOMP_CHOLESKY // DECOMP_EIG // DECOMP_SVD // DECOMP_QR // DECOMP_NORMAL

3. 退化問題分析

LOAM的作者Ji Zhang發表在2016年IROS上發表過一篇關于優化問題的退化處理，若是想更詳細的了解請參考：On Degeneracy of Optimization-based State Estimation Problems
定位的退化問題簡單來說就是因為約束的減少，而用線性代數的知識直觀的表述就是增量方程中的Hessian矩陣必須要正定。所以在求解增量方程時，可以將H矩陣的特征值和特征向量求解出來，那么較小的特征值對應的特征向量的方向就是退化的方向。

在第一次迭代時，求解Hessian矩陣的特征值和特征向量：
在OpenCV中，eigen函數參數分別為：帶分解的矩陣，輸出特征值（從大到小），對應的特征向量矩陣。而copyto函數的使用：A.copyto(B)，表示將A復制到B。

if (iterCount == 0) {//特征值1*6矩陣cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));//特征向量6*6矩陣cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));//求解特征值/特征向量cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;

若特征值小于設定的閾值，說明發生退化，將對應的特征向量置0 ，相當于舍棄該方向

//特征值取值門檻float eignThre[6] = {10, 10, 10, 10, 10, 10};for (int i = 5; i >= 0; i--) {//從小到大查找if (matE.at<float>(0, i) < eignThre[i]) {//特征值太小，則認為處在兼并環境中，發生了退化for (int j = 0; j < 6; j++) {//對應的特征向量置為0matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}

計算預測矩陣：

//計算P矩陣matP = matV.inv() * matV2;}

使用預測矩陣修正增量：

if (isDegenerate) {//如果發生退化，只使用預測矩陣P計算cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}

4. 姿態更新

使用迭代計算的增量進行姿態更新

//累加每次迭代的旋轉平移量transform[0] += matX.at<float>(0, 0);transform[1] += matX.at<float>(1, 0);transform[2] += matX.at<float>(2, 0);transform[3] += matX.at<float>(3, 0);transform[4] += matX.at<float>(4, 0);transform[5] += matX.at<float>(5, 0);

計算姿態是否合法，以及是否滿足迭代條件

for(int i=0; i<6; i++){if(isnan(transform[i]))//判斷是否非數字transform[i]=0;}//計算旋轉平移量，如果很小就停止迭代float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +pow(rad2deg(matX.at<float>(1, 0)), 2) +pow(rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));if (deltaR < 0.1 && deltaT < 0.1) {//迭代終止條件break;}}

5. 坐標轉換

float rx, ry, rz, tx, ty, tz;//求相對于原點的旋轉量,垂直方向上1.05倍修正?AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) - sin(rz) * (transform[4] - imuShiftFromStartY);float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) + cos(rz) * (transform[4] - imuShiftFromStartY);float z1 = transform[5] * 1.05 - imuShiftFromStartZ;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);//根據IMU修正旋轉量PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);//得到世界坐標系下的轉移矩陣transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;

歐拉角轉換成四元數：

geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);

發布里程計信息，一個用于建圖，一個用于rviz

//publish四元數和平移量laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserOdometry.pose.pose.orientation.x = -geoQuat.y;laserOdometry.pose.pose.orientation.y = -geoQuat.z;laserOdometry.pose.pose.orientation.z = geoQuat.x;laserOdometry.pose.pose.orientation.w = geoQuat.w;laserOdometry.pose.pose.position.x = tx;laserOdometry.pose.pose.position.y = ty;laserOdometry.pose.pose.position.z = tz;pubLaserOdometry.publish(laserOdometry);//廣播新的平移旋轉之后的坐標系(rviz)laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat);laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz));tfBroadcaster.sendTransform(laserOdometryTrans);

對點云的曲率比較大和比較小的點投影到掃描結束位置，但是對于全部點云，則是跳幀投影。

int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();for (int i = 0; i < cornerPointsLessSharpNum; i++) {TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);}int surfPointsLessFlatNum = surfPointsLessFlat->points.size();for (int i = 0; i < surfPointsLessFlatNum; i++) {TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}frameCount++;//點云全部點，每間隔一個點云數據相對點云最后一個點進行畸變校正if (frameCount >= skipFrameNum + 1) {int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++) {TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}}

將當前點云的曲率比較大和比較小的點保存為上一幀，并建立kd樹

//畸變校正之后的點作為last點保存等下個點云進來進行匹配pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();//點足夠多就構建kd-tree，否則棄用此幀，沿用上一幀數據的kd-treeif (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);}

按照跳幀數publich邊沿點，平面點以及全部點給laserMapping(每隔一幀發一次）

if (frameCount >= skipFrameNum + 1) {frameCount = 0;sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudFullRes3.header.frame_id = "/camera";pubLaserCloudFullRes.publish(laserCloudFullRes3);}}status = ros::ok();rate.sleep();}

總結

以上是生活随笔為你收集整理的loam源码解析5 ： laserOdometry（三）的全部內容，希望文章能夠幫你解決所遇到的問題。

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