LOAM论文和程序代码的解读（2）

文章：LOAM: Lidar Odometry and Mapping in Real-time

作者：Ji Zhang and Sanjiv Singh

编译：robinvista

来源：https://blog.csdn.net/robinvista/article/details/104379087?spm=1001.2014.3001.5501

欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。未经博主同意请勿擅自转载。

论文阅读模块将分享点云处理，SLAM，三维视觉，高精地图相关的文章。公众号致力于理解三维视觉领域相关内容的干货分享，欢迎各位加入我，我们一起每天一篇文章阅读，开启分享之旅,有兴趣的可联系微信dianyunpcl@163.com。

程序代码

第一篇文章在：LOAM论文和程序代码的解读　程序代码可见LOAM_NOTED，分为四个部分。这四个部分基本是独立的，只通过ROS的消息交换数据。

　　① scanRegistration.cpp的功能是对激光点云预处理，包括移除空点、对运动畸变补偿、计算每个点的水平和垂直角度并分成不同的scan、然后就是最重要的曲率计算、根据曲率提取出特征点，最后把特征点发布出来就不管了；

　　② laserOdometry.cpp的功能是通过求解最小二乘问题进行点云匹配，从而得到状态估计，包括畸变补偿（上面是用IMU补偿）、查找最近的对应特征点、计算点到直线和点到平面的距离、求雅克比矩阵、迭代解最小二乘问题、最后把估计的状态发布出来就不管了；

　　③ laserMapping.cpp的功能是不断拼接每帧点云建立地图，包括对点云坐标变换、然后还是求雅克比矩阵和解最小二乘问题这一套、最后把环境地图发布出来就不管了；

　　④ transformMaintenance.cpp的功能是用第3步得到的结果矫正第2步的状态估计，得到更准确的位姿，都是一些坐标变换和相乘，没什么太复杂的，最后把矫正后的位姿发布出来就不管了；

scanRegistration模块代码

　ShiftToStartIMU函数的功能是：利用IMU计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变，后面用来补偿。平移的畸变如下，变量前带imu的都是IMU测量的数据。imuShiftStart是开始时刻IMU测得的当前的雷达位置，开始时刻定义成每帧扫描开始的时候，imuShiftCur是IMU测得的当前时刻的雷达位置，我只用很小一段时间内的，所以不关心累积误差。这里用了一阶速度积分来近似计算中间时刻点雷达的运动位置。

imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)

TransformToStartIMU函数利用ShiftToStartIMU函数通过IMU得到的补偿值对点云进行补偿。先根据当前IMU测得的角度将点变换到全局坐标系，再用扫描开始时刻IMU测得的角度将点变换到激光雷达局部坐标系。

　　laserCloudHandler函数是话题调用函数，每当点云数据发布的时候就被调用。这个函数是整个文件最主要的部分，它对特征点进行了处理，其中调用了前面提到的ShiftToStartIMU、TransformToStartIMU等函数。

　　main函数就订阅velodyne_points和imu两个话题、发布laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat四个话题代表的特征点。main函数使用了ros::spin()而不是设置固定循环频率，这意味着它的运行频率取决于外部话题的频率。除此以外，还发布了velodyne_cloud_2话题，这是原始点云经过畸变补偿后的点云，我以后有机会对比一下看看补偿的效果怎么样。

imuHandler

imuHandler是个消息回调函数，如下。它的功能是处理IMU数据。虽然LOAM本身不用IMU也能跑，但是用了精度肯定会更好（ALOAM版本中为了简单就没有使用IMU）。IMU传感器自身的坐标系与激光雷达一样，也是X轴向前，Y轴向左，Z轴向上。

ros::Subscriber subImu = nh.subscribe("/imu/data", 50, imuHandler);

通过下面两行代码可以从IMU中提取出欧拉角来（存储在变量roll, pitch, yaw中）。这个欧拉角是相对于哪个坐标系的呢？自然是相对于ROS中的全局世界坐标系的。那这里就有一个问题了，getRPY是ROS自带的函数，那么ROS默认的坐标系姿态与LOAM规定的一样吗？应该不一样，因为LOAM使用的坐标系并不常见。LOAM使用的坐标系是Z轴向前，X轴向左，Y轴向上。所以这里要小心了！下面讲解一下。

tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

