[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

前言

最近看回来之前发的博客，发现了埋了很多坑，其中也有很多错误的地方，所以现在重新写一遍，一个个坑补回来。

一、LiDAR里程计原理

简述：目前用得比较多的还是LOAM中提出的方法，以一个粗糙度$c$值来区分边缘和平面，然后边缘点、平面点分别与边缘线、平面匹配，从而得到一个两帧之间的LiDAR位姿。

简单地说，就是提取锐利边缘特征点和平面特征点，并分别将特征点与边缘线段和平面曲面片进行匹配。

参考文献：Zhang J , Singh S . Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2):401-416.

1、坐标系定义

• 激光雷达坐标系$L$是3D坐标系，其原点位于激光雷达的几何中心。 $x$轴指向左侧，$y$轴指向上方，$z$轴指向前方。
• 世界坐标系$W$是在初始位置与$L$一致的3D坐标系。

2、特征点提取

LOAM论文中选择在锐利边缘和平面/曲面的特征点。设$i$为$P_k$中的一个点，$i\in P_k$，设$S$为LiDAR在同一扫描中返回的$i$的连续点集。定义一个“粗糙度”$c$来评估局部曲面的平滑度，用于边缘特征点、平面特征点和普通点的分类。
（这个粗糙度可以粗暴的理解为点$i$到邻点的距离之和）
$$c=\frac{1}{\left|\mathbf{S}\right|\cdot \Vert {\mathbf{X}}_{\left(k,i\right)}^L\Vert }\Vert \sum _{j\in \mathbf{S},j\ne i}({\mathbf{X}}_{(k,i)}^{\mathbf{L}}-{\mathbf{X}}_{(k,j)}^{\mathbf{L}})\Vert$$

• 扫描中的点根据$c$值进行排序，然后用最大$c$值（即边缘点）和最小$c$值（即平面点）选择特征点。
• 一次扫描分为四个均等的区域。每个区域中最多可提供2个边缘点和4个平面点。

两点约束：

• 不能位于与激光束大致平行的曲面片上
• 不能位于遮挡区域的边界上
  
  因为这些点通常被认为是不可靠的。

3、特征点匹配

设$t_k$为扫描$k$的开始时间，在开始时，$P_k$是一个空集，在扫描过程中随着接收到更多点而增加。在每次扫描结束时，在扫描过程中感知到的点云$P_k$重新投影到时间戳$t_{k+1}$，如上图所示。然后将重新投影的点云表示为${\overline{P}}_k$。在下一个扫描$k+1$期间，${\overline{P}}_k$与新接收的点云$P_{k+1}$一起估计激光雷达的运动。
假设${\overline{P}}_k$和$P_{k+1}$现在都可用，并开始寻找两个激光雷达点云之间的对应关系。从$P_{k+1}$中找到边缘特征点和平面特征点，设$E_{k+1}$和$H_{k+1}$分别为边缘特征点和平面特征点的集合。我们将从${\overline{P}}_k$中找边缘线作为$E_{k+1}$中的边缘特征点对应，以及从${\overline{P}}_k$中找平面块作为$H_{k+1}$中的平面特征点对应。
在扫描$k+1$开始时，$P_{k+1}$是一个空集，在扫描过程中随着接收到更多点而增加。激光雷达里程计递归地估计扫描过程中的6自由度运动，并随着$P_{k+1}$的增加逐渐包含更多的点。在每次迭代中，使用当前估计的变换，将$E_{k+1}$和$H_{k+1}$重新投影到扫描的开始。将重新投影的点集设为$E_{k+1}$和$H_{k+1}$。对于$E_{k+1}$和$H_{k+1}$中的每个点，我们将在${\overline{P}}_k$中找到最近的相邻点。这里，${\overline{P}}_k$存储在3D KDtree中，用于快速索引。

边缘特征点与边缘线对应
上图（a）表示寻找边缘线作为边缘特征点的对应关系的过程。设$i$为$E_{k+1}，i\in E_{k+1}$中的点。边缘线由两点表示。设$j$为$i$在${\overline{P}}_k$中的最近邻，$j\in {\overline{P}}_k$，设$l$为$i$在连续两次扫描中$i$的最近邻。$(j，l)$形成了$i$的对应关系。然后，为了验证$j$和$l$都是边缘点，我们根据$c$值检查局部表面的光滑度。在这里，我们特别要求$j$和$l$来自不同的扫描，因为单个扫描不能包含来自同一边缘线的多个特征点。边缘线在扫描平面上只有一个例外。但是，如果是这样，则边缘线将退化并在扫描平面上显示为直线，并且边缘线的特征点不应首先提取。
平面特征点与平面曲面块对应
上图（b）表示寻找平面曲面块作为平面特征点的对应关系的过程。设$i$为$H_{k+1}$中的点，$i\in H_{k+1}$。平面斑块由三个点表示。与上一部分相似，我们在${\overline{P}}_k$找到$i$的最近邻，表示为$j$。然后，我们找到另外两个点$l$和$m$，它们是$i$的最近邻，一个点与$j$在同一扫描中，另一个在两次连续扫描中直到$j$的扫描中。这保证了这三个点是非共线的。为了验证$j$，$l$和$m$均为平面点，我们基于$c$值再次检查局部表面的光滑度。

