loam算法_loam代码解析1

作为一名激光slam菜鸟，涉及到的首个开源算法就是loam。之前阅读过代码，时间久了容易记不清，现在记录下来以备随时查阅，也可为入门级菜鸟提供些许参考。

本人在阅读源码过程中参考了知乎众多loam解析文章，在此表示感谢。跟知乎上loam相关解析文章一样，本文按照loam代码文件分四部分介绍，各部分解析以求尽量详细，如有理解错误，欢迎指正。

本章首先介绍代码的第一部分scanRegistration。

1. 从main()函数看起：

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

   ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>      
                      ("/rslidar_points", 2, laserCloudHandler); //订阅雷达数据

   ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> 
                                   ("/imu/data", 50, imuHandler); //订阅imu数据

   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); //发布imu处理数据
   
   ros::spin();
   return 0;
}

main函数首先注册scanRegistration节点，然后订阅雷达话题/rslidar_points和惯导话题/imu/data，雷达和imu的话题名根据实际发布情况而定。接着发布按线分类后的点云，以及边界特征点和面特征点点云和处理后的惯导数据。

2. 订阅雷达话题后进入回调函数laserCloudHandler：

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
  /*延迟systemDelay帧数据后读取*/
  if (!systemInited) {                                    
    systemInitCount++;
    if (systemInitCount >= systemDelay) {
      systemInited = true;
    }
    return;
  }
  //N_SCANS(16)保存每一线的起始点和终止点索引
  std::vector<int> scanStartInd(N_SCANS, 0);               
  std::vector<int> scanEndInd(N_SCANS, 0);
  double timeScanCur = laserCloudMsg->header.stamp.toSec();     //帧时间戳

回调函数首先延迟systemDelay帧数据，然后定义两个vector容器分别存放N_SCANS线数据的始末点索引，并定义当前帧时间戳变量。

 /* 剔除异常点并计算起始和终止点的旋转角*/
 pcl::PointCloud<pcl::PointXYZ> laserCloudIn;      //将ros消息转换为pcl点云
 pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);            
 std::vector<int> indices;
 pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); //剔除异常点

 int cloudSize = laserCloudIn.points.size();
 float startOri = -atan2(laserCloudIn.points[0].y, 
                     laserCloudIn.points[0].x);                 //当前帧起始角
 float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,                   
                  laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; //当前帧终止角
 if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  } 

上段代码将ros中点云*laserCloudMsg转换为pcl点云laserCloudIn，然后去除 laserCloudIn中的异常点。startOri和endOri分别为当前帧点云的第一个point和最后一个point的旋转角，两者差值约束在Pi-3Pi范围内。

bool halfPassed = false;        
int count = cloudSize;         
PointType point;              //  pcl::PointXYZI
std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);//存储16线点云的vector
for (int i = 0; i < cloudSize; i++) {           
  point.x = laserCloudIn.points[i].y;          
  point.y = laserCloudIn.points[i].z;
  point.z = laserCloudIn.points[i].x;
  float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;   
  int scanID;
  int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); //计算激光点垂直角
  if (roundedAngle > 0){                                 
    scanID = roundedAngle;                               //该点对应的scanID
  }
  else {
    scanID = roundedAngle + (N_SCANS - 1);             
  }
  if (scanID > (N_SCANS - 1) || scanID < 0 ){      //scanID超出范围则丢弃该点
    count--; 
    continue; 
  }

  float ori = -atan2(point.x, point.z);              //计算点的水平旋转角
  if (!halfPassed) {                  
    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;
    if (ori < endOri - M_PI * 3 / 2) {
      ori += 2 * M_PI;
    } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
    } 
  }
  float relTime = (ori - startOri) / (endOri - startOri); //该点在一帧数据中的相对时间  0～1
  point.intensity = scanID + scanPeriod * relTime;      //scanID + 0.1 * (0~1)   

该部分首先定义halfPassed变量表示当前点是否过半，主要用于计算当前点角度确保处于startOri和endOri之间。容器laserCloudScans 用于存储N_SCANS线的数据。接着逐一计算当前帧每个点的垂直角并对应成0～N_SCANS-1线的scanID，剔除超出ID范围的激光点。然后计算该点的水平旋转角，和在当前帧中的相对时间，最后将scanID + scanPeriod * relTime值存在point变量的intensity成员中,，以备后续使用。

