LOAM学习-代码解析（二）点云数据配准 scanRegistration

前言

前一篇文章LOAM学习-代码解析（一）点云数据配准，对点云数据配准的源代码解析进行了一半，这里继续上一篇的工作。

LOAM代码(带中文注释)的地址：https://github.com/cuitaixiang/LOAM_NOTED

LOAM代码(带中文注释)的百度网盘链接：https://pan.baidu.com/s/1tVSNBxNQrxKJyd5c9mWFWw 提取码: wwxr

LOAM论文的百度网盘链接： https://pan.baidu.com/s/10ahqg8O3G2-xOt9QZ1GuEQ 提取码: hnri

LOAM流程：

点云数据配准-scanRegistration

点云数据配准的函数为scanRegistration.cpp，上一篇文章只解析到位移与速度校正，这里从接收点云数据开始。

接收点云数据

velodyne激光雷达坐标系安装为x轴向前，y轴向左，z轴向上的右手坐标系。这一段的代码非常长，需要一部分一部分的进行解析。

丢弃前20个点云数据，这里不知道为什么要这么操作，待补充。

if (!systemInited) {
     //丢弃前20个点云数据
    systemInitCount++;
    if (systemInitCount >= systemDelay) {
     
      systemInited = true;
    }
    return;
}

记录每个scan（一次扫描）有曲率的点的开始和结束索引，N_SCANS为激光雷达线数。

std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);

当前点云时间。补充一点c++的知识，结构体用’.’，结构体指针用箭头’->’。再补充一点ros的知识，msgs_header.stamp.sec就可以获取当前的时间，单位为秒。

//  模板
pcl::PointXYZ::PointXYZ ( float_x,
                  float_y,
                  float_z
                    ) 

double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;

ros消息转换成pcl数据存放。在ROS中表示点云的数据结构有： sensor_msgs::PointCloud、sensor_msgs::PointCloud2、pcl::PointCloud。

• 关于sensor_msgs::PointCloud2和 pcl::PointCloud之间的转换使用pcl::fromROSMsg和pcl::toROSMsg
• sensor_msgs::PointCloud 和 sensor_msgs::PointCloud2之间的转换使用sensor_msgs::convertPointCloud2ToPointCloud 和sensor_msgs::convertPointCloudToPointCloud2

pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

移除空点，使用pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices)，函数有三个参数，分别为输入点云，输出点云及对应保留的索引。

std::vector<int> indices;
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

计算点云点的数量

int cloudSize = laserCloudIn.points.size();

计算lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转，这里的正负号主要取决于安装激光雷达后扫描方向。atan2函数返回 y/x 的反正切值，以弧度表示，取值范围为(-π,π]。如上图所示，tan(θ) = y/x，θ = atan2(y, x)。

float startOri = -atan2(laserCloudIn.points[0].y, 
						laserCloudIn.points[0].x);

计算lidar scan结束点的旋转角，加2$\pi$使点云旋转周期为2$\pi$。这里不是很理解为什么要加2$\pi$，难道是因为一次scan的扫描角度只有[-15°,+15°]，如果有人阅读到这里，可以留言解答于我，十分感谢。

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扫描线是否旋转过半，标志为halfPassed，count为点云数量，N_SCANS为激光雷达线数。

// 初始化
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);

过滤点云数据，只挑选[-15°,+15°]范围内的点，scanID属于[0,15]。lidar安装时坐标系为z轴向前，x轴向左的右手坐标系。不理解这里的交换坐标轴的目标，待补充。
补充一点c++知识，angle<0.0?-0.5:+0.5表示如果小于零则减0.5，大于零则加0.5。

//坐标轴交换，velodyne lidar的坐标系也转换到z轴向前，x轴向左的右手坐标系
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);

根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿。确保$\pi$/2 < ori - startOri < 3$\pi$/2。不理解这里为什么要控制角度在[$\pi$/2，3$\pi$/2]之间，难道是要判断点在旋转未过半区域？ 确保-3$\pi$/2 < ori - endOri < $\pi$/2

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);

点强度=线号+点相对时间，即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间。匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间。不理解这里的点强度计算。

//扫描周期, velodyne频率10Hz，周期0.1s,scanPeriod=0.1
point.intensity = scanID + scanPeriod * relTime;

如果收到IMU数据,使用IMU矫正点云畸变

