系列文章目录

·【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp

---

写在前面

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

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

之后也打算录一个LOAM讲解的视频，大家可以关注一下。

---

---

整体框架

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

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

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

• IMU(IMU坐标系imu)：x轴向前，y轴向左，z轴向上
• LIDAR(激光雷达坐标系l)：x轴向前，y轴向左，z轴向上
• CAMERA(相机坐标系，也可以理解为里程计坐标系c)：z轴向前，x轴向左，y轴向上
• WORLD(世界坐标系w，也可以理解为C第一帧的坐标系init)：z轴向前，x轴向左，y轴向上

坐标变换约定： 为了清晰，变换矩阵的形式与《SLAM十四讲中一样》，即：$R_{A\_B}$表示B坐标系相对于A坐标系的变换，B中一个向量通过$R_{A\_B}$可以变换到A中的向量。

首先对照ros的节点图和论文中提到的算法框架来看一下：

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

• scanRegistration：对原始点云进行预处理，计算曲率，提取特征点
• laserOdometry：对当前sweep与上一次sweep进行特征匹配，计算一个快速(10Hz)但粗略的位姿估计
• laserMapping：对当前sweep与一个局部子图进行特征匹配，计算一个慢速(1Hz)比较精确的位姿估计
• transformMaintenance：对两个模块计算出的位姿进行融合，得到最终的精确地位姿估计

本文首先介绍scanRegistration模块，它的具体功能如下：

1. 将输入的无序点云转化为按照线号存储的有序点云
2. 接收IMU数据(如果有的话)，并基于匀速运动假设，使用IMU数据进行插值
3. 计算每个点云的曲率
4. 剔除论文中提到的不合要求的点
5. 提取edge point和planar point

一、main函数-main()

main函数很简单，主要是创建了一系列的发布者和订阅者，，然后ros::spin()进行无限循环，其中的主要程序都在回调函数中进行：

• imuHandler：接收IMU数据进行处理
• laserCloudHandler：接收激光雷达数据进行处理，这个是该程序的主要函数，包含了算法的核心部分。

int main(int argc, char** argv)
{ros::init(argc, argv, "scanRegistration");ros::NodeHandle nh;ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 2, laserCloudHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);ros::spin();return 0;
}

二、接收IMU的消息-imuHandler()

这一部分比较难理解的部分是去除重力影响，主要对这部分进行解释。

IMU坐标系为x轴向前，y轴向左，z轴向上的右手坐标系

首先，接收ROS的IMU话题，分解出PRY角，IMU系相对于全局坐标系的变换是XYZ的变换顺序，即，旋转矩阵$R_{w\_imu}=R_z{R}_y{R}_x$，重力为全局坐标系中一个向量$g_w=[0,0,9.81]$，需要先变换到IMU坐标系，有如下关系：
$$g_{imu}=R_{imu\_w}{g}_w=R_{w\_imu}^{-1}{g}_w=R_{-x}{R}_{-y}{R}_{-z}\ast g_w$$
计算出来的结果为：
$$g_{imu}=[-9.8sin\beta ,9.8sin\alpha cos\beta ,9.8cos\alpha cos\beta ]$$
而我们测量出来的加速度值是收到重力影响的，可以表述为：
$$a_{imu}^{measure}=a_{imu}^{true}+g_{imu}$$
所以加速度真值为：
$$a_{imu}^{true}=a_{imu}^{measure}-g_{imu}$$
最后进行坐标系交换，变换z轴向前，x轴向左，y轴向上，就有了代码中的样子。

注意这里没有进行坐标变换，只是换了一下坐标轴的位置，是为了后面计算全局坐标系下的速度和位移积分。

//接收imu消息，imu坐标系为x轴向前，y轴向左，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;//convert Quaternion msg to Quaterniontf::quaternionMsgToTF(imuIn->orientation, orientation);//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).//Here roll pitch yaw is in the global(init) frametf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//循环移位效果，形成环形数组imuPointerLast = (imuPointerLast + 1) % imuQueLength;     // const int imuQueLength = 200imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;AccumulateIMUShift();
}

三、积分IMU的速度与位移-AccumulateIMUShift()