3.1、计算对应特征的距离

利用找到的特征点的对应关系，现在我们导出表达式以计算从特征点到其对应关系的距离。下面将通过最小化特征点的总距离来估计激光雷达运动。
边缘特征点距离计算：
对于点$i\in E_{k+1}$，如果$(j，l)$是相应的边缘线$j，l\in {\overline{P}}_k$，则点到线的距离可以计算为

其中，$X_{(k+1,i)}^{\mathbf{L}}$，${\overline{X}}_{(k,j)}^{\mathbf{L}}$和${\overline{X}}_{(k,l)}^{\mathbf{L}}$分别是$L$中点$i$，$j$和$l$的坐标。
直观理解：公式的分子是两个向量的叉乘，而叉乘后求模就变成了求两个向量构成的三角形的面积。公式的分母是向量的模相当于三角形底边的长。三角形面积除以一条边就可以求出该边到对应顶点的距离，也就是边角点到边角线的距离。直观上的理解如下图所示：

平面特征点距离计算:
对于点$i\in H_{k+1}$，如果$(j，l，m)$是对应的平面曲面块$j，l，m\in {\overline{P}}_k$，则点到平面的距离为

其中，${\overline{X}}_{(k,m)}^{\mathbf{L}}$是$L$中点$m$的坐标。
直观理解：公式的分子包括两部分，上边是获得一个向量，下边也是获得一个向量，但两个向量叉乘再取模就表示的求下边得到三角形面积，上面表示立方体的高，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面积，得到的高就是平面点到平面的距离。直观上的理解如下图所示:

4 、用非线性优化方法进行运动估计

回想一下，上式计算$E_{k+1}$和$H_{k+1}$中的点之间的距离及其对应关系。结合边缘特征点距离计算式和上边的式子，我们可以得出$E_{k+1}$中的边缘点与相应边缘线之间的几何关系，

类似地，结合平面特征点距离计算式和上边的式子，我们可以在$H_{k+1}$中的一个平面特征点和相应的平面块之间建立另一个几何关系，

最后，利用非线性优化的方法求解激光雷达的运动。对于$E_{k+1}$和$H_{k+1}$中的每个特征点$f_E$与$f_H$相加，我们得到一个非线性函数。

其中$f$的每一行对应一个特征点，$d$包含相应的距离。我们计算相对于$T_{k+1}^{\mathbf{L}}$的$f$的雅可比矩阵，记为$J$，其中$J=\partial f/\partial T_{k+1}^{\mathbf{L}}$。然后将$d$最小化为零，通过使用高斯-牛顿法（GN法）$$J^{T}JX=-J^{T}f$$迭代来求解上式。这里涉及到一个帧间运动的雅克比矩阵$J$，下面我们来推导一下。

4.1、帧间运动的雅克比J的推导

