是以我們隻要獲得 特征點的世界坐标（三維坐标）、2D坐标（像素坐标）、相機内參矩陣、相機畸變參數矩陣 以上四個參數即可以解得相機與标志物之間的外參（R、T），并以此求得相機的世界坐标（以标志物為世界坐标平面，且原點為标志物已知某一點）。

轉載自：https://blog.csdn.net/cocoaqin/article/details/77848588

PnP 單目相機位姿估計（二）：solvePnP利用二維碼求解相機世界坐标

cocoaqin 2017-09-05 10:40:15 29341 收藏 114

分類專欄： 機器視覺 文章标簽： pnp 位姿 單目相機 世界坐标 相機坐标

版權

• • • 前言
    • 原理簡介
    • 輸入參數準備
      • 1 objectPoints特征點世界坐标
      • 2 imagePoints特征點在攝像頭下的像素點坐标
      • 3cameraMatrixdistCoeffs内參矩陣和畸變矩陣
    • 相機世界坐标的求解
      • 1求世界坐标中的點在相機坐标系下的坐标
      • 2求相機在世界坐标中的坐标
      • 3效果求解相機在世界坐标系下的坐标點
      • 4求解代碼
    • 更多

IDE：visual studio 2013

使用庫：Eigen opencv2.4.9

文檔版本：1.1

1.前言

　　很多時候，當我們利用單目相機擷取圖像時，往往隻能擷取圖像中特征物體的像素坐标。而在空間三維點計算時，我們也第一時間會想到深度相機（Kinect、Realsense等）、雙目相機等。

　　但是在實際中，我們亦可以通過單目相機來求得空間三維點，但這個往往有一個條件，已知特征物體的三個以上特征點參數。

在本章中，我們将使用二維碼作為标志物，通過二維碼四個角點，通過P4P求解相機的位姿及空間坐标。

特别說明：在本章以及未來的幾篇博文中都将以右手坐标系為基準，Z軸垂直于二維碼平面往裡。

　　

　　　　　　　　　　　　　　　二維碼參數：長140mm，寬140mm

在求解時不一定要使用二維碼，也可以用其它标志物代替，使用二維碼的好處是，通過識别二維碼内部的資訊可以更精準的比對以及識别，同時也能更容易的确定四個角點的順序。

　　

2.原理簡介

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

　　objectPoints：特征點的世界坐标，坐标值需為float型，不能為double型，可以為mat類型，也可以直接輸入vector

　　imagePoints：特征點在圖像中的像素坐标，可以輸入mat類型，也可以直接輸入vector，注意輸入點的順序要與前面的特征點的世界坐标一一對應

　　cameraMatrix：相機内參矩陣

　　distCoeffs：相機的畸變參數【Mat_(5, 1)】

　　rvec：輸出的旋轉向量

　　tvec：輸出的平移矩陣

　　最後的輸入參數有三個可選項：

　　CV_ITERATIVE，預設值，它通過疊代求出重投影誤差最小的解作為問題的最優解。

　　CV_P3P則是使用非常經典的Gao的P3P問題求解算法。

　　CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。

　　詳見 solvepnp三維位姿估算

　　是以我們隻要獲得 特征點的世界坐标（三維坐标）、2D坐标（像素坐标）、相機内參矩陣、相機畸變參數矩陣 以上四個參數即可以解得相機與标志物之間的外參（R、T），并以此求得相機的世界坐标（以标志物為世界坐标平面，且原點為标志物已知某一點）。

3.輸入參數準備

在上一節中我們已知solvePnP需要輸入四個參數，在這一節中，将介紹如何獲得這四個參數。

3.1 objectPoints特征點世界坐标

　　以特征點所在平面為世界坐标XY平面，并在該平面中确定世界坐标的原點，以我設計的二維碼為例，我設定二維碼的中心為世界坐标的原點，并按照順時針方向逐個輸入四個角點的世界坐标。

注意，輸入一定要按照順序輸入

m_markerCorners3d.push_back(cv::Point3f(-70.0f, -70.0f, 0));
    m_markerCorners3d.push_back(cv::Point3f(+70.0f, -70.0f, 0));    
    m_markerCorners3d.push_back(cv::Point3f(+70.0f, +70.0f, 0));
    m_markerCorners3d.push_back(cv::Point3f(-70.0f, +70.0f, 0));
           

3.2 imagePoints特征點在攝像頭下的像素點坐标

在這兒将獲得四個特征點對應2D的像素點坐标，而這個過程你可以人為的從圖像中逐個點獲得，也可以通過二維碼檢測程式檢測出，詳見 PnP 單目相機位姿估計（三）：二維碼四個角點的檢測

注意，這兒檢測到的四個像素點的輸入順序要和輸入的世界坐标的順序相同

3.3cameraMatrix，distCoeffs内參矩陣和畸變矩陣

我們這兒采用matlab的标定工具标定，并形成以下矩陣輸入solvePnP

camMatrix = (Mat_<double>(3, 3) << 598.29493, 0, 304.76898, 0, 597.56086, 233.34673, 0, 0, 1);
distCoeff = (Mat_<double>(5, 1) << -0.53572,1.35993,-0.00244,0.00620,0.00000);
           

4.相機世界坐标的求解

在上一節中，我們準備好了四個參數，把這四個參數輸入solvePnP中，即可獲得求得的R旋轉矩陣、T平移矩陣，拿到這兩個矩陣的值，我們就可以獲得許多我們想要的空間資訊啦~

已知 Pc=R*Po+T

定義Pc為相機坐标系的點值，Po為世界坐标系的點值，R、T為世界坐标系和相機坐标系的相對外參。

而我們利用solvePnP解得的R和T正是相機坐标系和世界坐标系的相對外參。

注意，solvePnP傳回的raux是旋轉向量，可通過羅德裡格斯變換成旋轉矩陣R。

4.1求世界坐标中的點在相機坐标系下的坐标

當Po=[0;0;0;]時，Pc=T即世界坐标原點（二維碼的中心）在相機坐标系下的坐标就為T

同樣的道理，當我們已知一點的世界坐标Po，我們就可以求得Pc

4.2求相機在世界坐标中的坐标

因為相機在相機坐标中相當于Pc=[0;0;0]，故Po=-R’*T 即可解得相機在世界坐标系下的坐标

其中R’為R的逆或者轉置矩陣（R是正交矩陣，R的逆等于R的轉置）

4.3效果（求解相機在世界坐标系下的坐标點）：

4.4求解代碼

Marker& m = detectedMarkers[i];
        cv::Mat Rvec;
        cv::Mat_<float> Tvec;
        cv::Mat raux, taux;

        cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff, raux, taux, false, CV_P3P);

        raux.convertTo(Rvec, CV_32F);    //旋轉向量
        taux.convertTo(Tvec, CV_32F);   //平移向量

        cv::Mat_<float> rotMat(3, 3);
        cv::Rodrigues(Rvec, rotMat);  //由于solvePnP傳回的是旋轉向量，故用羅德裡格斯變換變成旋轉矩陣

        //格式轉換
        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> R_n;
        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> T_n;
        cv2eigen(rotMat, R_n);
        cv2eigen(Tvec, T_n);
        Eigen::Vector3f P_oc;

        P_oc = -R_n.inverse()*T_n;
           

5.更多

相關文章

solvepnp三維位姿估算

PnP 單目相機位姿估計（一）：初識PnP問題

PnP 單目相機位姿估計（二）：solvePnP利用二維碼求解相機世界坐标

PnP 單目相機位姿估計（三）：二維碼角點檢測