lidar、深度相机与imu的相对位姿标定

lidar、深度相机与imu相对位姿标定

 

   如图所示， 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。

代码（略）