文章目錄

• 1.featureAssociation框架
• • 1.1節(jié)點代碼主體
  • 1.2 FeatureAssociation構(gòu)造函數(shù)
  • 1.3 runFeatureAssociation()主體函數(shù)
• 2.重要函數(shù)
• • 2.1 laserCloudHandler
  • 2.2 laserCloudInfoHandler
  • 2.3 outlierCloudHandler
  • 2.4 imuHandler
  • 2.5 adjustDistortion
  • 2.6 calculateSmoothness
  • 2.7 markOccludedPoints
  • 2.8 extractFeatures
  • 2.9 updateInitialGuess
  • 2.10 updateTransformation
  • 2.11 integrateTransformation

無人駕駛算法學習（九）：LeGo-LOAM激光雷達定位算法
LeGo-LOAM激光雷達定位算法源碼閱讀(一)

1.featureAssociation框架

1.1節(jié)點代碼主體

int main(int argc, char** argv) {ros::init(argc, argv, "lego_loam");ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");FeatureAssociation FA;//調(diào)用構(gòu)造函數(shù)ros::Rate rate(200);while (ros::ok()){ros::spinOnce();FA.runFeatureAssociation();//200hz調(diào)用主體函數(shù)rate.sleep();}ros::spin();return 0; }

1.2 FeatureAssociation構(gòu)造函數(shù)

作用:訂閱分割點云和imu數(shù)據(jù),存成類成員變量,為runFeatureAssociation()處理新數(shù)據(jù)做準備

//訂閱話題:"/segmented_cloud"(sensor_msgs::PointCloud2)，數(shù)據(jù)處理函數(shù)laserCloudHandler"/segmented_cloud_info"(cloud_msgs::cloud_info)，數(shù)據(jù)處理函數(shù)laserCloudInfoHandler"/outlier_cloud"(sensor_msgs::PointCloud2)，數(shù)據(jù)處理函數(shù)outlierCloudHandlerimuTopic = "/imu/data"(sensor_msgs::Imu)，數(shù)據(jù)處理函數(shù)imuHandler//發(fā)布話題，這些topic有："/laser_cloud_sharp"(sensor_msgs::PointCloud2)"/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)"/laser_cloud_flat"(sensor_msgs::PointCloud2)"/laser_cloud_less_flat"(sensor_msgs::PointCloud2)"/laser_cloud_corner_last"(sensor_msgs::PointCloud2)"/laser_cloud_surf_last"(cloud_msgs::cloud_info)"/outlier_cloud_last"(sensor_msgs::PointCloud2)"/laser_odom_to_init"(nav_msgs::Odometry)

1.3 runFeatureAssociation()主體函數(shù)

作用:特征提取和配準的主體

void runFeatureAssociation(){// 如果有新數(shù)據(jù)進來則執(zhí)行，否則不執(zhí)行任何操作if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){newSegmentedCloud = false;newSegmentedCloudInfo = false;newOutlierCloud = false;}else{return;}// 點云形狀調(diào)整 //主要進行的處理是將點云數(shù)據(jù)進行坐標變換，進行插補等工作adjustDistortion();// 計算曲率，并保存結(jié)果calculateSmoothness();// 標記瑕點,loam中:1.平行scan的點(可能看不見);2.會被遮擋的點markOccludedPoints();// 特征提取// 點云分類,然后分別保存到cornerPointsSharp等等隊列// 這一步中減少了點云數(shù)量，使計算量減少extractFeatures();// 發(fā)布cornerPointsSharp等4種類型的點云數(shù)據(jù)publishCloud();if (!systemInitedLM) {checkSystemInitialization();return;}// 預測位姿updateInitialGuess();// 更新變換updateTransformation();// 積分總變換integrateTransformation();publishOdometry();publishCloudsLast(); }

2.重要函數(shù)

2.1 laserCloudHandler

訂閱 "/segmented_cloud"的回調(diào)函數(shù)
作用:laserCloudHandler修改點云數(shù)據(jù)的時間戳，將點云數(shù)據(jù)從ROS定義的格式轉(zhuǎn)化到pcl的格式。
pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);

2.2 laserCloudInfoHandler

訂閱 "/segmented_cloud_info"的觸發(fā)回調(diào)函數(shù)laserCloudInfoHandler
newSegmentedCloudInfo = true;
作用:為runFeatureAssociation()做準備

2.3 outlierCloudHandler

訂閱 "/outlier_cloud"的回調(diào)函數(shù)
pcl::fromROSMsg(*msgIn, *outlierCloud); newOutlierCloud = true;
作用:為runFeatureAssociation()做準備

2.4 imuHandler

LOAM代碼里的那個重要函數(shù)

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){//通過接收到的imuIn里面的四元素得到roll,pitch,yaw三個角double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// 加速度去除重力影響，同時坐標軸進行變換float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//將歐拉角，加速度，速度保存到循環(huán)隊列中imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;//對速度，角速度，加速度進行積分，得到位移，角度和速度AccumulateIMUShiftAndRotation();}

2.5 adjustDistortion

void adjustDistortion(){bool halfPassed = false;int cloudSize = segmentedCloud->points.size();PointType point;for (int i = 0; i < cloudSize; i++) {// 這里xyz與laboshin_loam代碼中的一樣經(jīng)過坐標軸變換// imuhandler() 中相同的坐標變換point.x = segmentedCloud->points[i].y;point.y = segmentedCloud->points[i].z;point.z = segmentedCloud->points[i].x;// -atan2(p.x,p.z)==>-atan2(y,x)// ori表示的是偏航角yaw，因為前面有負號，ori=[-M_PI,M_PI)// 因為segInfo.orientationDiff表示的范圍是(PI,3PI)，在2PI附近// 下面過程的主要作用是調(diào)整ori大小，滿足start<ori<endfloat ori = -atan2(point.x, point.z);if (!halfPassed) {if (ori < segInfo.startOrientation - M_PI / 2)// start-ori>M_PI/2，說明ori小于start，不合理，// 正常情況在前半圈的話，ori-stat范圍[0,M_PI]ori += 2 * M_PI;else if (ori > segInfo.startOrientation + M_PI * 3 / 2)// ori-start>3/2*M_PI,說明ori太大，不合理ori -= 2 * M_PI;if (ori - segInfo.startOrientation > M_PI)halfPassed = true;} else {ori += 2 * M_PI;if (ori < segInfo.endOrientation - M_PI * 3 / 2)// end-ori>3/2*PI,ori太小ori += 2 * M_PI;else if (ori > segInfo.endOrientation + M_PI / 2)// ori-end>M_PI/2,太大ori -= 2 * M_PI;}// 用 point.intensity 來保存時間float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;if (imuPointerLast >= 0) {float pointTime = relTime * scanPeriod;imuPointerFront = imuPointerLastIteration;// while循環(huán)內(nèi)進行時間軸對齊while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {// 該條件內(nèi)imu數(shù)據(jù)比激光數(shù)據(jù)早，但是沒有更后面的數(shù)據(jù)// (打個比方,激光在9點時出現(xiàn)，imu現(xiàn)在只有8點的)// 這種情況上面while循環(huán)是以imuPointerFront == imuPointerLast結(jié)束的imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront]; } else {// 在imu數(shù)據(jù)充足的情況下可以進行插補// 當前timeScanCur + pointTime < imuTime[imuPointerFront]，// 而且imuPointerFront是最早一個時間大于timeScanCur + pointTime的imu數(shù)據(jù)指針int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);// 通過上面計算的ratioFront以及ratioBack進行插補// 因為imuRollCur和imuPitchCur通常都在0度左右，變化不會很大，因此不需要考慮超過2M_PI的情況// imuYaw轉(zhuǎn)的角度比較大，需要考慮超過2*M_PI的情況imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}// imu速度插補imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;// imu位置插補imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {// 此處更新過的角度值主要用在updateImuRollPitchYawStartSinCos()中,// 更新每個角的正余弦值imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;if (timeScanCur + pointTime > imuTime[imuPointerFront]) {// 該條件內(nèi)imu數(shù)據(jù)比激光數(shù)據(jù)早，但是沒有更后面的數(shù)據(jù)imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{// 在imu數(shù)據(jù)充足的情況下可以進行插補int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;}// 距離上一次插補，旋轉(zhuǎn)過的角度變化值imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;imuAngularRotationXLast = imuAngularRotationXCur;imuAngularRotationYLast = imuAngularRotationYCur;imuAngularRotationZLast = imuAngularRotationZCur;// 這里更新的是i=0時刻的rpy角，后面將速度坐標投影過來會用到i=0時刻的值updateImuRollPitchYawStartSinCos();} else {// 速度投影到初始i=0時刻VeloToStartIMU();// 將點的坐標變換到初始i=0時刻TransformToStartIMU(&point);}}segmentedCloud->points[i] = point;}imuPointerLastIteration = imuPointerLast;}

2.6 calculateSmoothness

// 計算光滑性，這里的計算沒有完全按照公式進行，// 缺少除以總點數(shù)i和r[i]void calculateSmoothness(){int cloudSize = segmentedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++) {float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]+ segInfo.segmentedCloudRange[i+5]; cloudCurvature[i] = diffRange*diffRange;// 在markOccludedPoints()函數(shù)中對該參數(shù)進行重新修改cloudNeighborPicked[i] = 0;// 在extractFeatures()函數(shù)中會對標簽進行修改，// 初始化為0，surfPointsFlat標記為-1，surfPointsLessFlatScan為不大于0的標簽// cornerPointsSharp標記為2，cornerPointsLessSharp標記為1cloudLabel[i] = 0;cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}

2.7 markOccludedPoints

// 阻塞點指點云之間相互遮擋，而且又靠得很近的點void markOccludedPoints(){int cloudSize = segmentedCloud->points.size();for (int i = 5; i < cloudSize - 6; ++i){float depth1 = segInfo.segmentedCloudRange[i];float depth2 = segInfo.segmentedCloudRange[i+1];int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));if (columnDiff < 10){// 選擇距離較遠的那些點，并將他們標記為1if (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);// 選擇距離變化較大的點，并將他們標記為1if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])cloudNeighborPicked[i] = 1;}}

2.8 extractFeatures

void extractFeatures(){cornerPointsSharp->clear();cornerPointsLessSharp->clear();surfPointsFlat->clear();surfPointsLessFlat->clear();for (int i = 0; i < N_SCAN; i++) {surfPointsLessFlatScan->clear();for (int j = 0; j < 6; j++) {// sp和ep的含義是什么???startPointer,endPointer?int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;if (sp >= ep)continue;// 按照cloudSmoothness.value從小到大排序std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {// 每次ind的值就是等于k??? 有什么意義?// 因為上面對cloudSmoothness進行了一次從小到大排序，所以ind不一定等于k了int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > edgeThreshold &&segInfo.segmentedCloudGroundFlag[ind] == false) {largestPickedNum++;if (largestPickedNum <= 2) {// 論文中nFe=2,cloudSmoothness已經(jīng)按照從小到大的順序排列，// 所以這邊只要選擇最后兩個放進隊列即可// cornerPointsSharp標記為2cloudLabel[ind] = 2;cornerPointsSharp->push_back(segmentedCloud->points[ind]);cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else if (largestPickedNum <= 20) {// 塞20個點到cornerPointsLessSharp中去// cornerPointsLessSharp標記為1cloudLabel[ind] = 1;cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {// 從ind+l開始后面5個點，每個點index之間的差值，// 確保columnDiff<=10,然后標記為我們需要的點int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 從ind+l開始前面五個點，計算差值然后標記int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSmoothness[k].ind;// 平面點只從地面點中進行選擇? 為什么要這樣做?if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < surfThreshold &&segInfo.segmentedCloudGroundFlag[ind] == true) {cloudLabel[ind] = -1;surfPointsFlat->push_back(segmentedCloud->points[ind]);// 論文中nFp=4，將4個最平的平面點放入隊列中smallestPickedNum++;if (smallestPickedNum >= 4) {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {// 從前面往后判斷是否是需要的鄰接點，是的話就進行標記int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 從后往前開始標記int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);}}}// surfPointsLessFlatScan中有過多的點云，如果點云太多，計算量太大// 進行下采樣，可以大大減少計算量surfPointsLessFlatScanDS->clear();downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.filter(*surfPointsLessFlatScanDS);*surfPointsLessFlat += *surfPointsLessFlatScanDS;}}

2.9 updateInitialGuess

void updateInitialGuess(){imuPitchLast = imuPitchCur;imuYawLast = imuYawCur;imuRollLast = imuRollCur;imuShiftFromStartX = imuShiftFromStartXCur;imuShiftFromStartY = imuShiftFromStartYCur;imuShiftFromStartZ = imuShiftFromStartZCur;imuVeloFromStartX = imuVeloFromStartXCur;imuVeloFromStartY = imuVeloFromStartYCur;imuVeloFromStartZ = imuVeloFromStartZCur;// 關(guān)于下面負號的說明：// transformCur是在Cur坐標系下的 p_start=R*p_cur+t// R和t是在Cur坐標系下的// 而imuAngularFromStart是在start坐標系下的，所以需要加負號if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY;transformCur[1] = - imuAngularFromStartZ;transformCur[2] = - imuAngularFromStartX;}// 速度乘以時間，當前變換中的位移if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0){transformCur[3] -= imuVeloFromStartX * scanPeriod;transformCur[4] -= imuVeloFromStartY * scanPeriod;transformCur[5] -= imuVeloFromStartZ * scanPeriod;}}

2.10 updateTransformation

void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();// 找到對應的特征平面// 然后計算協(xié)方差矩陣，保存在coeffSel隊列中// laserCloudOri中保存的是對應于coeffSel的未轉(zhuǎn)換到開始時刻的原始點云數(shù)據(jù)findCorrespondingSurfFeatures(iterCount1);if (laserCloudOri->points.size() < 10)continue;// 通過面特征的匹配，計算變換矩陣if (calculateTransformationSurf(iterCount1) == false)break;}for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();// 找到對應的特征邊/角點// 尋找邊特征的方法和尋找平面特征的很類似，過程可以參照尋找平面特征的注釋findCorrespondingCornerFeatures(iterCount2);if (laserCloudOri->points.size() < 10)continue;// 通過角/邊特征的匹配，計算變換矩陣if (calculateTransformationCorner(iterCount2) == false)break;}}

2.11 integrateTransformation

// 旋轉(zhuǎn)角的累計變化量void integrateTransformation(){float rx, ry, rz, tx, ty, tz; // AccumulateRotation作用// 將計算的兩幀之間的位姿“累加”起來，獲得相對于第一幀的旋轉(zhuǎn)矩陣// transformSum + (-transformCur) =(rx,ry,rz)AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);// 進行平移分量的更新float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) - sin(rz) * (transformCur[4] - imuShiftFromStartY);float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) + cos(rz) * (transformCur[4] - imuShiftFromStartY);float z1 = transformCur[5] - 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);// 與accumulateRotatio聯(lián)合起來更新transformSum的rotation部分的工作// 可視為transformToEnd的下部分的逆過程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;}

總結(jié)

以上是生活随笔為你收集整理的LeGo-LOAM激光雷达定位算法源码阅读(二)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。