LOAM源代码分析附公式推导之MultiScanRegistration

更新日志

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。

• 2020-12-07
  整理完成LaserOdometry节点内容，地址请转到LOAM源代码分析附公式推导之LaserOdometry
  最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！
• 2020-10-11
  整理完成MultiScanRegistration节点内容
• 2020-10-09
  第一次编辑，初版

1. 概述

整理一下LOAM中对激光点云数据、IMU数据的处理以及相关的公式推导，希望能帮助大家，同时供自己以后复习参考。由于相关的东西较多，可能会有疏漏，并且由于时间有限，精力有限，此博客长期更新，欢迎各位小伙伴批评指正。

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导，有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ）。

2. 前言

LOAM是 bulabula。
LOAM参考源代码仓库github地址为LOAM 源代码链接具体配置可以参照博客LOAM SLAM安装与配置
关于LOAM原始论文的解读可以参考LOAM SLAM之论文原理解读，
相关链接LOAM SLAM原理之防止非线性优化解退化。
其他参考链接，感谢各位在此之前分享对LOAM原理以及代码的分析理解
LOAM细节分析
LOAM:3D激光里程计及环境建图的方法和实现（一）

每个人对代码的理解是不一样，均有可能出现偏差，欢迎探讨！

3. 准备工作

在LOAM中，使用的坐标系是左上前坐标系，也就是X指向左，Y指向上，Z指向前。一定要注意坐标系问题，后面的一系列工作全都是基于这一基准坐标系开展的。

3.1 激光雷达

LOAM原始论文使用的是一个二维激光雷达，通过机械机构产生3D点云数据，但是现在的代码中大多使用的是Velodyne美国威力登公司的16线激光雷达：vlp16也称为PUCK。因此本文以代码中使用的vlp16雷达开展（代码使用的这个，如果想用其他的就自己改改吧，哈哈）。那么简单介绍一下vlp16：

• Velodyne PUCK（VLP-16）16通道
• TOF测量距离，最远100m，精度3cm
• 垂直分辨率2°
• 水平分辨率600、1200RPM对应0.2°、0.4°
• 旋转速率5-20Hz
• 最大点云数据量 360/0.4161200/60=288000（30万）

我们主要关注点云数据的结构，根据官方文档，具体结构示意如下图，在实际扫描中，激光雷达的激光束按照id号依次进行，例如发射id为0激光，发射id为1激光，最后发射id为15激光。
需要注意相邻激光束的ID差别较大，也就是激光每次发射激光时不是角度上依次发射的，而是隔了半个垂直视场的范围，id相邻的激光束空间上不相邻。

评论区有小伙伴对激光束发射提出疑问，此问题针对vlp16激光雷达，这里我解释一下，激光束是间断发射的16束垂直方向的激光，不是同时发射的，但是间断时间很短，你可以认为是同时发射的，只是认为。可以想一个问题，激光如果是同时发射的，如何根据反射的激光区分他们呢，所以一次只能发射一束激光。根据官方资料，在垂直方向发射激光时，相邻激光束时间差为$2.304\mu s$微秒，16束激光发射完成时会有$18.43\mu s$微秒的空闲时间，一次16束激光完成发射需要花费$16\ast 2.034\mu s+18.43\mu s=55.296\mu s$微秒。当水平旋转频率为600RPM时，对应的水平方向分辨率为$Azimut{h}_{resolution}=10rev/s\ast 360°/rev\ast 55.296\times 10^{-6}s/firingcycle=0.199°/firingcycle$，与之前给出的数据对应，这下应该都明白了吧。

vlp16的坐标系为右前上，并且扫描时顺时针旋转，通过安装可以将坐标系变换为前左上，因此我们在数据处理时只需要将前左上坐标系的点云数据转换为左上前坐标系即可。

3.2 IMU

根据代码可以看到IMU安装坐标系为前左上，因此实际使用时需要将测量的比力信号转换到左上前坐标系，并且注意对重力加速度的处理之后才是真正的激光运动加速度。

3.3 坐标变换

