对于已知3D点和2D点的匹配关系,进而计算相机位姿的方法有两大类,一类是直接求解法,包括DLS、PNP等;一类是基于BA的优化方法;
1、直接法
直接法可以利用opencv中已有的函数solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false)进行求解;
2、BA的方法
对于BA方法实际上是通过优化最小误差进行实现,而优化方法可以有多种方式,包括采用高斯-牛顿法、g2o、ceres等工具库,这里介绍高斯-牛顿法和g2o两种方法;
无论哪种方法首先需要给出优化误差,具体如下:
为了简化计算,这里将
考虑单个投影点误差:
对其线性化后:
雅可比的形式如下:
这里的P'为P在相机坐标系下的坐标:
对P'进行投影
这里