LOAM源代码中坐标变换部分的详细讲解

作者：K.Fire

写在前面

本系列文章将对LOAM源代码进行讲解，在讲解过程中，涉及到论文中提到的部分，会结合论文以及我自己的理解进行解读，尤其是对于其中坐标变换的部分，将会进行详细的讲解。

本来是懒得写的，一个是怕自己以后忘了，另外是我在学习过程中，其实没有感觉哪一个博主能讲解的通篇都能让我很明白，特别是坐标变换部分的代码，所以想着自己学完之后，按照自己的理解，也写一个LOAM解读，希望能对后续学习LOAM的同学们有所帮助。

整体框架

LOAM多牛逼就不用多说了，直接开始

先贴一下我详细注释的LOAM代码，在这个版本的代码上加入了我自己的理解。

我觉得最重要也是最恶心的一部分是其中的坐标变换，在代码里面真的看着头大，所以先明确一下坐标系(都是右手坐标系)：

IMU(IMU坐标系imu)：x轴向前，y轴向左，z轴向上

LIDAR(激光雷达坐标系l)：x轴向前，y轴向左，z轴向上

CAMERA(相机坐标系，也可以理解为里程计坐标系c)：z轴向前，x轴向左，y轴向上

WORLD(世界坐标系w，也叫全局坐标系，与里程计第一帧init重合)：z轴向前，x轴向左，y轴向上

MAP(地图坐标系map，一定程度上可以理解为里程计第一帧init)：z轴向前，x轴向左，y轴向上

坐标变换约定：为了清晰，变换矩阵的形式与《SLAM十四讲中一样》，即：表示B坐标系相对于A坐标系的变换，B中一个向量通过可以变换到A中的向量。首先对照ros的节点图和论文中提到的算法框架来看一下：

可以看到节点图和论文中的框架是一一对应的，这几个模块的功能如下：

scanRegistration：对原始点云进行预处理，计算曲率，提取特征点

laserOdometry：对当前sweep与上一次sweep进行特征匹配，计算一个快速(10Hz)但粗略的位姿估计

laserMapping：对当前sweep与一个局部子图进行特征匹配，计算一个慢速(1Hz)比较精确的位姿估计

transformMaintenance：对两个模块计算出的位姿进行融合，得到最终的精确地位姿估计

本文介绍laserOdometry模块，它相当于SLAM的前端，它其实是一个scan-to-scan的过程，可以得到高频率但精度略低的位姿变换，它的具体功能如下：

接收特征点话题、全部点云话题、IMU话题，并保存到对应的变量中

将当前sweep与上一次sweep进行特征匹配，得到edge point匹配对应的直线以及planar point匹配对应的平面

计算雅可比矩阵，使用高斯牛顿法(论文中说的是LM法)进行优化，得到估计出的相邻两帧的位姿变换

累积位姿变换，并用IMU修正，得到当前帧到初始帧的累积位姿变换

发布话题并更新tf变换

一、main()函数以及回调函数

main()函数是很简单的，就是定义了一系列的发布者和订阅者，订阅了来自scanRegistration节点发布的话题；然后定义了一个tf发布器，发布当前帧(/laser_odom)到初始帧(/camera_init)的坐标变换；然后定义了一些列下面会用到的变量。

其中有6个订阅者，分别看一下它们的回调函数。

