論文理論部分

特征提取不是從原始點(diǎn)云中進(jìn)行提取，而是從點(diǎn)云分割中分割出的地面點(diǎn)和分割點(diǎn)中進(jìn)行提取。參考：LeGO-LOAM論文翻譯（內(nèi)容精簡)
過程如下：

只看核心理論部分還是很好理解的。總體流程：（特征提取部分）訂閱傳感器數(shù)據(jù)->運(yùn)動(dòng)畸變?nèi)コ?>曲率計(jì)算->去除瑕點(diǎn)->提取邊、平面特征->發(fā)布特征點(diǎn)云；（特征關(guān)聯(lián)部分）將IMU信息作為先驗(yàn)->根據(jù)線特征、面特征計(jì)算位姿變換->聯(lián)合IMU數(shù)據(jù)進(jìn)行位姿更新->發(fā)布里程計(jì)信息
下面來看看源碼，剖析下該部分所用到的方法。

源碼部分

首先看main函數(shù)，結(jié)構(gòu)比較簡單，要注意的是處理函數(shù)并沒有以回調(diào)函數(shù)的方式運(yùn)行，而是以一定的頻率在循環(huán)中運(yùn)行。
ros::Rate rate(200)表示循環(huán)的頻率，
ros::spinOnce()與ros::spin()不同，ros::spinOnce()僅僅運(yùn)行一次。
rate.sleep()為了控制時(shí)間對(duì)齊。