前面谈到了坐标系问题，这里讲一下坐标变换的问题，网上相关的博客很多，为了不引起误解，对于LOAM，我们统一用下面的方式描述旋转变换：
左乘，旋转坐标轴，先Z再X后Y得到旋转矩阵$R_{ZXY}$。
举例说明：激光坐标系$L$和世界坐标系$W$原点开始时一样，经过绕Z轴旋转$rz$角度，绕X轴$rx$角度，绕Y轴$ry$角度（以坐标轴正方向为旋转正方向）之后得到激光坐标系$L^{\prime }$，那么根据上面的定义有
从世界坐标系$W$到激光坐标系$L^{\prime }$的变换$R_{ZXY}$为
根据左乘
$$R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$
根据旋转坐标轴矩阵
$$R_{ZXY}=\left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]$$
进一步得到
$$R_{ZXY}=\left[\begin{array}{ccc}cycz-sxsysz& cysz+sxsycz& -sycx\\ -cxsz& cxcz& sx\\ sycz+sxcysz& sysz-sxcycz& cycx\end{array}\right]$$
上面的公式以及以后的公式中$sy=sry=sin(ry),cy=cry=cos(ry)$
但有的时候，我们需要的是逆运算，也就是从激光坐标系$L^{\prime }$到世界坐标系$W$的变换$(R_{ZXY}{)}^{-1}$
根据变换的关系很容易得到，所有的变换角度取负数，顺序取反就可以得到
$$(R_{ZXY}{)}^{-1}=R_{-rz}{R}_{-rx}{R}_{-ry}$$

单独考虑先Y再X后Z左乘，旋转坐标轴的矩阵$R_{YXZ}$，有
根据左乘旋转坐标轴矩阵
$$R_{YXZ}=\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]$$
进一步得到
$$R_{YXZ}=\left[\begin{array}{ccc}cycz+sxsysz& cxsz& -sycz+sxcysz\\ -cysz+sxsycz& cxcz& sysz+sxcycz\\ cxsy& -sx& cxcy\end{array}\right]$$

很容易看到单独的$R_{YXZ}$和$(R_{ZXY}{)}^{-1}$差了一个负号，这是因为逆运算导致的。

上面的左乘，对应右乘，对右乘有$(R_{ZXY})=R_{rz}{R}_{rx}{R}_{ry}$，这里不考虑。
上面的旋转坐标轴，对应旋转向量，对旋转向量有
$$R_{ry}=\left[\begin{array}{ccc}cry& 0& sry\\ 0& 1& 0\\ -sry& 0& cry\end{array}\right]$$
其实就是旋转坐标轴的角度取反，可以认为是一组逆运算。相对运动，哈哈。这个在代码里体现为所有的rotateZXY和rotateYXZ函数都使用的是旋转向量矩阵，因此需要注意实际传入的角度信息。

同样的，对于变换
$$X_t=R{X}_0+T$$的逆运算为
$$X_0=R^{-1}(X_t-T)$$
这个后面具体用到的时候再说吧。

4. 插话

现在就开始进行我们的源代码阅读之旅，注意分析内容有可能较多，后面视情况再将内容划分。
总体概览和框架可以参考原始论文分析LOAM SLAM之论文原理解读，这里主要对代码进行详细分析。

5. 之MultiScanRegistration

MultiScanRegistration节点利用插值得到的IMU数据对激光运动补偿，得到近似匀速运动模型下的点云数据，同时对点云数据提取边缘特征，平面特征，供LaserOdometry节点使用。

5.1 参数配置

可设置参数以及默认参数有，第一列表示参数名称，第二列表示默认值或者可选值

      scanPeriod                 const float& scanPeriod_ = 0.1,//一次完整的激光数据，对应扫描频率10hzimuHistorySize             const int& imuHistorySize_ = 200,//IMU数据缓冲区大小，后面使用缓冲区数据进行插值featureRegions             const int& nFeatureRegions_ = 6,//同一激光束点云数据划分的子区域数量curvatureRegion            const int& curvatureRegion_ = 5,//计算每个点弯曲度时在当前点一边选取的附近点数，总点数为 region*2maxCornerSharp             const int& maxCornerSharp_ = 2,//子区域中选取的最大边缘点数maxCornerLessSharp         maxCornerLessSharp(10 * maxCornerSharp_)//子区域中选取的最大次边缘点数(弯曲度大于阈值的部分点)maxSurfaceFlat             const int& maxSurfaceFlat_ = 4,//子区域中选取的最大平面点数lessFlatFilterSize         const float& lessFlatFilterSize_ = 0.2,//次平面点滤波器使用的体素大小surfaceCurvatureThreshold  const float& surfaceCurvatureThreshold_ = 0.1//区分平面、边缘时使用的弯曲度阈值//激光设置可以选择下面的一种方式1.          lidar          VLP-16，HDL-32，HDL-64E       //velodyne具体产品，对应16线，32线，64线2.          minVerticalAngle || maxVerticalAngle || nScanRings  //非velodyne产品，根据具体的参数进行设置，并且至少需要2线激光雷达

