PNP的学习-P3P

PNP方法是为了解决在当前两帧图像中，已知前一帧图像上的3dLandmark点和当前帧的2d特征点，求取当前帧的pose。

PNP主要有P3P、EPNP、UPNP、DLT、MRE(LS Iteration,g2o).

一、p3p

p3p主要是根据针孔模型原理，即3d点投影到相机归一化坐标系再汇集到光心，如下图所示：

因此根据上右图中的模型，A,B,C为3D点，已知a'=|BC|,b'=|AC|,c'=|AB|，α=<PB,PC>,β=<PA,PB>,γ=<PA,PC>，其中角度可由归一化平面内对应的点求出。令a'²=ac'²,b'²=bc'².

再令未知数X=PA,YPB,Z=PC,X=xZ,Y=yZ,c'²=vZ²,p=2cosα,q=2cosβ,r=2cosγ,则a'²=ac'²=avZ²,b'²=bc'²=bvZ².

然后根据三角形的余弦定理，分别在三个三角形中列方程：

Y²+Z²-2YZcosα=a'²

X²+Z²-2XZcosβ=b'²

 Y²+X²-2YXcosγ=c'²

 同时有一个约束方程：P,A,B,C不共面：p²+q²+r²-pqr-1≠0;必须满足这个约束条件

再将三个方程替换成以下形式：

(1-a)y²-ax²+axyr-yp+1=0

(1-b)x²-by²+bxyr-xq+1=0

上式中，只有x和y是未知数，且为二元二次方程，采用吴零点分解方法（可参考论文），求解出x,y以及之前的v,最后求出X,Y,Z:

Z=c'/√v,X=xZ,Y=yZ;

求出3d点到光心的距离之后（即3d点在相机坐标系下的据光心的距离），根据三角形相似原理，分别求出A,B.C点在相机坐标系下的坐标。

 

最后就是ICP问题，3d-3d，求R,t.最后将求出的R，t 再做非线性迭代，即可得到精度一般的pose。最后附上p3p流程：