【計算機視覺】opencv靶標相機姿態解算2 根據四個特徵點估計相機姿態及實時位姿估計與三維重建相機姿態

阿新 • • 發佈：2018-11-02

https://blog.csdn.net/kyjl888/article/details/71305149

1 基本原理之如何解PNP問題

轉載基本原理之如何解PNP問題

http://www.cnblogs.com/singlex/p/pose_estimation_0.html

相機位姿估計0：基本原理之如何解PNP問題

關鍵詞：相機位姿估計 PNP問題求解

用途：各種位姿估計

文章型別：原理

@Author：VShawn([email protected])

@Date:2016-11-18

@Lab: [email protected]

《相機位姿估計0：基本原理之如何解PNP問題》

《相機位姿估計1：根據四個特徵點估計相機姿態》

《相機位姿估計1_1：OpenCV:solvePnP二次封裝與效能測試》

《相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態》

《相機位姿估計3：根據兩幅影象的位姿估計結果求某點的世界座標》

今天給大家講一講相機位姿估計的基本原理，說實話我本人也沒太瞭解，這裡權當做拋磚引玉了。本來我這個部落格是寫應用型文章的，但雖然不做理論研究，但你要使用別人的方法來解決問題，那麼也還是多多少少要對它的原理有點了解的。

關於PNP問題就是指通過世界中的N個特徵點與影象成像中的N個像點，計算出其投影關係，從而獲得相機或物體位姿的問題。

以下討論中設相機位於點Oc，P1、P2、P3……為特徵點。

Case1：當N=1時

當只有一個特徵點P1，我們假設它就在影象的正中央，那麼顯然向量OcP1就是相機座標系中的Z軸，此事相機永遠是面對P1，於是相機可能的位置就是在以P1為球心的球面上，再一個就是球的半徑也無法確定，於是有無數個解。

Case2：當N=2時

現在多了一個約束條件，顯然OcP1P2形成一個三角形，由於P1、P2兩點位置確定，三角形的變P1P2確定，再加上向量OcP1，OcP2從Oc點射線特徵點的方向角也能確定，於是能夠計算出OcP1的長度=r1，OcP2的長度=r2。於是這種情況下得到兩個球：以P1為球心，半徑為r1的球A；以P2為球心，半徑為r2的球B。顯然，相機位於球A，球B的相交處，依舊是無數個解。

Case3：當N=3時

與上述相似，這次又多了一個以P3為球心的球C，相機這次位於ABC三個球面的相交處，終於不再是無數個解了，這次應該會有4個解，其中一個就是我們需要的真解了。

Case4：當N大於3時

N=3時求出4組解，好像再加一個點就能解決這個問題了，事實上也幾乎如此。說幾乎是因為還有其他一些特殊情況，這些特殊情況就不再討論了。N>3後，能夠求出正解了，但為了一個正解就又要多加一個球D顯然不夠"環保"，為了更快更節省計算機資源地解決問題，先用3個點計算出4組解獲得四個旋轉矩陣、平移矩陣。根據公式：

將第四個點的世界座標代入公式，獲得其在影象中的四個投影（一個解對應一個投影），取出其中投影誤差最小的那個解，就是我們所需要的正解。

PNP問題的求解原理大致就是上面這樣了，至於具體的數學方法還是請大家自己去查閱文獻吧，本部落格對這個問題的分析就到此為止了。接下來請看通過解PNP問題，求解相機位姿的應用。

2 根據四個特徵點估計相機姿態

相機位姿估計1：根據四個特徵點估計相機姿態

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

用途：各種位姿估計

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

@Author：VShawn([email protected])

@Date:2016-11-18

@Lab: [email protected]

前言

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

3 OpenCV:solvePnP二次封裝與效能測試

轉自 http://www.cnblogs.com/singlex/p/pose_estimation_1_1.html

相機位姿估計1_1：OpenCV:solvePnP二次封裝與效能測試

關鍵詞：OpenCV::solvePnP

文章型別：方法封裝、測試

@Author：VShawn([email protected])

@Date:2016-11-27

@Lab: [email protected]

前言

今天給大家帶來的是一篇關於程式功能、效能測試的文章，讀過《相機位姿估計1：根據四個特徵點估計相機姿態》一文的同學應該會發現，直接使用OpenCV的solvePnP來估計相機位姿，在程式呼叫上相當麻煩，從一開始的引數設定到最後將計算出的矩陣轉化為相機的位姿引數，需要花費近兩百行程式碼。因此為了更方便地呼叫程式，今天我就給大家帶來一個我自己對solvePnP的封裝類PNPSolver，順便將OpenCV自帶的三種求解方法測試一遍。

類的封裝

封裝的思路我就不寫了，由於部落格更新速度趕不上我寫程式的速度，現在發上來的類已經修改過好幾次了，思路也換了幾次。不過大的方向沒變，目的就是隻需要輸入引數，輸入座標點後直接可以得到相機在世界座標系的座標。