激光束数量不同的激光雷达（16，32，64线等）在处理数据时稍有不同：需要将不同激光束点云数据区分开，供后续里程计，建图时使用。
因此做了下面工作：

• 默认支持VLP16，HDL32，HDL64E，创建scanMapper对象。
• 或者根据最大、最小垂直角度，激光线束构建创建scanMapper对象，这种可以用于非Velodyne品牌的激光雷达。

  if (privateNode.getParam("lidar", lidarName)) {if (lidarName == "VLP-16") {_scanMapper = MultiScanMapper::Velodyne_VLP_16();} else if (lidarName == "HDL-32") {_scanMapper = MultiScanMapper::Velodyne_HDL_32();} else if (lidarName == "HDL-64E") {_scanMapper = MultiScanMapper::Velodyne_HDL_64E();} else {ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str());return false;}ROS_INFO("Set  %s  scan mapper.", lidarName.c_str());if (!privateNode.hasParam("scanPeriod")) {config_out.scanPeriod = 0.1;ROS_INFO("Set scanPeriod: %f", config_out.scanPeriod);}} else {float vAngleMin, vAngleMax;int nScanRings;if (privateNode.getParam("minVerticalAngle", vAngleMin) &&privateNode.getParam("maxVerticalAngle", vAngleMax) &&privateNode.getParam("nScanRings", nScanRings)) {if (vAngleMin >= vAngleMax) {ROS_ERROR("Invalid vertical range (min >= max)");return false;} else if (nScanRings < 2) {ROS_ERROR("Invalid number of scan rings (n < 2)");return false;}_scanMapper.set(vAngleMin, vAngleMax, nScanRings);ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings);}}

scanMapper对象的作用使用一句话概括：根据角度获取激光束ID，后续用于分开不同激光束数据。

• 以vlp16为例

  /** \brief Construct a new multi scan mapper instance.** @param lowerBound - the lower vertical bound (degrees)* @param upperBound - the upper vertical bound (degrees)* @param nScanRings - the number of scan rings*/MultiScanMapper(const float& lowerBound = -15,const float& upperBound = 15,const uint16_t& nScanRings = 16);MultiScanMapper::MultiScanMapper(const float& lowerBound,const float& upperBound,const uint16_t& nScanRings): _lowerBound(lowerBound),_upperBound(upperBound),_nScanRings(nScanRings),_factor((nScanRings - 1) / (upperBound - lowerBound))
{}
// 根据垂直角度获取激光束ID：0,1,2、、、15
int MultiScanMapper::getRingForAngle(const float& angle) {return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5);
}

5.2 话题订阅

bool ScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out)
{if (!parseParams(privateNode, config_out))return false;// subscribe to IMU topic_subImu = node.subscribe<sensor_msgs::Imu>("/imu/data", 50, &ScanRegistration::handleIMUMessage, this);// advertise scan registration topics_pubLaserCloud            = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);_pubCornerPointsSharp     = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);_pubCornerPointsLessSharp = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);_pubSurfPointsFlat        = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);_pubSurfPointsLessFlat    = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);_pubImuTrans              = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);return true;
}

以及点云数据

  // subscribe to input cloud topic_subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

5.3 IMU数据处理

IMU数据的处理如下，IMU的坐标系为前左上，对测量的比力进行处理得到真正的加速度信号，可以自己推导一下，有需要的话我再把这部分内容添加上，转换坐标系为左上前坐标系。这里的偏航俯仰滚转角三个欧拉角都不会变化，可以考虑一下为什么。

void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn)
{tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);double roll, pitch, yaw;tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);Vector3 acc;acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);acc.z() = float(imuIn->linear_acceleration.x + sin(pitch)             * 9.81);IMUState newState;newState.stamp = fromROSTime( imuIn->header.stamp);newState.roll = roll;newState.pitch = pitch;newState.yaw = yaw;newState.acceleration = acc;updateIMUData(acc, newState);
}

对于陀螺仪而言，测量得到加速度信息是需要进行积分的，因此有
加速度信号转换到世界坐标系然后积分得到速度velocity，再积分得到位置position。
对于姿态角，不必积分，直接使用即可。
将这些数据存储到IMU环形容器_imuHistory中。用于后面的插值以及偏移计算。

