關(guān)鍵詞：位姿估計(jì) ?OpenCV::solvePnP

用途：各種位姿估計(jì)

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

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-18

@Lab: CvLab202@CSU

?

?

目錄

• 《相機(jī)位姿估計(jì)0：基本原理之如何解PNP問(wèn)題》
• 《相機(jī)位姿估計(jì)1：根據(jù)四個(gè)特征點(diǎn)估計(jì)相機(jī)姿態(tài)》
• 《相機(jī)位姿估計(jì)1_1：OpenCV:solvePnP二次封裝與性能測(cè)試》
• 《相機(jī)位姿估計(jì)2：[應(yīng)用]實(shí)時(shí)位姿估計(jì)與三維重建相機(jī)姿態(tài)》
• 《相機(jī)位姿估計(jì)3：根據(jù)兩幅圖像的位姿估計(jì)結(jié)果求某點(diǎn)的世界坐標(biāo)》

前言

本文通過(guò)迭代法解PNP問(wèn)題，得到相機(jī)坐標(biāo)系關(guān)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣R與平移矩陣T后，根據(jù)之前的文章《根據(jù)相機(jī)旋轉(zhuǎn)矩陣求解三個(gè)軸的旋轉(zhuǎn)角》獲得相機(jī)坐標(biāo)系的三軸旋轉(zhuǎn)角，實(shí)現(xiàn)了對(duì)相機(jī)位姿的估計(jì)。知道相機(jī)在哪后，我們就可以通過(guò)兩張照片，計(jì)算出照片中某個(gè)點(diǎn)的高度，實(shí)現(xiàn)對(duì)環(huán)境的測(cè)量。

先看演示視頻：

原理簡(jiǎn)介

相機(jī)位姿估計(jì)就是通過(guò)幾個(gè)已知坐標(biāo)的特征點(diǎn)，以及他們?cè)谙鄼C(jī)照片中的成像，求解出相機(jī)位于坐標(biāo)系內(nèi)的坐標(biāo)與旋轉(zhuǎn)角度，其核心問(wèn)題就在于對(duì)PNP問(wèn)題的求解，這部分本文不再啰嗦，參見(jiàn)本人之前的博客文章《相機(jī)位姿估計(jì)0：基本原理之如何解PNP問(wèn)題》。本文中對(duì)pnp問(wèn)題的求解直接調(diào)用了OpenCV的庫(kù)函數(shù)"solvePnP"，其函數(shù)原型為：

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

第一個(gè)輸入objectPoints為特征點(diǎn)的世界坐標(biāo)，坐標(biāo)值需為float型，不能為double型，可以輸入mat類型，也可以直接輸入vector<point3f>?。

第二個(gè)輸入imagePoints為特征點(diǎn)在圖像中的坐標(biāo)，需要與前面的輸入一一對(duì)應(yīng)。同樣可以輸入mat類型，也可以直接輸入vector<point3f>?。

第三個(gè)輸入cameraMatrix為相機(jī)內(nèi)參數(shù)矩陣，大小為3×3，形式為：

第四個(gè)輸入distCoeffs輸入為相機(jī)的畸變參數(shù)，為1×5的矩陣。

第五個(gè)rvec為輸出矩陣，輸出解得的旋轉(zhuǎn)向量。

第六個(gè)tvec為輸出平移向量。

第七個(gè)設(shè)置為true后似乎會(huì)對(duì)輸出進(jìn)行優(yōu)化。

最后的輸入?yún)?shù)有三個(gè)可選項(xiàng)：

CV_ITERATIVE，默認(rèn)值，它通過(guò)迭代求出重投影誤差最小的解作為問(wèn)題的最優(yōu)解。

CV_P3P則是使用非常經(jīng)典的Gao的P3P問(wèn)題求解算法。

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

流程

1.從函數(shù)的原型看出函數(shù)需要相機(jī)的內(nèi)參數(shù)與畸變參數(shù)，于是相機(jī)標(biāo)定是必不可少的，通過(guò)OpenCV自帶例程或者M(jìn)atlab的相機(jī)標(biāo)定工具箱都可以很方便地求出相機(jī)標(biāo)定參數(shù)。

2.準(zhǔn)備好四個(gè)特征點(diǎn)的世界坐標(biāo)，存入Mat矩陣

vector<cv::Point3f> Points3D; Points3D.push_back(cv::Point3f(0, 0, 0)); //P1 三維坐標(biāo)的單位是毫米 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.準(zhǔn)備好四個(gè)特征點(diǎn)在圖像上的對(duì)應(yīng)點(diǎn)坐標(biāo)，這個(gè)坐標(biāo)在實(shí)驗(yàn)中我是通過(guò)PhotoShop數(shù)出來(lái)的。注意，輸入坐標(biāo)的順序一定要與之前輸入世界坐標(biāo)的順序一致，就是說(shuō)點(diǎn)與點(diǎn)要對(duì)應(yīng)上，OpenCV的函數(shù)無(wú)法解決點(diǎn)與點(diǎn)匹配的問(wèn)題（對(duì)應(yīng)搜索問(wèn)題）。

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.創(chuàng)建輸出變量，即旋轉(zhuǎn)矩陣跟平移矩陣的變量。最后調(diào)用函數(shù)。

