論文理論部分

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

只看核心理論部分還是很好理解的。總體流程：（特征提取部分）訂閱傳感器數據->運動畸變去除->曲率計算->去除瑕點->提取邊、平面特征->發布特征點云；（特征關聯部分）將IMU信息作為先驗->根據線特征、面特征計算位姿變換->聯合IMU數據進行位姿更新->發布里程計信息
下面來看看源碼，剖析下該部分所用到的方法。

源碼部分

首先看main函數，結構比較簡單，要注意的是處理函數并沒有以回調函數的方式運行，而是以一定的頻率在循環中運行。
ros::Rate rate(200)表示循環的頻率，
ros::spinOnce()與ros::spin()不同，ros::spinOnce()僅僅運行一次。
rate.sleep()為了控制時間對齊。

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的私有對象以及變量定義（參考LeGO-LOAM源碼解析3： featureAssociation（一））：
訂閱者：

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

點云發布者：

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

pcl儲存變量：

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;

定義相關時間：

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

接受點云的標志：

bool newSegmentedCloud;bool newSegmentedCloudInfo;bool newOutlierCloud;

定義時間戳以及系統初始化標志：

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

點云平滑度及相關標簽定義：

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

imu相關數據：

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;//旋轉角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];

里程計相關發布器：

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;

保存上幀點云的定義：

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;

里程計和tf發布：

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;

退化相關：

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

了解完一些變量后，看一下消息回調。
segmented_cloud 分割好的點云（分塊點云：包括地面點和分割點）。
segmented_cloud_info 分割好的點云，和segmented_cloud的區別是，cloud_info的強度信息是距離，而segmented_cloud的是range image的行列信息。
outlier_cloud 離群點，也就是分割失敗的點（界外點）。
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);

三個點云的回調主要是將ros通信轉化為pcl點云信息

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){cloudHeader = laserCloudMsg->header;timeScanCur = cloudHeader.stamp.toSec();timeNewSegmentedCloud = timeScanCur;segmentedCloud->clear();pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);newSegmentedCloud = true;} // 注意這里沒有轉換為PCL點云，而是直接保存的消息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回調函數，接收到IMU消息之后保存到數組。IMU還做了一些角度的轉換

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);// 加速度去除重力影響，同時坐標軸進行變換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();}

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

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

經過上面的處理Imu數據已經變換到相機坐標系，下面首先要將數據變換到世界坐標系，也就是相機初始坐標系。需要注意的是這里的角度還是原來坐標系的角度，也就是說新坐標系的roll, pitch, yaw并不對應著數據上的roll, pitch, yaw（原坐標系里的roll, pitch, yaw），因為上一步賦值的時候沒有變換roll, pitch, yaw，這個理解了后面的坐標轉換就會迎刃而解。
繞z軸旋轉（原來的x軸），注意看角度：

// 先繞Z軸(原x軸)旋轉,下方坐標系示意imuHandler()中加速度的坐標軸交換// z->Y// ^ // | ^ y->X// | /// | /// | /// -----> x->Z//// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|// [x1,y1,z1]^T=Rz*[accX,accY,accZ]// 因為在imuHandler中進行過坐標變換，// 所以下面的roll其實已經對應于新坐標系中(X-Y-Z)的yawfloat x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;

繞x軸旋轉（原來的y軸），注意看角度

// 繞X軸(原y軸)旋轉// [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軸旋轉（原來的z軸），注意看角度:

// 最后再繞Y軸(原z軸)旋轉// |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;

計算位移，速度，角度：

// 進行位移，速度，角度量的累加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;}}

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

1、新數據進來進行坐標轉換和插補等工作.