類的呼叫順序：

1.初始化PNPSolver類；

2.呼叫SetCameraMatrix(),SetDistortionCoefficients()設定好相機內參數與鏡頭畸變引數；

3.向Points3D，Points2D中新增一一對應的特徵點對；

4.呼叫Solve()方法執行計算；

5.從屬性Theta_C2W中提取旋轉角，從Position_OcInW中提取出相機在世界座標系下的座標。

以下是類體：

PNPSolver.h

+ View Code

PNPSolver.cpp

+ View Code

一個典型的呼叫示例

//初始化PNPSolver類

PNPSolver p4psolver;

//初始化相機引數

p4psolver.SetCameraMatrix(fx, fy, u0, v0);

//設定畸變引數

p4psolver.SetDistortionCoefficients(k1, k2, p1, p2, k3);

//設定特徵點的世界座標

p4psolver.Points3D.push_back(cv::Point3f(0, 0, 0)); //P1三維座標的單位是毫米

p4psolver.Points3D.push_back(cv::Point3f(0, 200, 0)); //P2

p4psolver.Points3D.push_back(cv::Point3f(150, 0, 0)); //P3

//p4psolver.Points3D.push_back(cv::Point3f(150, 200, 0)); //P4

p4psolver.Points3D.push_back(cv::Point3f(0, 100, 105)); //P5

cout << "test2:特徵點世界座標 = " << endl << p4psolver.Points3D << endl;

//設定特徵點的影象座標

p4psolver.Points2D.push_back(cv::Point2f(2985, 1688)); //P1

p4psolver.Points2D.push_back(cv::Point2f(5081, 1690)); //P2

p4psolver.Points2D.push_back(cv::Point2f(2997, 2797)); //P3

//p4psolver.Points2D.push_back(cv::Point2f(5544, 2757)); //P4

p4psolver.Points2D.push_back(cv::Point2f(4148, 673)); //P5

cout << "test2:圖中特徵點座標 = " << endl << p4psolver.Points2D << endl;

if (p4psolver.Solve(PNPSolver::METHOD::CV_P3P) == 0)

cout << "test2:CV_P3P方法: 相機位姿→" << "Oc座標=" << p4psolver.Position_OcInW << " 相機旋轉=" << p4psolver.Theta_W2C << endl;

if (p4psolver.Solve(PNPSolver::METHOD::CV_ITERATIVE) == 0)

cout << "test2:CV_ITERATIVE方法: 相機位姿→" << "Oc座標=" << p4psolver.Position_OcInW << " 相機旋轉=" << p4psolver.Theta_W2C << endl;

if (p4psolver.Solve(PNPSolver::METHOD::CV_EPNP) == 0)

cout << "test2:CV_EPNP方法: 相機位姿→" << "Oc座標=" << p4psolver.Position_OcInW << " 相機旋轉=" << p4psolver.Theta_W2C << endl;

方法測試

OpenCV提供了三種方法進行PNP計算，三種方法具體怎麼計算的就請各位自己查詢opencv documentation以及相關的論文了，我看了個大概然後結合自己實際的測試情況給出一個結論，不一定正確，僅供參考：

方法名	說明	測試結論
CV_P3P	這個方法使用非常經典的Gao方法解P3P問題，求出4組可能的解，再通過對第四個點的重投影，返回重投影誤差最小的點。論文《Complete Solution Classification for the Perspective-Three-Point Problem》	可以使用任意4個特徵點求解，不要共面，特徵點數量不為4時報錯
CV_ITERATIVE	該方法基於Levenberg-Marquardt optimization迭代求解PNP問題，實質是迭代求出重投影誤差最小的解，這個解顯然不一定是正解。實測該方法只有用4個共面的特徵點時才能求出正確的解，使用5個特徵點或4點非共面的特徵點都得不到正確的位姿。	只能用4個共面的特徵點來解位姿
CV_EPNP	該方法使用EfficientPNP方法求解問題，具體怎麼做的當時網速不好我沒下載到論文，後面又懶得去看了。論文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》	對於N個特徵點，只要N>3就能夠求出正解。

測試截圖：

1.使用四個共面的特徵點，顯然三種方法都能得到正解，但相互之間略有誤差。

2使用四個非共面的特徵點，CV_ITERATIVE方法解錯了。

3.使用5個特徵點求解，只有CV_EPNP能夠用

效能測試

最後對三種方法的效能進行測試，通過對test1重複執行1000次獲得演算法的執行時間，從結果可以看出迭代法顯然是最慢的，Gao的P3P+重投影法用時最少，EPNP法其次。

總結

綜合以上的測試，推薦使用CV_P3P來解決實際問題，該方法對於有四個特徵點的情況限制少、運算速度快。當特徵點數大於4時，可以取多組4特徵點計算出結果再求平均值，或者為了簡單點就直接使用CV_EPNP法。

不推薦使用CV_ITERATIVE方法。