void BasicScanRegistration::updateIMUData(Vector3& acc, IMUState& newState)
{if (_imuHistory.size() > 0) {// accumulate IMU position and velocity over time//这里将数据转换到世界坐标系中，理论上应该取负数，但是这里代码中使用了旋转向量矩阵，因此就不需要取负数了rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);const IMUState& prevState = _imuHistory.last();float timeDiff = toSec(newState.stamp - prevState.stamp);newState.position = prevState.position+ (prevState.velocity * timeDiff)+ (0.5 * acc * timeDiff * timeDiff);newState.velocity = prevState.velocity+ acc * timeDiff;}_imuHistory.push(newState);
}

觉得太复杂直接记住下面的总结就行，我就不多讲了，容易混淆。简单总结就是对于IMU测量的姿态角有：

   　　　　　rotateYXZ(-yaw,-pitch,-roll)世界坐标系－－－－－－－－－－－－>>>>>IMU坐标系世界坐标系<<<<<－－－－－－－－－－－－IMU坐标系rotateZXY(roll,pitch,yaw)

5.4 点云数据处理

5.4.1 点云数据

整个点云注册环节核心部分就在这里，主要是对点云数据进一步处理，根据IMU数据校正得到近似匀速运动的点云数据，以供激光里程计使用。对，匀速运动假设，这个很重要，需要构造这样的条件，不然后面的处理会出现很大的误差。

具体的处理环节在下面的函数中

void MultiScanRegistration::process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn, const Time& scanTime)

原始的激光坐标系是右前上，顺时针旋转，点云数据的水平角度递减，但是可以通过安装将激光坐标系变换为前左上，依旧是顺时针旋转，点云数据水平角度递减，很简单对不对。这样就可以很方便的转换为论文中提到的统一坐标系左上前。

判断点的起始方位和终止方位,原始角度是顺时针变化，因此需要对角度进行取反，并对角度判断周期性。注意负号，因为原始是顺时针旋转的，这里取负号，后面也取负号，就统一变为逆时针旋转。

• atan2的值域是$-\pi$到$\pi$。

  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);// 必须满足终止角度 > 初始角度并且，近似一个周期2piif (endOri - startOri > 3 * M_PI) {endOri -= 2 * M_PI;} else if (endOri - startOri < M_PI) {endOri += 2 * M_PI;}

需要转换坐标系：前左上转换为左上前坐标系，不处理无效点数据。

    // 前左上 转换 为左上前坐标系point.x = laserCloudIn[i].y;point.y = laserCloudIn[i].z;point.z = laserCloudIn[i].x;// skip NaN and INF valued points// 去除无效点数据if (!pcl_isfinite(point.x) ||!pcl_isfinite(point.y) ||!pcl_isfinite(point.z)) {continue;}// skip zero valued pointsif (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {continue;}

根据垂直角度找到对应的激光束ID，这个时候坐标系已经发生了改变，注意使用的坐标轴数据

    // calculate vertical point angle and scan IDfloat angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));int scanID = _scanMapper.getRingForAngle(angle);if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){continue;}

接下来是求解每个点的水平角度，根据水平角度可以得到获取每个点时相对与开始点的时间，因为激光雷达是先垂直扫描再水平扫描的。但是角度函数atan2的值域是$-\pi$到$\pi$，因此必须要进行变换。这里将整个水平方向(360度)扫描的点分为两部分，第一部分与开始点起始角度startOri相差角度在$\pi$以内，第二部分与结束点终止角度endOri相差角度在$\pi$以内。这里默认使用的角度是逆时针增加。

求解每个点的水平角度，对于atan2的结果需要取负数，以和前面对应，将顺时针递减的角度转变为逆时针递增的顺序。
注意实际中的激光雷达数据不一定从角度0开始的，因此需要考虑到atan2解算的角度在$-\pi$和$\pi$变换时的突变会出问题，需要对角度进一步补偿。

例如相邻点云数据点的角度由atan2解算并取负数后为$\pi -0.1$，$-\pi +0.1$。在空间上是相邻的，由于三角函数的关系它们在数值上并不相邻，因此需要进行补偿，主要是对$-\pi +0.1$补偿。

• 考虑起始角度startOri为$\pi /2$，现在角度是逆时针增加，那么当前角度为$-\pi +0.1$时会出现$(-\pi +0.1)-(\pi /2)=-3\ast \pi /2+0.1$。因为$-3\ast \pi /2+0.1$小于0，这也就意味着角度不是增加了，根据角度周期性$2\pi$，实际角度应该为$-\pi +0.1-->\pi +0.1$。这样才有$(\pi +0.1)-(\pi /2)=1/2\ast \pi +0.1$，这表示角度增加。因此考虑到半个扫描周期并且包含有$\pi$到$-\pi$转换后有当$\theta -startOri<-1/2\ast \pi$时需要进行补偿$\theta +2\pi$可以参考图解。
  