void runFeatureAssociation(){// 如果有新數據進來則執行，否則不執行任何操作if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){newSegmentedCloud = false;newSegmentedCloudInfo = false;newOutlierCloud = false;}else{return;}// 主要進行的處理是將點云數據進行坐標變換，進行插補等工作adjustDistortion();// 不完全按照公式進行光滑性計算，并保存結果calculateSmoothness();// 標記阻塞點??? 阻塞點是什么點???// 參考了csdn若愚maimai大佬的博客，這里的阻塞點指過近的點// 指在點云中可能出現的互相遮擋的情況markOccludedPoints();// 特征抽取，然后分別保存到cornerPointsSharp等等隊列中去// 保存到不同的隊列是不同類型的點云，進行了標記的工作，// 這一步中減少了點云數量，使計算量減少extractFeatures();// 發布cornerPointsSharp等4種類型的點云數據publishCloud();if (!systemInitedLM) {checkSystemInitialization();return;}// 預測位姿updateInitialGuess();// 更新變換updateTransformation();// 積分總變換integrateTransformation();publishOdometry();publishCloudsLast(); } };

如果不是新數據或者數據不同步則不進行處理，直接返回。
如果是新數據，將數據標志記為0，順序執行操作。
首先是adjustDistortion()函數處理點云數據的坐標轉換、插補等工作。
遍歷點云坐標轉換：

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

針對每個點計算偏航角yaw：

// -atan2(p.x,p.z)==>-atan2(y,x)// ori表示的是偏航角yaw，因為前面有負號，ori=[-M_PI,M_PI)// 因為segInfo.orientationDiff表示的范圍是(PI,3PI)，在2PI附近// 下面過程的主要作用是調整ori大小，滿足start<ori<endfloat ori = -atan2(point.x, point.z);

對角度進行矯正，和點云分割中的操作類似。
四種情況：
沒有轉過一半，但是start-ori>M_PI/2
沒有轉過一半，但是ori-start>3/2M_PI,說明ori太大，不合理（正常情況在前半圈的話，ori-start范圍[0,M_PI]）
轉過一半，end-ori>3/2PI,ori太小
轉過一半，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;}

然后進行imu數據與激光數據的時間軸對齊操作。我不是很理解，可以看看這篇博客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循環內進行時間軸對齊while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {// 該條件內imu數據比激光數據早，但是沒有更后面的數據// (打個比方,激光在9點時出現，imu現在只有8點的)// 這種情況上面while循環是以imuPointerFront == imuPointerLast結束的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數據充足的情況下可以進行插補// 當前timeScanCur + pointTime < imuTime[imuPointerFront]，// 而且imuPointerFront是最早一個時間大于timeScanCur + pointTime的imu數據指針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轉的角度比較大，需要考慮超過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]) {// 該條件內imu數據比激光數據早，但是沒有更后面的數據imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{// 在imu數據充足的情況下可以進行插補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;}// 距離上一次插補，旋轉過的角度變化值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);}}

以上插值過程就是根據imuPointerFront和imuPointerBack對imu數據進行插值。然后將相對速度數據從世界坐標系轉換到當前坐標系，然后在坐標轉換到最后統一的激光坐標系。不懂得可以參考：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初始數據，此時i=0，也就是將imu的本來的世界坐標系（i=0的初始坐標系就是世界坐標系）更新到新的世界坐標系（插值的新的初始角度，這個時間在i=0之前）：

void VeloToStartIMU(){// imuVeloXStart,imuVeloYStart,imuVeloZStart是點云索引i=0時刻的速度// 此處計算的是相對于初始時刻i=0時的相對速度// 這個相對速度在世界坐標系下imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;// ！！！下面從世界坐標系轉換到start坐標系，roll,pitch,yaw要取負值// 首先繞y軸進行旋轉// |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;// 繞當前x軸旋轉(-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;// 繞當前z軸旋轉(-roll)的角度// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuVeloFromStartZCur = z2;}

將所有點云數據轉換到imu的新的初始時刻，首先變換到世界坐標系（i=0）：

void TransformToStartIMU(PointType *p){// 因為在adjustDistortion函數中有對xyz的坐標進行交換的過程// 交換的過程是x=原來的y，y=原來的z，z=原來的x// 所以下面其實是繞Z軸(原先的x軸)旋轉，對應的是roll角//// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|// [x1,y1,z1]^T=Rz*[x,y,z]//// 因為在imuHandler中進行過坐標變換，// 所以下面的roll其實已經對應于新坐標系中(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軸)旋轉// // [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軸)旋轉// |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坐標系：

// 下面部分的代碼功能是從imu坐標的原點變換到i=0時imu的初始時刻(從世界坐標系變換到start坐標系)// 變換方式和函數VeloToStartIMU()中的類似// 變換順序：Cur-->世界坐標系-->Start，這兩次變換中，// 前一次是正變換，角度為正，后一次是逆變換，角度應該為負// 可以參考：// 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時刻，另外需要加上imu的位移漂移// 后面加上的 imuShiftFromStart.. 表示從start時刻到cur時刻的漂移，// (imuShiftFromStart.. 在start坐標系下)p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;}

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

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

總結

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

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