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流程：