首先明确一点：在进行了坐标系转换(转换成了z轴向前，x轴向左，y轴向上)后，原来的当前帧(局部坐标系)到世界坐标系(全局坐标系)的变换变成了$R_{ZXY}=R_y{R}_x{R}_z$。

现在的加速度还是在局部坐标系(imu)中，现在需要将其变换到世界坐标系，然后与之前的速度、位移进行积分。
$$a_w=R_y{R}_x{R}_z\ast a_{imu}$$
下面就是根据初中学的公式进行积分：
$$\begin{gathered} x_i=x_{i-1}+v_{i-1}\ast \Delta t+\frac{1}{2}\ast a_i\ast \Delta t^2 \\ {v}_i=v_{i-1}+a_i\ast \Delta t \end{gathered}$$

//积分速度与位移
void AccumulateIMUShift()
{float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];// accX、accY、acc都是世界坐标系下float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)//绕z轴旋转(roll)float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//绕x轴旋转(pitch)float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;//绕y轴旋转(yaw)accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;//上一个imu点int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;//上一个点到当前点所经历的时间，即计算imu测量周期double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义if (timeDiff < scanPeriod) {//（隐含从静止开始运动）//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;}
}

---

四、接收点云数据-laserCloudHandler()

4.1 预处理与线性插值

首先接收到当前sweep的点云数据后，先计算一下点云的起始角度startOri和终止角度endOri，对应于下图的α角，然后将结束角与起始角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描。

下面这个for循环做了这些事情：

1. 将点云从雷达坐标系转换到相机坐标系(坐标系转换，不是坐标变换)
2. 计算每个点的俯仰角，对应于上图的ω角，用于计算scanID
3. 如果收到IMU数据，使用IMU进行插值
4. 计算了每个点由于加减速产生的位移和速度畸变
5. 将每个点变换到当前sweep的初始时刻start坐标系

要进行说明的部分如下：

点云强度保存：

point.intensity = scanID + scanPeriod * relTime;

这里点云强度值保存的格式为“线号 + 扫描周期(10Hz=0.1s) * 点云相对角度”，这样保存的好处就是：对强度值取整，可以得到线号；强度值-取整，可以计算出相对角度。

IMU去畸变：

 if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU进行插值float pointTime = relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}
......

这里的imuPointerLast表示最新收到IMU数据的位置，imuPointerFront表示时间戳刚好大于当前点云时间戳的位置，在对点云进行插值时，需要使用imuPointerFront和它之前一个位置imuPointerBack。

当找到了一个满足要求的imuPointerFront，就是用下式进行插值：
$$v_{current}=v_{i-1}+(v_i-v_{i-1})\ast \frac{t_{current}-t_{i-1}}{t_i-t_{i-1}}$$
文章中的式子如下：本质上是一样的：
$$v_{current}=v_i\ast \frac{t_{current}-t_{i-1}}{t_i-t_{i-1}}+v_{i-1}\ast \frac{t_i-t_{current}}{t_i-t_{i-1}}$$
第一个点云的数值都保存在以Start结尾的变量中。

ShiftToStartIMU(pointTime)函数：

这个函数用来计算相对于当前sweep初始时刻 由于加减速产生的位移畸变，注意这里的变量都是在全局坐标系下计算的 当前时刻相对于该sweep初始时刻的畸变量。

然后将畸变量由全局坐标系(init)变换到当前sweep的初始时刻坐标系(start)，而现在我们已知的量RPY角所构成的变换为：
$$R_{init\_start}=R_y{R}_x{R}_z$$
所以这里需要的变换为：
$$R_{start\_init}=R_{init\_start}^{-1}=R_{-z}{R}_{-x}{R}_{-y}$$
这样得到了如下代码。

//计算局部(start)坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
void ShiftToStartIMU(float pointTime)
{//计算相对于第一个点由于加减速产生的畸变位移(全局(init)坐标系下畸变位移量delta_Tg)//imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;/********************************************************************************delta_Tstart = Rz(roll).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tgtransfrom from the global(init) frame to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuShiftFromStartZCur = z2;
}

VeloToStartIMU()函数：
这个函数与上面函数的功能一致，是将计算由于加减速产生的速度畸变，并变换到start坐标系中，不再赘述。

