PNP algorithm

PNP algorithm

這個算法比我想想的要複雜的多，所以我還是從理論入手把，結合opencv的代碼。單從代碼講會忽略好多東西。以後有時間慢慢改。

最近用到了PNP算法，看了好多，網上搜索了一番，發現有很多種解法，在這裏，我就準備通過OPENCV源碼，看看OPENCV裏是怎麼實現的(我看的OPENCV的源碼是2.4，C++實現的)。

PNP用的比較多的就是P3P和EPNP兩種，在這裏，我也準備把OPENCV裏如何實現這兩種算法的過程詳細的記錄下來。至於PNP算法用來幹什麼的我就不說了，網上很多解釋。

OPENCV源碼

首先，看一下OPENCV裏的SolvePNP函數的API：

 C++: bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

其中參數爲：
Parameters:

• objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector can be also passed here.
• imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. vector can be also passed here.
• cameraMatrix – Input camera matrix A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1} .
• distCoeffs – Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
• rvec – Output rotation vector (see Rodrigues() ) that, together with tvec , brings points from the model coordinate system to the camera coordinate system.
• tvec – Output translation vector.
• useExtrinsicGuess – If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them.

• flags –

  Method for solving a PnP problem:

  • CV_ITERATIVE Iterative method is based on Levenberg-Marquardt optimization. In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
  • CV_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
  • CV_EPNP Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.

每個參數的意義已經寫的很清楚了，objectPoints是世界座標系的點，imagePoints是圖像座標系的點，cameraMatrix是相機內參，distCoeffs是相機畸變係數，rvec是旋轉矩陣，tvec是平移矩陣，支持三種方法P3P,EPNP,ITERATIVE，現在就來看P3P情況。

solvePnP源碼：

bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
                  InputArray _cameraMatrix, InputArray _distCoeffs,
                  OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();

    Mat rvec, tvec;
    if( flags != CV_ITERATIVE )
        useExtrinsicGuess = false;

    if( useExtrinsicGuess )
    {
        int rtype = _rvec.type(), ttype = _tvec.type();
        Size rsize = _rvec.size(), tsize = _tvec.size();
        CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
                   (ttype == CV_32F || ttype == CV_64F) );
        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
    }
    else
    {
        _rvec.create(3, 1, CV_64F);
        _tvec.create(3, 1, CV_64F);
    }
    rvec = _rvec.getMat();
    tvec = _tvec.getMat();

    if (flags == CV_EPNP)
    {
        cv::Mat undistortedPoints;
        cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        epnp PnP(cameraMatrix, opoints, undistortedPoints);

        cv::Mat R;
        PnP.compute_pose(R, tvec);
        cv::Rodrigues(R, rvec);
        return true;
    }
    else if (flags == CV_P3P)
    {
        CV_Assert( npoints == 4);
        cv::Mat undistortedPoints;
        cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        p3p P3Psolver(cameraMatrix);

        cv::Mat R;
        bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
        if (result)
            cv::Rodrigues(R, rvec);
        return result;
    }
    else if (flags == CV_ITERATIVE)
    {
        CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
        CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
        CvMat c_rvec = rvec, c_tvec = tvec;
        cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                     c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                     &c_rvec, &c_tvec, useExtrinsicGuess );
        return true;
    }
    else
        CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP");
    return false;
}

函數前三行

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();//將世界座標系點的數據和圖像座標系點的數據變爲Mat格式
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));//檢查Mat是不是由npoints個3維CV_32F/CV_64F向量組成的
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );// 對ipoints進行同樣的檢查，並對比兩組點的個數是否相同

下一句：

    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();//轉換爲Mat類型

P3P

我們先看P3P部分：

 else if (flags == CV_P3P)
    {
        // 看看是否有4組點對，CV_P3P使用第四個點對來獲得更好的結果
        CV_Assert( npoints == 4);
        //獲得圖像座標在畸變前的座標，我們的目的是看P3P算法怎麼實現的，這裏不用管。
        cv::Mat undistortedPoints;
        cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        //初始化一個p3p類型的對象
        p3p P3Psolver(cameraMatrix);

        cv::Mat R;
        //在這裏計算！
        bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
        if (result)
            cv::Rodrigues(R, rvec);
        return result;
    }

