//odometry计算的转移矩阵(实时高频量)
float transformSum[6] = {0};
//平移增量
float transformIncre[6] = {0};
//经过mapping矫正过后的最终的世界坐标系下的位姿
float transformMapped[6] = {0};
//mapping传递过来的优化前的位姿
float transformBefMapped[6] = {0};
//mapping传递过来的优化后的位姿
float transformAftMapped[6] = {0};

//ros发布、坐标系、里程计、位姿转换
ros::Publisher *pubLaserOdometry2Pointer = NULL;
tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
nav_msgs::Odometry laserOdometry2;
tf::StampedTransform laserOdometryTrans2;

二、位姿转换 transformAssociateToMap

位姿转换主要是将odometry的运动估计和mapping矫正量融合，主要步骤如下

步骤1：计算两次激光里程计的平移增量transformIncre。由于是基于匀速运动模型的假设，所以运动增量为transformBefMapped[3] - transformSum[3]。由于这两个数组中的位姿态都是基于世界坐标系（/camera_init）下的，所以需要将点云从世界坐标系转换到当前时刻的imu坐标系下，变换矩阵为 $R^{T}=R(z)^{T}R(x)^{T}R(y)^{T}$

$\begin{bmatrix} \Delta x_{Inc} \\ \Delta y_{Inc} \\ \Delta z_{Inc} \end{bmatrix} = \begin{bmatrix} cos\gamma & sin\gamma & 0 \\ -sin\gamma & cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\alpha & sin\alpha\\ 0 & -sin\alpha & cos\alpha \end{bmatrix} \begin{bmatrix} cos\beta & 0 & -sin\beta \\ 0 & 1 & 0 \\ sin\beta & 0 & cos\beta \end{bmatrix} \begin{bmatrix} x_{bef}-x_{sum}\\ y_{bef}-y_{sum}\\ z_{bef}-z_{sum} \end{bmatrix}$

步骤2：计算地图map与世界坐标系（/camera_init）的矩阵

$(T_{k}^{m})_{w-BefMap}= (T_{k-1}^{m})_{w-AftMap} \cdot ((T_{k-1}^{o})_{w-Odom})^T \cdot (T_{k}^{o})_{w-Odom}$

最终的旋转矩阵 $R=R(y)^{ }R(x)^{ }R(z)^{ }$

$\begin{bmatrix} cosey \cdot cosez + siney \cdot sinex \cdot sinez & cosez \cdot siney \cdot sinex - cosey \cdot sinez & cosex \cdot siney \\ cosex \cdot sinez & cosex \cdot cosez & -sinex \\ cosey \cdot sinex \cdot sinez - cosez \cdot siney & siney \cdot sinez + sinex \cdot cosey \cdot cosez & -cosey \cdot cosex \end{bmatrix}$

记为

$R= \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{bmatrix}$

则上述旋转矩阵转换成欧拉角的计算公式为

$\beta _{y} = \theta _{y} = -asin(R_{23})$

$\alpha _{x} = \psi _{x} = atan2 \left ( \frac{R_{13}}{cos\theta _{y}}, \frac{R_{33}}{cos\theta _{y}} \right )$

$\gamma _{z} = \phi _{x} = atan2 \left ( \frac{R_{21}}{cos\theta _{y}}, \frac{R_{22}}{cos\theta _{y}} \right )$

代码中的的对应关系如下

srx对应 $R_{23}$
srycrx对应 $R_{13}$
crycrx对应 $R_{33}$
srzcrx对应 $R_{21}$
crzcrx对应 $R_{22}$

步骤3：在得到imu里程计预测的map在世界坐标系（/camera_init）的位姿transformMapped（经过mapping矫正过后的最终的世界坐标系下的位姿），还需要加上步骤1中计算的世界坐标系下的平移增量，完成位姿更新。由于平移增量transformIncre是在当前时刻的imu坐标系下，需要转换到世界坐标系，变换矩阵为 $R=R(y)R(x)R(z)$ 。

$\begin{bmatrix} x_{fMap-new} \\ y_{fMap-new} \\ z_{fMap-new} \end{bmatrix} = \begin{bmatrix} x_{fMap} \\ y_{fMap} \\ z_{fMap} \end{bmatrix} - \begin{bmatrix} cos\beta & 0 & sin\beta \\ 0 & 1 & 0 \\ -sin\beta & 0 & cos\beta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\alpha & -sin\alpha\\ 0 & sin\alpha & cos\alpha \end{bmatrix} \begin{bmatrix} cos\gamma & -sin\gamma & 0 \\ sin\gamma & cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \Delta x_{Inc} \\ \Delta y_{Inc} \\ \Delta z_{Inc} \end{bmatrix}$

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

三、接收信息 Handler

接收laserOdometry的信息

//接收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的转换信息

//接收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;
}

四、主函数 main

main函数的作用如下

transformMaintenance节点的初始化
订阅laserOdometry节点发布的/laser_odom_to_init消息（Lidar里程计估计位姿到初始坐标系的变换）；订阅laserMapping节点发布的/aft_mapped_to_init消息（laserMapping节点优化后的位姿到初始坐标系的变换）
发布/integrated_to_init消息

//主函数
int main(int argc, char** argv)
{
  //ros节点初始化
  ros::init(argc, argv, "transformMaintenance");
  ros::NodeHandle nh;

  //ros订阅信息
  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发布信息
  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;
}

结语

至此，已经把transformMaintenance.cpp的内容解析完了意味着LOAM代码解析的已经结束。

上述内容还有几处不太理解的，如果有人能够解答，就请给我留言吧，十分感谢。

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

一些碎碎念

在代码解析过程中，作为一个从未接触过激光雷达的我来说，真的是需要学习非常多新的知识，论文中的难点就在于坐标系的转换，坐标系转换经常会把我整得晕乎乎的，我也有好几处地方没有完全弄懂。

虽然这件事情非常之难，没有什么大师指导，没有什么资金支持，但这件事情我没有感到丝毫后悔，因为我确实在这学习过程中得到许多，不仅仅是知识，更多的是对于一件事情、一个项目的处理。

只有不断学习新知识，才能是自己成长。

在这过程中，周围充斥这不理解和误会，总会站在他们的立场上认为我是傻子，为什么要去做一些这么难的事，好像就应该与他们一样，昏昏沉沉浑浑噩噩地得过且过即可，完成相应的任务，刷刷今天的新闻和热点，装作自己很忙，实际上做的都是“拧上一颗螺丝钉，卸下一颗螺丝钉，抱怨做得这么多，薪水却没有这么多”。

我不想成为这样的人。

标签：pose,transformMaintenance,LOAM,float,transformMapped,cblz,transformSum,cbly,解析
来源： https://blog.csdn.net/weixin_44041199/article/details/111166527