点时间=点云时间+周期时间，计算点的点云时间

float pointTime = relTime * scanPeriod; //计算点的点云时间

寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront。如果小于IMU的时间，则退出。若大于则imu点加1。

while (imuPointerFront != imuPointerLast) {
     
  if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
     
    break;
  }
  imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}

如果没找到，此时imuPointerFront==imtPointerLast，只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角。

如果找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角。

$$R_f=\frac{T_{scan-cur}+T_{point}-T_{imu-point-back}}{T_{imu-point-front}-T_{imu-point-back}}$$

$$R_b=\frac{T_{imu-point-front}-T_{scan-cur}-T_{point}}{T_{imu-point-front}-T_{imu-point-back}}$$

$$angl{e}_{imu-\ast -cur}=angl{e}_{imu-\ast -front}{R}_f+angl{e}_{imu-\ast -back}{R}_b$$

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;
  //按时间距离计算权重分配比率,也即线性插值
  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);
}

将每个补偿矫正的点放入对应线号的容器

laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器

至此，每个点云的矫正完成。

接着，获得有效范围内的点的数量

//获得有效范围内的点的数量
cloudSize = count;

pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {
     //将所有的点按照线号从小到大放入一个容器
  *laserCloud += laserCloudScans[i];
}
int scanCount = -1;

特征点提取

LOAM中提取的特征点有两种：Edge Point和Planar Point。两种特征使用曲率来进行区分。曲率最大的为Edge Point，曲率最小的为Planar Point。计算每个点的曲率，使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过。论文中计算曲率的公式如下：

$$c=\frac{1}{\left|S\right|\Vert X_{(k,i)}^L\Vert }\Vert \sum _{j\in S,j\ne i}\left(X_{(k,i)}^L-X_{(k,j)}^L\right)\Vert$$

下列代码中计算曲率的公式如下：
$$\Delta x=\sum _{k=1}^{5}x(i-k)+\sum _{k=1}^{5}x(i+k)-10x(i)$$

$$\Delta y=\sum _{k=1}^{5}y(i-k)+\sum _{k=1}^{5}y(i+k)-10y(i)$$

$$\Delta z=\sum _{k=1}^{5}z(i-k)+\sum _{k=1}^{5}z(i+k)-10z(i)$$

$$c=\Delta x^2+\Delta y^2+\Delta z^2$$

去除一些不可靠的点，主要包括论文里提到的两种情况，容易被斜面挡住的点以及离群点，如下
针对激光光束入射角与障碍物表面夹角小的点，即（a）图所示的情况。$A$、$B$为距离大于阈值（代码里为0.1米）的两个相邻点，其与激光器$O$的连线就是该点的距离值。$\theta$为前后两点连线的夹角，值越小表$AB$两点的连线越陡。当$\theta$值小于某一个阈值就认为该点是不可靠的。并且将前方或后方的5个点也置成不可选的点（即不会选为特征点）。去除公式如下
$$\Delta x=x(i+1)-x(i)$$

$$\Delta y=y(i+1)-y(i)$$

$$\Delta z=z(i+1)-z(i)$$

$$c=\Delta x^2+\Delta y^2+\Delta z^2$$

如果曲率$c$大于0.1，则计算点的深度，即点与远点的距离，公式如下

$$d=\sqrt{x^2+y^2+z^2}$$

如果$d(i)>d(i+1)$，意味着前后两点的距离过远，作者采取的方法是将深度较大的点进行往原点拉回，则计算距离和曲率的公式变为

$$\Delta x=x(i+1)-x(i)\frac{d(i+1)}{d(i)}$$

$$\Delta y=y(i+1)-y(i)\frac{d(i+1)}{d(i)}$$

$$\Delta z=z(i+1)-z(i)\frac{d(i+1)}{d(i)}$$

代码里将边长比用作判断前后两点连线的夹角$\theta$。边长比也即是弧度值，若小于0.1（对应角度值5°），说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上。

$$若\frac{\sqrt{x^2+y^2+z^2}}{d(i+1)}<0.1，弃该点和前五个点$$

如果$d(i)<d(i+1)$，则公式变为

$$\Delta x=x(i+1)\frac{d(i+1)}{d(i)}-x(i)$$

$$\Delta y=y(i+1)\frac{d(i+1)}{d(i)}-y(i)$$

$$\Delta z=z(i+1)\frac{d(i+1)}{d(i)}-z(i)$$

