[笔记]三维激光SLAM学习一(5)——LOAM代码transformMaintenance篇
[笔记]三维激光SLAM学习一(5)——LOAM代码transformMaintenance篇
这部分是LOAM代码的第四个节点,也是最后一篇。
先回顾一下节点图:
该节点订阅了以下消息:
- /laser_odom_to_init(Lidar里程计估计位姿到map坐标系的变换)
- /aft_mapped_to_init消息(laserMapping节点优化后的位姿到map坐标系的变换)
发布以下消息:
- /integrated_to_init
下面我们来分析一下它所发布的消息到底是什么含义。
Main函数
这个节点主函数比较简单
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 时,就发布一次该消息。
回调函数laserOdometryHandler
//接收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 优化后的Lidar位姿。
总结
这个 /integrated_to_init 消息实际就是融合后的Lidar轨迹。
说白了就是:
- 有优化结果:拿这一时刻的优化结果作为轨迹;
- 没有优化结果:直接拿里程计结果作为这一时刻的轨迹。
Ps.在下一篇文章里,会总结一下LOAM的细节和关键点,然后就转入下一个坑里。