3. 因为后续程序涉及到IMU数据处理，所以先看IMU的回调函数：

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);   
  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;

  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回调函数首先读取方向角四元素到orientation，然后转换成欧拉角roll/pitch/yaw，接着计算消除重力加速度影响的各个方向的线加速度，此时加速度是在IMU自身坐标系中。imuPointerLast作为imu各个参数数组的索引，循环累加，然后将当前imu数据及对应的时刻保存在各自数组中。最后调用 AccumulateIMUShift()函数，该函数代码如下：

void AccumulateIMUShift() 
{
  float roll = imuRoll[imuPointerLast];   
  float pitch = imuPitch[imuPointerLast];
  float yaw = imuYaw[imuPointerLast];
  float accX = imuAccX[imuPointerLast]; 
  float accY = imuAccY[imuPointerLast];
  float accZ = imuAccZ[imuPointerLast];

  //按照z/x/y轴旋转  将加速度由惯导坐标系旋转到局部地球坐标系         
  float x1 = cos(roll) * accX - sin(roll) * accY;
  float y1 = sin(roll) * accX + cos(roll) * accY;
  float z1 = accZ;

  float x2 = x1;
  float y2 = cos(pitch) * y1 - sin(pitch) * z1;
  float z2 = sin(pitch) * y1 + cos(pitch) * z1;

  accX = cos(yaw) * x2 + sin(yaw) * z2;
  accY = y2;
  accZ = -sin(yaw) * x2 + cos(yaw) * z2;

  int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;    
  double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; //前一时刻与后一时刻时间差
  if (timeDiff < scanPeriod) {
    imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff   
             + accX * timeDiff * timeDiff / 2;   //计算位移，匀加速运动  s=v*t+0.5*a*t^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;
  }
}

AccumulateIMUShift() 函数首先读取IMU回调函数中存储的各变量信息，然后通过绕z/x/y轴分别旋转将加速度从IMU坐标系转换至局部地球坐标系。timeDiff表示相邻两个惯导数据的时间差，假设在这个时间差内载体做匀加速运动，根据前一时刻三个坐标轴方向位移imuShift和速度imuVelo求当前时刻位移和速度信息。此时IMU信息转换到了局部地球坐标系。

4. 至此IMU数据处理完，接着回到雷达回调函数对应位置：

//后续位姿插值时假设机器人匀速运动，因此先消除激光点IMU加速度影响
if (imuPointerLast >= 0) {                    //有imu数据      
  float pointTime = relTime * scanPeriod;     //当前激光点偏移时间
  while (imuPointerFront != imuPointerLast) {  //时间同步  
    if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
      break;
    }
    imuPointerFront = (imuPointerFront + 1) % imuQueLength;
  }
  // 当前激光点的time大于找到的对应惯导数据的time,则惯导数据采用当前值
  if (timeScanCur + pointTime > imuTime[imuPointerFront]) {      
    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参数                                   
    int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
    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[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 {   // 当不为初始点时，去除IMU加速度影响
    ShiftToStartIMU(pointTime);
    VeloToStartIMU();
    TransformToStartIMU(&point);
  }
}

当imuPointerLast>=0即存在惯导数据，则执行上段代码，用于去除激光点IMU加速度影响。pointTime表示当前点在该帧数据中的偏移时刻，其中第一个点的时刻为0，最后一个点为scanPeriod。首先进行当前激光点和imu数据的时间同步，即在imu数组中找到与当前激光点时间一致的索引imuPointFront。imuPointerLast 表示imu循环数组最新索引，如果当前激光点的绝对时间大于imuPointFront对应imu时间，则将imuPointFront向imuPointerLast靠拢。如果激光点时间比imu最新数据对应的时间还大，即imuPointFront=imuPointerLast，则与之对应的imu就选最新值。这就是if (timeScanCur + pointTime > imuTime[imuPointerFront]) 所对应的情况，否则插值imuPointerBack(=imuPointerFront-1)与imuPointerFront得到激光点对应的imu数据。如果当前激光点为该帧第一个点，则赋值相关变量，否则依次调用ShiftToStartIMU(pointTime)， VeloToStartIMU()， TransformToStartIMU(&point)三个函数，下面分析各函数功能：

void ShiftToStartIMU(float pointTime)
{
  //IMU加速度造成的匀速运动位移偏差
  imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;       
  imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
  imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;

  float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur; 
  float y1 = imuShiftFromStartYCur;
  float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
 
  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  //位移偏差由局部地球坐标系旋转到初始点位姿坐标系
  imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;              
  imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
  imuShiftFromStartZCur = z2;
}

ShiftToStartIMU()函数首先求当前时刻imu位移与匀速运动假设情况下当前时刻imu位移之差imuShiftFromStartXCur，将世界坐标系下的该差值旋转到当前帧初始点坐标系。

//将当前时刻速度插值由局部世界坐标系旋转至初始点位姿坐标系
void VeloToStartIMU()
{
  imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; //世界坐标系下的速度差
  imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
  imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;

  float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
  float y1 = imuVeloFromStartYCur;
  float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;

  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
  imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
  imuVeloFromStartZCur = z2;
}

匀速运动情况下当前时刻速度应该与初始点速度一致，将由imu加速度造成的速度差imuVeloFromStartXCur由局部地球坐标系旋转到初始点坐标系。

void TransformToStartIMU(PointType *p)             
{
 //当前激光点旋转到局部地球坐标系
 float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y; 
 float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
 float z1 = p->z;

 float x2 = x1;
 float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
 float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;

 float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
 float y3 = y2;
 float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;

 //局部地球坐标系下激光点旋转至该帧初始点坐标系并消除imu加速度造成的位移偏差
 float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3; 
 float y4 = y3;
 float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;

 float x5 = x4;
 float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
 float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;

 p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;//加上IMU位置运动补偿 
 p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
 p->z = z5 + imuShiftFromStartZCur;
}

TransformToStartIMU()函数首先将当前激光点根据当前imu姿态角旋转到局部地球坐标系，然后由局部坐标系旋转至初始点坐标系方向，并加上imu计算得到的位移偏差，最终激光点转换到匀速运动假设情况下当前帧初始点坐标系，接着将激光点存入laserCloudScans对应的scanID中，从而完成按线分类工作。按照线号从小到大，通过laserCloud指针将N_SCANS线的激光数据组成pcl点云。

5. 针对按线分类后的点云，通过激光点左右各5个点进行曲率计算：

 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 = 。。。
   float diffZ = 。。。
   cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
   cloudSortInd[i] = i;
   cloudNeighborPicked[i] = 0;
   cloudLabel[i] = 0;
   //计算每一线的起始点和终止点  即扣除每一线始末各5个点
   if (int(laserCloud->points[i].intensity) != scanCount) {
     scanCount = int(laserCloud->points[i].intensity);
     if (scanCount > 0 && scanCount < N_SCANS) {
       scanStartInd[scanCount] = i + 5;         
       scanEndInd[scanCount - 1] = i - 5;      
     }
   }
 }
 scanStartInd[0] = 5;              //第一条线的起始位置
 scanEndInd.back() = cloudSize - 5;      //最后一条线的终止位置

