LOAM源码解析4——transformMaintenance

最新推荐文章于 2025-05-20 16:07:40 发布

原创

最新推荐文章于 2025-05-20 16:07:40 发布 · 876 阅读

5 ·

CC 4.0 BY-SA版权

本文详细解析LOAM（Lidar Odometry and Mapping）中transformMaintenance部分，主要关注两个关键回调函数laserOdometryHandler和odomAftMappedHandler。前者处理激光雷达里程计信息，将位姿转换到世界坐标系；后者负责融合不同频率的里程计和地图优化位姿，确保TransformMaintenance的发布频率与laserOdometry一致。

这一部分主要为了：融合后的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}

最低0.47元/天解锁文章