解决未知的二维旋转和平移

Question

我想在这里以及question的机器人技术变革中寻求帮助。到目前为止，这就是我从相机传感器检测到的特征点到地图点的转换过程，这里的特征是我通过将单应性矩阵与像素坐标相乘得出的二维均匀矢量。

map_point = T_world_origin.inverse() * T_world_gps * T_gps_camera * feature_point

在地图上有明显的重影（如下所示），并且由于T_world_gps是使用RTK GPS完成的，因此我怀疑T_gps_camera不正确。我希望解析地求解T_gps_camera（3x3同质矩阵），得出下面的等式，

M * T_world_origin.inverse() * T_world_gps_t * T_gps_camera * feature_point = T_world_origin.inverse() * T_world_gps_t+1 * T_gps_camera * feature_point

其中M是我使用时间t和t + 1处的地图点的ICP计算出的3x3矩阵。之后，我应该如何使用为T_GPS_camera解决的ICP信息？