关于LOAM中的 transformAssociateToMap（）函数

最新推荐文章于 2023-09-11 21:09:49 发布

匍匐的狗仔

最新推荐文章于 2023-09-11 21:09:49 发布

阅读量390

点赞数

本文链接：https://blog.csdn.net/er_dan_love/article/details/123669685

版权

雷达里程计位姿变换世界坐标系地图优化位姿融合

关键词由CSDN通过智能技术生成

由于每次都会忘记所以写下来，如果不对，望大家指正。

就是如何通过雷达里程计的当前位姿以及雷达里程计的上一帧的位姿和经过map优化之后的上一帧的位姿，注意以上都是在世界坐标系下的位姿状态。

先介绍一下简单的位姿转换的知识：假设我们已知第 $i$ 帧在世界坐标系下的位姿，记为 $T_{wi}$ ,读作 $i->w$ 的位姿变换（以下都是这样的含义），又知道第 $j$ 帧在世界坐标系下的位姿，记为 $T_{wj}$ ，时间顺序上先 $i$ 后 $j$ 。那么由第 $j$ 帧到第 $i$ 帧的位姿变换如何得到呢？有如下的公式：

$T_{ij}= T^{-1}_{wi} \cdot T_{wj}$ 。

// transformSum为当前里程计信息
// transformBefMapped 上次里程计信息
// transformAftMapped 上次map中的位置
// transformTobeMapped 为里程计估计的当前时刻的位置
//根据我们上面提到的公式来计算

假设有一个点 $P_{j}$ ，可以通过里程计的位姿信息来转到上一帧记为 $P_{i}$ 。

// transformSum 这里的这个量为当前里程计的位姿信息，可能我这几个物理量的理解有点错误，但是数学知
//识应该是没有错误的。
// transformBefMapped 上次里程计信息
// transformAftMapped 上次map中的位置
// transformTobeMapped 待优化量
//Pi = transformBefMapped.inv() * transformSum * Pj;
//也可以由优化之后的位姿变量来得到
//Pi = transformAftMapped.inv() * transformTobeMapped * Pj;
//那么就有
//transformAftMapped.inv() * transformTobeMapped = transformBefMapped.inv()*transformSum；
//transformTobeMapped=transformAftMapped*transformBefMapped.inv()*transformSum；