int main(int argc, char** argv) {ros::init(argc, argv, "lego_loam");ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");FeatureAssociation FA;ros::Rate rate(200);while (ros::ok()){ros::spinOnce();FA.runFeatureAssociation();rate.sleep();}ros::spin();return 0; }

查看 FeatureAssociation的私有對(duì)象以及變量定義（參考LeGO-LOAM源碼解析3： featureAssociation（一））：
訂閱者：

ros::NodeHandle nh;ros::Subscriber subLaserCloud;ros::Subscriber subLaserCloudInfo;ros::Subscriber subOutlierCloud;ros::Subscriber subImu;

點(diǎn)云發(fā)布者：

ros::Publisher pubCornerPointsSharp;ros::Publisher pubCornerPointsLessSharp;ros::Publisher pubSurfPointsFlat;ros::Publisher pubSurfPointsLessFlat;

pcl儲(chǔ)存變量：

pcl::PointCloud<PointType>::Ptr segmentedCloud;pcl::PointCloud<PointType>::Ptr outlierCloud;pcl::PointCloud<PointType>::Ptr cornerPointsSharp;pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp;pcl::PointCloud<PointType>::Ptr surfPointsFlat;pcl::PointCloud<PointType>::Ptr surfPointsLessFlat;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScanDS;pcl::VoxelGrid<PointType> downSizeFilter;

定義相關(guān)時(shí)間：

double timeScanCur;double timeNewSegmentedCloud;double timeNewSegmentedCloudInfo;double timeNewOutlierCloud;

接受點(diǎn)云的標(biāo)志：

bool newSegmentedCloud;bool newSegmentedCloudInfo;bool newOutlierCloud;

定義時(shí)間戳以及系統(tǒng)初始化標(biāo)志：

cloud_msgs::cloud_info segInfo;std_msgs::Header cloudHeader;int systemInitCount;bool systemInited;

點(diǎn)云平滑度及相關(guān)標(biāo)簽定義：

std::vector<smoothness_t> cloudSmoothness;float cloudCurvature[N_SCAN*Horizon_SCAN];int cloudNeighborPicked[N_SCAN*Horizon_SCAN];int cloudLabel[N_SCAN*Horizon_SCAN];

imu相關(guān)數(shù)據(jù)：

int imuPointerFront;int imuPointerLast;int imuPointerLastIteration;float imuRollStart, imuPitchStart, imuYawStart;float cosImuRollStart, cosImuPitchStart, cosImuYawStart, sinImuRollStart, sinImuPitchStart, sinImuYawStart;float imuRollCur, imuPitchCur, imuYawCur;float imuVeloXStart, imuVeloYStart, imuVeloZStart;float imuShiftXStart, imuShiftYStart, imuShiftZStart;float imuVeloXCur, imuVeloYCur, imuVeloZCur;float imuShiftXCur, imuShiftYCur, imuShiftZCur;float imuShiftFromStartXCur, imuShiftFromStartYCur, imuShiftFromStartZCur;float imuVeloFromStartXCur, imuVeloFromStartYCur, imuVeloFromStartZCur;float imuAngularRotationXCur, imuAngularRotationYCur, imuAngularRotationZCur;float imuAngularRotationXLast, imuAngularRotationYLast, imuAngularRotationZLast;float imuAngularFromStartX, imuAngularFromStartY, imuAngularFromStartZ;//旋轉(zhuǎn)角double imuTime[imuQueLength];float imuRoll[imuQueLength];float imuPitch[imuQueLength];float imuYaw[imuQueLength];//加速度float imuAccX[imuQueLength];float imuAccY[imuQueLength];float imuAccZ[imuQueLength];//速度float imuVeloX[imuQueLength];float imuVeloY[imuQueLength];float imuVeloZ[imuQueLength]; //偏移量float imuShiftX[imuQueLength];float imuShiftY[imuQueLength];float imuShiftZ[imuQueLength]; //角速度float imuAngularVeloX[imuQueLength];float imuAngularVeloY[imuQueLength];float imuAngularVeloZ[imuQueLength]; //角度float imuAngularRotationX[imuQueLength];float imuAngularRotationY[imuQueLength];float imuAngularRotationZ[imuQueLength];

里程計(jì)相關(guān)發(fā)布器：

ros::Publisher pubLaserCloudCornerLast;ros::Publisher pubLaserCloudSurfLast;ros::Publisher pubLaserOdometry;ros::Publisher pubOutlierCloudLast;int skipFrameNum;bool systemInitedLM;int laserCloudCornerLastNum;int laserCloudSurfLastNum;int pointSelCornerInd[N_SCAN*Horizon_SCAN];float pointSearchCornerInd1[N_SCAN*Horizon_SCAN];float pointSearchCornerInd2[N_SCAN*Horizon_SCAN];int pointSelSurfInd[N_SCAN*Horizon_SCAN];float pointSearchSurfInd1[N_SCAN*Horizon_SCAN];float pointSearchSurfInd2[N_SCAN*Horizon_SCAN];float pointSearchSurfInd3[N_SCAN*Horizon_SCAN];float transformCur[6];float transformSum[6];float imuRollLast, imuPitchLast, imuYawLast;float imuShiftFromStartX, imuShiftFromStartY, imuShiftFromStartZ;float imuVeloFromStartX, imuVeloFromStartY, imuVeloFromStartZ;

保存上幀點(diǎn)云的定義：

pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;pcl::PointCloud<PointType>::Ptr laserCloudOri;pcl::PointCloud<PointType>::Ptr coeffSel;pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast;pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast;

里程計(jì)和tf發(fā)布：

std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff;nav_msgs::Odometry laserOdometry;tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform laserOdometryTrans;

退化相關(guān)：

bool isDegenerate;cv::Mat matP;int frameCount;

了解完一些變量后，看一下消息回調(diào)。
segmented_cloud 分割好的點(diǎn)云（分塊點(diǎn)云：包括地面點(diǎn)和分割點(diǎn)）。
segmented_cloud_info 分割好的點(diǎn)云，和segmented_cloud的區(qū)別是，cloud_info的強(qiáng)度信息是距離，而segmented_cloud的是range image的行列信息。
outlier_cloud 離群點(diǎn)，也就是分割失敗的點(diǎn)（界外點(diǎn)）。
imuTopic 接收imu的消息。

subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);

