如何计算从GPS坐标系到SLAM坐标系的旋转的旋转矩阵

如何解决如何计算从GPS坐标系到SLAM坐标系的旋转的旋转矩阵

在我们的SLAM算法中，我们希望将GPS坐标旋转到SLAM坐标系，以便可以将这些GPS位置用于单目SLAM（比例校正）中的光束调整。

我们已按照以下步骤进行轮换。

1. 使用Att库将纬度，经度，XYZ GPS坐标转换为Geoconverter坐标。 （重置原点与SLAM坐标原点相同）
2. 因此，对于原点后的第一个矢量，使用rodrigues rotation公式计算XYZ GPS坐标和SLAM坐标之间的旋转矩阵。
3. 然后，对于每个XYZ GPS位置，使用旋转矩阵来计算SLAM坐标系中的GPS坐标。

但是，结果与预期不符，GPS坐标平面与SLAM坐标不对齐，因此束调整失败。

要检查旋转后GPS坐标的实际位置，我们绘制了它们（如下所示）。 Front view (GPS(olive),SLAM(blue),MapPoints(black)) Without MapPoints Side view to understand GPS and SLAM are in different plane Side view with MapPoints Side view different angle

您能指出该过程中的错误还是建议另一种过程来对齐GPS数据。

谢谢

Saurabh Kumar