SLAM：loam 3D激光里程计与三维环境地图的重建（四）

这篇文章是LOAM代码系列的最后一篇，分析一下代码的第四个节点transformMaintenance。打开代码一看，只有200+行，是不是很开心！

先拿出节点图来回顾一下节点间的关系:

可以看到该节点订阅了laserOdometry节点发布的/laser_odom_to_init消息（Lidar里程计估计位姿到初始坐标系的变换）以及laserMapping节点发布的/aft_mapped_to_init消息（laserMapping节点优化后的位姿到初始坐标系的变换），经过节点处理发布/integrated_to_init消息。

所以要搞清楚这个transformMaintenance节点到底是干嘛的，我们首先需要弄清楚它所发布的消息到底是什么含义。

这个节点的主函数非常简单：

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

可以看出/integrated_to_init消息是由发布器pubLaserOdometry2发布的，实际上就是由发布器pubLaserOdometry2Pointer发布的。我们顺藤摸瓜，找到pubLaserOdometry2Pointer的发布函数，发现每次接收到/laser_odom_to_init消息并调用回调函数laserOdometryHandler时，就发布一次该消息。

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;

  // Lidar里程计估计位姿转到世界坐标系下
  transformAssociateToMap();

  // Lidar在世界坐标系下的姿态
  geoQuat = tf::createQuaternionMsgFromRollPitchYaw
            (transformMapped[2], -transformMapped[0], -transformMapped[1]);

  // 发布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);

  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优化后的Lidar位姿。

但是！！！这里还是有个小坑的。正如我们刚才所说的，这个节点接收了两个消息，分别是laserOdometry节点和laserMapping节点发布的，而这两个节点发布的频率不同啊！！

看图可以知道，论文中说1Hz和10Hz，实际是5Hz和10Hz。所以这个/integrated_to_init消息到底是什么？实际它就是融合后的Lidar轨迹，说白了就是：有优化结果了就拿这一时刻的优化结果作为轨迹，没有优化结果只有里程计结果了，就直接拿里程计结果作为这一时刻的轨迹。

这部分真的很简单，剩下部分的代码也都比较直观了，相信大家都能读懂。

到此，LOAM的四个节点就都介绍完了，相信大家跟我一样，对这个方法都有了更为深刻的认识，也希望大家能多多探讨交流，思想的交流总会碰撞出意想不到的火花！

SLAM：loam 3D激光里程计与三维环境地图的重建（四）

猜你喜欢