曲率计算方法为求相邻10个点与当前点i在x/y/z方向的坐标差，并求平方和，存放在cloudCurvature[i]。cloudSortInd[i]表示当前点在点云中索引，cloudNeighborPicked[i]用于标记是否可作为特征点，cloudLabel[i]用于标记特征点为边界线还是平面特征点。通过当前点intensity中存放的scanID信息，求每一个线的起始点和终止点索引，其中第一线的起始索引和最后线的终止索引直接赋值即可。

6. 判断当前点和周边点是否可以作为特征点，判断依据见论文。

for (int i = 5; i < cloudSize - 6; i++) {
  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;           
  if (diff > 0.1) {   //前后两点距离的平方大于阈值  论文中b情况  可能存在遮挡    
    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) {    //当前点可能被遮挡
      //以短边i+1构成等腰三角形，diffX/diffY/diffZ平方根表示等腰三角形的底           
      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;
      //实际表示i与i+1夹角小于5.732度 sin(5.732) ~= 0.1 认为被遮挡
      if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {    
        cloudNeighborPicked[i - 5] = 1;    //当前点i被遮挡，则i及其往前5个点都不能作为特征点
        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; //当前点后一点i+1被遮挡，则i+1及其后5个点不能作为特征点
        cloudNeighborPicked[i + 2] = 1;
        cloudNeighborPicked[i + 3] = 1;
        cloudNeighborPicked[i + 4] = 1;
        cloudNeighborPicked[i + 5] = 1;
        cloudNeighborPicked[i + 6] = 1;
      }
    }
  }
  //针对论文fig4中(a)情况 
  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;
  if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
    cloudNeighborPicked[i] = 1;
  }
}

剔除点云前后各5个点，逐个计算当前激光点与后一激光点的距离平方，如果距离方diff大于阈值，则表示可能存在遮挡。进一步计算两个激光点的深度信息depth1和depth2，如果depth1>depth2，则当前点可能被遮挡。进一步以短边depth2构成等腰三角形并计算三角形的底边长，根据

