【LeGO-LOAM论文阅读（二）--特征提取（二）】
生活随笔
收集整理的這篇文章主要介紹了
【LeGO-LOAM论文阅读（二）--特征提取（二）】
小編覺得挺不錯(cuò)的,現(xiàn)在分享給大家,幫大家做個(gè)參考.
簡介
上篇博客介紹了特征提取的原理以及坐標(biāo)轉(zhuǎn)化和插值的源碼理解，這篇將介紹特征提取的后續(xù)算法模塊。
源碼
1、新數(shù)據(jù)進(jìn)來進(jìn)行坐標(biāo)轉(zhuǎn)換和插補(bǔ)等工作
見【LeGO-LOAM論文閱讀（二）–特征提取（一）】
2、進(jìn)行光滑度計(jì)算
光滑度計(jì)算公式：
S取10，代表參與計(jì)算點(diǎn)數(shù)，計(jì)算點(diǎn)左右各有5個(gè)，r表示深度距離，計(jì)算點(diǎn)云開始5個(gè)點(diǎn)后到點(diǎn)云結(jié)束5個(gè)點(diǎn)前的點(diǎn)云平滑度，計(jì)算方式與論文中給的并不一樣。
初始化點(diǎn)云標(biāo)簽，初始化為0，surfPointsFlat標(biāo)記為-1，surfPointsLessFlatScan為不大于0的標(biāo)簽，cornerPointsSharp標(biāo)記為2，cornerPointsLessSharp標(biāo)記為1。
// 在markOccludedPoints()函數(shù)中對(duì)該參數(shù)進(jìn)行重新修改cloudNeighborPicked[i] = 0;// 在extractFeatures()函數(shù)中會(huì)對(duì)標(biāo)簽進(jìn)行修改，// 初始化為0，surfPointsFlat標(biāo)記為-1，surfPointsLessFlatScan為不大于0的標(biāo)簽// cornerPointsSharp標(biāo)記為2，cornerPointsLessSharp標(biāo)記為1cloudLabel[i] = 0;cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}3、標(biāo)記阻塞點(diǎn)。
阻塞點(diǎn)指點(diǎn)云之間相互遮擋而且距離比較近的點(diǎn)。將被遮擋的點(diǎn)云連續(xù)五個(gè)標(biāo)志位cloudNeighborPicked標(biāo)記為1。depth大的那個(gè)就是被遮擋的那個(gè)。具體參數(shù)設(shè)置為什么是10和0.3我也不清楚，應(yīng)該是是試驗(yàn)過的，各位可以考慮話說弄成別的試試
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){// 選擇距離較遠(yuǎn)的那些點(diǎn)，并將他們標(biāo)記為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;}}接著排除共面的情況，舍棄距離變換比較大的點(diǎn)
float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);// 選擇距離變化較大的點(diǎn)，并將他們標(biāo)記為1if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])cloudNeighborPicked[i] = 1;}}4、特征抽取。
這部分對(duì)計(jì)算好平滑度的點(diǎn)云將進(jìn)行特征提取，提取出線特征和面特征。由于點(diǎn)云過于龐大，后面處理的時(shí)候?qū)c(diǎn)云進(jìn)行了下采樣來提高計(jì)算效率。
清空上一幀特征：
將每一幀點(diǎn)云數(shù)據(jù)按水平角度分為6份，每份是60°的激光數(shù)據(jù)，sp儲(chǔ)存的是每份數(shù)據(jù)的起點(diǎn)，ep儲(chǔ)存的是每份數(shù)據(jù)的終點(diǎn)。
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;將每份點(diǎn)云按照平滑度進(jìn)行排序（由小到大）：
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());提取線特征的條件：不是被淘汰的點(diǎn)（cloudNeighborPicked=0）并且平滑度大于設(shè)定的閾值還有不能是地面點(diǎn)：
int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {// 每次ind的值就是等于k??? 有什么意義?// 因?yàn)樯厦鎸?duì)cloudSmoothness進(jìn)行了一次從小到大排序，所以ind不一定等于k了int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > edgeThreshold &&segInfo.segmentedCloudGroundFlag[ind] == false) {在滿足條件的點(diǎn)云中，將最大平滑度的兩個(gè)點(diǎn)云加入到cornerPointsSharp中，并將標(biāo)志cloudLabel置為2
largestPickedNum++;if (largestPickedNum <= 2) {// 論文中nFe=2,cloudSmoothness已經(jīng)按照從小到大的順序排列，// 所以這邊只要選擇最后兩個(gè)放進(jìn)隊(duì)列即可// cornerPointsSharp標(biāo)記為2cloudLabel[ind] = 2;cornerPointsSharp->push_back(segmentedCloud->points[ind]);cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);將平滑度前20的點(diǎn)云加入到cornerPointsLessSharp中，并將cloudLabel置為1
} else if (largestPickedNum <= 20) {// 塞20個(gè)點(diǎn)到cornerPointsLessSharp中去// cornerPointsLessSharp標(biāo)記為1cloudLabel[ind] = 1;cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else {break;}將已經(jīng)加入到cornerPointsSharp和cornerPointsLessSharp中的點(diǎn)淘汰，后面不再用：
cloudNeighborPicked[ind] = 1;計(jì)算已經(jīng)被選為線特征點(diǎn)云前后5個(gè)點(diǎn)之間的距離差值，若距離較近
同樣淘汰（避免兩個(gè)特征點(diǎn)表示的是同一個(gè)線特征）：
提取面特征的條件，不能是淘汰點(diǎn)，平滑度小于設(shè)定閾值并且只能是地面點(diǎn)：
int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSmoothness[k].ind;// 平面點(diǎn)只從地面點(diǎn)中進(jìn)行選擇? 為什么要這樣做?if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < surfThreshold &&segInfo.segmentedCloudGroundFlag[ind] == true) {將滿足條件的點(diǎn)加入surfPointsFlat同時(shí)標(biāo)志cloudLabel設(shè)為-1：
cloudLabel[ind] = -1;surfPointsFlat->push_back(segmentedCloud->points[ind]);將最小平滑度的四個(gè)點(diǎn)放入surfPointsFlat，同時(shí)設(shè)置為淘汰點(diǎn)。
// 論文中nFp=4，將4個(gè)最平的平面點(diǎn)放入隊(duì)列中smallestPickedNum++;if (smallestPickedNum >= 4) {break;}cloudNeighborPicked[ind] = 1;同時(shí)判斷面特征點(diǎn)前后5個(gè)點(diǎn)并標(biāo)記（和線特征一樣）：
for (int l = 1; l <= 5; l++) {// 從前面往后判斷是否是需要的鄰接點(diǎn)，是的話就進(jìn)行標(biāo)記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--) {// 從后往前開始標(biāo)記int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}將cloudLabe<= 0的點(diǎn)先都加入 surfPointsLessFlatScan中：
for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);}}}surfPointsLessFlatScan中的點(diǎn)云會(huì)非常的多，為了提高計(jì)算效率，對(duì)點(diǎn)云進(jìn)行下采樣：
// surfPointsLessFlatScan中有過多的點(diǎn)云，如果點(diǎn)云太多，計(jì)算量太大// 進(jìn)行下采樣，可以大大減少計(jì)算量surfPointsLessFlatScanDS->clear();downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.filter(*surfPointsLessFlatScanDS);*surfPointsLessFlat += *surfPointsLessFlatScanDS;}}5、發(fā)布四種點(diǎn)云信息。
分別發(fā)布平滑度大的點(diǎn)云、平滑度較大的點(diǎn)云、平滑度小的點(diǎn)云、平滑度較小的點(diǎn)云：
void publishCloud(){sensor_msgs::PointCloud2 laserCloudOutMsg;if (pubCornerPointsSharp.getNumSubscribers() != 0){pcl::toROSMsg(*cornerPointsSharp, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubCornerPointsSharp.publish(laserCloudOutMsg);}if (pubCornerPointsLessSharp.getNumSubscribers() != 0){pcl::toROSMsg(*cornerPointsLessSharp, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubCornerPointsLessSharp.publish(laserCloudOutMsg);}if (pubSurfPointsFlat.getNumSubscribers() != 0){pcl::toROSMsg(*surfPointsFlat, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubSurfPointsFlat.publish(laserCloudOutMsg);}if (pubSurfPointsLessFlat.getNumSubscribers() != 0){pcl::toROSMsg(*surfPointsLessFlat, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubSurfPointsLessFlat.publish(laserCloudOutMsg);}}回到runFeatureAssociation函數(shù)里，判斷有沒有初始化·，即判斷是不是第一幀數(shù)據(jù)，systemInitedLM初始為0，：
if (!systemInitedLM) {checkSystemInitialization();return;}將第一幀的曲率較大的點(diǎn)保存為上一幀用于匹配的線特征：
void checkSystemInitialization(){// 交換cornerPointsLessSharp和laserCloudCornerLastpcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;將第一幀的曲率較小的點(diǎn)保存為上一幀用于匹配的面特征
// 交換surfPointsLessFlat和laserCloudSurfLastlaserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;將特征點(diǎn)保存到KD樹中：
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);記錄特征點(diǎn)數(shù)
laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();發(fā)布特征點(diǎn)云消息：
sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = cloudHeader.stamp;laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = cloudHeader.stamp;laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);保存累積旋轉(zhuǎn)量：
transformSum[0] += imuPitchStart;transformSum[2] += imuRollStart;checkSystemInitialization判斷完第一幀數(shù)據(jù)將systemInitedLM設(shè)為1
systemInitedLM = true; }6、更新位姿
這里主要是保存當(dāng)前點(diǎn)云中最后一個(gè)點(diǎn)的旋轉(zhuǎn)角、最后一個(gè)點(diǎn)相對(duì)于第一個(gè)點(diǎn)的位移以及速度：
void updateInitialGuess(){imuPitchLast = imuPitchCur;imuYawLast = imuYawCur;imuRollLast = imuRollCur;imuShiftFromStartX = imuShiftFromStartXCur;imuShiftFromStartY = imuShiftFromStartYCur;imuShiftFromStartZ = imuShiftFromStartZCur;imuVeloFromStartX = imuVeloFromStartXCur;imuVeloFromStartY = imuVeloFromStartYCur;imuVeloFromStartZ = imuVeloFromStartZCur;但是我發(fā)現(xiàn)最后一個(gè)點(diǎn)的位移變換函數(shù)根本沒有被調(diào)用，我覺得這個(gè)函數(shù)是要在插值和坐標(biāo)轉(zhuǎn)那被調(diào)用，可以并沒有，有沒有大佬能解釋下。
void ShiftToStartIMU(float pointTime){// 下面三個(gè)量表示的是世界坐標(biāo)系下，從start到cur的坐標(biāo)的漂移imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;// 從世界坐標(biāo)系變換到start坐標(biāo)系float x1 = cosImuYawStart * imuShiftFromStartXCur - sinImuYawStart * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sinImuYawStart * imuShiftFromStartXCur + cosImuYawStart * imuShiftFromStartZCur;float x2 = x1;float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;imuShiftFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuShiftFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuShiftFromStartZCur = z2;}更新旋轉(zhuǎn)量（相鄰兩幀）：
// 關(guān)于下面負(fù)號(hào)的說明：// transformCur是在Cur坐標(biāo)系下的 p_start=R*p_cur+t// R和t是在Cur坐標(biāo)系下的// 而imuAngularFromStart是在start坐標(biāo)系下的，所以需要加負(fù)號(hào)if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY;transformCur[1] = - imuAngularFromStartZ;transformCur[2] = - imuAngularFromStartX;更新位移量（兩幀之間）：
// 速度乘以時(shí)間，當(dāng)前變換中的位移if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0){transformCur[3] -= imuVeloFromStartX * scanPeriod;transformCur[4] -= imuVeloFromStartY * scanPeriod;transformCur[5] -= imuVeloFromStartZ * scanPeriod;}}7、更新變換矩陣。
如果線特征或者面特征太少，直接退出：
void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;尋找面特征對(duì)應(yīng)點(diǎn)findCorrespondingSurfFeatures， iterCount迭代次數(shù)？：
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();// 找到對(duì)應(yīng)的特征平面// 然后計(jì)算協(xié)方差矩陣，保存在coeffSel隊(duì)列中// laserCloudOri中保存的是對(duì)應(yīng)于coeffSel的未轉(zhuǎn)換到開始時(shí)刻的原始點(diǎn)云數(shù)據(jù)findCorrespondingSurfFeatures(iterCount1);面特征點(diǎn)數(shù)賦值：
void findCorrespondingSurfFeatures(int iterCount){int surfPointsFlatNum = surfPointsFlat->points.size();將特征點(diǎn)坐標(biāo)變換到開始時(shí)刻 TransformToStart：
for (int i = 0; i < surfPointsFlatNum; i++) {// 坐標(biāo)變換到開始時(shí)刻，參數(shù)0是輸入，參數(shù)1是輸出TransformToStart(&surfPointsFlat->points[i], &pointSel);注意假設(shè)了勻速運(yùn)動(dòng)模型，也就是說位姿的變化是線性的，而代碼中提到的s就是隨時(shí)間線性變化的因子。回顧之前點(diǎn)云強(qiáng)度的保存的數(shù)據(jù)的計(jì)算，point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;，再結(jié)合s的定義， float s = 10 * (pi->intensity - int(pi->intensity));，發(fā)現(xiàn)s就是relTime，為當(dāng)前點(diǎn)云時(shí)間減去初始點(diǎn)云相對(duì)于幀點(diǎn)云時(shí)間差的比值。
void TransformToStart(PointType const * const pi, PointType * const po){// intensity代表的是：整數(shù)部分ring序號(hào)，小數(shù)部分是當(dāng)前點(diǎn)在這一圈中所花的時(shí)間// 關(guān)于intensity， 參考 adjustDistortion() 函數(shù)中的定義// s代表的其實(shí)是一個(gè)比例，s的計(jì)算方法應(yīng)該如下：// s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD// ===> SCAN_PERIOD=0.1(雷達(dá)頻率為10hz)// 以上理解感謝github用戶StefanGlaser// https://github.com/laboshinl/loam_velodyne/issues/29float s = 10 * (pi->intensity - int(pi->intensity));計(jì)算當(dāng)前點(diǎn)云和開始時(shí)刻的旋轉(zhuǎn)角度以及位移，這里用了勻速運(yùn)動(dòng)模型：
float rx = s * transformCur[0];float ry = s * transformCur[1];float rz = s * transformCur[2];float tx = s * transformCur[3];float ty = s * transformCur[4];float tz = s * transformCur[5];計(jì)算特征點(diǎn)對(duì)應(yīng)于開始時(shí)刻坐標(biāo)系的坐標(biāo)，先平移在旋轉(zhuǎn)：
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;}每5此迭代重新匹配特征點(diǎn)：
if (iterCount % 5 == 0) {在上一幀曲率較小的點(diǎn)（平面特征點(diǎn)）組成的KD樹中尋找一個(gè)最近鄰點(diǎn)：
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;// sq:平方，距離的平方值// 如果nearestKSearch找到的1(k=1)個(gè)鄰近點(diǎn)滿足條件if (pointSearchSqDis[0] < nearestFeatureSearchSqDist) {closestPointInd = pointSearchInd[0];找到該最近鄰點(diǎn)的線束（第幾行第幾列）point.intensity保存里點(diǎn)云的行列值：
// point.intensity 保存的是什么值? 第幾次scan?// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;// fullInfoCloud->points[index].intensity = range;// 在imageProjection.cpp文件中有上述兩行代碼，兩種類型的值，應(yīng)該存的是上面一個(gè)int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);在最近鄰點(diǎn)i 所在線束尋找再尋找另外一個(gè)最近點(diǎn)，以及在最近鄰點(diǎn)i ii所在線束的相鄰線束中尋找第三個(gè)最近點(diǎn)，組成匹配平面：
// 主要功能是找到2個(gè)scan之內(nèi)的最近點(diǎn)，并將找到的最近點(diǎn)及其序號(hào)保存// 之前掃描的保存到minPointSqDis2，之后的保存到minPointSqDis2float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist, minPointSqDis3 = nearestFeatureSearchSqDist;首先在雷達(dá)點(diǎn)云存儲(chǔ)的正方向(ScanID增大的方向和雷達(dá)旋轉(zhuǎn)正方向)尋找其他兩個(gè)點(diǎn)，為什么加上2.5，因?yàn)樯舷聝蓚€(gè)scan相差2°：
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {// int類型的值加上2.5? 為什么不直接加上2?// 四舍五入if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) {break;}計(jì)算距離，找到最近鄰的點(diǎn)，將之前掃描到的點(diǎn)保存在minPointSqDis2中，之后掃描到的保存在minPointSqDis3中：
pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) {if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}} else {if (pointSqDis < minPointSqDis3) {minPointSqDis3 = pointSqDis;minPointInd3 = j;}}反方向(ScanID減小的方向和雷達(dá)旋轉(zhuǎn)反方向?qū)ふ尹c(diǎn)，并確定總的最近鄰點(diǎn)：
// 往前找for (int j = closestPointInd - 1; j >= 0; j--) {if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) {break;}pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) {if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}} else {if (pointSqDis < minPointSqDis3) {minPointSqDis3 = pointSqDis;minPointInd3 = j;}}}}找到平面匹配的三個(gè)點(diǎn)：
pointSearchSurfInd1[i] = closestPointInd;pointSearchSurfInd2[i] = minPointInd2;pointSearchSurfInd3[i] = minPointInd3;}找到特征點(diǎn)的對(duì)應(yīng)點(diǎn)以后的工作就是計(jì)算距離了，在這里還計(jì)算了平面法向量（就是點(diǎn)到平面的直線的方向向量）在各個(gè)軸的方向分解，在雅可比的計(jì)算中使用：
// 前后都能找到對(duì)應(yīng)的最近點(diǎn)在給定范圍之內(nèi)// 那么就開始計(jì)算距離// [pa,pb,pc]是tripod1，tripod2，tripod3這3個(gè)點(diǎn)構(gòu)成的一個(gè)平面的方向量，// ps是模長，它是三角形面積的2倍if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];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);float 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;// 距離沒有取絕對(duì)值// 兩個(gè)向量的點(diǎn)乘，分母除以ps中已經(jīng)除掉了，// 加pd原因:pointSel與tripod1構(gòu)成的線段需要相減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) {// [x,y,z]是整個(gè)平面的單位法量// intensity是平面外一點(diǎn)到該平面的距離coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;// 未經(jīng)變換的點(diǎn)放入laserCloudOri隊(duì)列，距離，法向量值放入coeffSellaserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}}面特征優(yōu)化（這塊我看暈了，以下參考LeGO-LOAM源碼解析5： featureAssociation（三））：
定義各個(gè)矩陣，matA表示J，matAtA表示H，matAtB表示b：
對(duì)每個(gè)線特征點(diǎn)求關(guān)于對(duì)應(yīng)三個(gè)變量的解雅可比矩陣A和損失向量B，拼裝成為一個(gè)大的求解矩陣：
float srx = sin(transformCur[0]);float crx = cos(transformCur[0]);float sry = sin(transformCur[1]);float cry = cos(transformCur[1]);float srz = sin(transformCur[2]);float crz = cos(transformCur[2]);float tx = transformCur[3];float ty = transformCur[4];float tz = transformCur[5];float a1 = crx*sry*srz; float a2 = crx*crz*sry; float a3 = srx*sry; float a4 = tx*a1 - ty*a2 - tz*a3;float a5 = srx*srz; float a6 = crz*srx; float a7 = ty*a6 - tz*crx - tx*a5;float a8 = crx*cry*srz; float a9 = crx*cry*crz; float a10 = cry*srx; float a11 = tz*a10 + ty*a9 - tx*a8;float b1 = -crz*sry - cry*srx*srz; float b2 = cry*crz*srx - sry*srz;float b5 = cry*crz - srx*sry*srz; float b6 = cry*srz + crz*srx*sry;float c1 = -b6; float c2 = b5; float c3 = tx*b6 - ty*b5; float c4 = -crx*crz; float c5 = crx*srz; float c6 = ty*c5 + tx*-c4;float c7 = b2; float c8 = -b1; float c9 = tx*-b2 - ty*-b1;// 構(gòu)建雅可比矩陣，求解for (int i = 0; i < pointSelNum; i++) {pointOri = laserCloudOri->points[i];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;matA.at<float>(i, 0) = arx;matA.at<float>(i, 1) = arz;matA.at<float>(i, 2) = aty;matB.at<float>(i, 0) = -0.05 * d2;}使用OpenCV函數(shù)進(jìn)行求解，利用QR分解加速：
cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);關(guān)于退化問題的討論，與LOAM中一致，請(qǐng)參考loam源碼解析5 ： laserOdometry（三）中的退化問題分析：
if (iterCount == 0) {cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;float eignThre[3] = {10, 10, 10};for (int i = 2; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 3; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}使用迭代計(jì)算的增量進(jìn)行姿態(tài)更新：
transformCur[1] += matX.at<float>(0, 0);transformCur[3] += matX.at<float>(1, 0);transformCur[5] += matX.at<float>(2, 0);for(int i=0; i<6; i++){if(isnan(transformCur[i]))transformCur[i]=0;}計(jì)算姿態(tài)是否合法，以及是否滿足迭代條件：
float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +pow(rad2deg(matX.at<float>(1, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(2, 0) * 100, 2));if (deltaR < 0.1 && deltaT < 0.1) {return false;}return true;}線特征匹配與優(yōu)化：
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();// 找到對(duì)應(yīng)的特征邊/角點(diǎn)// 尋找邊特征的方法和尋找平面特征的很類似，過程可以參照尋找平面特征的注釋findCorrespondingCornerFeatures(iterCount2);if (laserCloudOri->points.size() < 10)continue;// 通過角/邊特征的匹配，計(jì)算變換矩陣if (calculateTransformationCorner(iterCount2) == false)break;}}這塊和面特征匹配優(yōu)化的思想類似：
將曲率大的點(diǎn)（待匹配的特征點(diǎn)）的坐標(biāo)全部轉(zhuǎn)換到當(dāng)前幀的初始時(shí)刻：
每五次迭代重新匹配特征點(diǎn)：
if (iterCount % 5 == 0) {在上一幀曲率較大的點(diǎn)組成的KD樹中尋找一個(gè)最近鄰點(diǎn)，找到該最近鄰點(diǎn)的線束，
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1;if (pointSearchSqDis[0] < nearestFeatureSearchSqDist) {closestPointInd = pointSearchInd[0];int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);在最近鄰點(diǎn)i ii所在線束的相鄰線束中尋找第二個(gè)最近點(diǎn)，組成匹配直線：
首先在雷達(dá)點(diǎn)云存儲(chǔ)的正方向(ScanID增大的方向和雷達(dá)旋轉(zhuǎn)正方向)尋找另外一個(gè)點(diǎn)：
然后在雷達(dá)點(diǎn)云存儲(chǔ)的反方向(ScanID減小的方向和雷達(dá)旋轉(zhuǎn)反方向)尋找其他兩個(gè)點(diǎn)：
找到用于匹配的兩個(gè)點(diǎn)（形成邊緣線）：
pointSearchCornerInd1[i] = closestPointInd;pointSearchCornerInd2[i] = minPointInd2;}找到特征點(diǎn)的對(duì)應(yīng)點(diǎn)以后的工作就是計(jì)算距離了，在這里還計(jì)算了就是點(diǎn)到直線的方向向量在各個(gè)軸的方向分解，在雅可比的計(jì)算中使用：
if (pointSearchCornerInd2[i] >= 0) {tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = tripod1.x;float y1 = tripod1.y;float z1 = tripod1.z;float x2 = tripod2.x;float y2 = tripod2.y;float z2 = tripod2.z;float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));float a012 = sqrt(m11 * m11 + m22 * m22 + m33 * m33);float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));float la = ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;float ld2 = a012 / l12;float s = 1;if (iterCount >= 5) {s = 1 - 1.8 * fabs(ld2);}if (s > 0.1 && ld2 != 0) {coeff.x = s * la; coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;laserCloudOri->push_back(cornerPointsSharp->points[i]);coeffSel->push_back(coeff);}}}}線特征優(yōu)化（我已經(jīng)麻了）：
bool calculateTransformationCorner(int iterCount){int pointSelNum = laserCloudOri->points.size();cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));// 以下為開始計(jì)算A,A=[J的偏導(dǎo)],J的偏導(dǎo)的計(jì)算公式是什么?float srx = sin(transformCur[0]);float crx = cos(transformCur[0]);float sry = sin(transformCur[1]);float cry = cos(transformCur[1]);float srz = sin(transformCur[2]);float crz = cos(transformCur[2]);float tx = transformCur[3];float ty = transformCur[4];float tz = transformCur[5];float b1 = -crz*sry - cry*srx*srz; float b2 = cry*crz*srx - sry*srz; float b3 = crx*cry; float b4 = tx*-b1 + ty*-b2 + tz*b3;float b5 = cry*crz - srx*sry*srz; float b6 = cry*srz + crz*srx*sry; float b7 = crx*sry; float b8 = tz*b7 - ty*b6 - tx*b5;float c5 = crx*srz;for (int i = 0; i < pointSelNum; i++) {pointOri = laserCloudOri->points[i];coeff = coeffSel->points[i];float ary = (b1*pointOri.x + b2*pointOri.y - b3*pointOri.z + b4) * coeff.x+ (b5*pointOri.x + b6*pointOri.y - b7*pointOri.z + b8) * coeff.z;float atx = -b5 * coeff.x + c5 * coeff.y + b1 * coeff.z;float atz = b7 * coeff.x - srx * coeff.y - b3 * coeff.z;float d2 = coeff.intensity;// A=[J的偏導(dǎo)]; B=[權(quán)重系數(shù)*(點(diǎn)到直線的距離)] 求解公式: AX=B// 為了讓左邊滿秩，同乘At-> At*A*X = At*BmatA.at<float>(i, 0) = ary;matA.at<float>(i, 1) = atx;matA.at<float>(i, 2) = atz;matB.at<float>(i, 0) = -0.05 * d2;}// transpose函數(shù)求得matA的轉(zhuǎn)置matAtcv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// 通過QR分解的方法，求解方程AtA*X=AtB，得到Xcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);if (iterCount == 0) {cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));// 計(jì)算At*A的特征值和特征向量// 特征值存放在matE，特征向量matVcv::eigen(matAtA, matE, matV);matV.copyTo(matV2);// 退化的具體表現(xiàn)是指什么？isDegenerate = false;float eignThre[3] = {10, 10, 10};for (int i = 2; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 3; j++) {matV2.at<float>(i, j) = 0;}// 存在比10小的特征值則出現(xiàn)退化isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}transformCur[1] += matX.at<float>(0, 0);transformCur[3] += matX.at<float>(1, 0);transformCur[5] += matX.at<float>(2, 0);for(int i=0; i<6; i++){if(isnan(transformCur[i]))transformCur[i]=0;}float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(1, 0) * 100, 2) +pow(matX.at<float>(2, 0) * 100, 2));if (deltaR < 0.1 && deltaT < 0.1) {return false;}return true;}以上部分我不完全理解就不贅述了，總之經(jīng)過上面的騷操作我們得到了當(dāng)前幀的角度旋轉(zhuǎn)量和位移量。
8、積分總變換。
然后就算旋轉(zhuǎn)變化：
// 旋轉(zhuǎn)角的累計(jì)變化量void integrateTransformation(){float rx, ry, rz, tx, ty, tz; // AccumulateRotation作用// 將計(jì)算的兩幀之間的位姿“累加”起來，獲得相對(duì)于第一幀的旋轉(zhuǎn)矩陣// transformSum + (-transformCur) =(rx,ry,rz)AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);其中 AccumulateRotation功能就是計(jì)算旋轉(zhuǎn)變化的：
void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz){// 參考：https://www.cnblogs.com/ReedLW/p/9005621.html// 0--->(cx,cy,cz)--->(lx,ly,lz)// 從0時(shí)刻到(cx,cy,cz),然后在(cx,cy,cz)的基礎(chǔ)上又旋轉(zhuǎn)(lx,ly,lz)// 求最后總的位姿結(jié)果是什么？// R*p_cur = R_c*R_l*p_cur ==> R=R_c* R_l//// |cly*clz+sly*slx*slz clz*sly*slx-cly*slz clx*sly|// R_l=| clx*slz clx*clz -slx|// |cly*slx*slz-clz*sly cly*clz*slx+sly*slz cly*clx|// R_c=...// -srx=(ccx*scy,-scx,cly*clx)*(clx*slz,clx*clz,-slx)// ...// 然后根據(jù)R再來求(ox,oy,oz)float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);ox = -asin(srx);float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));oy = atan2(srycrx / cos(ox), crycrx / cos(ox));float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));}接著計(jì)算當(dāng)前幀與初始幀的位移變化：
// 進(jìn)行平移分量的更新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);修正累計(jì)位姿變換（公式又讓我麻了，后面要是有心情在先詳細(xì)解釋這些數(shù)學(xué)原理）：
// 與accumulateRotatio聯(lián)合起來更新transformSum的rotation部分的工作// 可視為transformToEnd的下部分的逆過程PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz); void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, float alx, float aly, float alz, float &acx, float &acy, float &acz){// 參考：https://www.cnblogs.com/ReedLW/p/9005621.html// -imuStart imuEnd 0// transformSum.rot= R * R * R// YXZ ZXY k+1// bcx,bcy,bcz (rx, ry, rz)構(gòu)成了 R_(k+1)^(0)// blx,bly,blz（imuPitchStart, imuYawStart, imuRollStart） 構(gòu)成了 R_(YXZ)^(-imuStart)// alx,aly,alz（imuPitchLast, imuYawLast, imuRollLast）構(gòu)成了 R_(ZXY)^(imuEnd)float sbcx = sin(bcx);float cbcx = cos(bcx);float sbcy = sin(bcy);float cbcy = cos(bcy);float sbcz = sin(bcz);float cbcz = cos(bcz);float sblx = sin(blx);float cblx = cos(blx);float sbly = sin(bly);float cbly = cos(bly);float sblz = sin(blz);float cblz = cos(blz);float salx = sin(alx);float calx = cos(alx);float saly = sin(aly);float caly = cos(aly);float salz = sin(alz);float calz = cos(alz);float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);acx = -asin(srx);float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);acy = atan2(srycrx / cos(acx), crycrx / cos(acx));float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*cblx*salz*sblz);float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - calx*calz*cblx*sblz);acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));}最終位姿變換：
transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;}9、發(fā)布里程計(jì)及上一幀點(diǎn)云信息
發(fā)布里程計(jì)：
void publishOdometry(){geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);// rx,ry,rz轉(zhuǎn)化為四元數(shù)發(fā)布laserOdometry.header.stamp = cloudHeader.stamp;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 = transformSum[3];laserOdometry.pose.pose.position.y = transformSum[4];laserOdometry.pose.pose.position.z = transformSum[5];pubLaserOdometry.publish(laserOdometry);// laserOdometryTrans 是用于tf廣播laserOdometryTrans.stamp_ = cloudHeader.stamp;laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));tfBroadcaster.sendTransform(laserOdometryTrans);}發(fā)布點(diǎn)云：
把特征點(diǎn)云投影到每幀的結(jié)尾時(shí)刻：
用KD樹存儲(chǔ)當(dāng)前幀點(diǎn)云：
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);}frameCount++;發(fā)布各類點(diǎn)云（發(fā)布界外點(diǎn)云、線特征點(diǎn)云、面特征點(diǎn)云）：
if (frameCount >= skipFrameNum + 1) {frameCount = 0;// 調(diào)整坐標(biāo)系，x=y,y=z,z=xadjustOutlierCloud();sensor_msgs::PointCloud2 outlierCloudLast2;pcl::toROSMsg(*outlierCloud, outlierCloudLast2);outlierCloudLast2.header.stamp = cloudHeader.stamp;outlierCloudLast2.header.frame_id = "/camera";pubOutlierCloudLast.publish(outlierCloudLast2);sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = cloudHeader.stamp;laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = cloudHeader.stamp;laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}}參考：
LeGO-LOAM源碼解析5： featureAssociation（三）
坐標(biāo)轉(zhuǎn)換與imu融合
總結(jié)
以上是生活随笔為你收集整理的【LeGO-LOAM论文阅读（二）--特征提取（二）】的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 小型直播系统系列-乐聊TV的开发（二）
- 下一篇： 2020年iOS 和Android程序员