4 實時位姿估計與三維重建相機姿態

前言

本文將展示一個實時相機位姿估計的例程，其中的原理在前文中已經說過了，再利用《相機位姿估計1_1：OpenCV、solvePnP二次封裝與效能測試》中構建的類，使得程式處理更加簡單。本例程利用HSV空間，跟蹤紅色的特徵點，將跟蹤到的特徵點用於解PNP問題，得到相機位姿（相機的世界座標與相機的三個旋轉角）。最後使用labview中的三維圖片控制元件，對整個系統進行3D重建。

處理流程

首先初始化工業相機，採集到實時影象，使用imshow顯示圖片。
在實時的相機採圖中，依次選取P1、P2、P3、P4（在前文《相機位姿估計1：根據共面四點估計相機姿態》中有提及），一定要按順序點，否則無法獲得正確位姿。選取完成後立即對該點進行追蹤。
當跟蹤的特徵點數量達到4個時，程式開始呼叫PNPSolver類估計相機位姿。
將得到的位姿資訊寫入txt，位於D盤根目錄（這就是上一篇文章中為什麼要寫檔案的原因）。
Labview程式執行後不斷讀取txt，將讀到的位姿資料應用到3D中，繪製出正確的三維場景。（這裡兩個程序通過txt通訊效率很低，但我偷懶了，沒有再去編寫更好的程式）

用流程圖來表示就是：

過程非常簡單，C++程式用來計算位姿，labview程式用於顯示。

（對於不懂labview的讀者：也可以通過OpenGL來實現顯示部分）

特徵點跟蹤方法

為了偷懶省事，這裡的特徵點跟蹤直接使用了最簡單的跟蹤顏色的方法。我做的標誌圖是這樣的：

每個特徵點都是紅色馬克筆塗出的紅點。

在實際操作中使用者首先在顯示介面中按照順序（程式中點的世界座標輸入順序）點選特徵點，得到特徵點的初始位置。根據初始位置，在其附近選取ROI，將BGR影象轉為HSV影象進行顏色分割，針對其H通道進行二值化，將紅色區域置為255，得到二值影象。在二值影象中查詢連通域，並計算出連通域的重心G的位置，將G的座標作為本次跟蹤結果返回，並作為下一次跟蹤的起點。

效果如下圖，圖中綠色的圈是以重心G為圓心繪製的。

函式如下：

+ View Code

位姿估計

當用戶點選了四個特徵點後，程式開始執行位姿估計部分。位姿具體的過程不再敘述，請參考前面的博文：

《相機位姿估計1：根據四個特徵點估計相機姿態》

《相機位姿估計1_1：OpenCV:solvePnP二次封裝與效能測試》

三維顯示

位姿估計完成後，會輸出兩個txt用於記錄相機當前的位姿。

Labview程式就是讀取這兩個txt的資訊，進而顯示出三維空間。labview程式的程式設計過程比較難敘述，思路便是首先建立世界座標系，然後在世界座標系中建立一個三維物體作為相機的三維模型。然後根據txt中的資訊，設定這個模型所在的位置（也就是三維座標），再設定該模型的三個自旋角，完成三維繪製。

上述流程可以執行專案資料夾中的：

~\用LabView重建相機位置\世界-手動調整引數設定相機位姿.vi

來手動設定引數，體驗繪圖的流程。

對該部分感興趣的人可以參考文件：

http://zone.ni.com/reference/zhs-XX/help/371361J-0118/lvhowto/create_3d_scene/

效果演示

這演示以前也有放出來過，就是實時跟蹤特徵點，再在右邊重建相機姿態。

程式下載

最後給出例程，例程C++部分基於VS2013開發，使用的是OpenCV2.4.X，三維重建部分使用Labview2012開發。OpenCV配置參考我的部落格《OpenCV2+入門系列（一）：OpenCV2.4.9的安裝與測試》，Labview則是直接安裝好就行。

例程下載後，需要將影象採集部分修改為你的相機驅動，然後修改相機引數、畸變引數就能夠使用了。

地址：

C++程式：Github

LabView程式：Github

程式下載地址：Github

【計算機視覺】opencv靶標相機姿態解算2 根據四個特徵點估計相機姿態 及 實時位姿估計與三維重建相機姿態

1 基本原理之如何解PNP問題

目錄

Case1：當N=1時

Case2：當N=2時

Case3：當N=3時

Case4：當N大於3時

2 根據四個特徵點估計相機姿態

前言

原理簡介

流程

實驗

例程

3 OpenCV:solvePnP二次封裝與效能測試

前言

類的封裝

方法測試

效能測試

總結

4 實時位姿估計與三維重建相機姿態

前言

處理流程

特徵點跟蹤方法

位姿估計

三維顯示

效果演示

程式下載

相關推薦

【計算機視覺】opencv靶標相機姿態解算2 根據四個特徵點估計相機姿態及實時位姿估計與三維重建相機姿態