我们来看featureAssociation的最后两个函数publishOdometry()和publishCloudsLast();
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}
};publishOdometry()
void publishOdometry(){geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);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.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);}需要注意的有几个点，第一是参数的顺序为什么是transformSum[2], -transformSum[0], -transformSum[1]，第二，为什么后两个参数都加了负号？
也就是输入的顺序其实是rz、rx、ry，所以geoQuat.x对应的是rz，这和laserOdometry.pose.pose.orientation.z对得上。
然后为什么加了负号？对应这两行代码前面也有负号，也对得上。
laserOdometry.pose.pose.orientation.x = -geoQuat.y;
laserOdometry.pose.pose.orientation.y = -geoQuat.z;
如果您有更好的理解，欢迎一起讨论，比心！！
然后来看publishCloudsLast()；
publishCloudsLast()
void publishCloudsLast(){updateImuRollPitchYawStartSinCos();int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();for (int i = 0; i < cornerPointsLessSharpNum; i++) {TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);}int surfPointsLessFlatNum = surfPointsLessFlat->points.size();for (int i = 0; i < surfPointsLessFlatNum; i++) {TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}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++;if (frameCount >= skipFrameNum + 1) {frameCount = 0;adjustOutlierCloud();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);}}首先来看updateImuRollPitchYawStartSinCos();
这个函数出现过两次，一次是在adjustDistortion()中，用第一个点的imu值初始化位姿的时候，用第一个点的imu值计算过一次。这一次是在TransformToEnd之前，也就是用当前点的imu值计算一次，所以update的意思就体现在更新。
updateImuRollPitchYawStartSinCos()
void updateImuRollPitchYawStartSinCos(){cosImuRollStart = cos(imuRollStart);cosImuPitchStart = cos(imuPitchStart);cosImuYawStart = cos(imuYawStart);sinImuRollStart = sin(imuRollStart);sinImuPitchStart = sin(imuPitchStart);sinImuYawStart = sin(imuYawStart);}这个函数本身并不复杂，没有什么可说的，唯一需要注意的就是imuStart的定义，是用imuCur的数据来定义的。
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;然后我们再来看TransformToEnd：
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();for (int i = 0; i < cornerPointsLessSharpNum; i++) {TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);}TransformToEnd()
void TransformToEnd(PointType const * const pi, PointType * const po){float s = 10 * (pi->intensity - int(pi->intensity));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];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;float x3 = cos(ry) * x2 - sin(ry) * z2;float y3 = y2;float z3 = sin(ry) * x2 + cos(ry) * z2;rx = transformCur[0];ry = transformCur[1];rz = transformCur[2];tx = transformCur[3];ty = transformCur[4];tz = transformCur[5];float x4 = cos(ry) * x3 + sin(ry) * z3;float y4 = y3;float z4 = -sin(ry) * x3 + cos(ry) * z3;float x5 = x4;float y5 = cos(rx) * y4 - sin(rx) * z4;float z5 = sin(rx) * y4 + cos(rx) * z4;float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;float z6 = z5 + tz;float x7 = cosImuRollStart * (x6 - imuShiftFromStartX) - sinImuRollStart * (y6 - imuShiftFromStartY);float y7 = sinImuRollStart * (x6 - imuShiftFromStartX) + cosImuRollStart * (y6 - imuShiftFromStartY);float z7 = z6 - imuShiftFromStartZ;float x8 = x7;float y8 = cosImuPitchStart * y7 - sinImuPitchStart * z7;float z8 = sinImuPitchStart * y7 + cosImuPitchStart * z7;float x9 = cosImuYawStart * x8 + sinImuYawStart * z8;float y9 = y8;float z9 = -sinImuYawStart * x8 + cosImuYawStart * z8;float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;float y10 = y9;float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;float x11 = x10;float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;po->z = z11;po->intensity = int(pi->intensity);}
这个代码转来转去也比较绕，我们来理清一下：
首先假设同一帧的点云都是同一位姿，也就是在扫描过程中小车匀速运动。
然后根据特征优化得到的位姿，按时间线性插值做改正，得到从Start初始位姿到当前点的位姿改正量。
float s = 10 * (pi->intensity - int(pi->intensity));
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];从x1y1z1到x3y3z3的过程是计算当前点的线性位姿改正量，从x3y3z3到x6y6z6是从当前点的坐标系又转回Start初始坐标系，你看1->3是zxy的旋转顺序，3->6是yxz的旋转顺序！！为什么要转换初始坐标系呢？
因为我们之前的假设不成立，扫描过程中不是一个匀速运动的过程。
我们要通过imu的数据插值，来对位姿进行修正：
从x6y6z6到x9y9z9的过程就是通过我们更新过后的cosimu和sinimu的值做位姿修正。
但是这个修正非常粗糙，没有用到imu预积分的内容，在lio-sam中对imu的处理更加细致，未来我们也会注释lio-sam的代码。
最后一次转换x9y9z9到po，就是为下一帧的特征匹配做准备，我们知道，当前帧的最后一个点对应的imu数值，也就是最后一个imu数值，是下一帧的初始坐标系。
我们将当前点转到下一帧的初始坐标系上，再将下一帧的特征点转到初始坐标系上，就可以循环优化位姿了！！
！！注意转换之后，更新了kd树！！这同样是为特征匹配优化位姿做准备！！
if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);}最后向建图的程序发布数据
if (frameCount >= skipFrameNum + 1) {frameCount = 0;adjustOutlierCloud();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);}发布的数据有离群点，角特征点和面特征点。
skipFrameNum初值为1。
所以frameCount == 2时才发布，也就是两帧一发布，建图的频率比雷达里程计慢一半。
！！终于把featureAssociation注释完啦！！
下一节进入mapOptmization！！