先看一下p3p類是怎麼初始化的：

p3p::p3p(cv::Mat cameraMatrix)
{
    //如果矩陣的depth()是CV_32F，則
    //depth可以理解爲數據類型
    if (cameraMatrix.depth() == CV_32F)
        init_camera_parameters<float>(cameraMatrix);
    else
        init_camera_parameters<double>(cameraMatrix);
    init_inverse_parameters();
}

init_camera_parameters函數將已知的內參賦值給p3p類裏的變量cx,cy,fx,fy

  void init_camera_parameters(const cv::Mat& cameraMatrix)
  {
    cx = cameraMatrix.at<T> (0, 2);  //攝像機內參的X offset
    cy = cameraMatrix.at<T> (1, 2);
    fx = cameraMatrix.at<T> (0, 0);  // focal length x
    fy = cameraMatrix.at<T> (1, 1);  // focal length y
  }

init_inverse_parameters方法：

不用多解釋了

void p3p::init_inverse_parameters()
{
    inv_fx = 1. / fx;
    inv_fy = 1. / fy;
    cx_fx = cx / fx;
    cy_fy = cy / fy;
}

之後我們來看看這一部分：

cv::Mat R;
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);

首先是p3p類裏的solve部分：

bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
{
    //聲明瞭rotation_matrix  translation
    double rotation_matrix[3][3], translation[3];

    std::vector<double> points;
    //根據不同的數據類型，進行數據點的提取
    if (opoints.depth() == ipoints.depth())
    {
        if (opoints.depth() == CV_32F)
            extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
        else
            extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
    }
    else if (opoints.depth() == CV_32F)
        extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
    else
        extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
    //計算開始了
    bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
          points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
          points[15], points[16], points[17], points[18], points[19]);
    cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
    cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
    return result;
}

extract_points模板方法

template <typename OpointType, typename IpointType>
  void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
  {
      points.clear();
      points.resize(20);
      //可以看到，這個方法把image points 和 object points都存到了同一個vector裏
      //每一對2D-3D的點對，有5個值(2D兩個，3D三個)，有四個點對，總共存20個
      //圖像座標系裏，把x*fx後再加偏移量cx
      //y也這麼做
      for(int i = 0; i < 4; i++)
      {
          points[i*5] = ipoints.at<IpointType>(0,i).x*fx + cx;
          points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
          points[i*5+2] = opoints.at<OpointType>(0,i).x;
          points[i*5+3] = opoints.at<OpointType>(0,i).y;
          points[i*5+4] = opoints.at<OpointType>(0,i).z;
      }
  }

之後就是重點了：
我們注意到，solve函數聲明瞭兩個數組：

double rotation_matrix[3][3], translation[3];

之後在

bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],points[15], points[16], points[17], points[18], points[19]);

裏，我們根據函數的參數列表，找到相應的調用，因爲solve被重載了好幾次。
跳轉到：

bool p3p::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)
{
    double Rs[4][3][3], ts[4][3];
    //這裏會跳轉到別的被重載的solve函數中
    //使用前三個點對，第四個點對並沒有傳入進去
    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0,  mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);

    if (n == 0)
        return false;

    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;
        double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
        if (i == 0 || min_reproj > reproj) {
            ns = i;
            min_reproj = reproj;
        }
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++)
            R[i][j] = Rs[ns][i][j];
        t[i] = ts[ns][i];
    }

    return true;
}

再繼續跳轉到這裏：

//沒有第四個點對
int p3p::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)
{
    double mk0, mk1, mk2;
    double norm;
   // inv_fx = 1. / fx;
   // inv_fy = 1. / fy;
   // cx_fx = cx / fx;
   // cy_fy = cy / fy;
   //mu0 = x
   //mv0 = y
   // 將圖像座標系的點投影到一個球面
    mu0 = inv_fx * mu0 - cx_fx;
    mv0 = inv_fy * mv0 - cy_fy;
    //求第一個座標x,y到原點的距離
    norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
    //將數據單位化
    //原來點的座標x,y被單位化了
    mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;

    //同樣，單位化第二個點mu1, mv1
    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, mv2
    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;

    double distances[3];
    //計算第1個3D點和第2個3D點的歐式距離
    distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
    //計算第0個3D點和第2個3D點的歐式距離
    distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
    //計算第0個3D點和第1個3D點的歐式距離
    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;

    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;

        if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
            continue;

        nb_solutions++;
    }

    return nb_solutions;
}

