精华内容
下载资源
问答
  • 相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确定摄像机在世界参考坐标系中的位置和方位,通常会产生4对解,然后使用第4对点消除歧义。大多数解决方案是先求出对应的2D点在当前...


    P3P简介

    相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确定摄像机在世界参考坐标系中的位置和方位,通常会产生4对解,然后使用第4对点消除歧义。大多数解决方案是先求出对应的2D点在当前相机坐标系下的3D坐标,然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿,即直接线性变换法。还有另一种P3P的解法,即Kneip算法,Kneip是一种close-form的P3P求解方式,主要思想是引入相机和世界坐标系的中间坐标系来计算他们之间的相对姿态和位置来得到相机的姿态,相比其他求解方法,速度快,求解稳定。

    参考论文:
    A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation of Absolute Camera Position and Orientation)

    直接线性变换法

    像点坐标与对应的3D点坐标的关系如下:
    在这里插入图片描述
    整理可得像素点坐标u,v的表达式,如下:
    在这里插入图片描述
    进一步推导可得:
    在这里插入图片描述
    一共12个未知参数,每对3D-2D对应点提供2个线性约束,因此共需要至少6对3D-2D对应点,才可以求得r1,r2,r3,即求得矩阵T=[KR,Kt]。

    接下来就是根据求得的矩阵T来求解内参、旋转、平移矩阵。

    已知T(:,1:3)=KR,且内参矩阵K为上三角矩阵,R为正交矩阵。对矩阵T(:,1:3)-1进行QR分解,分别得到R-1,以及K-1,进而得到内参矩阵K以及旋转矩阵R。

    已知T(:,4)=Kt,以及内参矩阵K,可得平移向量t。

    Kneip算法

    算法原理及流程

    • 创建新的相机坐标系
      在这里插入图片描述 =300x200如上图所示,P1,P2,P3是世界坐标系中的三个点,C为相机中心,相机分别指向三个点的单位向量为 f 1 ⃗ \vec{f1} f1 , f 2 ⃗ \vec{f2} f2 , f 3 ⃗ \vec{f3} f3 。创建新的相机坐标系为 τ \tau τ =(C, t x ⃗ \vec{tx} tx , t y ⃗ \vec{ty} ty , t z ⃗ \vec{tz} tz ),其中
      在这里插入图片描述
      通过旋转变换T = [ t x ⃗ \vec{tx} tx , t y ⃗ \vec{ty} ty , t z ⃗ \vec{tz} tz ]T将单位向量 ν \nu ν转换到 τ \tau τ
      在这里插入图片描述

    • 创建新的世界坐标系
      在这里插入图片描述
      如上图所示,创建新的世界坐标系 η \eta η=(P1, n x ⃗ \vec{nx} nx , n y ⃗ \vec{ny} ny , n z ⃗ \vec{nz} nz ),其中
      在这里插入图片描述
      通过旋转矩阵N=[ n x ⃗ \vec{nx} nx , n y ⃗ \vec{ny} ny , n z ⃗ \vec{nz} nz ]T将世界坐标系的点转换到 η \eta η坐标系下。
      在这里插入图片描述

    • 相机中心在平面中的表达
      在这里插入图片描述
      如上图P1,P2,C三个点构成一个平面Ⅱ,定义 β \beta β= f 1 ⃗ \vec{f1} f1 · f 2 ⃗ \vec{f2} f2
      根据几何约束
      在这里插入图片描述
      相机中心C在平面Ⅱ中可以表达为
      在这里插入图片描述

    • 相机中心和姿态在新建世界坐标系中表达
      在这里插入图片描述
      如上图,新建相机坐标系 τ \tau τ的基向量在平面中Ⅱ的表达为
      在这里插入图片描述
      n x ⃗ \vec{nx} nx 的旋转矩阵可以表达为
      在这里插入图片描述
      相机在中心C在坐标系 η \eta η的表达为
      在这里插入图片描述
      η \eta η τ \tau τ 的旋转表达为
      在这里插入图片描述

    • 将第3个点从新建世界坐标系 η \eta η转换到新建相机坐标系 τ \tau τ

      已知在这里插入图片描述定义
      在这里插入图片描述

      在这里插入图片描述

    • 构建四次方程并求解
      最终可以得到一个关于cos θ \theta θ的四次方程
      在这里插入图片描述
      其中
      在这里插入图片描述
      每一个cos θ \theta θ对应一个cot α \alpha α一共可以得到4组解
      在这里插入图片描述
      通过第4个点可以选择出正确的解

    基于RANSAC的Kneip算法

    基于RANSAC的Kneip算法流程如下:

    1. 计算RANSAC采样次数,设置内点阈值(重投影误差)
    2. 随机采样三对3D-2D对应点,计算相机的姿态
    3. 计算每个视角中的重投影误差,统计内点个数
    4. 重复2,3步直到满足采样次数,选择内点数最多的相机姿态
    展开全文
  • 相机位姿求解——P3P问题

    千次阅读 2018-01-24 17:06:00
    1、位姿求解是计算机视觉中经常遇到的,Perspective-n-Points, PnP(P3P)提供了一种解决方案,它是一种由3D-2D的位姿求解方式,即需要已知匹配的3D点和图像2D点。目前遇到的场景主要有两个,其一是求解相机相对于某2...

      1、位姿求解是计算机视觉中经常遇到的,Perspective-n-Points, PnP(P3P)提供了一种解决方案,它是一种由3D-2D的位姿求解方式,即需要已知匹配的3D点和图像2D点。目前遇到的场景主要有两个,其一是求解相机相对于某2维图像/3维物体的位姿,具体的如AR应用,人脸跟踪等;其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。

      这里要说明的是在场景1中,我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点,因此求得的是相机(相机坐标系)相对于真实物体(世界坐标系)的位姿,如图所示:

     

    而在场景2中,通常输入的是上一帧中的3D点(在上一帧的相机坐标系下表示的点)和这些3D点在当前帧中的投影得到的2D点,所以它求得的是当前帧相对于上一帧的位姿变换,如图所示:

     两种情况本质上是相同的,都是基于已知3D点和对应的图像2D点求解相机运动的过程。下面详细探讨P3P的求解过程。

      2、我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵,而是先求出对应的2D点在当前相机坐标系下的3D坐标,然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。P3P的求解是从余弦定理开始的,设相机坐标中心为点P,A、B、C为不共线的三个3D点,D为验证3D点,根据余弦定理有如下公式:

    接下来其实是对上述3个式子消元化简的过程,同时除以

    并且使得

    则可得:

    然后再次进行替换,另:

    可得:

    将第一个式子代入第2,3式,可以化简得到:

     

    接下来的过程就是如何通过上述两个式子求解A,B,C在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量,输入的是3D-2D的坐标,也即

     

    都是已知的。因为首先AB,BC,AC的距离都是可以根据输入的3D点求得,而输入的2D点可以求解三个余弦值(如何求解,像素坐标根据相机内参矩阵和畸变参数可以求得在归一化图像平面上的3D坐标,此时 z=1,故余弦值可求)。此时未知数仅x,y两个,所以理论上两个未知数两个方程,是可求的。(从x,y求PA,PB,PC也可求)

      3、具体的求解过程:

      3.1、首先是根据2D坐标求解余弦值得过程,首先是由像素坐标到归一化图像坐标的转变,根据就是相机模型

    然后是L2归一化的过程,我们知道求解角度的时候用的是归一化坐标(此归一化非彼归一化,上面是归一化到z值等于1的平面上,这里讲的是数学上的归一化)

    有了上述值就可以求解余弦值了

    同理可求。

      3.2、根据3D坐标求解AB,AC,BC的值,以AB为例

    AC,BC同理可求,所以v,w也可以求解。

       3.3、接下来就是一个二元二次方程的求解,比较难求,但是这在数学上是可以求解的,需要用到Wu Ritt的零点分解方法,它可以将原方程等效成一组特征列(Characteristic Serial, CS),凡是原方程组的解都会是CS的解,但是CS的解不一定是原方程的解,所以需要验证,这里的等效方程为:

    其中的未知数a1~a4都是已知的,因为原方程的系数是已知的,后文有系数附录,因此我们可以求得x,y的值,4次方程组理论上有4组解,但其实只有一组是合适的。

      3.4、求得了x,y的值,就可以求取PA,PB,PC的值,根据下面的公式,AB已知,可以先求PC,然后分别求解PB,PA:

     但是我们需要的是A,B,C在相机坐标系下的坐标,而不是PA,PB,PC的长度,所以还需根据长度求取点的坐标,求解方法是用向量公式:

    其中a是单位向量,||PA||是模值,所得即A在相机坐标系下的坐标。

     

      最后求得了A,B,C的坐标就可以通过世界坐标系到当前相机坐标的变换求解相机位姿,注意上面求得了4组解,这里需要使用D点确认哪组解是最合适的。

      4、代码对应:看看上述过程是如何代码实现的

        //像素坐标转变为归一化图像坐标;
        mu0 = inv_fx * mu0 - cx_fx;
        mv0 = inv_fy * mv0 - cy_fy;
        //归一化图像坐标归一化
        norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
        mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
    
        mu1 = inv_fx * mu1 - cx_fx;
        mv1 = inv_fy * mv1 - cy_fy;
        norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
        mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
    
        mu2 = inv_fx * mu2 - cx_fx;
        mv2 = inv_fy * mv2 - cy_fy;
        norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
        mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
    
        //世界坐标系中,ABC三点的距离;
        double distances[3];
        distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
        distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
        distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
    
        //三点之间的角度值;
        // Calculate angles
        double cosines[3];
        cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
        cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
        cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
    
        //吴消元法求解PA,PB,PC的值,有四组解;
        double lengths[4][3];
        int n = solve_for_lengths(lengths, distances, cosines);
    
        int nb_solutions = 0;
        for(int i = 0; i < n; i++) {
            double M_orig[3][3];
    
            //对每个点求坐标值,单位向量乘以距离;
            M_orig[0][0] = lengths[i][0] * mu0;
            M_orig[0][1] = lengths[i][0] * mv0;
            M_orig[0][2] = lengths[i][0] * mk0;
    
            M_orig[1][0] = lengths[i][1] * mu1;
            M_orig[1][1] = lengths[i][1] * mv1;
            M_orig[1][2] = lengths[i][1] * mk1;
    
            M_orig[2][0] = lengths[i][2] * mu2;
            M_orig[2][1] = lengths[i][2] * mv2;
            M_orig[2][2] = lengths[i][2] * mk2;
    
            //计算每个解对应的位姿矩阵R,t
            if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
                continue;
    
            nb_solutions++;
        }

      这里面主要是使用吴消元法求解PA,PB,PC的距离

     
      
    /// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
    /// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C).
    /// Returned distances are for |PA|, |PB|, |PC| respectively.
    /// Only the solution to the main branch.
    /// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
    /// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003
    /// \param lengths3D Lengths of line segments up to four solutions.
    /// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|.
    /// \param cosines Cosine of the angles /_BPC, /_APC, /_APB.
    /// \returns Number of solutions.
    /// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED

    int
    p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]) { //吴消元法,数据准备 double p = cosines[0] * 2; double q = cosines[1] * 2; double r = cosines[2] * 2; double inv_d22 = 1. / (distances[2] * distances[2]); double a = inv_d22 * (distances[0] * distances[0]); double b = inv_d22 * (distances[1] * distances[1]); double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r; double pr = p * r, pqr = q * pr; // Check reality condition (the four points should not be coplanar) if (p2 + q2 + r2 - pqr - 1 == 0) return 0; double ab = a * b, a_2 = 2*a; double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2; //A, B, C, D, E 为四次多项式的系数; // Check reality condition if (A == 0) return 0; double a_4 = 4*a; double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab); double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2; double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2); double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2; double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr); double b0 = b * temp * temp; // Check reality condition if (b0 == 0) return 0; //求解四次多项式; double real_roots[4]; int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]); if (n == 0) return 0; int nb_solutions = 0; double r3 = r2*r, pr2 = p*r2, r3q = r3 * q; double inv_b0 = 1. / b0; // For each solution of x for(int i = 0; i < n; i++) { double x = real_roots[i]; // Check reality condition if (x <= 0) continue; double x2 = x*x;     //对应附录中的b1 double b1 = ((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) * (((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x + (r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 + (r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x + 2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) + p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1))); // Check reality condition if (b1 <= 0) continue; double y = inv_b0 * b1; double v = x2 + y*y - x*y*r; if (v <= 0) continue; double Z = distances[2] / sqrt(v); double X = x * Z; double Y = y * Z; lengths[nb_solutions][0] = X; lengths[nb_solutions][1] = Y; lengths[nb_solutions][2] = Z; nb_solutions++; } return nb_solutions; }

      看看是如何从4组解中选择合适的解的:

        int ns = 0;
        double min_reproj = 0;
        for(int i = 0; i < n; i++) {
            double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
            double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
            double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
            double mu3p = cx + fx * X3p / Z3p;
            double mv3p = cy + fy * Y3p / Z3p;
            //通过R,t计算第4个点的重投影误差选择合理的解
            double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
            //选择重投影误差最小的解
            if (i == 0 || min_reproj > reproj) {
                ns = i;
                min_reproj = reproj;
            }
        }

    大概就酱。

    附:吴消元法求解系数

    参考:http://iplimage.com/blog/p3p-perspective-point-overview/#Appendix

     

    转载于:https://www.cnblogs.com/mafuqiang/p/8302663.html

    展开全文
  • P3P位姿估计推导

    千次阅读 2019-05-15 10:25:43
    之前博客已经讲过P3P算法了《学习笔记之——P3P与ICP位姿估计算法及实验》本博文是具体推导的整理 接下来的过程就是如何通过上述两个式子求解A,B,C在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量...

    之前博客已经讲过P3P算法了《学习笔记之——P3P与ICP位姿估计算法及实验》本博文是具体推导的整理

    接下来的过程就是如何通过上述两个式子求解ABC在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量,输入的是3D-2D的坐标,也即

     

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122152049725-1532823144.png

    都是已知的。因为首先ABBCAC的距离都是可以根据输入的3D点求得,而输入的2D点可以求解三个余弦值(如何求解,像素坐标根据相机内参矩阵和畸变参数可以求得在归一化图像平面上的3D坐标,此时 z=1,故余弦值可求)。此时未知数仅xy两个,所以理论上两个未知数两个方程,是可求的。(从xyOAOBOC也可求)

      具体的求解过程:

      首先是根据2D坐标求解余弦值得过程,首先是由像素坐标到归一化图像坐标的转变,根据就是相机模型

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153050662-494674376.png

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153057178-1276682373.png

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153106569-1487117855.png

    然后是L2归一化的过程,我们知道求解角度的时候用的是归一化坐标(此归一化非彼归一化,上面是归一化到z值等于1的平面上,这里讲的是数学上的归一化)

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153329115-1379923265.png

    有了上述值就可以求解余弦值了

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153430037-2143280350.png

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153447584-921099788.png同理可求。

      根据3D坐标求解ABACBC的值,以AB为例

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122153534069-280753284.png

    ACBC同理可求,所以vw也可以求解。

       接下来就是一个二元二次方程的求解,比较难求,但是这在数学上是可以求解的,需要用到Wu Ritt的零点分解方法,它可以将原方程等效成一组特征列(Characteristic Serial, CS),凡是原方程组的解都会是CS的解,但是CS的解不一定是原方程的解,所以需要验证,这里的等效方程为:

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122155518490-1944969239.png

    其中的未知数a1~a4都是已知的,因为原方程的系数是已知的,后文有系数附录,因此我们可以求得xy的值,4次方程组理论上有4组解,但其实只有一组是合适的。

      求得了xy的值,就可以求取PAPBPC的值,根据下面的公式,AB已知,可以先求PC,然后分别求解PBPA

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122163020740-1581414370.png

     但是我们需要的是ABC在相机坐标系下的坐标,而不是PAPBPC的长度,所以还需根据长度求取点的坐标,求解方法是用向量公式:

    https://images2017.cnblogs.com/blog/1138886/201801/1138886-20180122184650944-565540283.png

    其中a是单位向量,||PA||是模值,所得即A在相机坐标系下的坐标。

    最后求得了ABC的坐标就可以通过世界坐标系到当前相机坐标的变换求解相机位姿

    https://img-blog.csdnimg.cn/20190506192851937.png

     

    其中下标c表示Camera,w表示World。 

    https://img-blog.csdnimg.cn/20190506194254535.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d3cGxvdmVraW1p,size_16,color_FFFFFF,t_70

    https://img-blog.csdnimg.cn/2019050619445277.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d3cGxvdmVraW1p,size_16,color_FFFFFF,t_70

    如上图所示,当合作标志特征点个数为3时,且3点不共线时,记

    https://img-blog.csdnimg.cn/20190506194525959.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d3cGxvdmVraW1p,size_16,color_FFFFFF,t_70

    https://img-blog.csdnimg.cn/20190506194647980.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d3cGxvdmVraW1p,size_16,color_FFFFFF,t_70

    ​参考资料

    https://www.cnblogs.com/mafuqiang/p/8302663.html

     

    展开全文
  • Opencv 3.4 中P3P位姿估计算法解析

    千次阅读 2018-06-22 10:48:45
    先上图,Opencv3.4中用两种算法实现P3P位姿估计问题。一种是基于距离P3P算法问题(算法1:P3P),一种是基于矩阵P3P算法问题(算法2:aP3P),具体推导细节可以参看论文还以整理的本地关键技术文档。此处只对程序...

    先上图,Opencv3.4中用两种算法实现P3P位姿估计问题。一种是基于距离P3P算法问题(算法1:P3P),一种是基于矩阵P3P算法问题(算法2:aP3P),具体推导细节可以参看论文还以整理的本地关键技术文档。此处只对程序进行分析注释,方便以后使用。注意:此处只注释算法2,因为算法1没有完全搞明白(主要是高小山用wu-ritt算法推导我看不太懂)。

    外部调用接口函数:

    /*  max 注释
    *   函数功能:P3P位姿估计的接口函数,此函数可以利用不用的flags值,选择不同的算法。
    *             输入3个对应点,返回最多4组有效解
    *   参数:
    *   [in]    objectPoints                参考坐标系(或世界坐标系)中的3D点,需要3个即可,不要多也不要少。float和double都可以    
    *   [in]    imagePoints                 对应3D点在相机相平面上的投影点的图像坐标。需要3个即可,不要多也不要少。注意这个是2D的。float和double都可以
    *   [in]    cameraMatrix                相机内参矩阵
    *   [in]    distCoeffs                  相机畸变矩阵
    *   [out]   rvecs                       输出旋转矩阵,不用声明是多大矩阵-----输出是最多是4个解----每个解的旋转是用旋转向量表示的。------从参考坐标中的点到相机坐标系的点的变换
    *   [out]   tvecs                       输出平移矩阵,不用声明多大矩阵------输出最多是4个解。
    *   [in]    flags                       选择不同的结算位姿算法,SOLVEPNP_P3P是算法1,采用基于距离PnP算法。SOLVEPNP_AP3P算法2,采用基于矩阵PnP算法。
    *
    *    返回值:
    *            返回解的个数,最大不会超过4个。
    */
    int solveP3P(InputArray objectPoints, InputArray imagePoints,	InputArray cameraMatrix, InputArray distCoeffs,	OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,int flags);

    测试例子:

    #include "test_precomp.hpp"
    
    void generate3DPointCloud(std::vector<cv::Point3f>& points, cv::Point3f pmin = cv::Point3f(-1, -1, 5), cv::Point3f pmax = cv::Point3f(1, 1, 10))
    {
    	cv::RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points
    
    	for (size_t i = 0; i < points.size(); i++)
    	{
    		float _x = rng.uniform(pmin.x, pmax.x);
    		float _y = rng.uniform(pmin.y, pmax.y);
    		float _z = rng.uniform(pmin.z, pmax.z);
    		points[i] = cv::Point3f(_x, _y, _z);
    	}
    }
    
    void generateCameraMatrix(cv::Mat& cameraMatrix, cv::RNG& rng)
    {
    	const double fcMinVal = 1e-3;
    	const double fcMaxVal = 100;
    	cameraMatrix.create(3, 3, CV_64FC1);
    	cameraMatrix.setTo(cv::Scalar(0));
    	cameraMatrix.at<double>(0, 0) = rng.uniform(fcMinVal, fcMaxVal);
    	cameraMatrix.at<double>(1, 1) = rng.uniform(fcMinVal, fcMaxVal);
    	cameraMatrix.at<double>(0, 2) = rng.uniform(fcMinVal, fcMaxVal);
    	cameraMatrix.at<double>(1, 2) = rng.uniform(fcMinVal, fcMaxVal);
    	cameraMatrix.at<double>(2, 2) = 1;
    }
    
    void generateDistCoeffs(cv::Mat& distCoeffs, cv::RNG& rng)
    {
    	distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
    	for (int i = 0; i < 3; i++)
    		distCoeffs.at<double>(i, 0) = rng.uniform(0.0, 1.0e-6);
    }
    
    void generatePose(cv::Mat& rvec, cv::Mat& tvec, cv::RNG& rng)
    {
    	const double minVal = 1.0e-3;
    	const double maxVal = 1.0;
    	rvec.create(3, 1, CV_64FC1);
    	tvec.create(3, 1, CV_64FC1);
    	for (int i = 0; i < 3; i++)
    	{
    		rvec.at<double>(i, 0) = rng.uniform(minVal, maxVal);
    		tvec.at<double>(i, 0) = rng.uniform(minVal, maxVal / 10);
    	}
    }
    
    
    int main()
    {
    	std::vector<cv::Point3f> points;
    	points.resize(500);
    	generate3DPointCloud(points);
    
    
    	std::vector<cv::Mat> rvecs, tvecs;
    	cv::Mat trueRvec, trueTvec;
    	cv::Mat intrinsics, distCoeffs;
    
    	generateCameraMatrix(intrinsics, cv::RNG());	
    	generateDistCoeffs(distCoeffs, cv::RNG());
    
    	generatePose(trueRvec, trueTvec, cv::RNG());
    
    	std::vector<cv::Point3f> opoints;
    	opoints = std::vector<cv::Point3f>(points.begin(), points.begin() + 3);
    
    	std::vector<cv::Point2f> projectedPoints;
    	projectedPoints.resize(opoints.size());
    	projectPoints(cv::Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
    
    	std::cout << "intrinsics: " << intrinsics << std::endl;
    	std::cout << "distcoeffs: " << distCoeffs << std::endl;
    	std::cout << "trueRvec: " << trueRvec << std::endl;
    	std::cout << "trueTvec: " << trueTvec << std::endl;
    
    	std::cout << "oPoint: " << opoints << std::endl;
    	std::cout << "projectedPoints: " << projectedPoints << std::endl;
    
    
    
    	std::cout<<"result numbers A: :"<<solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_AP3P)<<std::endl;
    	//std::cout << "result numbers: :" << solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_P3P) << std::endl;
    
    	bool isTestSuccess = false;
    	double error = DBL_MAX;
    	for (unsigned int i = 0; i < rvecs.size() /*&& !isTestSuccess*/; ++i)
    	{
    		double rvecDiff = cvtest::norm(rvecs[i], trueRvec, cv::NORM_L2);
    		double tvecDiff = cvtest::norm(tvecs[i], trueTvec, cv::NORM_L2);
    		isTestSuccess = rvecDiff < 1.0e-4 && tvecDiff < 1.0e-4;
    		error = std::min(error, std::max(rvecDiff, tvecDiff));
    		std::cout << "i: " << i << std::endl;
    		std::cout << "error: " << error << std::endl;
    		std::cout << "rvec: " << rvecs[i] << std::endl;
    
    	}
    
    	system("pause");
    }

    算法2中代码描述,下面所描述的不是Opencv外部提供的接口,属于算法的内部实现。---代码在ap3p.h头文件中。


    #ifndef P3P_P3P_H
    #define P3P_P3P_H
    
    #include "precomp.hpp"
    
    namespace cv {
    class ap3p {
    private:
        template<typename T>
        void init_camera_parameters(const cv::Mat &cameraMatrix) {
            cx = cameraMatrix.at<T>(0, 2);
            cy = cameraMatrix.at<T>(1, 2);
            fx = cameraMatrix.at<T>(0, 0);
            fy = cameraMatrix.at<T>(1, 1);
        }
    
        template<typename OpointType, typename IpointType>
        void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
            points.clear();
            int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
            points.resize(5*npoints);
            for (int i = 0; i < npoints; i++) {
                points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
                points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
                points[i * 5 + 2] = opoints.at<OpointType>(i).x;
                points[i * 5 + 3] = opoints.at<OpointType>(i).y;
                points[i * 5 + 4] = opoints.at<OpointType>(i).z;
            }
        }
    
        void init_inverse_parameters();
    
        double fx, fy, cx, cy;
        double inv_fx, inv_fy, cx_fx, cy_fy;
    public:
    	// 3个初始化构造函数的目的是获取相机内参数,并且计算相机内参矩阵的逆。---------用于求解论文中的bearing measurement
        ap3p() : fx(0), fy(0), cx(0), cy(0), inv_fx(0), inv_fy(0), cx_fx(0), cy_fy(0) {}
    
        ap3p(double fx, double fy, double cx, double cy);
    
        ap3p(cv::Mat cameraMatrix);
    
    
    
    	/*  max 注释
    	*   函数功能:此处的位姿估计只返回一个解,并且此处输入的是4个点---注意并不是solveP3P所调用。调用顺序为①-->④-->③
    	*   参数:
    	*   [out]   R                输出单个旋转矩阵
    	*   [out]   tvec             输出单个平移向量
    	*   [in]    opoints          输入3D点---4个
    	*   [in]    ipoints          输入2D点---4个
    	*
    	*    返回值:
    	*            成功返回true,失败返回false
    	*/
        ① bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
    
    	/*  max 注释
    	*   函数功能:此处的位姿估计只返回最多4个解,是solveP3P所调用。调用顺序为②-->③
    	*   参数:
    	*   [out]   R                输出最多4个旋转矩阵
    	*   [out]   tvec             输出最多4平移向量
    	*   [in]    opoints          输入3D点---3个
    	*   [in]    ipoints          输入2D点---3个
    	*
    	*    返回值:
    	*            成功返回true,失败返回false
    	*/
        ② int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints);
    
        ③ int solve(double R[4][3][3], double t[4][3],
                  double mu0, double mv0, double X0, double Y0, double Z0,
                  double mu1, double mv1, double X1, double Y1, double Z1,
                  double mu2, double mv2, double X2, double Y2, double Z2);
    
        ④ bool solve(double R[3][3], double t[3],
                   double mu0, double mv0, double X0, double Y0, double Z0,
                   double mu1, double mv1, double X1, double Y1, double Z1,
                   double mu2, double mv2, double X2, double Y2, double Z2,
                   double mu3, double mv3, double X3, double Y3, double Z3);
    
    
    	// 此处完全按照论文的计算步骤来的,程序非常清晰,可以参考论文
        // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
        // See https://arxiv.org/pdf/1701.08237.pdf
        // featureVectors: 3 bearing measurements (normalized) stored as column vectors
        // worldPoints: Positions of the 3 feature points stored as column vectors
        // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
        // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
        int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3],
                         double solutionsT[4][3]);
    
    };
    }
    #endif //P3P_P3P_H
    


    展开全文
  • 相机位姿估计

    千次阅读 2019-12-10 14:26:52
    相机位姿求解前言旋转角度欧拉角相机位姿求解旋转矩阵和旋转向量之间的转换 前言 这部分内容博主也不是很熟悉,写下这篇博文想记录下自己当时的求解过程,也想让看到的朋友一起讨论,看看我做的对不对。下面我就...
  • 本博文为本人学习P3P算法的学习笔记~ 先给出论文的链接(https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=1217599) 《Complete solution classification for the perspective-three-point problem...
  • https://www.cnblogs.com/shang-slam/p/6481344.html EPnP在ORB-SLAM中主要用于Tracking线程中的...PnP问题解决了已知世界参考系下地图点以及相机参考系下投影点位置时3D-2D相机位姿估计问题,不需要使用对极约束(...
  • 关键词:相机位姿估计 OpenCV::solvePnP labview三维图片 文章类型:应用展示+Demo演示 @Author:VShawn(singlex@foxmail.com) @Date:2016-12-12 @Lab: CvLab202@CSU 目录 《相机位姿估计0:基本原理之...
  • 根据四个特征点求解相机位姿

    千次阅读 2017-05-28 18:37:02
    关键词:位姿估计 OpenCV::solvePnP 用途:各种位姿估计 文章类型:原理、流程、Demo示例 @Author:VShawn(singlex@foxmail.com) ...《相机位姿估计0:基本原理之如何解PNP问题》《相机位姿估计1:根据四
  • 关键词:位姿估计 OpenCV::solvePnP 用途:各种位姿估计 文章类型:原理、流程、Demo示例 ... @Date:2016-11-18 @Lab: CvLab202@CSU ...《相机位姿估计1:根据四个特征点估计相机姿态》 《相...
  • 关键词:相机位姿估计,单目尺寸测量,环境探知 用途:基于相机的环境测量,SLAM,单目尺寸测量 文章类型:原理说明、Demo展示 @Author:VShawn @Date:2016-11-28 @Lab: CvLab202@CSU 目录 《相机位姿...
  • 欢迎大家在周日来到泡泡机器人讲堂,本次我们将为大家介绍相机位姿问题的求解,相机位姿估计是指给定若干图像,估计其中相机运动的问题。求解方法通常分特征点法和直接法两种,这也是视觉里程计的两类基本方法。本次...
  • 关键词:相机位姿估计 PNP问题求解 用途:各种位姿估计 文章类型:原理 @Author:VShawn(singlex@foxmail.com) @Date:2016-11-18 @Lab: CvLab202@CSU 目录 《相机位姿估计0:基本原理之如何解PNP...
  • opencv中solvePnPRansac函数求解相机位姿

    万次阅读 2016-12-22 18:51:01
    PnP(Perspective n Points):2D—3D,求解相机位姿 一、函数介绍 源码位置:An example of how to use solvePNPRansac for object detection can be found at opencv_source_code/samples/cpp/tutorial_code/...
  • 关键词:相机位姿估计,单目尺寸测量,环境探知 用途:基于相机的环境测量,SLAM,单目尺寸测量 文章类型:原理说明、Demo展示 @Author:VShawn @Date:2016-11-28 @Lab: CvLab202@CSU 目录 《相机位姿...
  • PnP 单目相机位姿估计(二):solvePnP利用二维码求解相机世界坐标 cocoaqin 2017-09-05 10:40:15 29341 收藏 114 分类专栏: 机器视觉 文章标签: pnp 位姿 单目相机 世界坐标 相机坐标 版权 前言 ...
  • 【泡泡机器人原创专栏-位姿求解】相机位姿问题的特征点法求解 原创 2016-11-28 高翔 泡泡机器人SLAM http://mp.weixin.qq.com/s/2i5RwW5HxpQVdqQ25AqN1w 欢迎大家在周日来到泡泡机器人讲堂,本次我们将为大家...
  • 他描述了当知道n个三维空间点坐标及其二维投影位置时,如何估计相机位姿。我们可以想象,在一幅图像中,最少只要知道三个点的空间坐标即3D坐标,就可以用于估计相机的运动以及相机的姿态。 而特征点的空间坐标
  • BA优化模型如下:优化变量(空间位置P相机位姿),边(P在相机平面的投影像素坐标) // 调用OpenCV 的 PnP 求解,可选择EPNP,DLS等方法 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); cv::...
  • 相机姿态估计(三)--P3P

    万次阅读 2016-07-29 11:27:45
     我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵,而是先求出对应的2D点在当前相机坐标系下的3D坐标,然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。P3P的求解是从余弦...
  • 半稠密直接法和稀疏直接法求解过程基本上是一样的,只是稀疏直接法提取的是fast关键点,一般是几千个,而半稠密直接法提取的...用g2o求解的相机位姿。   转载于:https://www.cnblogs.com/talugirl/p/7388577.html
  • 二维码具有方向性(只要设计的不为中心对称),可以更好的辅助求解位姿 二维码具有可识别性,即可解析二维码内部黑白块的排列顺序,从而确定该二维码是否为我们所要用的二维码 二维码识别的流程 该流程以该二维码为...
  • 关键词:OpenCV::solvePnP ...今天给大家带来的是一篇关于程序功能、性能测试的文章,读过《相机位姿估计1:根据四个特征点估计相机姿态》一文的同学应该会发现,直接使用OpenCV的solvePnP来估计相...
  • CV_P3P则是使用非常经典的Gao的P3P问题求解算法。 CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。 详见 solvepnp三维位姿估算  因此我们只要获得 特征...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 462
精华内容 184
关键字:

p3p相机位姿