int main(int argc, char** argv) { ros::init(argc, argv, "laserOdometry"); ros::NodeHandle nh; ros::Subscriber subCornerPointsSharp = nh.subscribe
       
        ("/laser_cloud_sharp", 2, laserCloudSharpHandler); ros::Subscriber subCornerPointsLessSharp = nh.subscribe
        
         ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler); ros::Subscriber subSurfPointsFlat = nh.subscribe
         
          ("/laser_cloud_flat", 2, laserCloudFlatHandler); ros::Subscriber subSurfPointsLessFlat = nh.subscribe
          
           ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler); ros::Subscriber subLaserCloudFullRes = nh.subscribe
           
            ("/velodyne_cloud_2", 2, laserCloudFullResHandler); ros::Subscriber subImuTrans = nh.subscribe
            
             ("/imu_trans", 5, imuTransHandler); ros::Publisher pubLaserCloudCornerLast = nh.advertise
             
              ("/laser_cloud_corner_last", 2); ros::Publisher pubLaserCloudSurfLast = nh.advertise
              
               ("/laser_cloud_surf_last", 2); ros::Publisher pubLaserCloudFullRes = nh.advertise
               
                ("/velodyne_cloud_3", 2); ros::Publisher pubLaserOdometry = nh.advertise
                
                 ("/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
                 
                  pointSearchInd;//搜索到的点序 std::vector
                  
                   pointSearchSqDis;//搜索到的点平方距离 PointType pointOri, pointSel; /*选中的特征点*/ PointType tripod1, tripod2, tripod3; /*特征点的对应点*/ PointType pointProj; /*unused*/ PointType coeff; /*直线或平面的系数*/ //退化标志 bool isDegenerate = false; //P矩阵，预测矩阵，用来处理退化情况 cv::Mat matP(6, 6, CV_32F, cv::all(0)); int frameCount = skipFrameNum; // skipFrameNum = 1 ros::Rate rate(100); bool status = ros::ok();
                  
                 
                
               
              
             
            
           
          
         
        
       

接收特征点的回调函数

下面这五个回调函数的作用和代码结构都类似，都是接收scanRegistration发布的话题，分别接收了edge point、less edge point、planar point、less planar point、full cloud point(按scanID排列的全部点云)。

对于接收到点云之后都是如下操作：

记录时间戳

保存到相应变量中

滤除无效点

将接收标志设置为true

void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2) { timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec(); cornerPointsSharp->clear(); pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp); std::vector
       
        indices; pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices); newCornerPointsSharp = true; } void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2) { timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec(); cornerPointsLessSharp->clear(); pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp); std::vector
        
         indices; pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices); newCornerPointsLessSharp = true; } void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2) { timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec(); surfPointsFlat->clear(); pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat); std::vector
         
          indices; pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices); newSurfPointsFlat = true; } void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2) { timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec(); surfPointsLessFlat->clear(); pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat); std::vector
          
           indices; pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices); newSurfPointsLessFlat = true; } //接收全部点 void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) { timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); laserCloudFullRes->clear(); pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); std::vector
           
            indices; pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices); newLaserCloudFullRes = true; }
           
          
         
        
       

接收/imu_trans消息这个回调函数主要是接受了scanRegistration中发布的自定义imu话题，包括当前sweep点云数据的IMU起始角、终止角、由于加减速产生的位移和速度畸变，保存到相应变量中。

//接收imu消息 void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) { timeImuTrans = imuTrans2->header.stamp.toSec(); imuTrans->clear(); pcl::fromROSMsg(*imuTrans2, *imuTrans); //根据发来的消息提取imu信息 imuPitchStart = imuTrans->points[0].x; imuYawStart = imuTrans->points[0].y; imuRollStart = imuTrans->points[0].z; imuPitchLast = imuTrans->points[1].x; imuYawLast = imuTrans->points[1].y; imuRollLast = imuTrans->points[1].z; imuShiftFromStartX = imuTrans->points[2].x; imuShiftFromStartY = imuTrans->points[2].y; imuShiftFromStartZ = imuTrans->points[2].z; imuVeloFromStartX = imuTrans->points[3].x; imuVeloFromStartY = imuTrans->points[3].y; imuVeloFromStartZ = imuTrans->points[3].z; newImuTrans = true; }

二、特征匹配

2.1 初始化

接收到第一帧点云数据时，先进行一次初始化，因为第一帧点云没法匹配..

这里就是直接把这一帧点云发送给laserMapping节点。

//初始化：将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理 if (!systemInited) { //将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值到laserCloudCornerLast中下轮使用 pcl::PointCloud
       
        ::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; //将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值到laserCloudSurfLast中下轮使用 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; // IMU的y方向 transformSum[2] += imuRollStart; // IMU的x方向 systemInited = true; continue; }
       

2.2 TransformToStart()函数

在scanRegistration模块中，有一个TransformToStartIMU()函数，上篇文章提到它的作用是：没有对点云坐标系进行变换，第i个点云依然处在里程计坐标系下的curr时刻坐标系中，只是对点云的位置进行调整。在这里推荐工坊新课：（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

