分享

SLAM的那些坑——PNP

 木俊 2018-04-23



计算机视觉

1. 内参数标定   张正友标定法

2. 外参数标定即姿态估计问题。从一组2D点的映射中估计物体的3D姿态。

3. 从三个对应点中恢复姿态,需要的信息是最少的,称为“三点透视问题”即P3P。同理,扩展到N个点,就称为“PnP”。

4. 基于视觉的姿态估计根据使用的摄像机数目分为单目视觉和多目视觉。根据算法又可以分为基于模型的姿态估计和基于学习的姿态估计。

5. OpenCV中有solvePnP以及solvePnPRansac用来实现已知平面四点坐标确定摄像头相对世界坐标系的平移和旋转。cvPOSIT基于 正交投影,用仿射投影模型近似透视投影模型,不断迭代计算出估计值。此算法在物体深度相对于物体到相机的距离比较大的时候,算法可能不收敛。

6. 从世界坐标系到相机坐标系的转换,需要矩阵[R|t],其中R是旋转矩阵,t是位移向量。如果世界坐标系为X,相机坐标系对应坐标为X',那么X' = [R|t]*X。从相机坐标系到理想屏幕坐标系的变换就需要内参数矩阵C。那么理想屏幕坐标系L = C*[R|t]*X。如何获得[R|t],大致是已知模板上的几个关键点在世界坐标系的坐标即X已知,然后在摄像头捕获的帧里获得模板上对应点在屏幕坐标系的坐标即L已知,通过求解线性方程组得到[R|t]的初值,再利用非线性最小二乘法迭代求得最优变换矩阵[R|t]。

7. 大多数情况下,背景是二维平面,识别的物体也是二维平面。对于ARToolkit,识别的Targets就是平面的(但是这种方法鲁棒性不好)。如果内参数矩阵是已知的,那么知道4个或者更多共面不共线的点就可以计算出相机的姿态。

8. 相机姿态估计的问题就是寻找相机的外参数,即是最小化误差函数的问题。误差函数有的基于image-space,有的基于object-space。

9. RPP算法基于object-space为误差函数提供了一种可视化的方法。误差函数有两个局部极小值。在无噪声条件下,第一个局部极小值跟正确的姿态对 应。另外的误差函数的极小值就是标准姿态估计算法为什么会抖动的原因。由于姿态估计算法最小化误差函数总是要使用迭代算法,因此需要一个初值。如果初值接 近第二个局部极小值,那么迭代算法就收敛到错误的结果。

10. 估计第一个姿态,RPP算法使用任何已知的姿态估计算法,在这里里,使用迭代算法。从第一个姿态使用P3P算法估计第二个姿态。这个姿态跟误差函数的第二 个局部极小值接近。使用估算的第二个姿态作为初值,使用迭代算法获得第二个姿态。最终正确的姿态是有最小误差的那个。

11. 这类问题最终都是解线性方程组AX=b的问题。当b∈R(A)时,x=A的广义逆*b;当b∈不R(A)时,能否是Ax接近b呢,即是否有x使||Ax-b||最小,习惯上用2-范数即欧式范数来度量。最小二乘解常存在,然后这样的解未必是唯一的。在方程无解的情况下,要找到最优解。就是要最小化所有误差的平方和,要找拥有最小平方和的解,即最小二乘。最小化就是把误差向量的长度最小化。

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

问题的方案、解决思路

最小PnP问题

P3P问题中假设没有噪声,使用几何约束,可以解得相机的位姿。不具有唯一解。
P4P问题中分为线性方法和基于P3P的方法。

最小二乘的观点

  • 迭代最小化一个代价函数(平方误差)。这些方法相对于之前的方法更加准确,在一定的噪声的情况下,返回一个最大似然估计。
  • 直接最小二乘方法DLS

常用方法

  • NPL: The N-Point Linear (NPL) method of Ansar and Daniilidis [1].
  • EPnP: The approach of Lepitit et al. [16].
  • SDP: The Semi Definite Program (SDP) approach of Schweighofer and Pinz [23].
  • DLS: The Direct Least-Squares (DLS) solution presented in this paper. An open source implementation of DLS is available at www.umn.edu/ ̃joel
  • DLS-LM: Maximum-likelihood estimate, computed using iterative Levenberg-Marquardt (LM) minimization of the sum of the squared reprojection errors, initialized with DLS.


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


____P3P____  P3P的方法,是用非常经典的<Complete solution classification for the perspective-three-point problem> 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代入四组解验证就算重投影误差最小的一组解,即为输出的结果.

关于PNP问题就是指通过世界中的N个特征点与图像成像中的N个像点,计算出其投影关系,从而获得相机或物体位姿的问题。

 

以下讨论中设相机位于点Oc,P1、P2、P3……为特征点。

 

Case1:当N=1时

当只有一个特征点P1,我们假设它就在图像的正中央,那么显然向量OcP1就是相机坐标系中的Z轴,此事相机永远是面对P1,于是相机可能的位置就是在以P1为球心的球面上,再一个就是球的半径也无法确定,于是有无数个解。

Case2:当N=2时

现在多了一个约束条件,显然OcP1P2形成一个三角形,由于P1、P2两点位置确定,三角形的变P1P2确定,再加上向量OcP1,OcP2从Oc点射 线特征点的方向角也能确定,于是能够计算出OcP1的长度=r1,OcP2的长度=r2。于是这种情况下得到两个球:以P1为球心,半径为r1的球A;以 P2为球心,半径为r2的球B。显然,相机位于球A,球B的相交处,依旧是无数个解。

Case3:当N=3时

与上述相似,这次又多了一个以P3为球心的球C,相机这次位于ABC三个球面的相交处,终于不再是无数个解了,这次应该会有4个解,其中一个就是我们需要的真解了。

Case4:当N大于3时

N=3时求出4组解,好像再加一个点就能解决这个问题了,事实上也几乎如此。说几乎是因为还有其他一些特殊情况,这些特殊情况就不再讨论了。N>3 后,能够求出正解了,但为了一个正解就又要多加一个球D显然不够"环保",为了更快更节省计算机资源地解决问题,先用3个点计算出4组解获得四个旋转矩 阵、平移矩阵。根据公式:

将第四个点的世界坐标代入公式,获得其在图像中的四个投影(一个解对应一个投影),取出其中投影误差最小的那个解,就是我们所需要的正解。


位姿估计的使用范围非常广泛。主要解决的问题为:在给出2D-3D若干点对以及相片的内参信息,如何求得相机中心在世界坐标系下的坐标以及相机的方向(旋转矩阵)。




    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。
    转藏 分享 献花(0

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多