EPnP算法

EPnP算法

 

原文见附件：https://www.cnblogs.com/jian-li/p/5689122.html

复制后公式都看不清了。

 

相机坐标系用FcFc，世界坐标系用FwFw表示，任何一点可以用四个控制点pwipiw表示

 

pwi=∑j=14αijcwj,with∑j=14αij=1(1)(1)piw=∑j=14αijcjw,with∑j=14αij=1

对于相机坐标系同样成立

pci=∑j=14αijccj,with∑j=14αij=1(2)(2)pic=∑j=14αijcjc,with∑j=14αij=1

对于上面的公式，首先需要说明的是αijαij确实存在。因为cwjcjw或ccjcjc构成的方程组是欠定的，所以一定存在解。
理论上来说，控制点可以随便选择，这里选择控制点为参考点的中心，其他的点在PCA得到的主轴上单位长度处，从而提高算法的稳定性。

控制点在相机坐标系的坐标

根据投影方程得到世界坐标系中参考点坐标和相机坐标系中参考点的约束关系：

 

∀i,ωi[ui 1]=Apci=A∑j=14αijccj(3)(3)∀i,ωi[ui 1]=Apic=A∑j=14αijcjc

写成矩阵的形式为：

∀i,ωi[ui vi 1]=[fu0uc 0fvvc 001]∑j=14αij[xcj ycj zcj](4)(4)∀i,ωi[ui vi 1]=[fu0uc 0fvvc 001]∑j=14αij[xjc yjc zjc]

将等式的第三列代入第一二列，得到

∑j=14αijfuxcj+αij(uc−ui)zcj=0(5)(5)∑j=14αijfuxjc+αij(uc−ui)zjc=0

 

∑j=14αijfvycj+αij(vc−vi)zcj=0(6)(6)∑j=14αijfvyjc+αij(vc−vi)zjc=0

因此，可以得到下面的线性方程组：

Mx=0,withx=[ccT1,ccT2,ccT3,ccT4]T(7)(7)Mx=0,withx=[c1cT,c2cT,c3cT,c4cT]T

上面的方程中，四个控制点总共12个未知变量，MM为2n×122n×12的矩阵。因此，xx属于MM的右零空间，vivi为矩阵MM的右奇异向量，可以通过求解MTMMTM的零空间特征值得到。

x=∑i=1Nβivi(8)(8)x=∑i=1Nβivi

[说明]使用MTMMTM比使用MM计算量更少，因为MTMMTM是求解是常数复杂度，而MM是O(n3)O(n3)的复杂度，但是计算MTMMTM的复杂度是O(n)O(n)的。

选择合适的线性组合

上面求解的xx中，需要确定βiβi，也就是确定合适的线性组合。根据参考点的位置不同，矩阵MTMMTM的零空间维数可能为N=1→4N=1→4维。求解ββ的策略是控制点在坐标系FwFw和FcFc中，两两之间的距离是相同，而xx的3k+1−3k3k+1−3k分量表示分别表示不同的控制点在相机坐标系中的坐标，总共有C24=6C42=6个约束。
如果N=1N=1，则根据约束有

 

∥βv[i]−βv[j]∥2=∥cwi−cwj∥2(9)(9)‖βv[i]−βv[j]‖2=‖ciw−cjw‖2

所以

β=∑[i,j]∈[1;4]∥v[i]−v[j]∥⋅∥cwi−cwj∥∑[i,j]∈[1;4]∥v[i]−v[j]∥2β=∑[i,j]∈[1;4]‖v[i]−v[j]‖⋅‖ciw−cjw‖∑[i,j]∈[1;4]‖v[i]−v[j]‖2

如果N=2N=2，

∥β1v[i]1+β2v[i]2−(β1v[j]1+β2v[j]2)∥2=∥cwi−cwj∥2(10)(10)‖β1v1[i]+β2v2[i]−(β1v1[j]+β2v2[j])‖2=‖ciw−cjw‖2

由于β1β1和β2β2只以二次项出现在方程中，记β=[β21,β1β2,β22]Tβ=[β12,β1β2,β22]T， ρρ的每一项为∥cwi−cwj∥2‖ciw−cjw‖2，得到相面的方程

Lβ=ρ(11)(11)Lβ=ρ

其中LL是由v1v1和v2v2构成的6×36×3的矩阵。
上面的方程可以通过β=(LTL)−1LTρβ=(LTL)−1LTρ得到，然后通过选择合适的符号从β21,β1β2,β22β12,β1β2,β22使得所有的pcipic有正的zz坐标。

如果N=3N=3则和N=2N=2差不多，唯一的区别在于使用的是LL的逆，而不是伪逆，此时的LL为6×66×6的矩阵。

G-N优化

前面的步骤可以得到目标点在相机坐标系中的闭式解，作为G-N优化的初始值，优化的变量为β=[β1,⋯,βN]Tβ=[β1,⋯,βN]T，目标函数为

 

Error(β)=∑(i,j)s.t.i<j(∥cci−ccj∥2−∥cwi−cwj∥2)(12)(12)Error(β)=∑(i,j)s.t.i<j(‖cic−cjc‖2−‖ciw−cjw‖2)

该优化过程和参考点的数目无关，优化步骤和时间是常数。

计算R，t

前面的两步计算不同维数的零空间的误差，选择误差最小维数对应的ββ，从而得到xx，恢复出控制点在相机坐标系中的坐标并根据质心坐标系数得到参考点在相机坐标系的坐标。剩下的工作就是已知一组点云在两个坐标系中的坐标，求两个坐标系的位姿变换。
步骤如下：
（1）求中心点，pcc=∑picNpcc=∑pciN，pcw=∑piwNpwc=∑pwiN；
（2）去中心，qic=pic−pcc,qiw=piw−pcwqci=pci−pcc,qwi=pwi−pwc；
（3）计算HH矩阵，H=∑Ni=1qicqiTwH=∑i=1NqciqwiT
（4）对HH进行SVD，H=UΛVTH=UΛVT；
（5）计算X=VUTX=VUT，如果det(x)=1det(x)=1，则R=XR=X，t=Pcc−RPcwt=Pcc−RPwc。否则R(2,⋅)=−R(2,⋅)