对于一帧点云数据，有第一个点$p_s$和最后一个点$p_e$,以及任意时刻的点$p_t$。
首先，将任意时刻$t$的点重新投影到同一时刻$t_s$得到点$p_{st}$,则得到以下式子
$$p_t=R_{s:t}{p}_{st}+t_{s:t}$$$$p_{st}=R_{s:t}^{-1}(p_t-t_{s:t})$$
根据上边的坐标系定义，使用左乘旋转矩阵，旋转坐标轴顺序Z-X-Y得到旋转矩阵$R_{ZXY}$
$$R_{s:t}=R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$$$=\left[\begin{array}{ccc}cosry& 0& sinry\\ 0& 1& 0\\ -sinry& 0& cosry\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cosrx& -sinrx\\ 0& sinrx& cosrx\end{array}\right]\left[\begin{array}{ccc}cosrz& -sinrz& 0\\ sinrz& cosrz& 0\\ 0& 0& 1\end{array}\right]$$$$=\left[\begin{array}{ccc}cry\ast crz+sry\ast srx\ast srz& crz\ast sry\ast srx-cry\ast srz& crx\ast sry\\ crx\ast srz& crx\ast crz& -srx\\ cry\ast srx\ast srz-crz\ast sry& cry\ast crz\ast srx+sry\ast srz& cry\ast crx\end{array}\right]$$
下式用$c$代表$cos$函数，$s$代表$sin$函数：
$$R_{s:t}^{-1}=\left[\begin{array}{ccc}cry\ast crz-srx\ast sry\ast srz& cry\ast srz+srx\ast sry\ast crz& -sry\ast crx\\ -crx\ast srz& crx\ast crz& srx\\ sry\ast crz+srx\ast cry\ast srz& sry\ast srz-srx\ast cry\ast crz& cry\ast crx\end{array}\right]$$
对于$R_{s:t}^{-1}$有$R_{s:t}^{-1}=R_{ZXY}^{-1}=R_{YXZ}^T$
雅克比计算：
$$f(p_{st})=f(R_{s:t}^{-1}(p_t-t_{s:t}))=f(R_{s:t}^{-1},t_{s:t})=d$$
定义：$R_{s:t}^{-1}$对应的旋转角分别为$rx,ry,rz$即$$R_{s:t}^{-1}=R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$$t_{s:t}$分量分别为$t_x,t_y,t_z$即$$t_{s:t}=[t_x,t_y,t_z{]}^T$$
进一步得到：$$\frac{\partial f}{\partial T_{k+1}^L}=\frac{\partial f}{\partial (rx,ry,rz,tx,ty,tz)}=[\frac{\partial f}{\partial rx},\frac{\partial f}{\partial ry},\frac{\partial f}{\partial rz},\frac{\partial f}{\partial tx},\frac{\partial f}{\partial ty},\frac{\partial f}{\partial tz}]$$