$$若\frac{\sqrt{x^2+y^2+z^2}}{d(i)}<0.1，弃该点和后五个点$$

针对前后遮挡的情况，如（b）所示，是通过计算前后两点间距的平方是否大于该点距离值平方的万分之二来排除的,包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过。判断的依据是

$$diff>0.0002dis且diff2>0.0002dis$$

这里判断的阈值应该是根据实际使用设置的，本质就是判断前两点的距离差是否大于设定阈值，而这个阈值就是点云与原点的距离乘以一个系数。当然，阈值也可以取其他值。

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;
    }
  }
}

设置点，类型为pcl::PointCloud

pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;

特征点分类

遍历点云，计算所有点的曲率后，对曲率记性从小到大的排序。为了保证特征点的均匀选取，会将点云分成6个部分，每个部分选取一定数量的特征点。不理解这里为什么会分成6个部分。

曲率按从小到大冒泡排序，该冒泡排序没有优化，可以对代码进行一定的修改，减少执行的次数。

这一部分还是比较简单的。基本思路就是选取曲率大的点，去除该点前后各5各距离比较进的点；选取曲率小的点，去除该点前后各5各距离比较进的点。

值得一看的是，出现新的名词，体素栅格滤波。PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格（可把体素栅格想象为微小的空间三维立方体的集合），然后在每个体素（即，三维立方体）内，用体素中所有点的重心来近似显示体素中其他点，这样该体素就内所有点就用一个重心点最终表示，对于所有体素处理后得到过滤后的点云。

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;
}

发布特征点

发布消除非匀速运动畸变后的所有的点

sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);

发布消除非匀速运动畸变后的平面点和边沿点

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);

发布 IMU消息,由于循环到了最后，因此是Cur都是代表最后一个点，即最后一个点的欧拉角，畸变位移及一个点云周期增加的速度

  pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);
  //起始点欧拉角
  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);

接收imu消息

接收imu消息主要是imuHandler()函数完成。

为了得到xyz方向的加速度真实值，需要消除重力加速度的影响，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系。这一部分的理解参考chengwei0019的去除重力加速度的影响。

先回顾之前的旋转矩阵
$$R=R_y(\theta )R_x(\psi )R_z(\varphi )$$

重力加速度在世界坐标系的坐标为$[0,0,-9.8]$，根据坐标系转换有$[0,0,-9.8]=R\ast [x,y,z]$，转换到imu坐标系下的坐标变为$R^{-1}\ast [0,0,-9.8]$，而旋转矩阵的逆等于其装置，所以也为$R^T\ast [0,0,-9.8]$。使用上面旋转矩阵的转置乘以$[0,0,-9.8]$，可以得到重力加速度在imu坐标系下的坐标为
$$[gsin\theta ,gsin\psi cos\theta ,gcos\psi cos\theta ]$$

因此，在imu坐标系下的xyz方向加速度分别减去上式数值，即可得到消除重力影响的纯加速度，公式如下

$$a_{x-real}=a_{imu-y}-g\cdot sin(roll)cos(pitch)$$

$$a_{y-real}=a_{imu-z}-g\cdot cos(roll)cos(pitch)$$

$$a_{z-real}=a_{imu-x}-g\cdot sin(pitch)$$

//接收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 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;

  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();
}

主函数

啊 这…
我说为什么其他博客写得代码解析都是从main函数开始的，而我一直没找到main函数在哪，原来在scanRegistration.cpp的末尾，终于找到了。

这个main函数就很基础，了解ros的基本操作都可以看得明白，都是一些话题的订阅和发布。

首先初始化ros节点“scanRegistration”，指定了一个句柄nh。订阅了两个话题的消息，分别是“/velodyne_points”和“/imu/data”。发布了六个话题的消息，分别是"/velodyne_cloud_2"、"/laser_cloud_sharp"、"/laser_cloud_less_sharp"、"/laser_cloud_flat"、"/laser_cloud_less_flat"、"/imu_trans"。最后调用ROS消息回调处理函数ros::spin()，

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;
}

结语

至此，已经把scanRegistration.cpp函数的所有内容解析完了。上述内容还有几处不太理解的，如果有人能够解答，就请给我留言吧，十分感谢。

如果你看到这里，说明你已经下定决心要学习loam了，学习新知识的过程总是痛苦的，与君共勉吧！