而这个函数呢，就是要对点云的坐标系进行变换，变换到里程计坐标系下的初始时刻start坐标系中，用于与上一帧的点云数据进行匹配。

在这个函数中，首先根据每个点强度值intensity，提取出scanRegistration中计算的插值系数，下面开始了！

首先要明确：transform中保存的变量是上一帧相对于当前帧的变换，也就是

而这里也体现了匀速运动假设，因为我们在这里使用transform变量时，还没有对其进行更新(迭代更新过程在特征匹配和非线性最小二乘优化中才进行)，所以这里使用的transform变量与上上帧相对于上一帧的变换相同，这也就是作者假设了每个扫描周期车辆都进行了完全相同的运动，用数学公式表示如下：

那么问题如下：

现在我们已知的量是：当前时刻坐标系下保持imustart角的的点云，上一帧相对于当前帧的变换，也就是，使用s进行插值后有：。

需要求解的是当前sweep初始时刻坐标系的点云。

推导过程：

根据坐标变换公式有：

而为YXZ变换顺序(解释：当前帧相对于上一帧的变换顺序为ZXY，呢么反过来，这里是上一帧相当于当前帧的变换，就变成了YXZ)，所以，代入得：

这就推出了和代码一致的变换顺序：先减去，然后绕z周旋转-rz，绕x轴旋转-rx，绕y轴旋转-ry。

//将该次sweep中每个点通过插值，变换到初始时刻start void TransformToStart(PointType const * const pi, PointType * const po) { //插值系数计算，云中每个点的相对时间/点云周期10 float s = 10 * (pi->intensity - int(pi->intensity)); //线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数 //这里的transform数值上和上次一样，这里体现了匀速运动模型，就是每次时间间隔下，相对于上一次sweep的变换都是一样的 //而在使用该函数之前进行了一次操作：transform[3] -= imuVeloFromStartX * scanPeriod; //这个操作是在匀速模型的基础上，去除了由于加减速造成的畸变 // R_curr_start = R_YXZ float rx = s * transform[0]; float ry = s * transform[1]; float rz = s * transform[2]; float tx = s * transform[3]; float ty = s * transform[4]; float tz = s * transform[5]; /*pi是在curr坐标系下 p_curr，需要变换到当前sweep的初始时刻start坐标系下 * 现在有关系：p_curr = R_curr_start * p_start + t_curr_start * 变换一下有：变换一下有：p_start = R_curr_start^{-1} * (p_curr - t_curr_start) = R_YXZ^{-1} * (p_curr - t_curr_start) * 代入定义量：p_start = R_transform^{-1} * (p_curr - t_transform) * 展开已知角：p_start = R_-ry * R_-rx * R_-rz * (p_curr - t_transform) */ //平移后绕z轴旋转（-rz） 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); //绕x轴旋转（-rx） float x2 = x1; float y2 = cos(rx) * y1 + sin(rx) * z1; float z2 = -sin(rx) * y1 + cos(rx) * z1; //绕y轴旋转（-ry） po->x = cos(ry) * x2 - sin(ry) * z2; po->y = y2; po->z = sin(ry) * x2 + cos(ry) * z2; po->intensity = pi->intensity; }

2.2 edge point匹配

寻找匹配直线：

将该次sweep中每个点通过插值，变换到初始时刻start后，迭代五次时，重新查找最近点，相当于每个匹配迭代优化4次，A-LOAM代码中的Ceres代码的最大迭代次数为4。

然后先试用PCL中的KD-tree查找当前点在上一帧中的最近邻点，对应论文中提到的j点，最近邻在上一帧中的索引保存在pointSearchInd中，距离保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd对应论文中的j点，minPointInd2对应论文中的l点

int closestPointInd = -1, minPointInd2 = -1;

如果查找到了最近邻的j点，就按照论文中所说，需要查找上一帧中，与j线号scanID相邻且j的最近邻点l，j和l构成与待匹配点i的匹配直线edge。

下面这两个for循环做了这么一件事情：向索引增大的方向查找(scanID>=j点的scanID)，如果查找到了相邻帧有距离更小的点，就更新最小距离和索引；然后向索引减小的方向查找(scanID<=j点的scanID)，如果查找到了相邻帧有距离更小的点，就更新最小距离和索引。然后将每个待匹配点对应的j和l点的索引保存在pointSearchCornerInd1[]和pointSearchCornerInd2[]中。