//计算局部(start)坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变（增量）
void VeloToStartIMU()
{//计算相对于第一个点由于加减速产生的畸变速度(全局(init)坐标系下畸变速度增量delta_Vg)imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;/********************************************************************************delta_Vstart = Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vgtransfrom from the global(init) frame to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuVeloFromStartZCur = z2;
}

TransformToStartIMU(&point)函数：
这个函数对应于论文中提到的，将当前sweep中的所有点云都变换到初始时刻，得到的就是下图中的$\overline{P_k}$.

我们现在已知了当前时刻的PRY角，那么可以构成当前时刻坐标系(curr坐标系)相对于世界坐标系(init)的变换：
$$R_{ini{t}_{c}urr}=R_y{R}_x{R}_z$$
同样，已知了该sweep初始时刻的PRY角，可以构成世界坐标系init到该sweep的坐标变换，和上面畸变量变换类似：
$$R_{start\_init}=R_{init\_start}^{-1}=R_{-z}{R}_{-x}{R}_{-y}$$
那么最终的变换为：
$$p_{start}=R_{start\_init}\ast R_{init\_curr}\ast p_{curr}$$
最后再加上上面计算出的由于加减速产生的位移畸变量，就得到了如下代码。

//将当前时刻curr坐标系下的的点云变换到世界坐标系init，然后在变换到当前帧的初始时刻start坐标系下
void TransformToStartIMU(PointType *p)
{/********************************************************************************Pinit = Ry * Rx * Rz * Pcurrtransform current camara(curr) frame point to the global(init) frame*********************************************************************************///绕z轴旋转(imuRollCur)float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;//绕x轴旋转(imuPitchCur)float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;//绕y轴旋转(imuYawCur)float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;/********************************************************************************Pstart = Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pinittransfrom global(init) points to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;float y4 = y3;float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;//绕x轴旋转(-imuPitchStart)float x5 = x4;float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;//绕z轴旋转(-imuRollStart)，然后叠加平移量p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;
}

当上面这些操纵都做完之后，将得到的start坐标系下去畸变的点，放入按scanID存储的点云容器laserCloudScans，所有代码如下：

