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

系列文章目录

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

写在前面

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

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

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

文章目录

  • 系列文章目录
  • 写在前面
  • 整体框架
  • 一、main函数-main()
  • 二、接收IMU的消息-imuHandler()
  • 三、积分IMU的速度与位移-AccumulateIMUShift()
  • 四、接收点云数据-laserCloudHandler()
    • 4.1 预处理与线性插值
    • 4.2 计算曲率
    • 4.3 剔除不好的点
    • 4.4 特征提取
    • 4.5 发布话题
  • 总结

整体框架

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 R_{A\_B} RA_B​表示B坐标系相对于A坐标系的变换，B中一个向量通过 R A _ B R_{A\_B} RA_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 _ i m u = R z R y R x R_{w\_imu} = R_z R_y R_x Rw_imu​=Rz​Ry​Rx​，重力为全局坐标系中一个向量 g w = [ 0 , 0 , 9.81 ] g_w=[0,0,9.81] gw​=[0,0,9.81]，需要先变换到IMU坐标系，有如下关系：
g i m u = R i m u _ w g w = R w _ i m u − 1 g w = R − x R − y R − z ∗ g w g_{imu} = R_{imu\_w} g_w = R_{w\_imu}^{-1} g_w = R_{-x} R_{-y} R_{-z} * g_w gimu​=Rimu_w​gw​=Rw_imu−1​gw​=R−x​R−y​R−z​∗gw​
计算出来的结果为：
g i m u = [ − 9.8 s i n β , 9.8 s i n α c o s β , 9.8 c o s α c o s β ] g_{imu} = [-9.8sin\beta,9.8sin\alpha cos\beta,9.8cos\alpha cos\beta] gimu​=[−9.8sinβ,9.8sinαcosβ,9.8cosαcosβ]
而我们测量出来的加速度值是收到重力影响的，可以表述为：
a i m u m e a s u r e = a i m u t r u e + g i m u a_{imu}^{measure} = a_{imu}^{true} + g_{imu} aimumeasure​=aimutrue​+gimu​
所以加速度真值为：
a i m u t r u e = a i m u m e a s u r e − g i m u a_{imu}^{true} = a_{imu}^{measure} - g_{imu} aimutrue​=aimumeasure​−gimu​
最后进行坐标系交换，变换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 Quaternion
  tf::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) frame
  tf::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 = 200

  imuTime[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 Z X Y = R y R x R z R_{ZXY} = R_y R_x R_z RZXY​=Ry​Rx​Rz​。

现在的加速度还是在局部坐标系(imu)中，现在需要将其变换到世界坐标系，然后与之前的速度、位移进行积分。
a w = R y R x R z ∗ a i m u a_w = R_y R_x R_z * a_{imu} aw​=Ry​Rx​Rz​∗aimu​
下面就是根据初中学的公式进行积分：
x i = x i − 1 + v i − 1 ∗ Δ t + 1 2 ∗ a i ∗ Δ t 2 v i = v i − 1 + a i ∗ Δ t x_i = x_{i-1} + v_{i-1} * \Delta t + \frac{1}{2} * a_i * \Delta t^2 \\ v_i = v_{i-1} + a_i * \Delta t xi​=xi−1​+vi−1​∗Δt+21​∗ai​∗Δt2vi​=vi−1​+ai​∗Δt

//积分速度与位移
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位置:imuPointerFront
      while (imuPointerFront != imuPointerLast) {
        if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
          break;
        }
        imuPointerFront = (imuPointerFront + 1) % imuQueLength;
      }
......

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