计算直线参数：

当找到了j点和l点，就可以进行特征匹配，计算匹配直线的系数，对应论文中的公式为：

这个公式的分子是i和j构成的向量与i和l构成的向量的叉乘，得到了一个与ijl三点构成平面垂直的向量，而叉乘取模其实就是一个平行四边形面积；分母是j和l构成的向量，取模后为平行四边形面积的底，二者相除就得到了i到直线jl的距离，下面是图示：

代码中变量代表的含义：

x0：i点

x1：j点

x2：l点

a012：论文中残差的分子(两个向量的叉乘)

la、lb、lc：i点到jl线垂线方向的向量(jl方向的单位向量与ijl平面的单位法向量的叉乘得到)在xyz三个轴上的分量

ld2：点到直线的距离，即

下面这个s可以看做一个权重，距离越大权重越小，距离越小权重越大，得到的权重范围<=1,其实就是在求解非线性最小二乘问题时的核函数，s的本质定义如下：

最后将权重大(>0.1)的，即距离比较小，且距离不为零的点放入laserCloudOri，对应的直线系数放入coeffSel。

//论文中提到的Levenberg-Marquardt算法(L-M method)，其实是高斯牛顿算法 for (int iterCount = 0; iterCount < 25; iterCount++) { laserCloudOri->clear(); coeffSel->clear(); //处理当前点云中的曲率最大的特征点,从上个点云中曲率比较大的特征点中找两个最近距离点，一个点使用kd-tree查找，另一个根据找到的点在其相邻线找另外一个最近距离的点 for (int i = 0; i < cornerPointsSharpNum; i++) { //将该次sweep中每个点通过插值，变换到初始时刻start TransformToStart(&cornerPointsSharp->points[i], &pointSel); //每迭代五次，重新查找最近点，相当于每个匹配迭代优化4次，ALOAM代码中的Ceres代码的最大迭代次数为4 if (iterCount % 5 == 0) { std::vector
       
        indices; pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices); //kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波 kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd = -1, minPointInd2 = -1; //closestPointInd对应论文中的j点，minPointInd2对应论文中的l点 //循环是寻找相邻线最近点l //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2 if (pointSearchSqDis[0] < 25) {//找到的最近点距离的确很近的话 closestPointInd = pointSearchInd[0]; //提取最近点线号 int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity); float pointSqDis, minPointSqDis2 = 25;//初始门槛值25米，可大致过滤掉scanID相邻，但实际线不相邻的值 //寻找距离目标点最近距离的平方和最小的点 for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {//向scanID增大的方向查找 if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {//非相邻线 break; } ...
       

2.3 planar point匹配

寻找匹配平面：

将该次sweep中每个点通过插值，变换到初始时刻start后，迭代五次时，重新查找最近点，相当于每个匹配迭代优化4次，A-LOAM代码中的Ceres代码的最大迭代次数为4。

然后先试用PCL中的KD-tree查找当前点在上一帧中的最近邻点，对应论文中提到的j点，最近邻在上一帧中的索引保存在pointSearchInd中，距离保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd对应论文中的j点、minPointInd2对应论文中的l点、minPointInd3对应论文中的m点。

int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;

如果查找到了最近邻的j点，就按照论文中所说，需要查找上一帧中，与j线号scanID相同且j的最近邻点l，然后查找一个与j线号scanID相邻且最近邻的点m。

下面这两个for循环做了这么一件事情：向索引增大的方向查找(scanID>=j点的scanID)，如果查找到了相邻线号的点j和相同线号的点m，有距离更小的点，就更新最小距离和索引；然后向索引减小的方向查找(scanID<=j点的scanID)，如果查找到了相邻线号的点j和相同线号的点m，就更新最小距离和索引。然后将每个待匹配点对应的j、l点、m点的索引保存在pointSearchSurfInd1[]、pointSearchSurfInd2[]和pointSearchSurfInd3[]中。

计算平面参数：

当找到了j点、l点和m点，就可以进行特征匹配，计算匹配直线的系数，对应论文中的公式为：

