对LOAM算法原理和代码的理解

LOAM概况

LOAM主要算法流程

上图为LOAM算法的主要执行流程。下面分别说明。
Point Cloud Registration
1.使用IMU数据进行点云运动畸变去除

利用IMU去除激光传感器在运动过程中非匀速（加减速）部分造成的误差（运动畸变）。为什么这样做呢？因为LOAM是基于匀速运动的假设，但实际中激光传感器的运动肯定不是匀速的，因此使用IMU来去除非匀速运动部分造成的误差，以满足匀速运动的假设。

imuHandler（）函数接受IMU数据。IMU的数据可以提供给我们IMU坐标系三个轴相对于世界坐标系的欧拉角和三个轴上的加速度。但由于加速度受到重力的影响所以首先得去除重力影响。在去除重力影响后我们想要获得IMU在世界坐标系下的运动，因此在 AccumulateIMUShift()中，根据欧拉角就可以将IMU三轴上的加速度转换到世界坐标系下的加速度。 然后利用匀加速度公式$s1=s2+vt+1/2a{t}^2$来计算位移。因此我们可以求出每一帧IMU数据在世界坐标系下对应的位移和速度。代码中实际缓存了200帧这样的数据，为后面插值激光点运动畸变打下了基础。

这里使用IMU数据进行插值计算点云的中点的位置，消除由于非匀速运动造成的运动畸变。

//-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; //按时间距离计算权重分配比率,也即线性插值 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);//将每个补偿矫正的点放入对应线号的容器 } 

void ShiftToStartIMU(float pointTime)函数中主要计算一帧数据中某点相对于一帧的起始点由于加减速造成的运动畸变。因此我们首先要求出世界坐标系下的加减速造成的运动畸变，然后将运动畸变值经过绕y、x、z轴旋转后得到起始点坐标系下的运动畸变。这里的坐标系一定要搞清楚为什么要放的起始点的坐标系下。

//计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变 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 transfrom from the global frame to the local 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()函数，这个函数流程和上一个函数大致相同。它的作用就是求当前点的速度相对于点云起始点的速度畸变，先计算全局坐标系下的然后再转换到起始点的坐标系中。

//计算局部坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变（增量） 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(PointType *p)函数作用是将当前点先转换到世界坐标系下然后再由世界坐标转换到点云起始点坐标系下。 然后减去加减速造成的非匀速畸变的值。

//去除点云加减速产生的位移畸变 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; } 

这一部分主要引用：https://blog.csdn.net/liuyanpeng12333/article/details/中讲解IMU进行运动畸变的部分

 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; 

//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到 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; 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;//赋成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; } } } 

针对前后遮挡的情况，如b所示，是通过计算前后两点间距的平方是否大于该点距离值平方的万分之二来排除的。下面是代码实现。

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

遍历点云计算好曲率后会对曲率进行从小到大的排序。为了保证特征点的均匀获取，会将点云分成6个部分，每个部分选取一定数量的特征点。

Lidar Odometry
1.激光帧的时间同步

一帧激光帧经历的时间比较长。一个sweep间隔里，激光点云数据会由于激光器的运动产生畸变。所以作者将前后两帧激光数据都转换到同一时刻，然后再进行激光点云匹配。如上图所示，将完整的一帧激光点云$P_k$投影到$t_{t+1}$时刻。投影过程会去除点云的运动畸变。在上一个模块(Point Cloud Registration)中，已经用IMU的数据去除了非匀速运动的畸变。在本模块(Lidar Odometr)中,使用上一模块中得到的一个sweep内的平均速度去除匀速运动引起的畸变。最终得到的效果就好像，激光器在$t_{t+1}$时刻静止扫出来的点云。对于新接受的数据$P_{k+1}$使用同样的方法投影到$t_{t+2}$时刻。注意这里的投影过程是为了后面的点云匹配。所以只进行了特征点云的投影。而当匹配结束后会将当前帧的特征点云和完整的点云数据都投影到该帧最后一个时刻。

2.点云匹配与误差函数

对于Edge Point，是寻找离当前帧的Edge Point最近的上一帧里的两个Edge Point。同时需要保证这两个Edge Point不能在同一层激光里，正如上图(a)中的点$l$和$j$分别处于不同的层。而误差函数就是描述当前帧的$i$点到上一帧中$l$、$j$连线的距离。方程公式如下：

