3D激光SLAM:位姿融合输出,LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。这里主要讲解如何通过imu来进行位姿融合输出的。
LIO-SAM的全称是:Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping,从全称上可以看出,该算法是一个紧耦合的雷达惯导里程计(Tightly-coupled Lidar Inertial Odometry),借助的手段就是利用GT-SAM库中的方法。
LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。在之前的博客讲解了imu如何进行预积分,最终以imu的频率发布了imu的预测位姿里程计。
主要讲解,最终是如何进行位姿融合输出的。
Eigen::Affine3f
其中功能的核心在于位姿间的变换,所以要了解 Eigen::Affine3f 部分的内容。
Affine3f 是eighen库的仿射变换矩阵。实际上就是:平移向量+旋转变换组合而成,可以同时实现旋转,缩放,平移等空间变换。
Eigen库中,仿射变换矩阵的大致用法为:
创建Eigen::Affine3f 对象a;
创建类型为Eigen::Translation3f 对象b,用来存储平移向量;
创建类型为Eigen::Quaternionf 四元数对象c,用来存储旋转变换。
最后通过以下方式生成最终Affine3f变换矩阵:a=b*c.toRotationMatrix();一个向量通过仿射变换时的方法是result_vector=test_affine*test_vector;
仿射变换包括:平移、旋转、放缩、剪切、反射。平移(translation)和旋转(rotation)顾名思义,两者的组合称之为欧式变换(Euclidean transformation)或刚体变换(rigid transformation);
放缩(scaling)可进一步分为uniform scaling和non-uniform scaling,前者每个坐标轴放缩系数相同(各向同性),后者不同;如果放缩系数为负,则会叠加上反射(reflection)——reflection可以看成是特殊的scaling;
刚体变换+uniform scaling 称之为,相似变换(similarity transformation),即平移+旋转+各向同性的放缩。
位姿融合输出
在imu预积分的节点中,在main函数里面 还有一个类的实例对象,那就是
其主要功能是做位姿融合输出,最终输出imu的预测结果,与上节中的imu预测结果的区别就是:该对象的融合输出是基于全局位姿的基础上再进行imu的预测输出。全局位姿就经过回环检测后的lidar位姿。
建图优化会输出两种激光雷达的位姿:lidar增量位姿和lidar全局位姿。
lidar增量位姿就是通过lidar的匹配功能,优化出的帧间的相对位姿,通过相对位姿的累积,形成世界坐标系下的位姿。
lidar全局位姿则是在帧间位姿的基础上,通过回环检测,再次进行优化的 世界坐标系下的位姿,所以对于增量位姿,全局位姿更加精准。
在前面提到的发布的imu的预测位姿是在lidar的增量位姿上基础上预测的,那么为了更加准确,本部分功能就预测结果,计算到基于全局位姿的基础上面。首先看构造函数:
判断lidar帧和baselink(通常baselink指车体系)帧是不是同一个坐标系,如果不是,查询一下lidar和baselink 之间的 tf变换,ros::Time(0) 表示最新的,等待两个坐标系有了变换,更新两个的变换 lidar2Baselink:
订阅地图优化节点的全局位姿和预积分节点的增量位姿:
发布两个信息 odomTopic ImuPath,然后看第一个回调函数 lidarOdometryHandler:
将全局位姿保存下来,将ros的odom格式转换成 Eigen::Affine3f 的形式,将最新帧的时间保存下来,第二个回调函数是 imuOdometryHandler,imu预积分之后所发布的imu频率的预测位姿:
建图的话,可以认为map坐标系和odom坐标系是重合的(初始化时刻):
发布静态tf,odom系和map系,他们是重合的:
imu得到的里程计结果送入到这个队列中:
如果没有收到lidar位姿就return:
弹出时间戳小于最新lidar位姿时刻之前的imu里程计数据:
计算最新队列里imu里程计的增量:
增量补偿到lidar的位姿上去,就得到了最新的预测的位姿:
分解成平移 + 欧拉角的形式:
发送全局一致位姿的最新位姿:
更新tf:
更新odom到baselink的tf:
发布imu里程计的轨迹,控制一下更新频率,不超过10hz,将最新的位姿送入轨迹中,把lidar时间戳之前的轨迹全部擦除,发布轨迹,这个轨迹实践上是可视化imu预积分节点输出的预测值。
Result
其中粉色的部分就是imu的位姿融合输出path。