LOAM源码解析4——transformMaintenance

这一部分主要为了：融合后的Lidar轨迹（里程计位姿+地图优化的位姿）

main函数
订阅的两个消息节点：
1、laserOdometry节点发布的/laser_odom_to_init消息（Lidar里程计估计位姿到初始坐标系的变换）
2、laserMapping节点发布的/aft_mapped_to_init消息（laserMapping节点优化后的位姿到初始坐标系的变换）
发布的节点：发布/integrated_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;
}

接下来主要介绍两个回调函数。
先从重要参数开始:

//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::Publisher *pubLaserOdometry2Pointer = NULL;// 发布里程计
tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;// tf坐标转换
nav_msgs::Odometry laserOdometry2;// 里程计
tf::StampedTransform laserOdometryTrans2;//tf里程计转换

laserOdometryHandler()

处理激光雷达里程计信息。

1. 对收到的消息进行解析，得到roll,pitch,yaw
2. 得到旋转平移矩阵transformSum
3. 将Lidar里程计估计的位姿转换到世界坐标系下transformMapped
4. pubLaserOdometry2Pointer节点发布Lidar在世界坐标系下的位姿
5. tfBroadcaster2Pointer节点发送旋转平移量

//接收laserOdometry的信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
  // 1.对收到的消息进行解析，得到roll,pitch,yaw
  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);

  // 2.得到旋转平移矩阵transformSum
  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;

  // 3.将Lidar里程计估计的位姿转换到世界坐标系下transformMapped
  transformAssociateToMap();
  // Lidar在世界坐标系下的位姿geoQuat
  geoQuat = tf::createQuaternionMsgFromRollPitchYaw
            (transformMapped[2], -transformMapped[0], -transformMapped[1]);
  
  // 4.pubLaserOdometry2Pointer节点发布Lidar在世界坐标系下的位姿
  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);

  // 5.tfBroadcaster2Pointer节点发送旋转平移量
  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);
}

odomAftMappedHandler()

void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
  // 1.对收到的消息进行解析，得到roll,pitch,yaw
  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);
  // 2.transformAftMapped
  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;
  // 3.transformBefMapped
  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;
}

接收到来自里程计和地图的两个节点的频率是不同的，laser_odom为10HZ，aft_mapped为5HZ，有优化结果了就拿这一时刻的优化结果作为轨迹，没有优化结果只有里程计结果了，就直接拿里程计结果作为这一时刻的轨迹。最终TransformMaintenance的发布频率和laserOdometry的发布频率是一致的。

参考：
https://blog.csdn.net/qq_21842097/article/details/81094922
https://zhuanlan.zhihu.com/p/30003078