式1$$\frac{\partial f}{\partial rx}=\frac{\partial f}{\partial p_{st}}\frac{\partial p_{st}}{\partial rx}$$$$=[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}（p_t-t_{s:t}）$$$$=[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\left[\begin{array}{c}px\\ py\\ pz\end{array}\right]-[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\left[\begin{array}{c}tx\\ ty\\ tz\end{array}\right]$$

式2$$\frac{\partial f}{\partial tx}=\frac{\partial f}{\partial p_{st}}\frac{\partial p_{st}}{\partial tx}$$$$=[la,lb,lc]R_{s:t}^{-1}\left[\begin{array}{c}-1\\ 0\\ 0\end{array}\right]$$
其中$\frac{\partial f}{\partial p_{st}}$为梯度方向，对应直线的垂线方向和平面法向量；$p_t$为当前点。
至此，与代码里的arx、ary、arz、atx、aty、atz计算一致。

附录：
旋转矩阵偏导$\frac{\partial R_{s:t}^{-1}}{\partial rx}$计算：
$$\frac{\partial R_{s:t}^{-1}}{\partial rx}=\frac{\partial R_{zxy}}{\partial rx}$$$$=\frac{\partial \left[\begin{array}{ccc}cry\ast crz-srx\ast sry\ast srz& cry\ast srz+srx\ast sry\ast crz& -sry\ast crx\\ -crx\ast srz& crx\ast crz& srx\\ sry\ast crz+srx\ast cry\ast srz& sry\ast srz-srx\ast cry\ast crz& cry\ast crx\end{array}\right]}{\partial rx}$$$$=\left[\begin{array}{ccc}-crx\ast sry\ast srz& crx\ast sry\ast crz& srx\ast sry\\ srx\ast srz& -srx\ast crz& crx\\ crx\ast cry\ast srz& -crx\ast cry\ast crz& -cry\ast srx\end{array}\right]$$
旋转矩阵偏导$\frac{\partial R_{s:t}^{-1}}{\partial ry}$计算：
$$\frac{\partial R_{s:t}^{-1}}{\partial ry}=\frac{\partial R_{zxy}}{\partial ry}$$$$=\frac{\partial \left[\begin{array}{ccc}cry\ast crz-srx\ast sry\ast srz& cry\ast srz+srx\ast sry\ast crz& -sry\ast crx\\ -crx\ast srz& crx\ast crz& srx\\ sry\ast crz+srx\ast cry\ast srz& sry\ast srz-srx\ast cry\ast crz& cry\ast crx\end{array}\right]}{\partial ry}$$$$=\left[\begin{array}{ccc}-sry\ast crz-srx\ast cry\ast srz& -sry\ast srz+srx\ast cry\ast crz& -cry\ast crx\\ 0& 0& 0\\ cry\ast crz-srx\ast sry\ast srz& sry\ast srz+srx\ast sry\ast crz& -sry\ast crx\end{array}\right]$$
旋转矩阵偏导$\frac{\partial R_{s:t}^{-1}}{\partial rz}$计算：
$$\frac{\partial R_{s:t}^{-1}}{\partial ry}=\frac{\partial R_{zxy}}{\partial rz}$$$$=\frac{\partial \left[\begin{array}{ccc}cry\ast crz-srx\ast sry\ast srz& cry\ast srz+srx\ast sry\ast crz& -sry\ast crx\\ -crx\ast srz& crx\ast crz& srx\\ sry\ast crz+srx\ast cry\ast srz& sry\ast srz-srx\ast cry\ast crz& cry\ast crx\end{array}\right]}{\partial rz}$$$$=\left[\begin{array}{ccc}-cry\ast srz-srx\ast sry\ast crz& cry\ast crz-srx\ast sry\ast srz& 0\\ -crx\ast crz& -crx\ast srz& 0\\ -sry\ast srz-srx\ast cry\ast crz& sry\ast crz+srx\ast cry\ast srz& 0\end{array}\right]$$

二、LOAM的LiDAR里程计代码分析

主要是两部分：运动补偿和帧间配准。

• 利用相邻两帧的点云数据进行配准，即完成$t$时刻和$t+1$时刻点云数据的关联，并估计LiDAR的相对运动关系。
• 配准工作需要基于边缘点特征、平面特征点云进行：利用scanRegistration分别获得$t$时刻和$t+1$时刻点云中的特征点，然后建立这两部分点云的一一对应关系。

一些细节：

• 每帧激光都会参与（所以帧率同VLP16的扫描帧率，10hz）
• 通过对每一帧激光的配准,可以得到一个精度较差的odom；
• 帧间匹配的初始pose可以由IMU得到
• 若没有IMU的时候由匀速运动模型得到

1、ROS订阅和发布

先来看看main函数里怎么写的。

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserOdometry");
  ros::NodeHandle nh;

  ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_sharp", 2, laserCloudSharpHandler);

  ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                             ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);

  ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                      ("/laser_cloud_flat", 2, laserCloudFlatHandler);

  ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                          ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);

  ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> 
                                         ("/velodyne_cloud_2", 2, laserCloudFullResHandler);

  ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> 
                                ("/imu_trans", 5, imuTransHandler);

  ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_corner_last", 2);

  ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_surf_last", 2);

  ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> 
                                        ("/velodyne_cloud_3", 2);

  ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
  nav_msgs::Odometry laserOdometry;
  laserOdometry.header.frame_id = "/camera_init";
  laserOdometry.child_frame_id = "/laser_odom";

  tf::TransformBroadcaster tfBroadcaster;
  tf::StampedTransform laserOdometryTrans;
  laserOdometryTrans.frame_id_ = "/camera_init";
  laserOdometryTrans.child_frame_id_ = "/laser_odom";

  std::vector<int> pointSearchInd;//搜索到的点序
  std::vector<float> pointSearchSqDis;//搜索到的点平方距离

  PointType pointOri, pointSel/*选中的特征点*/, tripod1, tripod2, tripod3/*特征点的对应点*/, pointProj/*unused*/, coeff;

  //退化标志
  bool isDegenerate = false;
  //P矩阵，预测矩阵
  cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

  int frameCount = skipFrameNum;
  ros::Rate rate(100);
  bool status = ros::ok();
  while (status) 
  {
    ros::spinOnce();

    if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 
        newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
        fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {  //同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
      newCornerPointsSharp = false;
      newCornerPointsLessSharp = false;
      newSurfPointsFlat = false;
      newSurfPointsLessFlat = false;
      newLaserCloudFullRes = false;
      newImuTrans = false;
      /***** 一、初始化 *****/
      /***** 二、点云处理 *****/
      /***** 三、坐标转换 *****/
    }
  }

订阅器 ：订阅scanRegistration节点发布的消息并从ROS::Msg类型转换成程序可以处理的pcl点云类型；

• subCornerPointsSharp 订阅 /laser_cloud_sharp 调用laserCloudSharpHandler
• subCornerPointsLessSharp 订阅 /laser_cloud_less_sharp 调用laserCloudLessSharpHandler
• subSurfPointsFlat 订阅 pubSurfPointFlat 调用 laserCloudFlatHandler
• subSurfPointLessFlat 订阅 pubSurfPointLessFlat 调用 laserCloudLessFlatHandler
• subLaserCloudFullRes 订阅 /velodyne_cloud_2 调用laserCloudFullResHandler 处理全部点云数据
• subImuTrans 订阅 /imu_trans 调用imuTransHandler 处理IMU数据

