一、初始化
重新回到主函数，
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;
}
在 main 函数中创建 FeatureAssociation 类的实例时：
FeatureAssociation FA;FeatureAssociation 类的构造函数会被执行，也就是会接收点云信息sensor_msgs，也会发布处理后的点云信息sensor_msgs，同时 initializationValue() 方法也会被调用。
FeatureAssociation():nh("~"){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);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);initializationValue();}当初始化过程完成以后，我们进入正题！！！来看runFeatureAssociation()函数！！！
二、runFeatureAssociation()
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;}/**1. Feature Extraction*/adjustDistortion();calculateSmoothness();markOccludedPoints();extractFeatures();publishCloud(); // cloud for visualization/**2. Feature Association*/if (!systemInitedLM) {checkSystemInitialization();return;}updateInitialGuess();updateTransformation();integrateTransformation();publishOdometry();publishCloudsLast(); // cloud to mapOptimization}整体的代码逻辑非常清晰，
newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud 这三个分别对应三个回调函数的结果，如果点云回调函数被成功调用，那么newSegmentedCloud = true，只有回调函数没有成功调用！！或者（timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05，时间不同步，才return。。
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){cloudHeader = laserCloudMsg->header;timeScanCur = cloudHeader.stamp.toSec();timeNewSegmentedCloud = timeScanCur;segmentedCloud->clear();pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);newSegmentedCloud = true;}然后我们来看具体的代码细节，第一部分是特征提取！！
1. Feature Extraction
adjustDistortion();
第一个函数，去畸变。
借用一下lego-loam阅读理解笔记 二_void adjustdistortion()-CSDN博客这位大佬的图，这个图片非常好，能够清晰的看到这个函数是做什么的：
该函数实现点云的空间、时间都统一到IMU坐标系（IMU坐标在前面的博客，imu数据预处理的时候已经做过一次坐标转换了，这里指坐标转换后的坐标系！！！）中。也就是激光点云的畸变补偿。激光雷达在不断的扫描时，也在运动，所以一帧从头扫到尾，激光点坐标并没有在同一坐标系中，如图1所示。
void adjustDistortion(){bool halfPassed = false;int cloudSize = segmentedCloud->points.size();PointType point;for (int i = 0; i < cloudSize; i++) {point.x = segmentedCloud->points[i].y;point.y = segmentedCloud->points[i].z;point.z = segmentedCloud->points[i].x;float ori = -atan2(point.x, point.z);if (!halfPassed) {if (ori < segInfo.startOrientation - M_PI / 2)ori += 2 * M_PI;else if (ori > segInfo.startOrientation + M_PI * 3 / 2)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)ori += 2 * M_PI;else if (ori > segInfo.endOrientation + M_PI / 2)ori -= 2 * M_PI;}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 (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {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 {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]);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;}imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;if (timeScanCur + pointTime > imuTime[imuPointerFront]) {imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{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;updateImuRollPitchYawStartSinCos();} else {VeloToStartIMU();TransformToStartIMU(&point);}}segmentedCloud->points[i] = point;}imuPointerLastIteration = imuPointerLast;}第一步，把点云坐标系转换到imu变换之后的坐标系：
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
第二步，调整ori大小，满足start<ori<end！！
注意这个时候x/z,相当于之前的y/x。
float ori = -atan2(point.x, point.z);if (!halfPassed) {if (ori < segInfo.startOrientation - M_PI / 2)ori += 2 * M_PI;else if (ori > segInfo.startOrientation + M_PI * 3 / 2)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)ori += 2 * M_PI;else if (ori > segInfo.endOrientation + M_PI / 2)ori -= 2 * M_PI;}有关起始角和终止角的问题可以看我之前的博客！！
lego-loam imageProjection.cpp源码注释（一）-CSDN博客
第三步，用 point.intensity 来保存时间
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;segInfo.orientationDiff是终止角减起始角，relTime是ori占一次完整扫描时间的比例。
首先获取原始点云（segmentedCloud）中第 i 个点的强度（intensity），然后将其转换为整数类型。接着，它将扫描周期（scanPeriod）与relTime的乘积加到这个强度值上。
extern const float scanPeriod = 0.1;
第四步，imu数据插值！！
if (imuPointerLast >= 0) {float pointTime = relTime * scanPeriod;imuPointerFront = imuPointerLastIteration;while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {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 {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]);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;}imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;if (timeScanCur + pointTime > imuTime[imuPointerFront]) {imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{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;updateImuRollPitchYawStartSinCos();} else {VeloToStartIMU();TransformToStartIMU(&point);}}segmentedCloud->points[i] = point;}imuPointerLastIteration = imuPointerLast;}分块来看：参考自LeGO-LOAM源码解析4： featureAssociation（二）_lego-loam曲率计算-CSDN博客
pointTime是在一个扫描周期内的相对时间。
imuPointerFront是一个指针，指向扫描开始时的imu数据。
所以将imuPointerFront的初值为上一次迭代的末尾，也就是上一个imuPointerLast。
利用while循环不断让imuPointerFront的值递增！！直到找到超越当前点云点时间的IMU时间。
没有找到则imuPointerFront为imuPointerLast:
if (imuPointerLast >= 0) {float pointTime = relTime * scanPeriod;imuPointerFront = imuPointerLastIteration;while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}
若没有找到超越点云时间的IMU时间，则保存最新IMU数据与当前点云的这个点对应，最新的imu数据也就是上一次迭代的最后一个imu数据！！
然后将imuPointerFront的值作为当前imu的值。
主要包括RPY角，XYZ线速度，XYZ偏移量。分别对应imu预积分中的R、V、P。
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {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]; 若找到超越点云时间的IMU时间，利用前后的IMU时间对该点云点的IMU数据进行插补：
imuPointerFront 指向的时间点是比 imuPointerBack 更靠后的。换句话说，imuPointerFront 表示的时间戳比 imuPointerBack 的时间戳更晚。
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
lego-loam中通过利用当前点云前后的IMU数据对当前点云的IMU数据进行插值！！！
分别对旋转角、速度、位移进行线性插值，得到点云中每个点的IMU数据，需要注意的是，再进行旋转角的插值的时候，roll和pitch的角度变化一般都是比较小的，但是yaw变化可能比较大，所以要进行判断和补偿：
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;
}imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;宗旨就是结束角比起始角不能超过半圈！！
imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI//如果大于半圈
那么imuYaw[imuPointerBack] + 2 * M_PI，这样就小于半圈了。。
imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI//如果小于半圈
那么imuYaw[imuPointerBack] - 2 * M_PI，就不小于半圈了！！
第五步，处理第一个点的imu数据
if (i == 0) {imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;if (timeScanCur + pointTime > imuTime[imuPointerFront]) {imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{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;}如果是第一个点，i==0，保存对应IMU信息。
IMU时间>第一个点时间则插值，否则就保存最近的imu信息。
计算与上一帧的角度变化：
imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;
imuAngularRotationXLast = imuAngularRotationXCur;
imuAngularRotationYLast = imuAngularRotationYCur;
imuAngularRotationZLast = imuAngularRotationZCur;updateImuRollPitchYawStartSinCos();
感觉这一步是为了更新旋转矩阵，提前把cos和sin值算出来。计算第一个点RPY的三角函数值。
void updateImuRollPitchYawStartSinCos(){cosImuRollStart = cos(imuRollStart);cosImuPitchStart = cos(imuPitchStart);cosImuYawStart = cos(imuYawStart);sinImuRollStart = sin(imuRollStart);sinImuPitchStart = sin(imuPitchStart);sinImuYawStart = sin(imuYawStart);}第六步，处理除第一个点以外的点
参考自LeGO-LOAM源码解析4： featureAssociation（二）_lego-loam曲率计算-CSDN博客
速度投影VeloToStartIMU()
因为在adjustDistortion函数中有对xyz的坐标进行交换的过程
交换的过程是x=原来的y，y=原来的z，z=原来的x
从世界坐标系转换到start，也就是第一个点的坐标系，roll,pitch,yaw要取负值
// 首先绕y轴进行旋转，因为取负值，sin变号// |cosry 0 -sinry|// Ry=|0 1 0|// |sinry 0 cosry|
// 绕当前x轴旋转(-pitch)的角度// |1 0 0|// Rx=|0 cosrx sinrx|// |0 -sinrx cosrx|
// 绕当前z轴旋转(-roll)的角度// |cosrz sinrz 0|// Rz=|-sinrz cosrz 0|// |0 0 1|void VeloToStartIMU(){imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sinImuYawStart * imuVeloFromStartXCur + cosImuYawStart * imuVeloFromStartZCur;float x2 = x1;float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuVeloFromStartZCur = z2;}点云投影TransformToStartIMU(&point)
将所有点云点转换到世界坐标系下：
绕Z轴(原先的x轴)旋转，对应的是roll角//// |cosrz -sinrz 0|// Rz=|sinrz cosrz 0|// |0 0 1|
// 因为在imuHandler中进行过坐标变换，
// 所以roll其实已经对应于新坐标系中(X-Y-Z)的yaw// 绕X轴(原先的y轴)旋转// |1 0 0|// Rx=|0 cosrx -sinrx|// |0 sinrx cosrx|// 最后再绕Y轴(原先的Z轴)旋转// |cosry 0 sinry|// Ry=|0 1 0|// |-sinry 0 cosry| float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;将点云点从世界坐标系下投影到点云中第一个点的坐标系下：
变换顺序：Cur-->世界坐标系-->Start，这两次变换中，
// 前一次是正变换，角度为正，后一次是逆变换，角度应该为负
// 首先绕y轴进行旋转，因为取负值，sin变号// |cosry 0 -sinry|// Ry=|0 1 0|// |sinry 0 cosry|
// 绕当前x轴旋转(-pitch)的角度// |1 0 0|// Rx=|0 cosrx sinrx|// |0 -sinrx cosrx|// 绕当前z轴旋转(-roll)的角度// |cosrz sinrz 0|// Rz=|-sinrz cosrz 0|// |0 0 1|// 绕z轴(原先的x轴)变换角度到初始imu时刻，另外需要加上imu的位移漂移
// 后面加上的 imuShiftFromStart.. 表示从start时刻到cur时刻的漂移，
// (imuShiftFromStart.. 在start坐标系下)
float 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;
p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;
p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;
p->z = z5 + imuShiftFromStartZCur;最后：
此时点云被投影到第一个点的imu坐标系中。
segmentedCloud->points[i] = point;补充：！！！
imu的测量值是变化，不可以直接累积，在imu数据预处理中，通过一重积分、二重积分，算出积分值再累积！！！
imuPointerLast = -1; //imuPointLast的初值-1。
imuPointerLastIteration = 0;
在回调函数中：
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
记录IMU时间的位置，等待下一帧点云进来，IMU以这个时间为起点：
imuPointerLastIteration = imuPointerLast;}插值细节补充！！！