• 上图，绿色线表示起始方位，黑色表示当前点的解算方位，由于atan2的原因，角度会出现突变，导致角度需要进行补偿，可以看到突变的角度为角度减少$2\pi$，但是根据半个周期的约束，如果包含该突变，起始角度必须位于$[0,\pi ]$之间，也就是说当前解算方位和起始方位最大相差$-\pi$，同时考虑不与顺时针旋转冲突，起始方位角有偏差。因此实际设定只要相差$-\pi /2$就认为当前解算方位和起始方位还没有相差半个周期角度$\pi$，需要补偿当前方位角。而当前解算方位和起始方位相差半个周期角度$\pi$以上时，默认会让解算角度增加 。

• 同样的，考虑当角度变换顺时针减少时，需要补偿条件为($\theta$) - startOri > 3/2*pi。

其实可以看到应该有两个判断半周期的条件，一个是ori - startOri > M_PI，一个是ori - startOri < -M_PI。但是velodyne只需要使用ori - startOri > M_PI，估计也就没有添加了。
或者说ori > startOri + M_PI * 3 / 2是多余的，没有用处，因为已经保证了角度增加。

同样的可以得到endOri和ori的关系，其实就是一一对应罢了。可以参考下面的不等式：

• ori < startOri - M_PI / 2
• ori > endOri + M_PI / 2 --> ori - M_PI / 2> endOri

    // calculate horizontal point angle// atan2的角度，值位于-pi到pi之间float ori = -std::atan2(point.x, point.z);if (!halfPassed) {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;if (ori < endOri - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > endOri + M_PI / 2) {ori -= 2 * M_PI;}}

因此根据水平角度可以得到每个点相对于开始点的相对时间relTime，并存储到intensit中。
对点云数据格式信息的利用达到最大化：把pcl::PointXYZI中的intensity设置为相对开始点的时间＋对应线束id，一个数据包含了两个信息！！后面还有类似的操作。

// calculate relative scan time based on point orientationfloat relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);point.intensity = scanID + relTime;

那这个相对时间relTime有什么用呢？马上就会用到：和IMU数据一起近似去除激光的非匀速运动，构建匀速运动模型。

5.4.2 近似匀速运动模型

当IMU数据存在时，使用IMU数据积分得到速度位置对点云数据进行补偿，得到激光匀速运动模型下的坐标。
首先是获取IMU数据，使用插值得到精确时间_scanTime + relTime的IMU数据(包含速度，位置，三个姿态角)

// 根据relTime进行IMU数据插值，得到速度，位置信息。
void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState)
{// IMU数据和点云点的时间差：点云点时间 - IMU时间double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {_imuIdx++;// IMU数据和点云点的时间差：点云点时间 - IMU时间timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;}if (_imuIdx == 0 || timeDiff > 0) {// 不能插值，1._imuIdx == 0：使用最旧的数据  2. timeDiff > 0：使用最新的数据outputState = _imuHistory[_imuIdx];} else {// 如果IMU缓冲区中可以插值，就直接插值float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);}
}

进行插值时特别注意偏航角，这里和刚才有点类似，$-\pi$和$\pi$的转换，这是因为一般偏航角度可以360度，必须考虑角度的转换，而俯仰、滚转角度变换较小，不会存在$-\pi$和$\pi$的转换。

    /** \brief Interpolate between two IMU states.** @param start the first IMUState* @param end the second IMUState* @param ratio the interpolation ratio* @param result the target IMUState for storing the interpolation result*/static void interpolate(const IMUState& start,const IMUState& end,const float& ratio,IMUState& result){float invRatio = 1 - ratio;result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;if (start.yaw.rad() - end.yaw.rad() > M_PI){result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio;}else if (start.yaw.rad() - end.yaw.rad() < -M_PI){result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio;}else{result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio;}result.velocity = start.velocity * invRatio + end.velocity * ratio;result.position = start.position * invRatio + end.position * ratio;};

经过上面处理，因此我们拥有了当前点时间(_scanTime + relTime)对应的激光位置和速度，
进一步和开始点(_scanTime + 0)的激光位置、速度进行处理有
Startposition + PositionShift + 匀速模型(开始点对应的速度) = Curposition。可以考虑激光加速运动下，PositionShift表示了开始点时刻到当前时刻激光由于加速移动的距离。