发布器 ：其中/laser_odom_to_init消息的发布频率高，其余三个消息每接收到三次scanRegistration节点的消息才发布一次。

• pubLaserCloudCornerLast 发布 /laser_cloud_corner_last
• pubLaserCloudSurfLast 发布 /laser_cloud_surf_last
• pubLaserCloudFullRes 发布 /velodyne_cloud_3
• pubLaserOdometry 发布 /laser_odom_to_init

总体计算过程分为三步：初始化、点云处理、坐标转换。下面一步一步来

2、初始化

       /********* Initialization ********/
      //将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理
      if (!systemInited) 
      {
        //将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用
        pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
        cornerPointsLessSharp = laserCloudCornerLast;
        laserCloudCornerLast = laserCloudTemp;

        //将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用
        laserCloudTemp = surfPointsLessFlat;
        surfPointsLessFlat = laserCloudSurfLast;
        laserCloudSurfLast = laserCloudTemp;

        //使用上一帧的特征点构建kd-tree
        kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合
        kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合

        //将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMapping
        sensor_msgs::PointCloud2 laserCloudCornerLast2;
        pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
        laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudCornerLast2.header.frame_id = "/camera";
        pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

        sensor_msgs::PointCloud2 laserCloudSurfLast2;
        pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
        laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudSurfLast2.header.frame_id = "/camera";
        pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

        //记住原点的翻滚角和俯仰角
        transformSum[0] += imuPitchStart;
        transformSum[2] += imuRollStart;

        systemInited = true;
        continue;
      }

      //T平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）
      transform[3] -= imuVeloFromStartX * scanPeriod;
      transform[4] -= imuVeloFromStartY * scanPeriod;
      transform[5] -= imuVeloFromStartZ * scanPeriod;
      /**********************************************/

主要是等待下一时刻的点云再做处理。

3、点云处理——点云配准与运动估计

这部分是整个laserOdometry节点的重中之重。假设你现在已经得到了两坨点云，对他们进行处理之前你首先得保证这些特征点足够多，否则你带了两坨没有任何特征的点，那可就真的爱莫能助了，用程序表达就是设定一个阈值进行判断。

在点云足够多的条件下，终于要开始正式工作了。这里我们设定整个L-M运动估计的迭代次数为25次，以保证运算效率。迭代部分又可分为：对特征边/面上的点进行处理，构建Jaccobian矩阵，L-M运动估计求解。

      if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) 
      {
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
        int cornerPointsSharpNum = cornerPointsSharp->points.size();
        int surfPointsFlatNum = surfPointsFlat->points.size();
        
        //Levenberg-Marquardt算法(L-M method)，非线性最小二乘算法，最优化算法的一种
        //最多迭代25次
        for (int iterCount = 0; iterCount < 25; iterCount++) 
        {
          laserCloudOri->clear();
          coeffSel->clear();
        /***** 1. 边缘特征上的点配准并构建Jaccobian *****/
        
        /***** 2. 平面特征上的点配准并构建Jaccobian  *****/

        /***** 3. L-M运动估计求解 *****/
        }
      }

代码中是使用Gauss-Newton优化（相比传统的GN法，代码中增加了一个阻尼因子，代码中的s），这里关键在于如何把点云配准和运动估计的问题转换为优化求解的问题。
主要思路：

1. 构建约束方程
2. 约束方程求偏导构建Jaccobian矩阵
3. 求解

下面再一步一步来看：关于构建约束方程的问题就是这节标题中提到的点云配准的问题，其基本思想就是从上一帧点云中找到一些边/面特征点，在当前帧点云中同样找这么一些点，建立他们之间的约束关系。

