關鍵詞：位姿估計 OpenCV::solvePnP
用途：各種位姿估計
文章類型：原理、流程、Demo示例
@Author：VShawn(singlex@foxmail.com)
@Date:2016-11-18
@Lab: CvLab202@CSU
目錄
前言
本文通過迭代法解PNP問題，得到相機坐標系關於世界坐標系的旋轉矩陣R與平移矩陣T后，根據之前的文章《根據相機旋轉矩陣求解三個軸的旋轉角》獲得相機坐標系的三軸旋轉角，實現了對相機位姿的估計。知道相機在哪后，我們就可以通過兩張照片，計算出照片中某個點的高度，實現對環境的測量。
先看演示視頻：
原理簡介
相機位姿估計就是通過幾個已知坐標的特征點，以及他們在相機照片中的成像，求解出相機位於坐標系內的坐標與旋轉角度，其核心問題就在於對PNP問題的求解，這部分本文不再啰嗦，參見本人之前的博客文章《相機位姿估計0：基本原理之如何解PNP問題》。本文中對pnp問題的求解直接調用了OpenCV的庫函數"solvePnP"，其函數原型為：
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<point3f> 。
第二個輸入imagePoints為特征點在圖像中的坐標，需要與前面的輸入一一對應。同樣可以輸入mat類型，也可以直接輸入vector<point3f> 。
第三個輸入cameraMatrix為相機內參數矩陣，大小為3×3，形式為：
第四個輸入distCoeffs輸入為相機的畸變參數，為1×5的矩陣。
第五個rvec為輸出矩陣，輸出解得的旋轉向量。
第六個tvec為輸出平移向量。
第七個設置為true后似乎會對輸出進行優化。
最后的輸入參數有三個可選項：
CV_ITERATIVE，默認值，它通過迭代求出重投影誤差最小的解作為問題的最優解。
CV_P3P則是使用非常經典的Gao的P3P問題求解算法。
CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。
流程
1.從函數的原型看出函數需要相機的內參數與畸變參數，於是相機標定是必不可少的，通過OpenCV自帶例程或者Matlab的相機標定工具箱都可以很方便地求出相機標定參數。
2.准備好四個特征點的世界坐標，存入Mat矩陣
vector<cv::Point3f> Points3D;
Points3D.push_back(cv::Point3f(0, 0, 0)); //P1 三維坐標的單位是毫米
Points3D.push_back(cv::Point3f(0, 200, 0)); //P2
Points3D.push_back(cv::Point3f(150, 0, 0)); //P3
Points3D.push_back(cv::Point3f(150, 200, 0)); //P4
3.准備好四個特征點在圖像上的對應點坐標，這個坐標在實驗中我是通過PhotoShop數出來的。注意，輸入坐標的順序一定要與之前輸入世界坐標的順序一致，就是說點與點要對應上，OpenCV的函數無法解決點與點匹配的問題（對應搜索問題）。
vector<cv::Point2f> Points2D;
Points2D.push_back(cv::Point2f(3062, 3073)); //P1 單位是像素
Points2D.push_back(cv::Point2f(3809, 3089)); //P2
Points2D.push_back(cv::Point2f(3035, 3208)); //P3
Points2D.push_back(cv::Point2f(3838, 3217)); //P4
4.創建輸出變量，即旋轉矩陣跟平移矩陣的變量。最后調用函數。
//初始化輸出矩陣
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
//三種方法求解
solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE); //實測迭代法似乎只能用共面特征點求位置
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P); //Gao的方法可以使用任意四個特征點
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);
5將輸出的旋轉向量轉變為旋轉矩陣
//旋轉向量變旋轉矩陣
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);
6.最后根據《根據相機旋轉矩陣求解三個軸的旋轉角》一文求出相機的三個旋轉角，根據《子坐標系C在父坐標系W中的旋轉》求出相機在世界坐標系中的位置。
至此，我們就求出了相機的位姿。
實驗
本人在實驗中先后使用了兩台相機做測試，一台是畸變較小的sony a6000微單+35mm定焦鏡頭，另一台是畸變較重的130w的工業相機+6mm定焦廣角鏡頭，實驗中兩台相機都得到了正確的位姿結果，此處為了方便只用α6000微單做演示說明。
如上圖所示，四個特征點P1-P4的世界坐標與像素坐標都已在圖中標明，P5用於重投影驗證位姿解是否正確。
相機實際位姿大約為：
粗略讀出卷尺讀數，得到相機的世界坐標大約為(520,0,330)。細心的讀者應該發現了，上面幾張圖的特征點不一樣了，其實是我中途重新做了一張特征點圖，重新安放實驗裝置的時候已經盡量按照(520,0,330)這個坐標去安放了，但誤差肯定是不可避免的。
把參數輸入例程中，得到結果，計算出相機的世界坐標：
也就是(528.6,-2.89,358.6)，跟實際情況還是差不多的。
同時還得到了x y z軸的三個旋轉角
自己動手轉一轉相機，發現也是對的。
對P5點重投影，投影公式為：
結果為：
誤差在10pix以內，結果也是正確的，於是驗證完畢。
P.S.經本人測試發現，solvePnP提供的三種算法都能對相機位姿進行估計，雖然三者直接解出的結果略有不同，但都在誤差范圍之內。其中solvePnP的默認方法迭代法，似乎只能使用共面的四個特征點求位姿，一旦有一個點不共面，解出的結果就會不對。
例程
最后給出例程，例程基於VS2013開發，使用的是OpenCV2.4.X，大家運行前需要將opencv的路徑重新配置成自己電腦上的，不懂的話參考我的博客《OpenCV2+入門系列（一）：OpenCV2.4.9的安裝與測試》。例程中提供兩張照片，其中DSC03323就是"實驗"中所用圖片，例程在計算完成后，會在D盤根目錄下生成兩個txt，分別存儲：相機在世界坐標系的坐標、相機的三個旋轉角。
下載地址：
CSDN：http://download.csdn.net/detail/wx2650/9688155
GIT：https://github.com/vshawn/Shawn_pose_estimation_by_opencv
最后
我現在在外地出差，演示視頻里的東西就下一篇文章中再說了，敬請期待。