当找到了一个满足要求的imuPointerFront，就是用下式进行插值：
v c u r r e n t = v i − 1 + ( v i − v i − 1 ) ∗ t c u r r e n t − t i − 1 t i − t i − 1 v_{current} = v_{i-1} + (v_i-v_{i-1})*\frac{t_{current}-t_{i-1}}{t_i-t_{i-1}} vcurrent​=vi−1​+(vi​−vi−1​)∗ti​−ti−1​tcurrent​−ti−1​​
文章中的式子如下：本质上是一样的：
v c u r r e n t = v i ∗ t c u r r e n t − t i − 1 t i − t i − 1 + v i − 1 ∗ t i − t c u r r e n t t i − t i − 1 v_{current} = v_i * \frac{t_{current}-t_{i-1}}{t_i-t_{i-1}} + v_{i-1} * \frac{t_i - t_{current}}{t_i-t_{i-1}} vcurrent​=vi​∗ti​−ti−1​tcurrent​−ti−1​​+vi−1​∗ti​−ti−1​ti​−tcurrent​​
第一个点云的数值都保存在以Start结尾的变量中。

ShiftToStartIMU(pointTime)函数：

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

然后将畸变量由全局坐标系(init)变换到当前sweep的初始时刻坐标系(start)，而现在我们已知的量RPY角所构成的变换为：
R i n i t _ s t a r t = R y R x R z R_{init\_start} = R_y R_x R_z Rinit_start​=Ry​Rx​Rz​
所以这里需要的变换为：
R s t a r t _ i n i t = R i n i t _ s t a r t − 1 = R − z R − x R − y R_{start\_init} = R_{init\_start}^{-1} = R_{-z} R_{-x} R_{-y} Rstart_init​=Rinit_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_Tg
  transfrom from the global(init) frame to the local(start) frame
  *********************************************************************************/

  //绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
  float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
  float y1 = imuShiftFromStartYCur;
  float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;

  //绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  //绕z轴旋转(-imuRollStart)，即Rz(pitch).inverse
  imuShiftFromStartXCur = 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_Vg
    transfrom from the global(init) frame to the local(start) frame
  *********************************************************************************/
  
  //绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
  float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
  float y1 = imuVeloFromStartYCur;
  float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;

  //绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  //绕z轴旋转(-imuRollStart)，即Rz(pitch).inverse
  imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
  imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
  imuVeloFromStartZCur = z2;
}

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

在这里插入图片描述
我们现在已知了当前时刻的PRY角，那么可以构成当前时刻坐标系(curr坐标系)相对于世界坐标系(init)的变换：
R i n i t c u r r = R y R x R z R_{init_curr} = R_y R_x R_z Rinitc​urr​=Ry​Rx​Rz​
同样，已知了该sweep初始时刻的PRY角，可以构成世界坐标系init到该sweep的坐标变换，和上面畸变量变换类似：
R s t a r t _ i n i t = R i n i t _ s t a r t − 1 = R − z R − x R − y R_{start\_init} = R_{init\_start}^{-1} = R_{-z} R_{-x} R_{-y} Rstart_init​=Rinit_start−1​=R−z​R−x​R−y​
那么最终的变换为：
p s t a r t = R s t a r t _ i n i t ∗ R i n i t _ c u r r ∗ p c u r r p_{start} = R_{start\_init} * R_{init\_curr} * p_{curr} pstart​=Rstart_init​∗Rinit_curr​∗pcurr​
最后再加上上面计算出的由于加减速产生的位移畸变量，就得到了如下代码。

