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

前言

在看完Zhang Ji写的LOAM论文，对LOAM的基本原理有了大概的了解，但对具体实现有点疑问，所以就想一步一步完整解析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，首先看到的是对于坐标系的解释说明，如下

1. imu为x轴向前,y轴向左,z轴向上的右手坐标系；
2. velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系；
3. scanRegistration会把两者通过交换坐标轴，都统一到z轴向前,x轴向左,y轴向上的右手坐标系。
   这是J. Zhang的论文里面使用的坐标系，交换后

R = Ry(yaw)*Rx(pitch)*Rz(roll)

初始化

设置扫描周期，velodyne频率10Hz，周期0.1s

const double scanPeriod = 0.1;

初始化控制变量

const int systemDelay = 20;//弃用前20帧初始数据
int systemInitCount = 0;
bool systemInited = false;

设置激光雷达线数

const int N_SCANS = 16;

设置一帧点云中点的最大数量

float cloudCurvature[40000];

点对应的序号

int cloudSortInd[40000];

点是否筛选过标志：0-未筛选过，1-筛选过

int cloudNeighborPicked[40000];

点分类标号：

• 2 - 曲率很大
• 1 - 曲率比较大
• 0 - 曲率比较小
• -1 - 曲率很小

int cloudLabel[40000];

imu时间戳的位置

int imuPointerFront = 0;

imu最新收到的点在数组中的位置

int imuPointerLast = -1;

imu循环队列长度

const int imuQueLength = 200;

点云数据开始点和当前点的欧拉角、速度、位移

float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; // 开始点的欧拉角
float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0; // 当前点的欧拉角
float imuVeloXStart = 0, imuVeloYStart = 0, imuVeloZStart = 0; // 开始点的速度
float imuShiftXStart = 0, imuShiftYStart = 0, imuShiftZStart = 0; // 开始点的位移
float imuVeloXCur = 0, imuVeloYCur = 0, imuVeloZCur = 0; // 当前点的位移
float imuShiftXCur = 0, imuShiftYCur = 0, imuShiftZCur = 0; // 当前点的位移

每次点云数据当前点相对于开始第一个点的畸变位移，速度

float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0;
float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0;

IMU信息：时间、欧拉角、加速度、速度、位移

double imuTime[imuQueLength] = {0};
float imuRoll[imuQueLength] = {0};
float imuPitch[imuQueLength] = {0};
float imuYaw[imuQueLength] = {0};

float imuAccX[imuQueLength] = {0};
float imuAccY[imuQueLength] = {0};
float imuAccZ[imuQueLength] = {0};

float imuVeloX[imuQueLength] = {0};
float imuVeloY[imuQueLength] = {0};
float imuVeloZ[imuQueLength] = {0};

float imuShiftX[imuQueLength] = {0};
float imuShiftY[imuQueLength] = {0};
float imuShiftZ[imuQueLength] = {0};

ros发布消息

ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubImuTrans;

位移畸变计算-ShiftToStartIMU

计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变，ShiftToStartIMU()函数主要计算一帧数据中某点相对于一帧的起始点由于加减速造成的运动畸变。因此首先要求出世界坐标系下的加减速造成的运动畸变，然后将运动畸变值经过绕y、x、z轴旋转后得到起始点坐标系下的运动畸变。根据以下公式

设俯仰角α（pitch），围绕Y轴旋转的；偏航角β（yaw），围绕Z轴旋转的角度，滚转角γ（roll）围绕X轴旋转的角度。对于x,y,z三个轴的不同旋转顺序一共有(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)六种组合。论文和代码里写的是y-x-z。

• 左乘： 坐标系不动，点动，则左乘。

• 右乘： 点不动，坐标系动，则右乘。
  

• 世界坐标系下的畸变位移

$$\Delta s_{world-sta2cur-x}=s_{cur-x}-s_{sta-x}-v_x\Delta t$$

$$\Delta s_{world-sta2cur-y}=s_{cur-y}-s_{sta-y}-v_y\Delta t$$

$$\Delta s_{world-sta2cur-z}=s_{cur-z}-s_{sta-z}-v_z\Delta t$$