三個(gè)點(diǎn)云的回調(diào)主要是將ros通信轉(zhuǎn)化為pcl點(diǎn)云信息

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){cloudHeader = laserCloudMsg->header;timeScanCur = cloudHeader.stamp.toSec();timeNewSegmentedCloud = timeScanCur;segmentedCloud->clear();pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);newSegmentedCloud = true;} // 注意這里沒有轉(zhuǎn)換為PCL點(diǎn)云，而是直接保存的消息cloud_msgs::cloud_info segInfovoid laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn){timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();segInfo = *msgIn;newSegmentedCloudInfo = true;} void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){timeNewOutlierCloud = msgIn->header.stamp.toSec();outlierCloud->clear();pcl::fromROSMsg(*msgIn, *outlierCloud);newOutlierCloud = true;}

接下來看IMU回調(diào)函數(shù)，接收到IMU消息之后保存到數(shù)組。IMU還做了一些角度的轉(zhuǎn)換

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// 加速度去除重力影響，同時(shí)坐標(biāo)軸進(jìn)行變換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;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();}

上面首先是對(duì)重力加速度的去除，然后進(jìn)行坐標(biāo)轉(zhuǎn)化。這塊我也是懵逼了好久，后來慢慢看了一下三維旋轉(zhuǎn)理解了。
防止文章過長，不知道原因的可以去看我的這篇博客：Logo-loam中imu消除重力影響
知道的可以跳過。
坐標(biāo)轉(zhuǎn)換主要過程是首先將重力加速度轉(zhuǎn)換到當(dāng)前imu坐標(biāo)系，然后imu加速度坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系上。主要是因?yàn)橄鄼C(jī)坐標(biāo)系和imu坐標(biāo)系并不統(tǒng)一，然后將歐拉角，加速度，速度保存到循環(huán)隊(duì)列中；AccumulateIMUShiftAndRotation()對(duì)速度，角速度，加速度進(jìn)行積分，得到位移，角度和速度；
賦值操作：

void AccumulateIMUShiftAndRotation(){float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];

經(jīng)過上面的處理Imu數(shù)據(jù)已經(jīng)變換到相機(jī)坐標(biāo)系，下面首先要將數(shù)據(jù)變換到世界坐標(biāo)系，也就是相機(jī)初始坐標(biāo)系。需要注意的是這里的角度還是原來坐標(biāo)系的角度，也就是說新坐標(biāo)系的roll, pitch, yaw并不對(duì)應(yīng)著數(shù)據(jù)上的roll, pitch, yaw（原坐標(biāo)系里的roll, pitch, yaw），因?yàn)樯弦徊劫x值的時(shí)候沒有變換roll, pitch, yaw，這個(gè)理解了后面的坐標(biāo)轉(zhuǎn)換就會(huì)迎刃而解。
繞z軸旋轉(zhuǎn)（原來的x軸），注意看角度：

// 先繞Z軸(原x軸)旋轉(zhuǎn),下方坐標(biāo)系示意imuHandler()中加速度的坐標(biāo)軸交換// z->Y// ^ // | ^ y->X// | /// | /// | /// -----> x->Z//// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|// [x1,y1,z1]^T=Rz*[accX,accY,accZ]// 因?yàn)樵趇muHandler中進(jìn)行過坐標(biāo)變換，// 所以下面的roll其實(shí)已經(jīng)對(duì)應(yīng)于新坐標(biāo)系中(X-Y-Z)的yawfloat x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;

繞x軸旋轉(zhuǎn)（原來的y軸），注意看角度

// 繞X軸(原y軸)旋轉(zhuǎn)// [x2,y2,z2]^T=Rx*[x1,y1,z1]// |1 0 0|// Rx=|0 cosrx -sinrx|// |0 sinrx cosrx|float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;

繞y軸旋轉(zhuǎn)（原來的z軸），注意看角度:

// 最后再繞Y軸(原z軸)旋轉(zhuǎn)// |cosry 0 sinry|// Ry=|0 1 0|// |-sinry 0 cosry|accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;

計(jì)算位移，速度，角度：

// 進(jìn)行位移，速度，角度量的累加int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff < scanPeriod) {imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;}}

