相機位姿求解——P3P問題

　　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