//接收点云数据，velodyne雷达坐标系安装为x轴向前，y轴向左，z轴向上的右手坐标系
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{if (!systemInited) {//丢弃前20个点云数据systemInitCount++;if (systemInitCount >= systemDelay) {systemInited = true;}return;}//记录每个scan有曲率的点的开始和结束索引std::vector<int> scanStartInd(N_SCANS, 0);std::vector<int> scanEndInd(N_SCANS, 0);//当前sweep的时间double timeScanCur = laserCloudMsg->header.stamp.toSec();pcl::PointCloud<pcl::PointXYZ> laserCloudIn;//消息转换成pcl数据存放pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vector<int> indices;//移除空点pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);//点云点的数量int cloudSize = laserCloudIn.points.size();//lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pifloat endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;//结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描//正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正if (endOri - startOri > 3 * M_PI) {endOri -= 2 * M_PI;} else if (endOri - startOri < M_PI) {endOri += 2 * M_PI;}//lidar扫描线是否旋转过半bool halfPassed = false;int count = cloudSize;PointType point;std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);/* 这个for循环做了这些事情：*    1.将点云从雷达坐标系转换到相机坐标系*    2.计算每个点的俯仰角，用于计算scanID*    3.如果收到IMU数据,使用IMU进行插值*    4.计算了每个点由于加减速产生的位移和速度畸变*    5.将每个点变换到当前sweep的初始时刻start坐标系*/for (int i = 0; i < cloudSize; i++) {//把雷达坐标系(前-左-上)中的点云转换到相机坐标系(左上前) X->Z Y->X Z->Ypoint.x = laserCloudIn.points[i].y;point.y = laserCloudIn.points[i].z;point.z = laserCloudIn.points[i].x;//计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;int scanID;//仰角四舍五入(加减0.5截断效果等于四舍五入)int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); if (roundedAngle > 0){scanID = roundedAngle;}else {scanID = roundedAngle + (N_SCANS - 1);}//过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15]if (scanID > (N_SCANS - 1) || scanID < 0 ){count--;continue;}//该点的旋转角float ori = -atan2(point.x, point.z);if (!halfPassed) {//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿//确保-pi/2 < ori - startOri < 3*pi/2if (ori < startOri - M_PI / 2) {ori += 2 * M_PI;} else if (ori > startOri + M_PI * 3 / 2) {ori -= 2 * M_PI;}if (ori - startOri > M_PI) {halfPassed = true;}} else {ori += 2 * M_PI;//确保-3*pi/2 < ori - endOri < pi/2if (ori < endOri - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > endOri + M_PI / 2) {ori -= 2 * M_PI;} }//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）float relTime = (ori - startOri) / (endOri - startOri);//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间point.intensity = scanID + scanPeriod * relTime;if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU进行插值float pointTime = relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;   // imuPointerBack是imuPointerFront的上一个位置//按时间距离计算权重分配比率,也即线性插值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[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFrontimuVeloXCur = 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;} else {ShiftToStartIMU(pointTime);VeloToStartIMU();TransformToStartIMU(&point);//将当前时刻curr坐标系下的的点云变换到世界坐标系init，然后在变换到当前帧的初始时刻start坐标系下}}laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器}

4.2 计算曲率

这一部分对应论文中提到的曲率计算公式：
$$c=\frac{1}{|S|\cdot ||X_{(k,i)}^L||}||\sum _{j\in S,j\ne i}(X_{(k,i)}^L-X_{(k,j)}^L)||$$
$X_{(k,i)}^L$是第i个点云的位置，$X_{(k,j)}^L$是$X_{(k,i)}^L$左右两边各5个点，注意它们两个都是矢量，那么$(X_{(k,i)}^L-X_{(k,j)}^L)$就是一个$X_{(k,i)}^L$指向$X_{(k,j)}^L$的向量。

如上图所示，如果一个点是edge point，那么向量求和后得到结果的模很大；如果一个点是planar point那么与两侧五个向量求和后结果是0，通过这种方式，区分特征点。

求解完所有点的曲率后，scanStartInd[]和scanEndInd[]数组用于记录下每个scanID有曲率的起始点和终止点的索引。

//获得有效范围内的点的数量cloudSize = count;//这里就按照scanID变成了有序点云pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());//将所有的点按照线号从小到大放入一个容器for (int i = 0; i < N_SCANS; i++) {*laserCloud += laserCloudScans[i];}int scanCount = -1;//使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过for (int i = 5; i < cloudSize - 5; i++) {float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x+ laserCloud->points[i + 5].x;float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y+ laserCloud->points[i + 5].y;float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z+ laserCloud->points[i + 5].z;//曲率计算cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;//记录曲率点的索引cloudSortInd[i] = i;//初始时，点全未筛选过cloudNeighborPicked[i] = 0;//初始化为less flat点cloudLabel[i] = 0;//每个scan，只有第一个符合的点会进来，因为每个scan的点都在一起存放if (int(laserCloud->points[i].intensity) != scanCount) {scanCount = int(laserCloud->points[i].intensity);//控制每个scan只进入第一个点//曲率只取同一个scan计算出来的，跨scan计算的曲率非法，排除，也即排除每个scan的前后五个点if (scanCount > 0 && scanCount < N_SCANS) {scanStartInd[scanCount] = i + 5;scanEndInd[scanCount - 1] = i - 5;}}}//第一个scan曲率点有效点序从第5个开始，最后一个激光线结束点序size-5scanStartInd[0] = 5;scanEndInd.back() = cloudSize - 5;

4.3 剔除不好的点

这部分对应于论文中提到的，计算完曲率后，最终的特征点需要满足以下要求：

• 所选边缘点或平面点的个数不能超过子区域的最大值-------保证取点均匀，这一点下面会讲到
• 它周围的点都没有被选中-------保证点不过于密集
• 它不能位于与激光束大致平行的平面上，也不能位于遮挡区域的边界上-------剔除一些不好的点(下面的操作)

下面代码中的这个if用于剔除类似于图b中A点这样的易遮挡点

if (diff > 0.1)

当传感器在这个角度时，A点看起来是edge point，但稍微移动时，A点变为planar或者不可见，这种是不靠谱的，需要剔除。

代码中的意思是：如果A和B距离相差0.1米以上，就求解它们两个的深度，将深度大的点放缩到同一距离水平，然后用"深度距离(距离很小，近似为弧长)/深度"，这个得到的就是两者夹角的弧度值，如果这个夹角很小，说明就是图b中的情况，A很容易被遮挡。

下面代码中的这个if用于剔除类似于图a中B点这样的所在平面与激光束近似平行的点

if (diff > 0.0002 * dis && diff2 > 0.0002 * dis)

diff和diff2分别是与后一个点、前一个点距离的平方，如果出现图a中B点这样的情况，diff和diff2值会很大，如果大于当前点深度的0.0002，则认为出现图a中的情况，需要剔除。

  //这个for循环：排除容易被斜面挡住的点、所在平面近似与激光束平行的点以及离群点(噪点)for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值，所以减6float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;//计算有效曲率点与后一个点之间的距离平方和float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;//排除一些易遮挡的点，对应论文中图4(b)的A点if (diff > 0.1) {//前提:两个点之间距离要大于0.1//点的深度float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y +laserCloud->points[i].z * laserCloud->points[i].z);//后一个点的深度float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);//按照两点的深度的比例，将深度较大的点拉回后计算距离if (depth1 > depth2) {diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;//边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点//该点及前面五个点（大致都在斜面上）全部置为筛选过cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}} else {diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}}float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;//与前一个点的距离平方和float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;//点深度的平方和float dis = laserCloud->points[i].x * laserCloud->points[i].x+ laserCloud->points[i].y * laserCloud->points[i].y+ laserCloud->points[i].z * laserCloud->points[i].z;//与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用//对应于论文中的图4(a)中的Bif (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {cloudNeighborPicked[i] = 1;}}

4.4 特征提取

这部分与论文中说的有点不一样，论文中说将当前sweep分为4个相同区域，而代码中是分为了6个区域，每个区域的起始点和终止点索引分别为sp和ed，其计算本质如下：

六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6

六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6

1.按照曲率从小到大进行冒泡排序，A-LOAM中使用的是sort函数。

2.然后，如果曲率值大于0.1则选择为edge point(边缘特征点)或less edge point(没那么尖锐的边缘特征点)，edge point对应论文中提到的每个区域选择2个，less edge point每个区域选择20个。

3.每选择一个点后就将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀。

4.然后，如果曲率值小于0.1则选择为planar point(平面特征点)或less planar point(没那么平坦的平面特征点)，planar point对应论文中提到的每个区域选择4个，而该区域剩下的全都归入less edge point。

5.同样的，每选择一个点后就将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀。

6.最后，由于less planar point点最多，对每个区域less planar point的点进行体素栅格滤波

  pcl::PointCloud<PointType> cornerPointsSharp;pcl::PointCloud<PointType> cornerPointsLessSharp;pcl::PointCloud<PointType> surfPointsFlat;pcl::PointCloud<PointType> surfPointsLessFlat;//这个for循环：将每条线上的点分入相应的类别：边沿点和平面点for (int i = 0; i < N_SCANS; i++) {pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点for (int j = 0; j < 6; j++) {//六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6int ep = (scanStartInd[i] * (5 - j)  + scanEndInd[i] * (j + 1)) / 6 - 1;//按曲率从小到大冒泡排序for (int k = sp + 1; k <= ep; k++) {for (int l = k; l >= sp + 1; l--) {//如果后面曲率点大于前面，则交换if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {int temp = cloudSortInd[l - 1];cloudSortInd[l - 1] = cloudSortInd[l];cloudSortInd[l] = temp;}}}//挑选每个分段的曲率很大和比较大的点int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {int ind = cloudSortInd[k];  //曲率最大点的点序//如果曲率大的点，曲率的确比较大，并且未被筛选过滤掉if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1) {largestPickedNum++;// 这里对应选点规则第二点if (largestPickedNum <= 2) {//挑选曲率最大的前2个点放入sharp点集合cloudLabel[ind] = 2;//2代表点曲率很大cornerPointsSharp.push_back(laserCloud->points[ind]);cornerPointsLessSharp.push_back(laserCloud->points[ind]);} else if (largestPickedNum <= 20) {//挑选曲率最大的前20个点放入less sharp点集合cloudLabel[ind] = 1;//1代表点曲率比较尖锐cornerPointsLessSharp.push_back(laserCloud->points[ind]);} else {break;}cloudNeighborPicked[ind] = 1;//筛选标志置位//这里对应选点规则第三点//将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀for (int l = 1; l <= 5; l++) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}}}//挑选每个分段的曲率很小比较小的点int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSortInd[k];//如果曲率的确比较小，并且未被筛选出if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1) {cloudLabel[ind] = -1;//-1代表曲率很小的点surfPointsFlat.push_back(laserCloud->points[ind]);smallestPickedNum++;if (smallestPickedNum >= 4) {//只选最小的四个，剩下的Label==0,就都是曲率比较小的break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {//同样防止特征点聚集float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {break;}cloudNeighborPicked[ind + l] = 1;}}}//将剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(laserCloud->points[k]);}}}//由于less flat点最多，对每个分段less flat的点进行体素栅格滤波pcl::PointCloud<PointType> surfPointsLessFlatScanDS;pcl::VoxelGrid<PointType> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);//less flat点汇总surfPointsLessFlat += surfPointsLessFlatScanDS;}

4.5 发布话题

最后这部分就是ROS中的发布话题，没什么可讲的，总结一下发布的话题都是什么：

• /velodyne)cloud_2：经过处理后的所有点云(按照scanID排序的有序点云)
• /laser_cloud_sharp：edge point特征点，一共1662=192个
• /laser_cloud_less_sharp：less edge point特征点，一共16620=1920个
• /laser_cloud_flat：planar point特征点，一共1664=384个
• /laser_cloud_less_flat：less planar point特征点，其余的满足要求的个
• /imu_trans：包含了：当前sweep的IMU测量的起始RPY角、终止RPY角、最后一个点相对于第一个点的畸变位移和速度

  //publich消除非匀速运动畸变后的所有的点sensor_msgs::PointCloud2 laserCloudOutMsg;pcl::toROSMsg(*laserCloud, laserCloudOutMsg);laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubLaserCloud.publish(laserCloudOutMsg);//publish消除非匀速运动畸变后的平面点和边沿点sensor_msgs::PointCloud2 cornerPointsSharpMsg;pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsSharpMsg.header.frame_id = "/camera";pubCornerPointsSharp.publish(cornerPointsSharpMsg);sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsLessSharpMsg.header.frame_id = "/camera";pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);sensor_msgs::PointCloud2 surfPointsFlat2;pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsFlat2.header.frame_id = "/camera";pubSurfPointsFlat.publish(surfPointsFlat2);sensor_msgs::PointCloud2 surfPointsLessFlat2;pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsLessFlat2.header.frame_id = "/camera";pubSurfPointsLessFlat.publish(surfPointsLessFlat2);//publich IMU消息,由于循环到了最后，因此是Cur都是代表最后一个点，即最后一个点的欧拉角，畸变位移及一个点云周期增加的速度pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);    // 1行4列的有序点云//起始点欧拉角imuTrans.points[0].x = imuPitchStart;imuTrans.points[0].y = imuYawStart;imuTrans.points[0].z = imuRollStart;//最后一个点的欧拉角imuTrans.points[1].x = imuPitchCur;imuTrans.points[1].y = imuYawCur;imuTrans.points[1].z = imuRollCur;//最后一个点相对于第一个点的畸变位移和速度imuTrans.points[2].x = imuShiftFromStartXCur;imuTrans.points[2].y = imuShiftFromStartYCur;imuTrans.points[2].z = imuShiftFromStartZCur;imuTrans.points[3].x = imuVeloFromStartXCur;imuTrans.points[3].y = imuVeloFromStartYCur;imuTrans.points[3].z = imuVeloFromStartZCur;sensor_msgs::PointCloud2 imuTransMsg;pcl::toROSMsg(imuTrans, imuTransMsg);imuTransMsg.header.stamp = laserCloudMsg->header.stamp;imuTransMsg.header.frame_id = "/camera";pubImuTrans.publish(imuTransMsg);
}

总结

本篇文章介绍了LOAM代码的第一个节点文件scanRegistration，我感觉这个代码还是很好懂得，坐标变换部分也不是很复杂，对照论文慢慢看就能看懂。

下一篇将介绍laserOdometry.cpp，也就是雷达里程计部分