上图的公式表示点到直线的距离。在二维空间中，向量$a$叉乘向量$b$的模长等于由向量a和向量b构成的平行四边形的面积。

图片引用于https://huangwang.github.io/2019/01/05/%E5%90%91%E9%87%8F%E7%82%B9%E7%A7%AF%E5%8F%89%E7%A7%AF%E5%8F%8A%E5%85%B6%E5%87%A0%E4%BD%95%E6%84%8F%E4%B9%89/

方程中的分子相当于向量$a$和向量$b$围成的平行四边形面积。分母就是向量$ab$的模长。平行四边形的面积除以边长$ab$就是向量$a$和向量$b$共同端点到直线$ab$的距离。

对于Planar Point，如Fig7图中(b)所示，找到离当前帧中点$i$最近的上一帧的3个Planar Point。需要保证这3个Planar Point不在同一层中。这样3个点就形成了一个面。而误差函数就是描述当前帧的$i$点到上一帧中$l$、$j$和$m$点所在平面的距离。方程公式如下：

在三维几何中，向量a和向量b的叉乘结果是一个向量，有个更通俗易懂的叫法是法向量，该向量垂直于a和b向量构成的平面。方程中点乘的下面那部分会得到单位化的法向量。而向量$ij$点乘单位法向量就相当于向量$ij$在法向量上的投影，即点$i$到平面的距离。

最终的误差函数是两项误差的和。最后用LM方法迭代（代码中最大迭代25次）求解出前后两帧激光的位姿差。

Lidar Mapping

上图中，$Q_k$表示到第$k$次sweep为止累积的点云地图。${\bar{Q}}_{k+1}$表示投影到map坐标系下的第$k$次sweep的激光数据。${\mathbf{T}}_k^W$表示map坐标系下激光器的位姿。${\mathbf{T}}_{k+1}^L$表示${\mathbf{T}}_k^W$坐标系下激光器的位姿。

从Lidar Odometry模块中，我们可以得到一帧已经去除畸变的激光点云数据和相对于前一帧的粗糙位姿变换。Lidar Mapping模块的任务就是用${\bar{Q}}_{k+1}$与地图$Q_k$进行匹配得到一个更为精确的帧间位姿变换，然后建图，为下次匹配做好准备。这里使用的匹配方法与Lidar Odometry模块中是一致的。不同的地方在于这里提取的特征点数量是Lidar Odometry模块的10倍。特征点的匹配不是找对应的2个或者3个特征点。而是相对于当前帧，在地图$Q_k$中对应位置附近10m10m10m的Cubic中提取所有的特征点。筛选出最近的5个特征点，然后计算协方差。对协方差进行特征值分解，最大特征值对应的向量就是需要匹配的Edge line方向向量，最小特征值对应的向量就是需要匹配的Planar patch方向向量。分别在Edge line上选取两个点，在Planar patch上选取三个点，按照点到线的距离公式和点到面的距离公式构建误差方程。同样使用LM方法迭代（最多迭代10次）求解，得到一个更为精确的帧间位姿变换。

Transform Integration
Transform Integration模块主要是融合了Lidar Mapping得到的位姿变换和Lidar Odometry得到的位姿变换。最终发布一个频率与Lidar Odometry发布频率一致的位姿变换。

LOAM的相关材料

针对LOAM和A-LOAM在KITT上排名差别很大的几点看法

1.LOAM认为一帧激光点云的间隔时间内激光器是匀加减速运动的，并且在点云畸变去除时使用了IMU数据来去除非匀速部分的畸变，然后在Lidar Mapping模块中还利用IMU数据求出平均速度来去除匀速运动引起的畸变。而A-LOAM只认为一帧激光点云的间隔时间内激光器是匀速运动的。然后直接根据时间进行线性插值来进行点云运动畸变去除。很显然LOAM对激光器的运动描述的更为准确，所以效果会更好。

2.A-LOAM使用ceres进行非线性优化求解。而LOAM是作者手写的非线性求解器。这一部分我看不太懂，但猜测代码的工程实现上会有优化，代码执行效率可能更高。

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。