//初始化輸出矩陣 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); //實(shí)測(cè)迭代法似乎只能用共面特征點(diǎn)求位置 //solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P); //Gao的方法可以使用任意四個(gè)特征點(diǎn) //solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP); 5將輸出的旋轉(zhuǎn)向量轉(zhuǎn)變?yōu)樾D(zhuǎn)矩陣

double rm[9]; cv::Mat rotM(3, 3, CV_64FC1, rm); Rodrigues(rvec, rotM);

6.最后根據(jù)《根據(jù)相機(jī)旋轉(zhuǎn)矩陣求解三個(gè)軸的旋轉(zhuǎn)角》一文求出相機(jī)的三個(gè)旋轉(zhuǎn)角，根據(jù)《子坐標(biāo)系C在父坐標(biāo)系W中的旋轉(zhuǎn)》求出相機(jī)在世界坐標(biāo)系中的位置。

至此，我們就求出了相機(jī)的位姿。

實(shí)驗(yàn)

本人在實(shí)驗(yàn)中先后使用了兩臺(tái)相機(jī)做測(cè)試，一臺(tái)是畸變較小的sony a6000微單+35mm定焦鏡頭，另一臺(tái)是畸變較重的130w的工業(yè)相機(jī)+6mm定焦廣角鏡頭，實(shí)驗(yàn)中兩臺(tái)相機(jī)都得到了正確的位姿結(jié)果，此處為了方便只用α6000微單做演示說(shuō)明。

如上圖所示，四個(gè)特征點(diǎn)P1-P4的世界坐標(biāo)與像素坐標(biāo)都已在圖中標(biāo)明，P5用于重投影驗(yàn)證位姿解是否正確。

相機(jī)實(shí)際位姿大約為：

?

粗略讀出卷尺讀數(shù)，得到相機(jī)的世界坐標(biāo)大約為(520,0,330)。細(xì)心的讀者應(yīng)該發(fā)現(xiàn)了，上面幾張圖的特征點(diǎn)不一樣了，其實(shí)是我中途重新做了一張?zhí)卣鼽c(diǎn)圖，重新安放實(shí)驗(yàn)裝置的時(shí)候已經(jīng)盡量按照(520,0,330)這個(gè)坐標(biāo)去安放了，但誤差肯定是不可避免的。

?

把參數(shù)輸入例程中，得到結(jié)果，計(jì)算出相機(jī)的世界坐標(biāo)：

也就是(528.6,-2.89,358.6)，跟實(shí)際情況還是差不多的。

同時(shí)還得到了x y z軸的三個(gè)旋轉(zhuǎn)角

自己動(dòng)手轉(zhuǎn)一轉(zhuǎn)相機(jī)，發(fā)現(xiàn)也是對(duì)的。

對(duì)P5點(diǎn)重投影，投影公式為：

結(jié)果為：

誤差在10pix以內(nèi)，結(jié)果也是正確的，于是驗(yàn)證完畢。

?

P.S.經(jīng)本人測(cè)試發(fā)現(xiàn)，solvePnP提供的三種算法都能對(duì)相機(jī)位姿進(jìn)行估計(jì)，雖然三者直接解出的結(jié)果略有不同，但都在誤差范圍之內(nèi)。其中solvePnP的默認(rèn)方法迭代法，似乎只能使用共面的四個(gè)特征點(diǎn)求位姿，一旦有一個(gè)點(diǎn)不共面，解出的結(jié)果就會(huì)不對(duì)。

例程

最后給出例程，例程基于VS2013開(kāi)發(fā)，使用的是OpenCV2.4.X，大家運(yùn)行前需要將opencv的路徑重新配置成自己電腦上的，不懂的話參考我的博客《OpenCV2+入門(mén)系列（一）：OpenCV2.4.9的安裝與測(cè)試》。例程中提供兩張照片，其中DSC03323就是"實(shí)驗(yàn)"中所用圖片，例程在計(jì)算完成后，會(huì)在D盤(pán)根目錄下生成兩個(gè)txt，分別存儲(chǔ)：相機(jī)在世界坐標(biāo)系的坐標(biāo)、相機(jī)的三個(gè)旋轉(zhuǎn)角。

?

下載地址：

CSDN：http://download.csdn.net/detail/wx2650/9688155

GIT：https://github.com/vshawn/Shawn_pose_estimation_by_opencv

?

最后

我現(xiàn)在在外地出差，演示視頻里的東西就下一篇文章中再說(shuō)了，敬請(qǐng)期待。

作者：VShawn

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

本文版權(quán)歸作者所有，歡迎轉(zhuǎn)載，但未經(jīng)博客作者同意必須保留此段聲明，且在文章頁(yè)面明顯位置給出原文連接，否則保留追究法律責(zé)任的權(quán)利。

分類:?機(jī)器視覺(jué)應(yīng)用,位姿估計(jì),應(yīng)用數(shù)學(xué)

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。