【3D激光SLAM】LOAM源代码解析--transformMaintenance.cpp

系列文章目录

·【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp
·【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp
·【3D激光SLAM】LOAM源代码解析–laserMapping.cpp
·【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp

---

写在前面

本系列文章将对LOAM源代码进行讲解，在讲解过程中，涉及到论文中提到的部分，会结合论文以及我自己的理解进行解读，尤其是对于其中坐标变换的部分，将会进行详细的讲解。

本来是懒得写的，一个是怕自己以后忘了，另外是我在学习过程中，其实没有感觉哪一个博主能讲解的通篇都能让我很明白，特别是坐标变换部分的代码，所以想着自己学完之后，按照自己的理解，也写一个LOAM解读，希望能对后续学习LOAM的同学们有所帮助。

之后也打算录一个LOAM讲解的视频，大家可以关注一下。

---

---

整体框架

LOAM多牛逼就不用多说了，直接开始

先贴一下我详细注释的LOAM代码，在这个版本的代码上加入了我自己的理解。

我觉得最重要也是最恶心的一部分是其中的坐标变换，在代码里面真的看着头大，所以先明确一下坐标系(都是右手坐标系)：

• IMU(IMU坐标系imu)：x轴向前，y轴向左，z轴向上
• LIDAR(激光雷达坐标系l)：x轴向前，y轴向左，z轴向上
• CAMERA(相机坐标系，也可以理解为里程计坐标系c)：z轴向前，x轴向左，y轴向上
• WORLD(世界坐标系w，也叫全局坐标系，与里程计第一帧init重合)：z轴向前，x轴向左，y轴向上
• MAP(地图坐标系map，一定程度上可以理解为里程计第一帧init)：z轴向前，x轴向左，y轴向上

坐标变换约定： 为了清晰，变换矩阵的形式与《SLAM十四讲中一样》，即：$R_{A\_B}$表示B坐标系相对于A坐标系的变换，B中一个向量通过$R_{A\_B}$可以变换到A中的向量。

首先对照ros的节点图和论文中提到的算法框架来看一下：

可以看到节点图和论文中的框架是一一对应的，这几个模块的功能如下：

• scanRegistration：对原始点云进行预处理，计算曲率，提取特征点
• laserOdometry：对当前sweep与上一次sweep进行特征匹配，计算一个快速(10Hz)但粗略的位姿估计
• laserMapping：对当前sweep与一个局部子图进行特征匹配，计算一个慢速(1Hz)比较精确的位姿估计
• transformMaintenance：对两个模块计算出的位姿进行融合，得到最终的精确地位姿估计

本文介绍transformMaintenance模块，它就是将laserOdometry和laserMapping两个模块优化得到的当前帧相对于初始帧的坐标变换进行融合，从而得到最终的最优的坐标变换结果。

---

一、变量含义

首先，介绍一下本程序用到变量的含义，与laserMapping一致：

• transformBefMapped[6]：从laserMapping模块接收到的，优化前的当前帧相对于初始时刻的位姿变换$T_{init\_end}$
• transformSum[6]：从laserOdometry模块接收到的，当前帧相对于初始时刻的变换$T_{init\_start}$
• transformAftMapped[6]：经过laserMapping模块优化后的，当前帧相对于初始时刻的位姿变换$T_{map\_end}$
• transformMapped[6]：融合后的当前帧相对于初始帧的坐标变换

一些理解：虽然transformAftMapped[6]我上面写的是$T_{map\_end}$，看起来好像是把坐标系换成了map坐标系，但是我觉得这里有两种理解都可以：

1. AftMapped可以理解为经过laserMapping模块优化后的里程计坐标系下的当前帧end相对于初始帧的坐标变换
2. 也可以理解为经过laserMapping模块优化，变到了map坐标系

二、main()函数

main函数依然很简单，就是定义了一些订阅者和发布者，接收/laser_odom_to_init和/aft_mapped_to_init两个坐标变换话题，然后进入相应的回调函数进行融合；然后发布融合后的当前帧相当于初始帧的坐标变换，以及坐标变换。

int main(int argc, char** argv)
{ros::init(argc, argv, "transformMaintenance");ros::NodeHandle nh;ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> ("/aft_mapped_to_init", 5, odomAftMappedHandler);ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);pubLaserOdometry2Pointer = &pubLaserOdometry2;laserOdometry2.header.frame_id = "/camera_init";laserOdometry2.child_frame_id = "/camera";tf::TransformBroadcaster tfBroadcaster2;tfBroadcaster2Pointer = &tfBroadcaster2;laserOdometryTrans2.frame_id_ = "/camera_init";laserOdometryTrans2.child_frame_id_ = "/camera";ros::spin();return 0;
}

三、接收laserMapping的转换信息