回到主函數(shù)中，主體函數(shù)是runFeatureAssociation()，緊接著下面查看該函數(shù)的結(jié)構(gòu)以及功能，新數(shù)據(jù)指的是點(diǎn)云分割隨后發(fā)布的三種數(shù)據(jù)，確保它們是同一時(shí)間的數(shù)據(jù)后才進(jìn)行下一步的處理。
主要過程為：
1、新數(shù)據(jù)進(jìn)來進(jìn)行坐標(biāo)轉(zhuǎn)換和插補(bǔ)等工作.
2、進(jìn)行光滑度計(jì)算。
3、標(biāo)記阻塞點(diǎn)。
4、特征抽取。
5、發(fā)布四種點(diǎn)云信息。
6、預(yù)測位姿，
7、更新變換位姿。
8、積分總變換。
9、發(fā)布里程計(jì)及上一幀點(diǎn)云信息。

1、新數(shù)據(jù)進(jìn)來進(jìn)行坐標(biāo)轉(zhuǎn)換和插補(bǔ)等工作.

void runFeatureAssociation(){// 如果有新數(shù)據(jù)進(jìn)來則執(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;}// 主要進(jìn)行的處理是將點(diǎn)云數(shù)據(jù)進(jìn)行坐標(biāo)變換，進(jìn)行插補(bǔ)等工作adjustDistortion();// 不完全按照公式進(jìn)行光滑性計(jì)算，并保存結(jié)果calculateSmoothness();// 標(biāo)記阻塞點(diǎn)??? 阻塞點(diǎn)是什么點(diǎn)???// 參考了csdn若愚maimai大佬的博客，這里的阻塞點(diǎn)指過近的點(diǎn)// 指在點(diǎn)云中可能出現(xiàn)的互相遮擋的情況markOccludedPoints();// 特征抽取，然后分別保存到cornerPointsSharp等等隊(duì)列中去// 保存到不同的隊(duì)列是不同類型的點(diǎn)云，進(jìn)行了標(biāo)記的工作，// 這一步中減少了點(diǎn)云數(shù)量，使計(jì)算量減少extractFeatures();// 發(fā)布cornerPointsSharp等4種類型的點(diǎn)云數(shù)據(jù)publishCloud();if (!systemInitedLM) {checkSystemInitialization();return;}// 預(yù)測位姿updateInitialGuess();// 更新變換updateTransformation();// 積分總變換integrateTransformation();publishOdometry();publishCloudsLast(); } };

如果不是新數(shù)據(jù)或者數(shù)據(jù)不同步則不進(jìn)行處理，直接返回。
如果是新數(shù)據(jù)，將數(shù)據(jù)標(biāo)志記為0，順序執(zhí)行操作。
首先是adjustDistortion()函數(shù)處理點(diǎn)云數(shù)據(jù)的坐標(biāo)轉(zhuǎn)換、插補(bǔ)等工作。
遍歷點(diǎn)云坐標(biāo)轉(zhuǎn)換：

void adjustDistortion(){bool halfPassed = false;int cloudSize = segmentedCloud->points.size();PointType point;for (int i = 0; i < cloudSize; i++) {// 這里xyz與laboshin_loam代碼中的一樣經(jīng)過坐標(biāo)軸變換// imuhandler() 中相同的坐標(biāo)變換point.x = segmentedCloud->points[i].y;point.y = segmentedCloud->points[i].z;point.z = segmentedCloud->points[i].x;

針對(duì)每個(gè)點(diǎn)計(jì)算偏航角yaw：

// -atan2(p.x,p.z)==>-atan2(y,x)// ori表示的是偏航角yaw，因?yàn)榍懊嬗胸?fù)號(hào)，ori=[-M_PI,M_PI)// 因?yàn)閟egInfo.orientationDiff表示的范圍是(PI,3PI)，在2PI附近// 下面過程的主要作用是調(diào)整ori大小，滿足start<ori<endfloat ori = -atan2(point.x, point.z);

對(duì)角度進(jìn)行矯正，和點(diǎn)云分割中的操作類似。
四種情況：
沒有轉(zhuǎn)過一半，但是start-ori>M_PI/2
沒有轉(zhuǎn)過一半，但是ori-start>3/2M_PI,說明ori太大，不合理（正常情況在前半圈的話，ori-start范圍[0,M_PI]）
轉(zhuǎn)過一半，end-ori>3/2PI,ori太小
轉(zhuǎn)過一半，ori-end>M_PI/2,太大

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;}

然后進(jìn)行imu數(shù)據(jù)與激光數(shù)據(jù)的時(shí)間軸對(duì)齊操作。我不是很理解，可以看看這篇博客LeGo-LOAM源碼閱讀筆記(三)—featureAssociation.cpp

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)進(jìn)行時(shí)間軸對(duì)齊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ù)// (打個(gè)比方,激光在9點(diǎn)時(shí)出現(xiàn)，imu現(xiàn)在只有8點(diǎn)的)// 這種情況上面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ù)充足的情況下可以進(jìn)行插補(bǔ)// 當(dāng)前timeScanCur + pointTime < imuTime[imuPointerFront]，// 而且imuPointerFront是最早一個(gè)時(shí)間大于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]);// 通過上面計(jì)算的ratioFront以及ratioBack進(jìn)行插補(bǔ)// 因?yàn)閕muRollCur和imuPitchCur通常都在0度左右，變化不會(huì)很大，因此不需要考慮超過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速度插補(bǔ)imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;// imu位置插補(bǔ)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()中,// 更新每個(gè)角的正余弦值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ù)充足的情況下可以進(jìn)行插補(bǔ)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;}// 距離上一次插補(bǔ)，旋轉(zhuǎn)過的角度變化值imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;imuAngularRotationXLast = imuAngularRotationXCur;imuAngularRotationYLast = imuAngularRotationYCur;imuAngularRotationZLast = imuAngularRotationZCur;// 這里更新的是i=0時(shí)刻的rpy角，后面將速度坐標(biāo)投影過來會(huì)用到i=0時(shí)刻的值updateImuRollPitchYawStartSinCos();} else {// 速度投影到初始i=0時(shí)刻VeloToStartIMU();// 將點(diǎn)的坐標(biāo)變換到初始i=0時(shí)刻TransformToStartIMU(&point);}}