//将当前时刻curr坐标系下的的点云变换到世界坐标系init，然后在变换到当前帧的初始时刻start坐标系下
void TransformToStartIMU(PointType *p)
{
  /********************************************************************************
    Pinit = Ry * Rx * Rz * Pcurr
    transform 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 * Pinit
    transfrom 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*pi
  float 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->Y
    point.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/2
      if (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/2
      if (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位置:imuPointerFront
      while (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])*ratioFront
        imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
        imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
        imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

        imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
        imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
        imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
      }

      if (i == 0) {//如果是第一个点,记住点云起始位置的速度，位移，欧拉角
        imuRollStart = imuRollCur;
        imuPitchStart = imuPitchCur;
        imuYawStart = imuYawCur;

        imuVeloXStart = imuVeloXCur;
        imuVeloYStart = imuVeloYCur;
        imuVeloZStart = imuVeloZCur;

        imuShiftXStart = imuShiftXCur;
        imuShiftYStart = imuShiftYCur;
        imuShiftZStart = imuShiftZCur;
      } else {
        ShiftToStartIMU(pointTime);
        VeloToStartIMU();
        TransformToStartIMU(&point);//将当前时刻curr坐标系下的的点云变换到世界坐标系init，然后在变换到当前帧的初始时刻start坐标系下
      }
    }
    laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器
  }

4.2 计算曲率

这一部分对应论文中提到的曲率计算公式：
c = 1 ∣ S ∣ ⋅ ∣ ∣ X ( k , i ) L ∣ ∣ ∣ ∣ ∑ j ∈ S , j ≠ i ( X ( k , i ) L − X ( k , j ) L ) ∣ ∣ c = \frac{1}{|S|·||X_{(k,i)}^L||} ||\sum _{j \in S, j\ne i} (X_{(k,i)}^L - X_{(k,j)}^L)|| c=∣S∣⋅∣∣X(k,i)L​∣∣1​∣∣j∈S,j=i∑​(X(k,i)L​−X(k,j)L​)∣∣
X ( k , i ) L X_{(k,i)}^L X(k,i)L​是第i个点云的位置， X ( k , j ) L X_{(k,j)}^L X(k,j)L​是 X ( k , i ) L X_{(k,i)}^L X(k,i)L​左右两边各5个点，注意它们两个都是矢量，那么 ( X ( k , i ) L − X ( k , j ) L ) (X_{(k,i)}^L - X_{(k,j)}^L) (X(k,i)L​−X(k,j)L​)就是一个 X ( k , i ) L X_{(k,i)}^L X(k,i)L​指向 X ( k , j ) L X_{(k,j)}^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-5
  scanStartInd[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++) {//与后一个点差值，所以减6
    float 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)中的B
    if (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/6
      int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;
      //六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
      int 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，也就是雷达里程计部分

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/81494.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

回归预测 | MATLAB实现TSO-SVM金枪鱼群算法优化支持向量机多输入单输出回归预测（多指标，多图）

回归预测 | MATLAB实现TSO-SVM金枪鱼群算法优化支持向量机多输入单输出回归预测（多指标，多图）

回归预测 | MATLAB实现TSO-SVM金枪鱼群算法优化支持向量机多输入单输出回归预测&#xff08;多指标&#xff0c;多图&#xff09; 目录 回归预测 | MATLAB实现TSO-SVM金枪鱼群算法优化支持向量机多输入单输出回归预测&#xff08;多指标&#xff0c;多图&#xff09;效果一览基…
阅读更多...
springcloud3 hystrix实现服务监控显示3(了解)

springcloud3 hystrix实现服务监控显示3(了解)

一 hystrix的服务监控调用 1.1 hystrix的服务监控调用 hystrix提供了准实时的监控调用&#xff08;hystrix dashbord&#xff09;&#xff0c;Hystrix 会持续的记录所有通过hystrix发送的请求的执行信息&#xff0c;并以统计报表和图形的形式展示给用户&#xff0c;包括每秒执…
阅读更多...
不是说嵌入式是风口吗，那为什么工作还那么难找？

不是说嵌入式是风口吗，那为什么工作还那么难找？

最近确实有很多媒体、机构渲染嵌入式可以拿高薪&#xff0c;这在行业内也是事实&#xff0c;但前提是你有足够的竞争力&#xff0c;真的懂嵌入式。 时至今日&#xff0c;能做嵌入式程序开发的人其实相当常见&#xff0c;尤其是随着树莓派、Arduino等开发板的普及&#xff0c;甚…
阅读更多...
【Java】智慧工地SaaS平台源码：AI/云计算/物联网/智慧监管

【Java】智慧工地SaaS平台源码：AI/云计算/物联网/智慧监管

智慧工地是指运用信息化手段&#xff0c;围绕施工过程管理&#xff0c;建立互联协同、智能生产、科学管理的施工项目信息化生态圈&#xff0c;并将此数据在虚拟现实环境下与物联网采集到的工程信息进行数据挖掘分析&#xff0c;提供过程趋势预测及专家预案&#xff0c;实现工程…
阅读更多...
互联网发展历程：保护与隔离，防火墙的安全壁垒

互联网发展历程：保护与隔离，防火墙的安全壁垒

互联网的快速发展&#xff0c;不仅带来了便利和连接&#xff0c;也引发了越来越多的安全威胁。在数字时代&#xff0c;保护数据和网络安全变得尤为重要。然而&#xff0c;在早期的网络中&#xff0c;安全问题常常让人担忧。 安全问题的困扰&#xff1a;网络威胁日益增加 随着互…
阅读更多...
分布式websocket解决方案

分布式websocket解决方案

1、websocket问题由来 websocket基础请自行学习，本文章是解决在分布式环境下websocket通讯问题。 在单体环境下，所有web客户端都是连接到某一个微服务上，这样消息都是到达统一服务端，并且也是由一个服务端进行响应，所以不会出现问题。 但是在分布式环境下，我们很容易发现…
阅读更多...
Postgresql源码（112）plpgsql执行sql时变量何时替换为值

Postgresql源码（112）plpgsql执行sql时变量何时替换为值

相关 《Postgresql源码&#xff08;41&#xff09;plpgsql函数编译执行流程分析》 《Postgresql源码&#xff08;46&#xff09;plpgsql中的变量类型及对应关系》 《Postgresql源码&#xff08;49&#xff09;plpgsql函数编译执行流程分析总结》 《Postgresql源码&#xff08;5…
阅读更多...
Android AppCompatActivity标题栏操作

Android AppCompatActivity标题栏操作

使用 AndroidStudio 新建的工程默认用 AppCompatActivity &#xff0c;是带标题栏的。 记录下 修改标题栏名称 和 隐藏标题栏 的方法。 修改标题栏名称 Override protected void onCreate(Bundle savedInstanceState) {super.onCreate(savedInstanceState);setContentView(R…
阅读更多...
Eureka注册中心

Eureka注册中心

全部流程 注册服务中心 添加maven依赖 <!--引用注册中心--> <dependency><groupId>org.springframework.cloud</groupId><artifactId>spring-cloud-starter-netflix-eureka-server</artifactId> </dependency> 配置Eureka 因为自…
阅读更多...
W6100-EVB-PICO 做UDP Client 进行数据回环测试（八）

W6100-EVB-PICO 做UDP Client 进行数据回环测试（八）

前言 上一章我们用开发板作为UDP Server进行数据回环测试&#xff0c;本章我们让我们的开发板作为UDP Client进行数据回环测试。 连接方式 使开发板和我们的电脑处于同一网段&#xff1a; 开发板通过交叉线直连主机开发板和主机都接在路由器LAN口 测试工具 网路调试工具&a…
阅读更多...
prompt-engineering-note(面向开发者的ChatGPT提问工程学习笔记)

prompt-engineering-note(面向开发者的ChatGPT提问工程学习笔记)

介绍&#xff1a; ChatGPT Prompt Engineering Learning Notesfor Developers (面向开发者的ChatGPT提问工程学习笔记) 课程简单介绍了语言模型的工作原理&#xff0c;提供了最佳的提示工程实践&#xff0c;并展示了如何将语言模型 API 应用于各种任务的应用程序中。 此外&am…
阅读更多...
idea gerrit 插件使用指引

idea gerrit 插件使用指引

IDEA安装gerrit插件 在线安装&#xff08;推荐&#xff09; 直接搜索gerrit&#xff0c;安装即可离线安装 可以到github下载离线包&#xff1a;https://github.com/uwolfer/gerrit-intellij-plugin/releases&#xff0c;不过可能会有版本不兼容问题&#xff0c;还是推荐在线安装…
阅读更多...
CSAPP Lab2:Bomb Lab

CSAPP Lab2:Bomb Lab

说明 6关卡&#xff0c;每个关卡需要输入相应的内容&#xff0c;通过逆向工程来获取对应关卡的通过条件 准备工作 环境 需要用到gdb调试器 apt-get install gdb系统: Ubuntu 22.04 本实验会用到的gdb调试器的指令如下 r或者 run或者run filename 运行程序,run filename就…
阅读更多...
AIGC绘画：基于Stable Diffusion进行AI绘图

AIGC绘画：基于Stable Diffusion进行AI绘图

文章目录 AIGC深度学习模型绘画系统stable diffusion简介stable diffusion应用现状在线网站云端部署本地部署Stable Diffusion AIGC深度学习模型绘画系统 stable diffusion简介 Stable Diffusion是2022年发布的深度学习文本到图像生成模型&#xff0c;它主要用于根据文本的描述…
阅读更多...
基于互斥锁的生产者消费者模型

基于互斥锁的生产者消费者模型

文章目录 生产者消费者 定义代码实现 / 思路完整代码执行逻辑 / 思路 局部具体分析model.ccfunc&#xff08;消费者线程&#xff09; 执行结果 生产者消费者 定义 生产者消费者模型 是一种常用的 并发编程模型 &#xff0c;用于解决多线程或多进程环境下的协作问题。该模型包含…
阅读更多...
Segment Anything论文阅读笔记

Segment Anything论文阅读笔记

Segment Anything论文阅读笔记 1. Segment Anything论文基本信息2. Segment Anything论文阅读2.1 第一遍阅读 Segment Anything2.2. 第二遍阅读 Segment Anything2.2.1. Segment Anything中相关的图表 1. Segment Anything论文基本信息 论文地址https://arxiv.org/abs/2304.02…
阅读更多...
基于VUE3+Layui从头搭建通用后台管理系统（前端篇）八：自定义组件封装上

基于VUE3+Layui从头搭建通用后台管理系统（前端篇）八：自定义组件封装上

一、本章内容 本章实现一些自定义组件的封装，包括数据字典组件的封装、下拉列表组件封装、复选框单选框组件封装、单选框组件封装、文件上传组件封装、级联选择组件封装、富文本组件封装等。 1. 详细课程地址： 待发布 2. 源码下载地址： 待发布 二、界面预览 ![在这里插入图…
阅读更多...
数据可视化diff工具jsondiffpatch使用学习

数据可视化diff工具jsondiffpatch使用学习

1.jsondiffpatch 简介 jsondiffpatch 是一个用于比较和生成 JSON 数据差异的 JavaScript 库。它可以将两个 JSON 对象进行比较&#xff0c;并生成一个描述它们之间差异的 JSON 对象。这个差异对象可以用于多种用途&#xff0c;例如&#xff1a; 生成可视化的差异报告应用差异…
阅读更多...
【TypeScript】tsc -v 报错 —— 在此系统上禁止运行脚本

【TypeScript】tsc -v 报错 —— 在此系统上禁止运行脚本

在 VS Code 终端中执行 tsc -v &#xff0c;报错 —— 在此系统上禁止运行脚本 然后 windows x &#xff0c;打开终端管理员&#xff0c;出现同样的问题 解决方法&#xff1a; 终端&#xff08;管理员&#xff09;执行以下命令&#xff1a; 出现 RemoteSigned 则代表更改成功…
阅读更多...
SpringBoot引入外部jar打包失败解决，SpringBoot手动引入jar打包war后报错问题

SpringBoot引入外部jar打包失败解决，SpringBoot手动引入jar打包war后报错问题

前言 使用外部手动添加的jar到项目&#xff0c;打包时出现jar找不到问题解决 处理 例如项目结构如下 引入方式换成这种 <!-- 除了一下这两种引入外部jar&#xff0c;还是可以将外部jar包添加到maven中&#xff08;百度查&#xff09;--><!-- pdf转word --><…
阅读更多...
最新文章

Copyright @ 2022~2023