SLAM的那些坑——PNP

跟随这高博的脚步走到了pose estimation 求相机的外参, 参考了几篇blog和paper, 总算稍微有点管中窥豹的感觉, 暂且记录下来以待后续

PNP 相机位姿估计pose estimation就是通过几个已知坐标(世界坐标)的特征点,结合他们在相机照片中的成像(像素坐标),求解出相机所在的世界坐标以及旋转角度(这两个货叫相机的外参), 用旋转矩阵(R)和平移矩阵(t)表示. PnP, which is short for perspective n point就是其中的一种2D-3D解决上述问题的算法, 也就是说, 这个算法的输入是一组点的三维世界坐标和二维像素坐标,输出是相机的旋转矩阵(R)和平移矩阵(t). OpenCV有相应的库函数solvePnP()和solvePnPRansac(),后者利用了ransac的思想计算更加精确的结果,不过据说速度很慢,完全没法做到实时.

PNP具体细分为几种, 直接线性变换 P3P EPnP UPnP等等, OpenCV目前提供了3种方法进行PNP计算, CV_P3P, CV_ITERATIVE, CV_EPNP.

____P3P____  P3P的方法,是用非常经典的 published by Gao in 2003的一篇论文,用空间中3个已知世界坐标和像素坐标的点,通过构造四个三角形的边角关系联立方程组,求出4组可能的解,再通过第四个已知点对四组解进行验证,返回重投影误差最小的点.

单纯看这个图真的会很费解,其实就是三角形三边关系公式 a²+b²-2ab*cos∠c=c²,假如我们做特征匹配的两帧图是frame1和frame2,那么ABC三个点就是匹配的3对特征点的世界地图坐标位置,即frame1上的A1.B1.C1匹配frame2上的A2B2C2. P点是frame2的相机所在位置的相机光心. 这个方程组里的已知条件是|AB||BC||AC|的长度,因为ABC三点的世界坐标已知; 和∠α.∠β,∠γ,因为A2B2C2的像素坐标已知,(通过像素坐标和内参矩阵求到光心距离然后构造三角形求角).
右侧的make是一堆为了化简式子所做的assumptions, 最后得到的二元二次方程组里,除了x,y剩下的量都是已知,解这个方程组需要用到二十世纪八十年代的一篇论文介绍的吴消元法, 简单看了一下,大体上的思路基本都没看懂,就交给opencv吧...反正我知道这个方程组最多有四组x,y的解, 然后用frame1和frame2上的另外一组匹配点D1D2代入四组解验证就算重投影误差最小的一组解,即为输出的结果.