• 绕y轴旋转
  $${\left[\begin{array}{c}x1\\ y1\\ z1\end{array}\right]}^T={\left[\begin{array}{c}\Delta s_x\\ \Delta s_y\\ \Delta s_z\end{array}\right]}^T{R}_y(\beta )={\left[\begin{array}{c}\Delta s_{world-x}\\ \Delta s_{world-y}\\ \Delta s_{world-z}\end{array}\right]}^T\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& sin\beta \end{array}\right]$$

• 绕x轴旋转
  $${\left[\begin{array}{c}x2\\ y2\\ z2\end{array}\right]}^T={\left[\begin{array}{c}x1\\ y1\\ z1\end{array}\right]}^T{R}_x(\alpha )={\left[\begin{array}{c}x1\\ y1\\ z1\end{array}\right]}^T\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]$$

• 绕z轴旋转
  $${\left[\begin{array}{c}\Delta s_{local-x}\\ \Delta s_{local-y}\\ \Delta s_{local-z}\end{array}\right]}^T={\left[\begin{array}{c}x2\\ y2\\ z2\end{array}\right]}^T{R}_z(\gamma )={\left[\begin{array}{c}x2\\ y2\\ z2\end{array}\right]}^T\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$

综上，得到

• 局部坐标系下的畸变位移

$${\left[\begin{array}{c}\Delta s_{local-x}\\ \Delta s_{local-y}\\ \Delta s_{local-z}\end{array}\right]}^T={\left[\begin{array}{c}\Delta s_{world-sta2cur-x}\\ \Delta s_{world-sta2cur-y}\\ \Delta s_{world-sta2cur-z}\end{array}\right]}^T{R}_y(\beta )R_x(\alpha )R_z(\gamma )$$

void ShiftToStartIMU(float pointTime)
{
  //计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)
  //imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
  imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
  imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
  imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;

  /********************************************************************************
  Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tg
  从全局坐标系到局部坐标系的转换
  *********************************************************************************/

  //绕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

与位移畸变的计算同理，速度畸变由VeloToStartIMU()函数完成，主要求解当前点相对于起始点的速度畸变，先计算全局坐标系下速度畸变，再根据旋转矩阵求解起始点的局部坐标系中。

• 世界坐标系下的畸变位移

$$\Delta v_{world-sta2cur-x}=v_{cur-x}-v_{sta-x}$$

$$\Delta v_{world-sta2cur-y}=v_{cur-y}-v_{sta-y}$$

$$\Delta v_{world-sta2cur-z}=v_{cur-z}-v_{sta-z}$$

• 局部坐标系下的畸变位移

$${\left[\begin{array}{c}\Delta v_{local-x}\\ \Delta v_{local-y}\\ \Delta v_{local-z}\end{array}\right]}^T={\left[\begin{array}{c}\Delta v_{world-sta2cur-x}\\ \Delta v_{world-sta2cur-y}\\ \Delta v_{world-sta2cur-z}\end{array}\right]}^T{R}_y(\beta )R_x(\alpha )R_z(\gamma )$$

$$R_y(\beta )=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]$$

$$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]$$

$$R_z(\gamma )=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$