首先研究一下函数getRPY，它的定义如下图所示。图中说的很清楚了，roll, pitch, yaw分别是绕一个固定坐标系的XYZ轴的转动角度。如果没错的话，这个固定的坐标系就是ROS的全局坐标系了，它是X轴向东，Y轴向北，Z轴向上的。明白了这个规定，我们就知道了，IMU的坐标系相对于ROS全局坐标系的姿态就是通过首先绕（全局坐标系的）X轴转动roll，再绕（全局坐标系的）Y轴转动pitch，再绕（全局坐标系的）Z轴转动yaw得到的。

　　我们可以用欧拉角roll, pitch, yaw像上面这样构造一个旋转矩阵看看对不对，在Mathematica软件中输入以下代码即可：

{X,Y,Z}=IdentityMatrix[3];
R=RotationMatrix[yaw,Z].RotationMatrix[pitch,Y].RotationMatrix[roll,X]

如果我们想把全局坐标系的一个向量表示在IMU的坐标系中，可以利用逆运算。例如，全局坐标系下的重力向量肯定是沿Z轴的反方向的，即(0,0,-9.81)，那么表示在在IMU的坐标系中就是如下图所示的结果：

　　三个轴再交换一下，转换成LOAM要求的坐标系，刚好就是imuHandler函数中出现的样子：

  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;

laserOdometry代码

TransformToStart函数的功能与TransformToStartIMU差不多，但是后者是用IMU进行补偿、前者是用激光雷达自身估计的状态进行补偿。显然用IMU更准确、TransformToStart适用于速度比较平滑的情况。

　　main函数是主体、功能是状态估计、也可以叫里程计算。其中使用了ros::Rate rate(100)，说明这个程序的运行是固定间隔的，按照rate中设定的频率循环运行。注意这里使用了spinOnce()，要区分与spin()的不同。

　　在机器人领域，坐标系之间的变换是很常见同时也是容易出错的，即便老手也是一样。在论文和程序中采用了z轴朝前、x轴朝左、y轴朝上的坐标系，雷达局部坐标系L LL和全局世界坐标系W都是这样的，作者这样定义应该是想与相机一致，因为相机的坐标系一般定义成z 轴朝前、垂直于镜头。而velodyne 16激光雷达默认采用的坐标系是x轴朝前、y 轴朝左、z轴上，所以需要进行坐标变换。laserOdometry中在计算雅克比矩阵时出现了一坨坐标变换。根据这篇文章，我们知道作者采用了Y_1，X_2， Z_3 欧拉角表示旋转变换，如下。至于作者为什么选择这种表达方式，我也不知道。注意旋转变换的表示方式与坐标系的定义是两码事，别搞混了。

　　可以在数学软件中验证它是怎么来的，我以Mathematica软件为例，输入以下命令：

RotationMatrix["1", {0, 1, 0}].RotationMatrix["2", {1, 0, 0}].RotationMatrix["3", {0, 0, 1}] /. {Cos[x_] :>Subscript[c, x], Sin[x_] :> Subscript[s, x]} // MatrixForm

　　计算结果如下，可见与上面的公式一样。

掌握齐次变换矩阵的同学都知道，矩阵依次在右侧相乘表示每次变换都在前一次的基础上，矩阵依次在左侧相乘表示每次变换都在最开始的基础上，不管怎么理解，结果总是一样的。

　　计算雅克比矩阵需要对这个矩阵求导，我们在数学软件中求导试试。关于x轴转动变量的求导命令如下。我也不知道为什么要加负号。

D[(RotationMatrix[-y, {0, 1, 0}].RotationMatrix[-x, {1, 0, 0}].RotationMatrix[-z, {0, 0, 1}]), x] /. {Cos[x_] :> Subscript[c, x], Sin[x_] :> Subscript[s, x]} // MatrixForm

　　求导的结果如下：

　　与代码中的arx中的计算公式完全相同，我已经检验过了。剩余的ary、arz计算过程同理，我就不列举了。

laserMapping代码

main函数是主体，其中也使用了ros::Rate rate(100)，100的意思是一秒钟运行100次，也就是说这个程序也是按照固定的频率运行的。但是程序执行显然不会到100Hz这么快，所以这里设置为100是为了及时处理消息，有消息来就马上处理，不等待。

参考文献

[1]　An ICP variant using a point-to-line metric.