关于LIO与RTK对齐

[minghongss]

我看了您这里是fix了第一帧。后面当gpsfactor数量超过了20的时候，会将gps加入执行优化，从而实现对齐，那么在gpsfactor数量超过20的前面这一段时间的位姿cloudKeyPoses6D和cloudKeyPoses3D应该都是在lio局部的世界坐标系下。 在加入了gpsfactor后，后面新加入的位姿才会因为转到enu系下，那么在这之前的位姿是不进行更新了吗？这样的话岂不是就出现了位姿跳变的情况吗

[JokerJohn]

@hongSS0919 注意，这是批量的图优化，优化结果朝误差最小化的方向去，如果你定义的残差e = T_gps.translation - T_lio.translation，你的factor graph中，LIO只做between factor约束两帧的相对位姿(并不是绝对！！！)，RTK作为位置先验prior factor（绝对位置）。按这个推导雅可比的话，并且你的noise model设置合理的话，误差最小会朝着RTK位置的方向(ENU系)上走，图优化后的位姿就会在RTK坐标系下，并且连续帧位姿靠近LIO相对位姿，这才是融合，注意区分不同因子类型。

“在加入了gpsfactor后，后面新加入的位姿才会因为转到enu系下，那么在这之前的位姿是不进行更新了吗？这样的话岂不是就出现了位姿跳变的情况吗”，因子图整体更新，而非滤波器只考虑2帧，如果你的LIO相对位姿约束够强的话，就不会跳变。

[JokerJohn]

@hongSS0919 看看GPS factor怎么实现的吧，自己写一遍解析求导，整个过程你就明白了，这是SLAMer的基本能力，趁这个简单案例练一下，再和源代码对比。但在实际工程中，有的时候需要快速验证，直接数值求导就行了(看截图2)，不需要自己推导雅可比了。

参考我知乎的评论。 有哪些GPS和Lidar SLAM融合的建图算法？ - Catalina的回答 - 知乎 https://www.zhihu.com/question/507251261/answer/2291977850

[minghongss]

感谢您的回答! “并且连续帧位姿靠近LIO相对位姿，这才是融合，”这句话我没太理解大佬。 我知道因子图会整体更新，但是给的图中的红色框位姿，我看这个是个先验因子，按照我浅薄的理解，这个是为了固定第一帧，避免轨迹整体漂移的情况。那这个你在加入了gps factor之后第一帧这个位姿都被fix了，他是如何实现对齐的呢？ 因子图会整体更新，但是我看了一下代码cloudKeyPoses6D和cloudKeyPoses3D这两个容器里存的历史位姿并没有更新，只有触发回环的时候才会更新，要是没有回环，岂不是就一直不更新了么

[JokerJohn]

@hongSS0919 回顾下这个因子图，闭环与这里讨论的无关不考虑。LIO的相对位姿构建了between factor，它是一个二元因子，同时约束连续的两个位姿节点。正常来说固定第一帧，你的因子图才能求解(这里还涉及到可观性的一些内容，看状态估计书)，不然会欠定，所以一般第一帧会给个先验因子（一元因子，约束位姿节点位置+旋转）。现在GPS只约束位置(x y z)，形成了gps factor，它也是个一元因子，但只约束对应node的位置，所以你可以认为他就是先验因子，只是命名为GPS Factor而已，你也可以自己命名PriorPositon factor。这里你弄混淆了。 每一次ISAM增量更新后，会在correctPoses函数里main循环中逐个更新历史帧位姿信息。初始化并不意味着就已经没有误差了，随着后续GPS factor越来越多，前面的历史位姿都会跟着调整的，就会对齐得越来越好，这与闭环无关。

[minghongss]

感谢回复！ 第二个问题更新已经解决。correctPoses函数中的aLoopIsClosed在每次加入gps factor时都会置为true。最开始我只看变量名的时候以为是产生回环才会触发历史关键帧位姿的更新。抱歉，这是我的疏忽！ 第一个问题我还是没有很理解大佬，我是不是可以理解成这样，下面这两个先验因子在isam因子图中其实就是同一个因子，因为他两的索引是一样（都是为0），因此在我加入了第0个gps factor时，他只是改变了因子图中对应第1个先验因子位置部分的值，而后面没有存在对应索引的先验因子，就直接添加，而不是改变。 我这样理解有问题吗大佬

大佬我想问问lio轨迹和gps轨迹为啥能是同一个起点？按理来说lio轨迹的起点应该是单位阵的旋转＋（0，0，0）的位置

[JokerJohn]

@hongSS0919 哎，因为第一帧与第二帧之间有LIO的相对位姿约束，权重为w1，第一帧同时有先验因子，权重w2，第一帧如果有GPS，还有GPS位置约束，权重为w3. 第一帧位置究竟在哪里，取决于这三者共同作用。一般w1约束最强。你回去看看factor graph的一些基本理论吧。

[minghongss]

@hongSS0919 哎，因为第一帧与第二帧之间有LIO的相对位姿约束，权重为w1，第一帧同时有先验因子，权重w2，第一帧如果有GPS，还有GPS位置约束，权重为w3. 第一帧位置究竟在哪里，取决于这三者共同作用。一般w1约束最强。你回去看看factor graph的一些基本理论吧。

好的，谢谢

[JokerJohn]

@hongSS0919 这里索引0对应的node节点，表示的是一个个的状态，新加入的一个factor代表一个残差，整个因子图优化最终会变为最小二乘问题。看看我这个论文的uncertainty propagate in factor graph章节，https://ieeexplore.ieee.org/document/10480308

[minghongss]

@hongSS0919 这里索引0对应的node节点，表示的是一个个的状态，新加入的一个factor代表一个残差，整个因子图优化最终会变为最小二乘问题。看看我这个论文的uncertainty propagate in factor graph章节，https://ieeexplore.ieee.org/document/10480308

嗯嗯，我下来在琢磨琢磨，看下您的论文，十分感谢！