• relSweepTime其实等于relTime，表示当前点相对于开始点的时间差。

  // 得到非匀速运动下的激光位置偏移量。 Startposition + PositionShift + 匀速模型(开始点对应的速度) = Curpositionfloat relSweepTime = toSec(_scanTime - _sweepStart) + relTime;// _scanTime - _sweepStart = 0_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

对于位置偏移，需要在世界坐标系下对点云数据进行处理，这里使用了之前提到的IMU局部坐标系和世界坐标系的转换，并且由于点云测量坐标和激光运动趋势相反，因此这里需要加上位置偏移量，特别注意不是减！然后进一步得到IMU局部坐标系下的点云坐标。

void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point)
{// rotate point to global IMU system// IMU局部坐标系到世界坐标系转换rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);// add global IMU position shift// 加上位置偏移量，这里 加 是因为激光运动和点云运动是相反的。// 可以考虑加速运动下，PositionShift为正，并且点云测量坐标系变小，因此需要让点云坐标变大。point.x += _imuPositionShift.x();point.y += _imuPositionShift.y();point.z += _imuPositionShift.z();// rotate point back to local IMU system relative to the start IMU state// 世界坐标系到IMU局部坐标系转换rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
}

然后将近似匀速运动模型下IMU局部坐标系下的点云数据进行存储得到

_laserCloudScans[scanID].push_back(point);

5.4.3 特征提取

对所有点云数据进行运动补偿之后就是进行特征点提取了。首先变换点云数据存储格式，得到一维状态下的点云数据。使用_scanIndices来确定每束数据的开始位置，结束位置。

 pcl::PointCloud<pcl::PointXYZI> _laserCloud;std::vector<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;

void BasicScanRegistration::processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans)
{// reset internal buffers and set IMU start state based on current scan time// 为下一次处理做准备，同时清空当前特征点容器reset(scanTime);  // construct sorted full resolution cloud// 将不同激光束点云数据合并为一维形式，使用_scanIndices来确定每束数据的开始位置，结束位置。size_t cloudSize = 0;for (int i = 0; i < laserCloudScans.size(); i++) {_laserCloud += laserCloudScans[i];IndexRange range(cloudSize, 0);cloudSize += laserCloudScans[i].size();range.second = cloudSize > 0 ? cloudSize - 1 : 0;_scanIndices.push_back(range);}extractFeatures();updateIMUTransform();
}

现在最重要的环节就是提取每个激光束的特征点，包含平面特征点和边缘特征点，具体可以参考下面
特征点的选择以及注意事项可以参考论文理解部分，这里不过多讲解。在

void BasicScanRegistration::extractFeatures(const uint16_t& beginIdx)

中进行特征点提取。
提取每个激光束点云数据在一维容器中的的开始索引，结束索引，并且保证点云数据的数量足够能够进行弯曲度计算。

    // 每个激光束点云数据在一维容器中的的开始索引，结束索引size_t scanStartIdx = _scanIndices[i].first;size_t scanEndIdx = _scanIndices[i].second;// skip empty scans// 激光束中数据不够if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {continue;}

对不不可靠点进行标记，0表示未被选择，可以作为特征点，1表示不能被选择为特征点。

• 不考虑每次激光束的前curvatureRegion，最后curvatureRegion个点，因为不能计算弯曲度。
• 相邻点距离过大，说明可能不在同一个平面上。加权后的点近似相邻，但是空间中不相邻，因此存在被遮挡的可能，平面过于倾斜的可能，属于不可靠点，需要标记，不能作为特征点。注意标记是对应标记的区域是根据点的远近来决定的。
• 考虑A，B，C相邻三个点，AB距离，BC距离均大于B到激光距离一定比例，表明B点可能为离群点，位于过于倾斜的平面，不可靠的测量数据。