这个公式的分母是一个lj向量和mj向量叉乘的形式，表示的是jlm平面的法向量的模；而分子是ij向量与jlm平面的法向量的向量积，向量积有一个意义是一个向量在另一个向量方向上的投影，这里就表示ij向量在jlm平面的法向量方向上的投影，也就是i到平面的距离，下面是图示：

代码中变量代表的含义：

tripod1：j点

tripod2：l点

tripod3：m点

pa、pb、pc：jlm平面的法向量在xyz三个轴上的分量，也可以理解为平面方程Ax+By+Cz+D=0的ABC系数

pd：为平面方程的D系数

ps：法向量的模

pd2：点到平面的距离(将平面方程的系数归一化后，代入点的坐标，其实就是点到平面距离公式，即可得到点到平面的距离)

下面这个s可以看做一个权重，对应于论文中的这一部分

距离越大权重越小，距离越小权重越大，得到的权重范围<=1,其实就是在求解非线性最小二乘问题时的核函数，s的本质定义如下：

最后将权重大(>0.1)的，即距离比较小，且距离不为零的点放入laserCloudOri，对应的平面系数放入coeffSel。

//对本次接收到的曲率最小的点,从上次接收到的点云曲率比较小的点中找三点组成平面，一个使用kd-tree查找，另外一个在同一线上查找满足要求的，第三个在不同线上查找满足要求的 for (int i = 0; i < surfPointsFlatNum; i++) { //将该次sweep中每个点通过插值，变换到初始时刻start TransformToStart(&surfPointsFlat->points[i], &pointSel); if (iterCount % 5 == 0) { //kd-tree最近点查找，在经过体素栅格滤波之后的平面点中查找，一般平面点太多，滤波后最近点查找数据量小 kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); //closestPointInd对应论文中的j、minPointInd2对应论文中的l、minPointInd3对应论文中的m 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; } ...

三、高斯牛顿优化

3.1 计算雅可比矩阵并求解增量

在代码中，作者是纯手推的高斯牛顿算法，这种方式相比于使用Ceres等工具，会提高运算速度，但是计算雅克比矩阵比较麻烦，需要清晰的思路和扎实的数学功底，下面我们一起来推导一下。

以edge point匹配为例，planar point是一样的。

设误差函数(点到直线的距离)为：

其中，X为待优化变量，也就是transform[6]中存储的变量，表示3轴旋转rx、ry、rz和3轴平移量tx、ty、tz；D()表示两点之间的距离，其计算公式为：

表示start时刻坐标系下待匹配点i；表示start时刻坐标系下点i到直线jl的垂点；另外根据之前TransformToStart()函数推导过的坐标变换有：

根据链式法则，f(x)对X求导有：

对其中每一项进行计算：

D对求导的结果其实就是进行归一化后的点到直线向量，它在xyz三个轴的分量就是前面求解出来的la、lb、lc变量。

用上面的结果，分别对rx,ry,rz,tx,ty,tz求导，将得到的结果(3x6矩阵)再与D对求导的结果(1x3矩阵)相乘，就可以得到代码中显示的结果(1x6矩阵)，分别赋值到matA的6个位置，matB是残差项。

最后使用opencv的QR分解求解增量X即可。

//满足要求的特征点至少10个，特征匹配数量太少弃用此帧数据，不再进行优化步骤 if (pointSelNum < 10) { continue; } // Hx=g cv::Mat matA(pointSelNum, 6, CV_32F, cv::all(0)); // J cv::Mat matAt(6, pointSelNum, CV_32F, cv::all(0)); // J^T cv::Mat matAtA(6, 6, CV_32F, cv::all(0)); // H = J^T * J cv::Mat matB(pointSelNum, 1, CV_32F, cv::all(0)); // e cv::Mat matAtB(6, 1, CV_32F, cv::all(0)); // g = -J * e cv::Mat matX(6, 1, CV_32F, cv::all(0)); // x //计算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
       
        (i, 0) = arx; matA.at
        
         (i, 1) = ary; matA.at
         
          (i, 2) = arz; matA.at
          
           (i, 3) = atx; matA.at
           
            (i, 4) = aty; matA.at
            
             (i, 5) = atz; matB.at
             
              (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);
             
            
           
          
         
        
       

3.2 退化处理

代码中使用的退化处理在Ji Zhang的这篇论文(《On Degeneracy of Optimization-based State Estimation Problems》)中有详细论述。

