相机姿态估计(七)--UPnP
论文:Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation
整体来看,UPnP跟EPnP差不多,只是同时估计了焦距,因此,适合未标定场合,Uncalibrated PnP.
问题定义:
p世界坐标系点参考点,C控制点,R,t姿态矩阵,u是p对应的2D投射点。建模为最小化P点的投影误差。
其中u表示为:
k是尺度因子。
跟EPnP类似,增加控制点约束:
注意,这里如上图所示,c是在世界坐标系下。同样,相机坐标系下,c的上标变为c。
3式带入2式,展开得:
到这里,跟EPnP的差别在于焦距, 本质 是一样的,也得到线性方程组:
M也是2n*12的矩阵,得到x:
上面的方程中,四个控制点总共12个未知变量,M为2n×12的矩阵。因此,x属于M的右零空间,vi为矩阵M的右奇异向量,可以通过求解MTM 的零空间特征值得到。
[说明]使用MTM比使用M计算量更少,因为MTM是求解是常数复杂度,而M是O(n3)的复杂度,但是计算MTM的复杂度是O(n)的。
上面求解的x中,需要确定βk,也就是确定合适的线性组合。根据参考点的位置不同,矩阵MTM的零空间维数可能为N=1→4维。求解β的策略是控制点在坐标系Fw和Fc中,两两之间的距离是相同,而x的3k+1−3k分量表示分别表示不同的控制点在相机坐标系中的坐标,总共有C24=6个约束。
同样,也是分类讨论。
N=1,线性化,此时只要求解β1和f,假定β11=β1^2,βff11=f^2β^2,根据控制点距离约束:
可以得到6个如下形式的方程:
其中b=[β11,βff11]T .
计算得到:
当N=2时,穷举线性化,我们需要求β1,β2,f. 则类似N=1的情况,此时b变为:
与N=1类似,只是在相同的相机内外参数约束下,不断穷举,例如:
当N=3时:穷举再线性化。
再线性化技术如下:
以定义线性系统开始:
其中,
具体算法伪代码如下:
后面,为了提高精度,增加迭代优化:
完整的对比结果:
不共面:
上图可以看出,在点数超过20时,其实UPnP与EPnP没有太大差距,但是,UPnP更适合大焦距图像的姿态估计。当f>4000时,差距就出现了,但一般常见的f在500~2000之间,也就是近似估计为480i~1080p范围。在这种情况下,EPnP与UPnP性能差不多。
共面:
共面情况下,结论也类似,2者差距不大。
顺便提一下时间:
作者在实测一个任务时,得到的结果如下:
另外:
其中颜色标注对应上面不共面情况下的算法类别。
可以看到,UPnP在时间上跟EPnP比,是没有优势的。