關鍵詞：位姿估計 ?OpenCV::solvePnP

用途：各種位姿估計

文章類型：原理、流程、Demo示例

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-18

@Lab: CvLab202@CSU

?

?

目錄

• 《相機位姿估計0：基本原理之如何解PNP問題》
• 《相機位姿估計1：根據四個特征點估計相機姿態》
• 《相機位姿估計1_1：OpenCV:solvePnP二次封裝與性能測試》
• 《相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態》
• 《相機位姿估計3：根據兩幅圖像的位姿估計結果求某點的世界坐標》

前言

本文通過迭代法解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

?

最后

我現在在外地出差，演示視頻里的東西就下一篇文章中再說了，敬請期待。

作者：VShawn

出處：http://www.cnblogs.com/singlex/

本文版權歸作者所有，歡迎轉載，但未經博客作者同意必須保留此段聲明，且在文章頁面明顯位置給出原文連接，否則保留追究法律責任的權利。

分類:?機器視覺應用,位姿估計,應用數學

總結

以上是生活随笔為你收集整理的根据四个特征点求解相机位姿的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。