简单来说，作者认为退化只可能发生在第一次迭代时，先对矩阵求特征值，然后将得到的特征值与阈值(代码中为10)进行比较，如果小于阈值则认为该特征值对应的特征向量方向发生了退化，将对应的特征向量置为0，然后按照论文中所述，计算一个P矩阵：

是原来的特征向量矩阵，是将退化方向置为0后的特征向量矩阵，然后用P矩阵与原来得到的解相乘，得到最终解：

if (iterCount == 0) { //特征值1*6矩阵 cv::Mat matE(1, 6, CV_32F, cv::all(0)); //特征向量6*6矩阵 cv::Mat matV(6, 6, CV_32F, cv::all(0)); cv::Mat matV2(6, 6, CV_32F, cv::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
       
        (0, i) < eignThre[i]) {//特征值太小，则认为处在退化环境中，发生了退化 for (int j = 0; j < 6; j++) {//对应的特征向量置为0 matV2.at
        
         (i, j) = 0; } isDegenerate = true; } else { break; } } //计算P矩阵 matP = matV.inv() * matV2; // 论文中对应的Vf^-1 * V_u` } if (isDegenerate) {//如果发生退化，只使用预测矩阵P计算 cv::Mat matX2(6, 1, CV_32F, cv::all(0)); matX.copyTo(matX2); matX = matP * matX2; }
        
       

3.3 迭代更新

最后进行迭代更新，如果更新量很小则终止迭代。

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

再次总结一下整个流程：

一共迭代25次，每次分为edge point和planar point两个处理过程

每迭代5次时，重新寻找最近点

其他情况时，根据找到的最近点和待匹配点，计算匹配得到的直线和平面方程

根据计算匹配得到的直线和平面方程，计算雅可比矩阵，并求解迭代增量

如果是第一次迭代，判断是否发生退化

更新迭代增量，并判断终止条件

四、发布话题和坐标变换

4.1 发布话题

这一部分首先奖transformSum更新为修正后的值，然后将欧拉角转换成四元数，发布里程计话题、广播tf坐标变换，然后将点云的曲率比较大和比较小的点投影到扫描结束位置；如果当前帧特征点数量足够多，就将其插入到KD-tree中用于下一次特征匹配；然后发布话题，发布的话题有：

/laser_cloud_corner_last：曲率较大的点--less edge point

/laser_cloud_surf_last：曲率较小的点--less planar point

/velodyne_cloud_3：全部点云--full cloud point

/laser_odom_to_init：里程计坐标系下，当前时刻end相对于初始时刻的坐标变换

//得到世界坐标系下的转移矩阵 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); //广播新的平移旋转之后的坐标系(rviz) laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat); laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz)); tfBroadcaster.sendTransform(laserOdometryTrans); //对点云的曲率比较大和比较小的点投影到扫描结束位置 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]); } frameCount++; //点云全部点，每间隔一个点云数据相对点云最后一个点进行畸变校正 if (frameCount >= skipFrameNum + 1) { // skipFrameNum = 1 int laserCloudFullResNum = laserCloudFullRes->points.size(); for (int i = 0; i < laserCloudFullResNum; i++) { TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); } } //畸变校正之后的点作为last点保存等下个点云进来进行匹配 pcl::PointCloud
       
        ::Ptr laserCloudTemp = cornerPointsLessSharp; cornerPointsLessSharp = laserCloudCornerLast; laserCloudCornerLast = laserCloudTemp; laserCloudTemp = surfPointsLessFlat; surfPointsLessFlat = laserCloudSurfLast; laserCloudSurfLast = laserCloudTemp; laserCloudCornerLastNum = laserCloudCornerLast->points.size(); laserCloudSurfLastNum = laserCloudSurfLast->points.size(); //点足够多就构建kd-tree，否则弃用此帧，沿用上一帧数据的kd-tree if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { kdtreeCornerLast->setInputCloud(laserCloudCornerLast); kdtreeSurfLast->setInputCloud(laserCloudSurfLast); } //按照跳帧数publich边沿点，平面点以及全部点给laserMapping(每隔一帧发一次) if (frameCount >= skipFrameNum + 1) { frameCount = 0; 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); sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudFullRes3.header.frame_id = "/camera"; pubLaserCloudFullRes.publish(laserCloudFullRes3); } } status = ros::ok(); rate.sleep(); }
       