3.1、边缘特征上的点配准

          /* 边缘特征匹配 */
         //处理当前点云中的曲率最大的特征点,从上个点云中曲率比较大的特征点中找两个最近距离点，一个点使用kd-tree查找，另一个根据找到的点在其相邻线找另外一个最近距离的点
         for (int i = 0; i < cornerPointsSharpNum; i++)
         {
           TransformToStart(&cornerPointsSharp->points[i], &pointSel);

           //每迭代五次，重新查找最近点
           if (iterCount % 5 == 0)
           {
             std::vector<int> indices;
             pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
             //kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波
             kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
             int closestPointInd = -1, minPointInd2 = -1;

             //寻找相邻线距离目标点距离最小的点
             //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
             if (pointSearchSqDis[0] < 25)
             {//找到的最近点距离的确很近的话
               closestPointInd = pointSearchInd[0];
               //提取最近点线号
               int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

               float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值
               //寻找距离目标点最近距离的平方和最小的点
               for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
               {//向scanID增大的方向查找
                 if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5)
                 {//非相邻线
                   break;
                 }

                 pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                              (laserCloudCornerLast->points[j].x - pointSel.x) + 
                              (laserCloudCornerLast->points[j].y - pointSel.y) * 
                              (laserCloudCornerLast->points[j].y - pointSel.y) + 
                              (laserCloudCornerLast->points[j].z - pointSel.z) * 
                              (laserCloudCornerLast->points[j].z - pointSel.z);

                 if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan)
                 {//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
                   if (pointSqDis < minPointSqDis2)
                   {//距离更近，要小于初始值5米
                       //更新最小距离与点序
                     minPointSqDis2 = pointSqDis;
                     minPointInd2 = j;
                   }
                 }
               }

               //同理
               for (int j = closestPointInd - 1; j >= 0; j--) {//向scanID减小的方向查找
                 if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5)
                 {
                   break;
                 }

                 pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                              (laserCloudCornerLast->points[j].x - pointSel.x) + 
                              (laserCloudCornerLast->points[j].y - pointSel.y) * 
                              (laserCloudCornerLast->points[j].y - pointSel.y) + 
                              (laserCloudCornerLast->points[j].z - pointSel.z) * 
                              (laserCloudCornerLast->points[j].z - pointSel.z);

                 if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan)
                 {
                   if (pointSqDis < minPointSqDis2)
                   {
                     minPointSqDis2 = pointSqDis;
                     minPointInd2 = j;
                   }
                 }
               }
             }

             //记住组成线的点序
             pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距离点，-1表示未找到满足的点
             pointSearchCornerInd2[i] = minPointInd2;//另一个最近的，-1表示未找到满足的点
           }

           if (pointSearchCornerInd2[i] >= 0)
           {//大于等于0，不等于-1，说明两个点都找到了
             tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
             tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

             //选择的特征点记为O，kd-tree最近距离点记为A，另一个最近距离点记为B
             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;

             //向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）
             //向量OA OB的向量积(即叉乘)为：
             //|  i      j      k  |
             //|x0-x1  y0-y1  z0-z1|
             //|x0-x2  y0-y2  z0-z2|
             //模为：
             float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                        * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                        + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                        * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                        + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                        * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

             //两个最近距离点之间的距离，即向量AB的模
             float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

             //点到线的距离，d = |向量OA 叉乘 向量OB|/|AB|
             float ld2 = a012 / l12;


             //AB方向的单位向量与OAB平面的单位法向量的向量积在各轴上的分量（d的方向）
             //x轴分量i
             float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                      + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

             //y轴分量j
             float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                      - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

             //z轴分量k
             float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                      + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

             //unused
             pointProj = pointSel;
             pointProj.x -= la * ld2;
             pointProj.y -= lb * ld2;
             pointProj.z -= lc * ld2;

             //权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1
             float s = 1;
             if (iterCount >= 5)
             {//5次迭代之后开始增加权重因素 fabs返回 x 的绝对值
               s = 1 - 1.8 * fabs(ld2);
             }

             //考虑权重
             coeff.x = s * la;
             coeff.y = s * lb;
             coeff.z = s * lc;
             coeff.intensity = s * ld2;

             if (s > 0.1 && ld2 != 0)
             {//只保留权重大的，也即距离比较小的点，同时也舍弃距离为零的
               laserCloudOri->push_back(cornerPointsSharp->points[i]);
               coeffSel->push_back(coeff);
             }
           }
         }