void BasicScanRegistration::setScanBuffersFor(const size_t& startIdx, const size_t& endIdx)
{// resize bufferssize_t scanSize = endIdx - startIdx + 1;// 0表示未被选择，可以作为特征点_scanNeighborPicked.assign(scanSize, 0);// mark unreliable points as picked// 不考虑每次激光束的前curvatureRegion，最后curvatureRegion个点，因为不能计算弯曲度for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);const pcl::PointXYZI& point = (_laserCloud[i]);const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);float diffNext = calcSquaredDiff(nextPoint, point);// 相邻点距离过大，说明可能不在同一个平面上，需要考虑if (diffNext > 0.1) {float depth1 = calcPointDistance(point);float depth2 = calcPointDistance(nextPoint);// 将距离远的点 坐标比例缩放重投影 到 距离近的点平面if (depth1 > depth2) {// 加权下的相邻点距离  point点较远float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;// 这里加权距离小说明，加权后的点近似相邻，但是空间中不相邻，因此存在被遮挡的可能，平面过于倾斜的可能，// 属于不可靠点，需要标记，不能作为特征点// point远，那么point左边这一部分区域点都不能作为特征点if (weighted_distance < 0.1) {std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);continue;}} else {// 和上面的分析类似float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;// nextPoint远，那么nextPoint右边这一部分区域点都不能作为特征点if (weighted_distance < 0.1) {std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);}}}float diffPrevious = calcSquaredDiff(point, previousPoint);float dis = calcSquaredPointDistance(point);// 考虑A，B，C相邻三个点，AB距离，BC距离均大于B到激光距离一定比例，表明B点可能为离群点，位于过于倾斜的平面，不可靠的测量数据，if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {_scanNeighborPicked[i - startIdx] = 1;}}
}

将同一激光束索引划分子区域，得到子区域索引sp，ep，

      // 将同一激光束索引划分子区域，得到子区域索引sp epsize_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)+ (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)+ (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;// skip empty regions// 跳过空的子区域if (ep <= sp) {continue;}

根据论文中向量的归一化模值公式计算子区域中每个点的弯曲度并进行排序，弯曲度越大，对应索引存储位置越靠后。初始所有点的特征标志为次平面点。特征点的种类存放在_regionLabel中(边缘点，次边缘点，平面点，次平面点)

void BasicScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx)
{// resize bufferssize_t regionSize = endIdx - startIdx + 1;_regionCurvature.resize(regionSize);_regionSortIndices.resize(regionSize);// 初始所有点的标志为 次平面点_regionLabel.assign(regionSize, SURFACE_LESS_FLAT);// calculate point curvatures and reset sort indicesfloat pointWeight = -2 * _config.curvatureRegion;// 根据论文中 向量的归一化模值 计算每个点的弯曲度并存储for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) {float diffX = pointWeight * _laserCloud[i].x;float diffY = pointWeight * _laserCloud[i].y;float diffZ = pointWeight * _laserCloud[i].z;for (int j = 1; j <= _config.curvatureRegion; j++) {diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;}_regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;_regionSortIndices[regionIdx] = i;}// sort point curvatures// 根据弯曲度将索引排序，弯曲度越大，对应索引存储位置越靠后for (size_t i = 1; i < regionSize; i++) {for (size_t j = i; j >= 1; j--) {if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) {std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);}}}
}

然后根据弯曲度选取边缘点，具体操作如下

• 根据弯曲度大小提取边缘点，弯曲度越大越可能为边缘点，因此从后面开始寻找
• 未被标记 + 弯曲度满足阈值 + 边缘点最大数量未满足 --> 可被选为边缘点
• 边缘点，次边缘点附近左右curvatureRegion区域点进行标记

      // extract corner featuresint largestPickedNum = 0;// 根据弯曲度大小提取边缘点，弯曲度越大越可能为边缘点，因此从后面开始寻找for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) {// idx为_laserCloud中的索引  用于提取点云数据// scanIdx为_scanNeighborPicked中的索引   用于标记能够作为特征点// regionIdx为_regionCurvature，_regionLabel中的索引  用于提取弯曲度，标识特征类型size_t idx = _regionSortIndices[--k];size_t scanIdx = idx - scanStartIdx;size_t regionIdx = idx - sp;// 未被标记 + 弯曲度满足阈值 + 边缘点最大数量未满足 --> 可被选为边缘点// 边缘点，次边缘点附近左右curvatureRegion区域点进行标记if (_scanNeighborPicked[scanIdx] == 0 &&_regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) {largestPickedNum++;// 数量为达到则为边缘点CORNER_SHARP，否则为次边缘点CORNER_LESS_SHARPif (largestPickedNum <= _config.maxCornerSharp) {_regionLabel[regionIdx] = CORNER_SHARP;// 只存放边缘点_cornerPointsSharp.push_back(_laserCloud[idx]);} else {_regionLabel[regionIdx] = CORNER_LESS_SHARP;}// 存放边缘点和次边缘点_cornerPointsLessSharp.push_back(_laserCloud[idx]);// 对选取点附近左右curvatureRegion区域点进行标记markAsPicked(idx, scanIdx);}}

对平面特征点同样有