4.2 将点云变换到结束时刻end--TransformToEnd()函数

这里对应于论文中体到的，变换到下图所示的将变换到。

在匀速运动假设的前提下，从Start时刻到End时刻，点云都将保持imuRPYStart的位置姿态，然后对其中里程计坐标系下curr时刻的点进行以下操作：

1.首先进行了一个TransformToStart()函数的过程，将当前点变换到里程计坐标系下start时刻坐标系下，得到x3、y3、z3：

2.然后变换到里程计坐标系下end时刻坐标系下，再次提醒，得到x6、y6、z6：

但是事实上，由于加减速情况的存在，会产生运动畸变，这就导致匀速运动假设不再成立，也就是End时刻的imuRPY角与Start时刻的imuRPY角不相等，需要使用IMU去除畸变。

3.上面的过程，总体上看的结果就是将所有点保持imuRPYStrat的姿态，搬运到了雷达坐标系下的end时刻，由于运动畸变的存在，下面要使用IMU进行去畸变，首先将所有点转换到世界坐标系下，但仍然是里程计坐标系的end时刻，只是使用IMU进行了修正：

4.然后将所有点通过测量得到的imuRPYLast变换到全局(w)坐标系下的当前帧终止时刻，并且对应了imuRPYLast姿态角：

我理解的这个过程如下：

//将上一帧点云中的点相对结束位置去除因匀速运动产生的畸变，效果相当于得到在点云扫描结束位置静止扫描得到的点云 void TransformToEnd(PointType const * const pi, PointType * const po) { //插值系数计算 float s = 10 * (pi->intensity - int(pi->intensity)); //R_curr_start float rx = s * transform[0]; float ry = s * transform[1]; float rz = s * transform[2]; float tx = s * transform[3]; float ty = s * transform[4]; float tz = s * transform[5]; //将点curr系下的点，变换到初始时刻start //平移后绕z轴旋转（-rz） 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); //绕x轴旋转（-rx） float x2 = x1; float y2 = cos(rx) * y1 + sin(rx) * z1; float z2 = -sin(rx) * y1 + cos(rx) * z1; //绕y轴旋转（-ry） float x3 = cos(ry) * x2 - sin(ry) * z2; float y3 = y2; float z3 = sin(ry) * x2 + cos(ry) * z2;//求出了相对于起始点校正的坐标 //R_end_start = R_YXZ rx = transform[0]; ry = transform[1]; rz = transform[2]; tx = transform[3]; ty = transform[4]; tz = transform[5]; //变换到end坐标系 //绕y轴旋转（ry） float x4 = cos(ry) * x3 + sin(ry) * z3; float y4 = y3; float z4 = -sin(ry) * x3 + cos(ry) * z3; //绕x轴旋转（rx） float x5 = x4; float y5 = cos(rx) * y4 - sin(rx) * z4; float z5 = sin(rx) * y4 + cos(rx) * z4; //绕z轴旋转（rz），再平移 float x6 = cos(rz) * x5 - sin(rz) * y5 + tx; float y6 = sin(rz) * x5 + cos(rz) * y5 + ty; float z6 = z5 + tz; //使用IMU去除由于加减速产生的运动畸变，然后变换到全局(w)坐标系下 //平移后绕z轴旋转（imuRollStart） float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) - sin(imuRollStart) * (y6 - imuShiftFromStartY); float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) + cos(imuRollStart) * (y6 - imuShiftFromStartY); float z7 = z6 - imuShiftFromStartZ; //绕x轴旋转（imuPitchStart） float x8 = x7; float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7; float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7; //绕y轴旋转（imuYawStart） float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8; float y9 = y8; float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8; //然后变换到全局(w)坐标系下的当前帧终止时刻 //绕y轴旋转（-imuYawLast） float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9; float y10 = y9; float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9; //绕x轴旋转（-imuPitchLast） float x11 = x10; float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10; float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10; //绕z轴旋转（-imuRollLast） po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11; po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11; po->z = z11; //只保留线号 po->intensity = int(pi->intensity); }

总结

感觉如果去掉IMU的话，整个代码就很清晰，但是加上IMU部分就很容易让人懵逼。

编辑：黄飞