3.2、平面特征上的点配准

          /* 平面特征匹配 */
          //对本次接收到的曲率最小的点,从上次接收到的点云曲率比较小的点中找三点组成平面;1、一个使用kd-tree查找;2、另外一个在同一线上查找满足要求的;3、第三个在不同线上查找满足要求的
          for (int i = 0; i < surfPointsFlatNum; i++)
          {
            TransformToStart(&surfPointsFlat->points[i], &pointSel);

            if (iterCount % 5 == 0)
            {
                //kd-tree最近点查找，在经过体素栅格滤波之后的平面点中查找，一般平面点太多，滤波后最近点查找数据量小
              kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
              int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
              if (pointSearchSqDis[0] < 25)
              {
                closestPointInd = pointSearchInd[0];
                int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);

                float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
                for (int j = closestPointInd + 1; j < surfPointsFlatNum; 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;
                     }
                  }
                }
                //同理
                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;
                    }
                  }
                }
              }

              pointSearchSurfInd1[i] = closestPointInd;//kd-tree最近距离点,-1表示未找到满足要求的点
              pointSearchSurfInd2[i] = minPointInd2;//同一线号上的距离最近的点，-1表示未找到满足要求的点
              pointSearchSurfInd3[i] = minPointInd3;//不同线号上的距离最近的点，-1表示未找到满足要求的点
            }

            if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0)
            {//找到了三个点
              tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//A点
              tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//B点
              tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//C点

              //向量AB = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
              //向量AC = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)

              //向量AB AC的向量积（即叉乘），得到的是法向量
              //x轴方向分向量i
              float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 
                       - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
              //y轴方向分向量j
              float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 
                       - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
              //z轴方向分向量k
              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 pb pc为法向量各方向上的单位向量
              pa /= ps;
              pb /= ps;
              pc /= ps;
              pd /= ps;

              //点到面的距离：向量OA与与法向量的点积除以法向量的模
              float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

              //unused
              pointProj = pointSel;
              pointProj.x -= pa * pd2;
              pointProj.y -= pb * pd2;
              pointProj.z -= pc * pd2;

              //同理计算权重
              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));
              }

              //考虑权重
              coeff.x = s * pa;
              coeff.y = s * pb;
              coeff.z = s * pc;
              coeff.intensity = s * pd2;

              if (s > 0.1 && pd2 != 0) {
                  //保存原始点与相应的系数
                laserCloudOri->push_back(surfPointsFlat->points[i]);
                coeffSel->push_back(coeff);
              }
            }
          }

3.3、构建Jaccobian并进行运动估计求解

