我已经通过icp计算出激光雷达各相邻帧之间的旋转平移矩阵,现想通过位姿图做回环优化以达到整个建图首尾相连的效果。总共有62帧,第62帧和第0帧我也求出了转换矩阵。但目前我测试时发现,在我没加回环时,及没添加62和0的边,g2o也对我的所有转换矩阵进行优化,但是往更糟糕的方向优化了。并且更离谱的是,将0到24所有帧添加到顶点和边,就没有进行优化,但是0到25所有帧添加到顶到和边,就进行了负优化。
关键代码如下:






以下是0-62帧优化前的建图:可以证明保存到文件里的矩阵是没有问题的。

以下是0-24帧优化后的建图:正常

以下是0-25帧优化后的建图:不正常,连带影响到了前面的帧

第25帧是肯定没问题,我也做了25-35帧建图优化,是正常的,而且,就算25帧的旋转矩阵有问题,但是我还没添加回环,按理说还没有优化项,g2o不应该进行优化才对,第25帧也不应该会影响到前面的帧啊。
实在是想不通为什么了,故来请教下各位,望解惑,万分感谢!