如图所示， O1、O2为IMU的传感器或者旋转平台定义的坐标系统，O1经过T2 变换到O2；S1、S2为深度相机或激光lidar的坐标系，S1经过T1 变换到S2;其中T1为寻找两次观测同名点解算得到，T2为IMU的传感器，或者编码器测量得到。
原理
首先根据几何关系很容易得到一下四个公式：
将(3)(4)代入(2)中，再将(1)代入得到： 所以重点是如何求解方程（5）中的X，这个问题可看成是求解最小二乘问题：
求解求使误差平方和达到极小的X。当然这个最小二乘问题也是比较容易求解的，我们可以做如下转化：
所以AY=0就属于齐次坐标的最小二乘问题，对A做SVD分解，就可以的得到Y。
代码（略）