以上插值過程就是根據(jù)imuPointerFront和imuPointerBack對(duì)imu數(shù)據(jù)進(jìn)行插值。然后將相對(duì)速度數(shù)據(jù)從世界坐標(biāo)系轉(zhuǎn)換到當(dāng)前坐標(biāo)系，然后在坐標(biāo)轉(zhuǎn)換到最后統(tǒng)一的激光坐標(biāo)系。不懂得可以參考：LeGO-LOAM源碼解析4： featureAssociation（二）
更新imu開始角度正余弦指：

void updateImuRollPitchYawStartSinCos(){cosImuRollStart = cos(imuRollStart);cosImuPitchStart = cos(imuPitchStart);cosImuYawStart = cos(imuYawStart);sinImuRollStart = sin(imuRollStart);sinImuPitchStart = sin(imuPitchStart);sinImuYawStart = sin(imuYawStart);}

更新imu初始數(shù)據(jù)，此時(shí)i=0，也就是將imu的本來的世界坐標(biāo)系（i=0的初始坐標(biāo)系就是世界坐標(biāo)系）更新到新的世界坐標(biāo)系（插值的新的初始角度，這個(gè)時(shí)間在i=0之前）：

void VeloToStartIMU(){// imuVeloXStart,imuVeloYStart,imuVeloZStart是點(diǎn)云索引i=0時(shí)刻的速度// 此處計(jì)算的是相對(duì)于初始時(shí)刻i=0時(shí)的相對(duì)速度// 這個(gè)相對(duì)速度在世界坐標(biāo)系下imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;// ！！！下面從世界坐標(biāo)系轉(zhuǎn)換到start坐標(biāo)系，roll,pitch,yaw要取負(fù)值// 首先繞y軸進(jìn)行旋轉(zhuǎn)// |cosry 0 sinry|// Ry=|0 1 0|// |-sinry 0 cosry|float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sinImuYawStart * imuVeloFromStartXCur + cosImuYawStart * imuVeloFromStartZCur;// 繞當(dāng)前x軸旋轉(zhuǎn)(-pitch)的角度// |1 0 0|// Rx=|0 cosrx -sinrx|// |0 sinrx cosrx|float x2 = x1;float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;// 繞當(dāng)前z軸旋轉(zhuǎn)(-roll)的角度// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuVeloFromStartZCur = z2;}

將所有點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到imu的新的初始時(shí)刻，首先變換到世界坐標(biāo)系（i=0）：

void TransformToStartIMU(PointType *p){// 因?yàn)樵赼djustDistortion函數(shù)中有對(duì)xyz的坐標(biāo)進(jìn)行交換的過程// 交換的過程是x=原來的y，y=原來的z，z=原來的x// 所以下面其實(shí)是繞Z軸(原先的x軸)旋轉(zhuǎn)，對(duì)應(yīng)的是roll角//// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|// [x1,y1,z1]^T=Rz*[x,y,z]//// 因?yàn)樵趇muHandler中進(jìn)行過坐標(biāo)變換，// 所以下面的roll其實(shí)已經(jīng)對(duì)應(yīng)于新坐標(biāo)系中(X-Y-Z)的yawfloat x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;// 繞X軸(原先的y軸)旋轉(zhuǎn)// // [x2,y2,z2]^T=Rx*[x1,y1,z1]// |1 0 0|// Rx=|0 cosrx -sinrx|// |0 sinrx cosrx|float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;// 最后再繞Y軸(原先的Z軸)旋轉(zhuǎn)// |cosry 0 sinry|// Ry=|0 1 0|// |-sinry 0 cosry|float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;

然后變換到新的start坐標(biāo)系：

// 下面部分的代碼功能是從imu坐標(biāo)的原點(diǎn)變換到i=0時(shí)imu的初始時(shí)刻(從世界坐標(biāo)系變換到start坐標(biāo)系)// 變換方式和函數(shù)VeloToStartIMU()中的類似// 變換順序：Cur-->世界坐標(biāo)系-->Start，這兩次變換中，// 前一次是正變換，角度為正，后一次是逆變換，角度應(yīng)該為負(fù)// 可以參考：// https://blog.csdn.net/wykxwyc/article/details/101712524float x4 = cosImuYawStart * x3 - sinImuYawStart * z3;float y4 = y3;float z4 = sinImuYawStart * x3 + cosImuYawStart * z3;float x5 = x4;float y5 = cosImuPitchStart * y4 + sinImuPitchStart * z4;float z5 = -sinImuPitchStart * y4 + cosImuPitchStart * z4;// 繞z軸(原先的x軸)變換角度到初始imu時(shí)刻，另外需要加上imu的位移漂移// 后面加上的 imuShiftFromStart.. 表示從start時(shí)刻到cur時(shí)刻的漂移，// (imuShiftFromStart.. 在start坐標(biāo)系下)p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;}

以上坐標(biāo)變換以及插值工作完成。
以下工作下一篇文章再分享：
2、進(jìn)行光滑度計(jì)算
3、標(biāo)記阻塞點(diǎn)。
4、特征抽取。
5、發(fā)布四種點(diǎn)云信息。
6、預(yù)測位姿，
7、更新變換位姿。
8、積分總變換。
9、發(fā)布里程計(jì)及上一幀點(diǎn)云信息。

參考：
LeGO-LOAM論文翻譯（內(nèi)容精簡)
Lego-LOAM IMU坐標(biāo)系變換的詳細(xì)記錄
Logo-loam中imu消除重力影響
LeGo-LOAM源碼閱讀筆記(三)—featureAssociation.cpp
LeGo-LOAM源碼篇:源碼分析(2)
LeGO-LOAM源碼解析3： featureAssociation（一）
loam源碼解析1 ： scanRegistration（一）
LeGO-LOAM源碼解析4： featureAssociation（二）

總結(jié)

以上是生活随笔為你收集整理的【LeGO-LOAM论文阅读（二）--特征提取（一）】的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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