• 根据弯曲度大小提取平面点，弯曲度越小越可能为边缘点，因此从前面开始寻找
• 未被标记 + 弯曲度满足阈值 + 边缘点最大值未满足 --> 可被选为平面点
• 平面点附近左右curvatureRegion区域点进行标记

     // extract flat surface featuresint smallestPickedNum = 0;// 根据弯曲度大小提取平面点，弯曲度越小越可能为边缘点，因此从前面开始寻找for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) {// idx为_laserCloud中的索引  用于提取点云数据// scanIdx为_scanNeighborPicked中的索引   用于标记能够作为特征点// regionIdx为_regionCurvature，_regionLabel中的索引  用于提取弯曲度，标识特征类型size_t idx = _regionSortIndices[k];size_t scanIdx = idx - scanStartIdx;size_t regionIdx = idx - sp;// 未被标记 + 弯曲度满足阈值 + 边缘点最大值未满足 --> 可被选为平面点// 平面点点附近左右curvatureRegion区域点进行标记if (_scanNeighborPicked[scanIdx] == 0 &&_regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {smallestPickedNum++;_regionLabel[regionIdx] = SURFACE_FLAT;// 只存放平面点_surfacePointsFlat.push_back(_laserCloud[idx]);markAsPicked(idx, scanIdx);}}// extract less flat surface features// 存放所有平面点，次平面点。也就是非边缘点，次边缘点的所有点for (int k = 0; k < regionSize; k++) {if (_regionLabel[k] <= SURFACE_LESS_FLAT) {surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);}}

最后对所有的次平面点进行降采样

    // down size less flat surface point cloud of current scan// 对次平面点进行降采样得到_surfacePointsLessFlatpcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);downSizeFilter.filter(surfPointsLessFlatScanDS);_surfacePointsLessFlat += surfPointsLessFlatScanDS;

到此，所有的特征点存放在_cornerPointsSharp、_cornerPointsLessSharp、_surfacePointsFlat、_surfacePointsLessFlat(经过滤波)中。

5.4.4 打包IMU数据

这个时候需要对IMU变换数据进行存储打包，把imu信息存储在点云格式的数据中，在激光里程计中还会加以使用。

• 获取点云中第一个点时对应的激光姿态角_imuStart，获取点云中最后一个点时对应的激光姿态角_imuCur
• 获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 位置偏移 开始点对应的IMU局部坐标系imuShiftFromStart
• 获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 速度偏移 开始点对应的IMU局部坐标系imuVelocityFromStart
• 点云已经去除了非匀速运动，为什么还需要呢？因为后面估计的是激光运动，不是点云运动，因此必须要在激光里程计中恢复原来的非匀速运动偏移

void BasicScanRegistration::updateIMUTransform()
{// 更新IMU变换的数据，可以供激光里程计中使用// 获取点云中第一个点时对应的激光姿态角_imuTrans[0].x = _imuStart.pitch.rad();_imuTrans[0].y = _imuStart.yaw.rad();_imuTrans[0].z = _imuStart.roll.rad();// 获取点云中最后一个点时对应的激光姿态角_imuTrans[1].x = _imuCur.pitch.rad();_imuTrans[1].y = _imuCur.yaw.rad();_imuTrans[1].z = _imuCur.roll.rad();Vector3 imuShiftFromStart = _imuPositionShift;// 世界坐标系到IMU局部坐标系转换rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);// 获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 位置偏移 开始点对应的IMU局部坐标系_imuTrans[2].x = imuShiftFromStart.x();_imuTrans[2].y = imuShiftFromStart.y();_imuTrans[2].z = imuShiftFromStart.z();Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;// 世界坐标系到IMU局部坐标系转换rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);// 获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 速度偏移 开始点对应的IMU局部坐标系_imuTrans[3].x = imuVelocityFromStart.x();_imuTrans[3].y = imuVelocityFromStart.y();_imuTrans[3].z = imuVelocityFromStart.z();
}

5.5发布数据

最后将得到点云数据以及IMU数据进行发布。_pubLaserCloud是近似匀速模型下的所有点云数据。其他的前面已经涉及到了，就不再赘述。

void ScanRegistration::publishResult()
{auto sweepStartTime = toROSTime(sweepStart());// publish full resolution and feature point clouds// 发布点云数据，特征点云数据publishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera");publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera");publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera");publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera");publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera");// publish corresponding IMU transformation information// 发布IMU测量的激光位姿变换数据publishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera");
}

更新日志

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。

• 2020-12-07
  整理完成LaserOdometry节点内容，地址请转到LOAM源代码分析附公式推导之LaserOdometry
  最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！
• 2020-10-11
  整理完成MultiScanRegistration节点内容
• 2020-10-09
  第一次编辑，初版