void VeloToStartIMU()
{
  //计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)
  imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
  imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
  imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;

  /********************************************************************************
    Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vg
    transfrom from the global frame to the local 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

在完成位移畸变的计算后，就可以去除点云加减速产生的位移畸变。方法还是与上同理，先将点云数据（xyz）在当前点转换到世界坐标系下，然后在从世界坐标系转换到点云起始坐标系下，最后减去加减速导致的位移值。核心思想就是借助世界坐标系来进行坐标转换。

• 当前坐标系转换到世界坐标系

$${\left[\begin{array}{c}{s}_{world-x}\\ s_{world-y}\\ s_{world-z}\end{array}\right]}^T={\left[\begin{array}{c}{s}_{local-cur-x}\\ s_{local-cur-y}\\ s_{local-cur-z}\end{array}\right]}^T{R}_z(\gamma {)}^{-1}{R}_x(\alpha {)}^{-1}{R}_y(\beta {)}^{-1}$$

$$R_z(\gamma {)}^{-1}=\left[\begin{array}{ccc}cos\gamma & sin\gamma & 0\\ -sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$

$$R_x(\alpha {)}^{-1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & sin\alpha \\ 0& -sin\alpha & cos\alpha \end{array}\right]$$

$$R_y(\beta {)}^{-1}=\left[\begin{array}{ccc}cos\beta & 0& -sin\beta \\ 0& 1& 0\\ sin\beta & 0& cos\beta \end{array}\right]$$

• 世界坐标系转换到起始坐标系，并减去位移畸变的值

$${\left[\begin{array}{c}{s}_{local-sta-x}\\ s_{local-sta-y}\\ s_{local-sta-z}\end{array}\right]}^T={\left[\begin{array}{c}{s}_{world-x}\\ s_{world-y}\\ s_{world-z}\end{array}\right]}^T{R}_y(\beta )R_x(\alpha )R_z(\gamma )+{\left[\begin{array}{c}\Delta s_{world-sta2cur-x}\\ \Delta s_{world-sta2cur-y}\\ \Delta s_{world-sta2cur-z}\end{array}\right]}^T$$

$$R_y(\beta )=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]$$

$$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]$$

$$R_z(\gamma )=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$

void TransformToStartIMU(PointType *p)
{
  /********************************************************************************
    Ry*Rx*Rz*Pl, transform point to the global 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;

  /********************************************************************************
    Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pg
    transfrom global points to the local 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;
}

位移与速度校正-AccumulateIMUShift

积分速度与位移的功能是通过AccumulateIMUShift()函数来实现的，将当前时刻的加速度值绕ZXY轴（原XYZ而安装后变为ZXY）分别旋转(roll: γ, pitch: α, yaw: β)角，转换得到世界坐标系下的加速度值(right hand rule)。

• 当前坐标系转换世界坐标系，并减去位移畸变的值

$${\left[\begin{array}{c}{a}_{world-x}\\ a_{world-y}\\ a_{world-z}\end{array}\right]}^T={\left[\begin{array}{c}{a}_{local-x}\\ a_{local-y}\\ a_{local-z}\end{array}\right]}^T{R}_z(\gamma {)}^{-1}{R}_x(\alpha {)}^{-1}{R}_y(\beta {)}^{-1}$$

$$R_z(\gamma {)}^{-1}=\left[\begin{array}{ccc}cos\gamma & sin\gamma & 0\\ -sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$

$$R_x(\alpha {)}^{-1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & sin\alpha \\ 0& -sin\alpha & cos\alpha \end{array}\right]$$

$$R_y(\beta {)}^{-1}=\left[\begin{array}{ccc}cos\beta & 0& -sin\beta \\ 0& 1& 0\\ sin\beta & 0& cos\beta \end{array}\right]$$

• 上一个Imu点到当前点所经历时间

$$\Delta t_{}=t_{last}-t_{back}$$

• imu频率比lidar高才进行校正，分别是位移校正和速度校正

$$\begin{gathered} s_{last-x}=s_{back-x}+v_{back-x}\Delta t_{}+\frac{1}{2}{a}_{back-x}\Delta t_{}^2 \\ {s}_{last-y}=s_{back-y}+v_{back-y}\Delta t_{}+\frac{1}{2}{a}_{back-y}\Delta t_{}^2 \\ {s}_{last-z}=s_{back-z}+v_{back-z}\Delta t_{}+\frac{1}{2}{a}_{back-z}\Delta t_{}^2 \end{gathered}$$

$$\begin{gathered} v_{last-x}=v_{back-x}+a_{back-x}\Delta t_{} \\ {v}_{last-y}=v_{back-y}+a_{back-y}\Delta t_{} \\ {v}_{last-z}=v_{back-z}+a_{back-z}\Delta t_{} \end{gathered}$$

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

  //将当前时刻的加速度值绕交换过的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

我看了一下这个函数，很长，这一章估计写不完了，留到下一章吧。

结语

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