,求得当前激光点与后一点的夹角，如果夹角小于阈值，则认为存在遮挡，从而被遮挡的当前激光点及其往前5个点不可作为特征点。同理，如果depth1<depth2则后一点可能被遮挡，计算方法同上。考虑完遮挡点情形后，再考虑激光入射角，即论文fig4(a)所示情况。diff2表示当前点与前一点距离方，diff表示与后一点距离方，diss为当前点深度平方，如果当前点与前后两点距离都大于c倍当前点深度，则可认为当前点入射角与物理表面平行，当前点标记为不可作特征点。

7. 前述工作得到了匀速运动假设后的按线分类的点云，且标记了不可作为特征点的激光点，现在进行特征点分类。

for (int i = 0; i < N_SCANS; i++) {  //按线处理
  pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);    //用于滤波降低数据量
  //每个线分6段，sp/ep分别为各段起始和终止位置
  for (int j = 0; j < 6; j++) {
    int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 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;
        }
      }
    }

特征点按照每一线进行选取，首先将该线分成6段，根据每一线的始末激光点索引计算6段的始末点索引，然后每一段内部激光点按照曲率升序排列。

//从每一段最大曲率(每一段末尾)处往前判断，用于确定特征线点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
  int ind = cloudSortInd[k];   //排序后的点在点云中的索引
  if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1) {//没有被标记为不可取特征点且曲率大于0.1
    largestPickedNum++;      
    if (largestPickedNum <= 2) {             //2个特征线点
      cloudLabel[ind] = 2;             //每个点标记为2
      cornerPointsSharp.push_back(laserCloud->points[ind]); //保存sharp点云中 
      cornerPointsLessSharp.push_back(laserCloud->points[ind]);//同时保存在lessSharp      
    } else if (largestPickedNum <= 20) { //20个lesssharp点(包括前面2个标记为2的点)
      cloudLabel[ind] = 1;            //后18个lesssharp特征线点，标记为1
      cornerPointsLessSharp.push_back(laserCloud->points[ind]); //保存在lessSharp
    } else {
      break;
    }
    cloudNeighborPicked[ind] = 1;    //该点被选为特征点后，标记为不可用
    //对ind点周围的点是否能作为特征点进行判断   除非距离大于阈值，否则前后各五个点不能作为特征点
    for (int l = 1; l <= 5; l++) {      //后5个点   
      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--) {     //前5个点
      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;
    }
  }
}

上段代码根据每段曲率由大到小筛选边界线特征点，如果该点没有被标记为不可取特征点且曲率大于阈值，则曲率最大的前2个点标记为SharpPoints，保存在cornerPointsSharp和cornerPointsLessSharp中，接着的18个激光点标记为LessSharpPoints，保存在cornerPointsLessSharp中。当前点被选为特征点后，对其前后各5个点进行标记，除非相邻点的距离大于阈值，否则标记为不可作特征点。

//从每一段曲率最小(前端)开始查找    用于确定平面点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
  int ind = cloudSortInd[k];
  if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) {//没有被标记为不可取特征点  且曲率小于0.1
    cloudLabel[ind] = -1;                                  
    surfPointsFlat.push_back(laserCloud->points[ind]);      //4个平面点  标记为-1     
    smallestPickedNum++;
    if (smallestPickedNum >= 4) {           
      break;
    }
    cloudNeighborPicked[ind] = 1; 
    // 判断附近点是否需要标记为不可用特征点    
    。。。。。
    。。。。。
  }
}
for (int k = sp; k <= ep; k++) {//没被标记过的点和前述标记的平面点，作为lessFlat平面点
  if (cloudLabel[k] <= 0) {           
    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
  }
}

对于平面特征点，通过每一段线中曲率小端开始查找。如果当前点没有被标记且曲率小于阈值，则该点为FlatPoints，保存在surfPointsFlat中，平面特征点每段提取4个。同样的，需要判断前后各5个点是否需要标记为不可用特征点。最后，除了被标记过的点(本身不能作为特征点以及被标记为边界线的特征点)，其它点都作为lessFlatPoints存储到surfPointsLessFlatScan中，并进一步滤波降采样(lessFlat点太多),，最后存储到surfPointsLessFlat中。

最后，将各类消息发布出去。主要包括按线分类后的当前帧点云laserCloud，边界线上的sharp特征点/lessSharp特征点，平面上的flat特征点/lessFlat特征点，以及imu处理后的当前帧起始点和终止点的姿态角/转换到当前帧初始点坐标系的终止点位移偏差和速度偏差。

小结一下本章的主要内容，scanRegistration主要完成点云的按线分类，当有imu数据时完成对应激光点的由加速度造成的匀速运动假设参数补偿，接着完成激光点曲率计算并提取特征点，最后发布处理结果。