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

推荐学习3D视觉工坊推出的课程（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

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

1. 将点云从雷达坐标系转换到相机坐标系(是坐标系转换，不是坐标变换)

2. 计算每个点的俯仰角，对应于上图的ω角，用于计算scanID

3. 如果收到IMU数据，使用IMU进行插值

4. 计算了每个点由于加减速产生的位移和速度畸变

5. 将每个点变换到当前sweep的初始时刻里程计的start时刻坐标系

==要进行说明的部分如下：==

点云强度保存：

point.intensity = scanID + scanPeriod * relTime;

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

推荐学习3D视觉工坊推出的课程（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

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，就是用下式进行插值：

文章中的式子如下：本质上是一样的：

第一个点云的数值都保存在以Start结尾的变量中。

ShiftToStartIMU(pointTime)函数：

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

推荐学习3D视觉工坊推出的课程（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

然后将畸变量由全局坐标系w变换到当前sweep的初始时刻坐标系(start)，而现在我们已知的量RPY角所构成的变换为：

所以这里需要的变换为：

这样得到了如下代码。

//计算局部(start)坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变void ShiftToStartIMU(float pointTime){  //计算相对于第一个点由于加减速产生的畸变位移(全局(w)坐标系下畸变位移量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(w) 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(){  //计算相对于第一个点由于加减速产生的畸变速度(全局(w)坐标系下畸变速度增量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(w) 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中的所有点云去除由于加减速产生的运动畸变，然后都变换到里程计坐标系w下的初始时刻。

我们现在已知当前时刻的PRY角，那么可以构成当前时刻坐标系(curr坐标系)相对于世界坐标系(w)的变换：

同样，已知了该sweep初始时刻的PRY角，可以构成世界坐标系w到该sweep的坐标变换，和上面畸变量变换类似：

表示curr坐标系下对应于imustart姿态静止扫描得到的点，那么最终的变换为：

最后再加上上面计算出的由于加减速产生的位移畸变量，就得到了如下代码。

这个过程完成的工作应该如下图所示：

得到了一个去畸变后的点云结果，效果相当于：得到了，在点云扫描开始位置静止扫描，得到的点云位置，也就是说没有对点云坐标系进行变换，第i个点云依然处在里程计坐标系下的curr时刻坐标系中，这一步的操作只是对点云的位置进行调整，然后这一帧点云扫描过程中，按照这个调整后位置送入传感器中。

还有一种解释是说：这个函数操作完了之后，current时刻的坐标系变成了：仍然以current时刻为原点，坐标轴的姿态与初始时刻start的姿态相同，我觉得这个理解也可以。

//将当前时刻curr坐标系下的的点云变换到世界坐标系w，然后在变换到当前帧的初始时刻start坐标系下void TransformToStartIMU(PointType *p){  /********************************************************************************    Pinit = Ry * Rx * Rz * Pcurr    transform current camara(curr) frame point to the global(w) 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(w) 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坐标系下的的点云变换到世界坐标系w，然后在变换到当前帧的初始时刻start坐标系下      }    }    laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器  }

这一部分对应论文中提到的曲率计算公式：

是第i个点云的位置，是左右两边各5个点，注意它们两个都是==矢量==，那么就是一个指向的向量。

如上图所示，如果一个点是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;

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

• 所选边缘点或平面点的个数不能超过子区域的最大值-------保证取点均匀，这一点下面会讲到

• 它周围的点都没有被选中-------保证点不过于密集

• 它不能位于与激光束大致平行的平面上，也不能位于遮挡区域的边界上-------剔除一些不好的点(下面的操作)

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

if (diff > 0.1)

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

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

推荐学习3D视觉工坊推出的课程（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化

下面代码中的这个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;    }  }

这部分与论文中说的有点不一样，论文中说将当前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;  }

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

• /velodyne)cloud_2：经过处理后的所有点云(按照scanID排序的有序点云)

• /laser_cloud_sharp：edge point特征点，一共162=192个

• /laser_cloud_less_sharp：less edge point特征点，一共1620=1920个

• /laser_cloud_flat：planar point特征点，一共164=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，我感觉这个代码还是很好懂得，坐标变换部分也不是很复杂，对照论文慢慢看就能看懂。

标签：