接收/aft_mapped_to_init话题的回调函数很简单，就是将接收到的数据，赋值给transformAftMapped[6]和transformBefMapped[6]变量，这两个变量的含义与laserMapping中一致，就不过多解释了。

//接收laserMapping的转换信息
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformAftMapped[0] = -pitch;transformAftMapped[1] = -yaw;transformAftMapped[2] = roll;transformAftMapped[3] = odomAftMapped->pose.pose.position.x;transformAftMapped[4] = odomAftMapped->pose.pose.position.y;transformAftMapped[5] = odomAftMapped->pose.pose.position.z;transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}

四、接收laserOdometry的信息

这个回调函数主要是接收到/laser_odom_to_init话题后进行，先根据接收到的数据对相关变量进行赋值操作，然后进入到transformAssociateToMap()函数进行位姿变换融合，最后将融合后的位姿变换发布出去，发布的话题为：

• /integrated_to_init：融合后的当前帧相对于初始帧(世界坐标系)的位姿变换

另外，广播了/camera相对于/camera_init的坐标变换

//接收laserOdometry的信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);//得到旋转平移矩阵transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;transformSum[3] = laserOdometry->pose.pose.position.x;transformSum[4] = laserOdometry->pose.pose.position.y;transformSum[5] = laserOdometry->pose.pose.position.z;transformAssociateToMap();geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);laserOdometry2.header.stamp = laserOdometry->header.stamp;laserOdometry2.pose.pose.orientation.x = -geoQuat.y;laserOdometry2.pose.pose.orientation.y = -geoQuat.z;laserOdometry2.pose.pose.orientation.z = geoQuat.x;laserOdometry2.pose.pose.orientation.w = geoQuat.w;laserOdometry2.pose.pose.position.x = transformMapped[3];laserOdometry2.pose.pose.position.y = transformMapped[4];laserOdometry2.pose.pose.position.z = transformMapped[5];pubLaserOdometry2Pointer->publish(laserOdometry2);//发送旋转平移量laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}

五、位姿融合

这里的位姿融合部分与laserMapping中的求解地图坐标系中end时刻到初始时刻的初始猜测–transformAssociateToMap()函数完全一致。

1.求解位移增量
"transformBefMapped - transformSum"的含义是上一帧相对于初始帧的位移量 与 当前帧相对于初始帧的位移量 的差值，得到的结果是初始帧init坐标系下的位移增量$t_{init}^{start-end}$。

然后将其变换到end时刻：
$$\begin{gathered} t_{init}^{start-end}=R_{end\_init}\ast t_{init}^{start-end}=R_{init\_end}^{-1}\ast t_{init}^{start-end} \\ {R}_{init\_end}^{-1}=R_{ZXY}^{-1}=R_{-rz}{R}_{-rx}{R}_{-ry} \end{gathered}$$
对应于下面代码中所示的变换。

2.求解旋转部分的融合
现在这里的变量含义分别表示为：

• transformSum：laserOdometry模块的当前帧相对于初始帧的变换$R_{init\_end}^L$
• transformBefMapped：laserMapping模块的当前帧相对于初始帧的变换$R_{init\_end}^M$
• transformAftMapped：laserMapping模块的优化后的当前帧相对于初始帧的变换，也可以理解为当前帧相对于地图坐标系的变换$R_{map\_start}^M$
• transformMapped：融合后的当前帧相对于初始帧的坐标变换$R_{map\_end}^F$

那么有如下坐标变换关系：
$$R_{map\_end}^F=R_{map\_end}^M\ast R_{init\_end}^{M-1}\ast R_{init\_end}^L=R_{ZXY}\ast R_{ZXY}^{-1}\ast R_{ZXY}$$

这里的计算公式与laserOdometry模块中的IMU修正部分完全一样：
$$R_{map\_end}^F=\left[\begin{array}{ccc}cacycacz+sacxsacysacz& cacysacz+sacxsacycacz& cacxsacy\\ cacxsacz& cacxcacz& -sacx\\ -sacycacz+sacxcacysacz& sacysacz+sacxcacycacz& cacxcacy\end{array}\right]$$
$$R_{map\_end}^M=\left[\begin{array}{ccc}cbcycbcz+sbcxsbcysbcz& cbcysbcz+sbcxsbcycbcz& cbcxsbcy\\ cbcxsbcz& cbcxcbcz& -sbcx\\ -sbcycbcz+sbcxcbcysbcz& sbcysbcz+sbcxcbcycbcz& cbcxcbcy\end{array}\right]$$
$$R_{init\_end}^{M-1}=\left[\begin{array}{ccc}cblycblz-sblxsblysblz& -cblxsblz& sblycblz+sblxcblysblz\\ -cblysblz+sblxsblycblz& cblxcblz& sblysblz-sblxcblycblz\\ -cblxsbly& sblx& cblxcbly\end{array}\right]$$
$$R_{init\_end}^L=\left[\begin{array}{ccc}calycalz+salxsalysalz& calysalz+salxsalycalz& calxsaly\\ calxsalz& calxcalz& -salx\\ -salycalz+salxcalysalz& salysalz+salxcalycalz& calxcaly\end{array}\right]$$