根据上一部分推导的公式，我们有了这两坨点云的约束方程，构建Jaccobian矩阵就是直接对待估参数$X_{(k,i)}^{\mathbf{L}}$求偏导了。
再看对应程序中如何对应的:
$$arx=\frac{\partial f}{\partial rx}，ary=\frac{\partial f}{\partial ry}，arz=\frac{\partial f}{\partial rz}$$$$atx=\frac{\partial f}{\partial tx}，aty=\frac{\partial f}{\partial ty}，atz=\frac{\partial f}{\partial tz}$$
以$arx$为例：
$$arx=\frac{\partial f}{\partial rx}=[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\left[\begin{array}{c}px\\ py\\ pz\end{array}\right]-[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\left[\begin{array}{c}tx\\ ty\\ tz\end{array}\right]$$$=\left[\begin{array}{ccc}la& lb& lc\end{array}\right]\left[\begin{array}{ccc}-crx\ast sry\ast srz& crx\ast sry\ast crz& srx\ast sry\\ srx\ast srz& -srx\ast crz& crx\\ crx\ast cry\ast srz& -crx\ast cry\ast crz& -cry\ast srx\end{array}\right]\left[\begin{array}{c}px-tx\\ py-ty\\ pz-tz\end{array}\right]$
$=(-crx\ast sry\ast srz\ast px+crx\ast crz\ast sry\ast py+srx\ast sry\ast pz$
$+tx\ast crx\ast sry\ast srz-ty\ast crx\ast crz\ast sry-tz\ast srx\ast sry)\ast la$
$+(srx\ast srz\ast px-crz\ast srx\ast py+crx\ast pz$
$+ty\ast crz\ast srx-tz\ast crx-tx\ast srx\ast srz)\ast lb$
$+(crx\ast cry\ast srz\ast px-crx\ast cry\ast crz\ast py-cry\ast srx\ast pz$
$+tz\ast cry\ast srx+ty\ast crx\ast cry\ast crz-tx\ast crx\ast cry\ast srz)\ast lc$

          int pointSelNum = laserCloudOri->points.size();
          //满足要求的特征点至少10个，特征匹配数量太少弃用此帧数据
          if (pointSelNum < 10)
          {
            continue;
          }
          /* LM优化过程 QR分解求解T向量*/
          cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));
          cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));
          cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
          cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
          cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
          cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

          //计算matA,matB矩阵
          for (int i = 0; i < pointSelNum; i++)
          {
            pointOri = laserCloudOri->points[i];
            coeff = coeffSel->points[i];

            float s = 1;

            float srx = sin(s * transform[0]);
            float crx = cos(s * transform[0]);
            float sry = sin(s * transform[1]);
            float cry = cos(s * transform[1]);
            float srz = sin(s * transform[2]);
            float crz = cos(s * transform[2]);
            float tx = s * transform[3];
            float ty = s * transform[4];
            float tz = s * transform[5];

            float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z
                      + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
                      + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
                      + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
                      + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
                      + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;

            float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x
                      + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z
                      + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx)
                      + s*tz*crx*cry) * coeff.x
                      + ((s*cry*crz - s*srx*sry*srz)*pointOri.x
                      + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
                      + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry)
                      - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;

            float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
                      + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
                      + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
                      + s*ty*crx*srz + s*tx*crx*crz) * coeff.y
                      + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
                      + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;

            float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y
                      - s*(crz*sry + cry*srx*srz) * coeff.z;

            float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y
                      - s*(sry*srz - cry*crz*srx) * coeff.z;

            float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;

            float d2 = coeff.intensity;

            matA.at<float>(i, 0) = arx;
            matA.at<float>(i, 1) = ary;
            matA.at<float>(i, 2) = arz;
            matA.at<float>(i, 3) = atx;
            matA.at<float>(i, 4) = aty;
            matA.at<float>(i, 5) = atz;
            matB.at<float>(i, 0) = -0.05 * d2;
          }
          cv::transpose(matA, matAt);
          matAtA = matAt * matA;
          matAtB = matAt * matB;
          //求解matAtA * matX = matAtB
          cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

          if (iterCount == 0)
          {
            //特征值1*6矩阵 Mat(int rows, int cols, int type, const Scalar& s)
            cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
            //特征向量6*6矩阵
            cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

            //求解特征值/特征向量
            cv::eigen(matAtA, matE, matV);
            matV.copyTo(matV2);

            isDegenerate = false;
            //特征值取值门槛
            float eignThre[6] = {10, 10, 10, 10, 10, 10};
            for (int i = 5; i >= 0; i--)
            {//从小到大查找
              if (matE.at<float>(0, i) < eignThre[i])
              {//特征值太小，则认为处在兼并环境中，发生了退化
                for (int j = 0; j < 6; j++) {//对应的特征向量置为0
                  matV2.at<float>(i, j) = 0;
                }
                isDegenerate = true;
              }
              else
              {
                break;
              }
            }

            //计算P矩阵
            matP = matV.inv() * matV2;
          }

          if (isDegenerate)
          {//如果发生退化，只使用预测矩阵P计算
            cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
            matX.copyTo(matX2);
            matX = matP * matX2;
          }

          //累加每次迭代的旋转平移量
          transform[0] += matX.at<float>(0, 0);
          transform[1] += matX.at<float>(1, 0);
          transform[2] += matX.at<float>(2, 0);
          transform[3] += matX.at<float>(3, 0);
          transform[4] += matX.at<float>(4, 0);
          transform[5] += matX.at<float>(5, 0);

          for(int i=0; i<6; i++)
          {
            if(isnan(transform[i]))//判断是否非数字
              transform[i]=0;
          }
          //计算旋转平移量，如果很小就停止迭代
          float deltaR = sqrt(
                              pow(rad2deg(matX.at<float>(0, 0)), 2) +
                              pow(rad2deg(matX.at<float>(1, 0)), 2) +
                              pow(rad2deg(matX.at<float>(2, 0)), 2));
          float deltaT = sqrt(
                              pow(matX.at<float>(3, 0) * 100, 2) +
                              pow(matX.at<float>(4, 0) * 100, 2) +
                              pow(matX.at<float>(5, 0) * 100, 2));

          if (deltaR < 0.1 && deltaT < 0.1)
          {//迭代终止条件跳出循环
            break;
          }

4、坐标转换

计算出了两坨点云间的相对运动，但他们是在这两帧点云的局部坐标系下的，我们需要把它转换到世界坐标系下，因此需要进行转换。
这部分内容较为简单，直接上代码了：

      float rx, ry, rz, tx, ty, tz;
      //求相对于原点的旋转量,垂直方向上1.05倍修正?
      AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                         -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);

      float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 
               - sin(rz) * (transform[4] - imuShiftFromStartY);
      float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 
               + cos(rz) * (transform[4] - imuShiftFromStartY);
      float z1 = transform[5] * 1.05 - 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);

      //根据IMU修正旋转量
      PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                        imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

      //得到世界坐标系下的转移矩阵
      transformSum[0] = rx;
      transformSum[1] = ry;
      transformSum[2] = rz;
      transformSum[3] = tx;
      transformSum[4] = ty;
      transformSum[5] = tz;

      //欧拉角转换成四元数
      geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);

      //publish四元数和平移量
      laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
      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 = tx;
      laserOdometry.pose.pose.position.y = ty;
      laserOdometry.pose.pose.position.z = tz;
      pubLaserOdometry.publish(laserOdometry);

至此我们接完成了整个laserOdometry的计算。