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> 模板
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$$