歡迎使用Markdown編輯器寫博客

本Markdown編輯器使用StackEdit修改而來，用它寫博客，將會帶來全新的體驗哦：

• 列表內容
• Markdown和擴展Markdown簡潔的語法
• 代碼塊高亮
• 圖片鏈接和圖片上傳
• LaTex數學公式
• UML序列圖和流程圖
• 離線寫博客
• 導入導出Markdown文件
• 豐富的快捷鍵

---

快捷鍵

• 加粗 Ctrl + B
• 斜體 Ctrl + I
• 引用 Ctrl + Q
• 插入鏈接 Ctrl + L
• 插入代碼 Ctrl + K
• 插入圖片 Ctrl + G
• 提升標題 Ctrl + H
• 有序列表 Ctrl + O
• 無序列表 Ctrl + U
• 橫線 Ctrl + R
• 撤銷 Ctrl + Z
• 重做 Ctrl + Y
• sdfk
• sdflk

---

dkskkf

這裏寫代碼片

Markdown及擴展

Markdown 是一種輕量級標記語言，它允許人們使用易讀易寫的純文本格式編寫文檔，然後轉換成格式豐富的HTML頁面。 —— [ 維基百科 ]

使用簡單的符號標識不同的標題，將某些文字標記爲粗體或者斜體，創建一個鏈接等，詳細語法參考幫助？。

本編輯器支持 Markdown Extra , 　擴展了很多好用的功能。具體請參考Github.

表格

Markdown　Extra　表格語法：

項目	價格
Computer	$1600
Phone	$12
Pipe	$1

可以使用冒號來定義對齊方式：

項目	價格	數量
Computer	1600 元	5
Phone	12 元	12
Pipe	1 元	234

定義列表

Markdown　Extra　定義列表語法：

項目１

項目２

定義 A

定義 B

項目３

定義 C

定義 D

定義D內容

代碼塊

代碼塊語法遵循標準markdown代碼，例如：

@requires_authorization
def somefunc(param1='', param2=0):
    '''A docstring'''
    if param1 > param2: # interesting
        print 'Greater'
    return (param2 - param1 + 1) or None
class SomeClass:
    pass
>>> message = '''interpreter
... prompt'''

腳註

生成一個腳註1.

目錄

用 [TOC]來生成目錄：

數學公式

使用MathJax渲染LaTex 數學公式，詳見math.stackexchange.com.

• 行內公式，數學公式爲：Γ(n)=(n−1)!∀n∈ℕ 。
• 塊級公式：

x=−b±b2−4ac‾‾‾‾‾‾‾‾√2a

更多LaTex語法請參考 這兒.

UML 圖:

可以渲染序列圖：

或者流程圖：

• 關於 序列圖 語法，參考 這兒,
• 關於 流程圖 語法，參考 這兒.

離線寫博客

即使用戶在沒有網絡的情況下，也可以通過本編輯器離線寫博客（直接在曾經使用過的瀏覽器中輸入write.blog.csdn.net/mdeditor即可。Markdown編輯器使用瀏覽器離線存儲將內容保存在本地。

用戶寫博客的過程中，內容實時保存在瀏覽器緩存中，在用戶關閉瀏覽器或者其它異常情況下，內容不會丟失。用戶再次打開瀏覽器時，會顯示上次用戶正在編輯的沒有發表的內容。

博客發表後，本地緩存將被刪除。　

用戶可以選擇 把正在寫的博客保存到服務器草稿箱，即使換瀏覽器或者清除緩存，內容也不會丟失。

注意：雖然瀏覽器存儲大部分時候都比較可靠，但爲了您的數據安全，在聯網後，請務必及時發表或者保存到服務器草稿箱。

瀏覽器兼容

1. 目前，本編輯器對Chrome瀏覽器支持最爲完整。建議大家使用較新版本的Chrome。
2. IE９以下不支持
3. IE９，１０，１１存在以下問題
   
   1. 不支持離線功能
   2. IE9不支持文件導入導出
   3. IE10不支持拖拽文件導入

---

---

1. 這裏是 腳註 的 內容. ↩