然后使用对应位置的值相等，就得到了修正后的累计变换acx、acy、acz，计算如下：
$$\begin{gathered} acx=-arcsin(R_{2,3})=-arcsin(-sbcx\ast (salx\ast sblx+calx\ast caly\ast cblx\ast cbly+calx\ast cblx\ast saly\ast sbly)-cbcx\ast cbcz\ast (calx\ast saly\ast (cbly\ast sblz-cblz\ast sblx\ast sbly)-calx\ast caly\ast (sbly\ast sblz+cbly\ast cblz\ast sblx)+cblx\ast cblz\ast salx)-cbcx\ast sbcz\ast (calx\ast caly\ast (cblz\ast sbly-cbly\ast sblx\ast sblz)-calx\ast saly\ast (cbly\ast cblz+sblx\ast sbly\ast sblz)+cblx\ast salx\ast sblz)) \\ acy=arctan(R_{1,3}/R_{3,3}) \\ acz=arctan(R_{2,1}/R_{2,2}) \end{gathered}$$

3.将位移增量转换到map坐标系
$$\begin{gathered} t_{map}^{increment}=R_{map\_end}^F\ast t_{end}^{increment} \\ {R}_{map\_end}^F=R_{ZXY}=R_y{R}_x{R}_z \end{gathered}$$

4.求解平移部分的初始猜测
这里注意一点：上面求出来的增量使用的事start时刻的累积位移减去end时刻的累计位移，所以这里在求解时也是减号，如下：
$$t_{map\_end}^F=t_{map\_end}^M+t_{map}^{end-start}=t_{map\_start}^M-t_{map}^{start-end}$$

我在上面声明变量时提到了：地图坐标系map，一定程度上可以理解为里程计第一帧init，这个意思就是可以理解为map坐标系和初始时刻坐标系init以及世界坐标系w是重合的，而laserMapping中虽然写的是变换到了map坐标系，也可以理解为仍然是当前帧end相对于初始帧init的坐标变换，只是经过了laserMapping模块优化，所以这里的$t_{map\_end}^F$也可以写成$t_{init\_end}^F$，这个解释只是为了符合作者代码中坐标变换时发布的是/camera_init到/camera的变换，所以这里写$t_{map\_end}^F$也没问题。

//odometry的运动估计和mapping矫正量融合之后得到的最终的位姿transformMapped
void transformAssociateToMap()
{//平移后绕y轴旋转（-transformSum[1]）float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 = transformBefMapped[4] - transformSum[4];float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);//绕x轴旋转（-transformSum[0]）float x2 = x1;float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;//绕z轴旋转（-transformSum[2]）transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;transformIncre[5] = z2;float sbcx = sin(transformSum[0]);float cbcx = cos(transformSum[0]);float sbcy = sin(transformSum[1]);float cbcy = cos(transformSum[1]);float sbcz = sin(transformSum[2]);float cbcz = cos(transformSum[2]);float sblx = sin(transformBefMapped[0]);float cblx = cos(transformBefMapped[0]);float sbly = sin(transformBefMapped[1]);float cbly = cos(transformBefMapped[1]);float sblz = sin(transformBefMapped[2]);float cblz = cos(transformBefMapped[2]);float salx = sin(transformAftMapped[0]);float calx = cos(transformAftMapped[0]);float saly = sin(transformAftMapped[1]);float caly = cos(transformAftMapped[1]);float salz = sin(transformAftMapped[2]);float calz = cos(transformAftMapped[2]);float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);transformMapped[0] = -asin(srx);float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0]));float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0]));x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];z1 = transformIncre[5];x2 = x1;y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;transformMapped[3] = transformAftMapped[3] - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);transformMapped[4] = transformAftMapped[4] - y2;transformMapped[5] = transformAftMapped[5] - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
}

总结

到此为止，整个LOAM的讲解就结束了！！

我的感觉就是看LOAM的论文，有一种“作者说的好有道理，确实就是这样啊”的感觉，但是如果要是让自己想，就想不出来这么牛逼的算法，它的代码也写的比较漂亮。

代码的运行就不单独开一篇文章写了，只要装好了依赖，编译很顺畅，也没报什么错，我找了一个数据集测试了一下，也没问题，测试的数据里放在了文章开头提到的我的github仓库的bag文件夹中，运行结果点云图放在了pcl文件夹中，放一张结果截图。