本博客內(nèi)容來源于網(wǎng)絡(luò)以及其他書籍彭雾，結(jié)合自己學(xué)習(xí)的心得進行重編輯碟刺，因為看了很多文章不便一一標(biāo)注引用，如圖片文字等侵權(quán)薯酝，請告知刪除南誊。
學(xué)習(xí)筆記目錄----->傳送門 <-----
PNP問題簡介
PNP問題的描述以及定義是相對簡單的，他的目的就是求解3D-2D點對運動的方法蜜托。簡單來說抄囚，就是在已知n個三維空間點坐標(biāo)（相對于某個指定的坐標(biāo)系A(chǔ)）及其二維投影位置的情況下，如何估計相機的位姿（即相機在坐標(biāo)系A(chǔ)下的姿態(tài)）橄务。舉個例子幔托，我們在一幅圖像中，知道其中至少四個圖像中確定的點在3D空間下的相對坐標(biāo)位置，我們就可以估計出相機相對于這些點的姿態(tài)重挑，或者說估計出這些3D點在相機坐標(biāo)系下姿態(tài)嗓化。（上述說的姿態(tài)或者位姿，包括位置以及方向谬哀，即一個6自由度的狀態(tài)）
PNP問題的可解性
從上面的問題簡介中刺覆，我們可以了解到整個問題比較清晰，那么該怎么求解呢史煎？雖然前面定義說明了在已知4對點對的情況下谦屑，是可以求出問題的解，但是在了解怎么求解前篇梭，我們應(yīng)該先了解這個問題是不是真的有解氢橙？這將有助于我們理解下面的PNP問題的求解方法。
在分析PNP問題的可解性之前恬偷，應(yīng)該需要知道小孔相機的參數(shù)模型悍手，后續(xù)會在我的其他文章中給出。
1. 當(dāng)PNP中N=1時袍患，即只知道一對3D-2D點對的情況下：
當(dāng)只有一個特征點P1坦康，我們假設(shè)它就在圖像的正中央，那么顯然向量OcP1就是相機坐標(biāo)系中的Z軸诡延，此事相機永遠(yuǎn)是面對P1滞欠，于是相機可能的位置就是在以P1為球心的球面上，再一個就是球的半徑也無法確定孕暇，于是有無數(shù)個解。
如上圖（2D示意圖）的示例赤兴，Oc有可能在O1球上的任何一點妖滔，或者O2或O3球上的任何一點。
2. 當(dāng)PNP中N=2時桶良，即只知道兩對3D-2D點對的情況下：
現(xiàn)在多了一個約束條件座舍，顯然OcP1P2形成一個三角形，由于P1陨帆、P2兩點位置確定曲秉，三角形的邊P1P2確定，再加上向量OcP1疲牵，OcP2啄糙，根據(jù)小孔模型急侥，從Oc點射線特征點的方向角也能確定，于是能夠計算出OcP1的長度=r1，OcP2的長度=r2僧免。于是這種情況下得到兩個球：以P1為球心，半徑為r1的球A；以P2為球心，半徑為r2的球B神妹。顯然，相機位于球A家妆，球B的相交處,應(yīng)該是一個圓圈鸵荠，依舊是無數(shù)個解。
3. 當(dāng)PNP中N=3時伤极，即只知道三對3D-2D點對的情況下：
與上述相似蛹找，這次又多了一個以P3為球心的球C，相機這次位于ABC三個球面的相交處塑荒，終于不再是無數(shù)個解了熄赡，但是這次應(yīng)該會有4個解，其中一個就是我們需要的真解了齿税。
4. 當(dāng)PNP中N>3時彼硫，即知道三對以上3D-2D點對的情況下：
N=3時求出4組解，對于一般的方程組再加一組數(shù)據(jù)就可以解決問題凌箕，事實上也幾乎如此拧篮。這里還有其他一些特殊情況，這些特殊情況就比較復(fù)雜了牵舱，暫不詳解串绩。我們就可以認(rèn)定，N>3后芜壁，能夠求出正解了礁凡。但當(dāng)直接使用4個點時，由于精度誤差慧妄，并不能直接接觸精確解顷牌，而是先用3個點計算出4組解獲得四個旋轉(zhuǎn)矩陣、平移矩陣塞淹。根據(jù)公式：
將第四個點的世界坐標(biāo)代入公式窟蓝，獲得其在圖像中的四個投影（一個解對應(yīng)一個投影），取出其中投影誤差最小的那個解饱普，就是我們所需要的正解运挫。
至此，我們就能確定在我們有至少4個點時套耕，就PNP問題就可以結(jié)算出一個相對正確的解谁帕。
PNP問題求解方法
PNP 的問題是一致的，不同的就是在已知3D-2D的點對的情況下冯袍，怎么求出相機的位姿或者說點對在相機坐標(biāo)系下的姿態(tài)雇卷。常見的PNP問題的求解方法，有以下幾種:
- 直接線性變換DLT
- EPnP
- SDP
- P3P
- UPnP
- 非線性優(yōu)化方法等……
下面我們來一一詳細(xì)的解釋其中幾種方法求解的思路和過程，其中EPNP詳解关划，其他方法簡要介紹思路小染。
PNP 問題EPNP解法詳解
有關(guān)EPNP的解釋裤翩，主要是從文章深入EPnP算法學(xué)習(xí)而來，下面文字主要來自這篇博客调榄，并進行重新排版踊赠。(就是感覺CSDN上廣告太多了，導(dǎo)致排版不清晰)
算法的大概思路如下：
利用已知的3d點每庆，通過PCA選擇4個控制點筐带，建立新的局部坐標(biāo)系，從而將3d坐標(biāo)用新的控制點表示出來缤灵。然后伦籍，利用相機投影模型和2d點，轉(zhuǎn)換到相機坐標(biāo)系中腮出，再在相機坐標(biāo)系中建立和世界坐標(biāo)系同樣關(guān)系（每個點在相機坐標(biāo)系和世界坐標(biāo)系下控制點處的坐標(biāo)一致）的4個控制點帖鸦，求解出相機坐標(biāo)系下的四個控制點的坐標(biāo)，進而利用ICP求解pose胚嘲。(先有個初步印象作儿，具體一些概念，我們接下來看公式慢慢理解)馋劈。
控制點和重心坐標(biāo)系
本文中分別用上標(biāo)w和c表示在世界坐標(biāo)系和攝像頭坐標(biāo)系中的坐標(biāo)攻锰，那么，3D參考點在世界坐標(biāo)系中的坐標(biāo)為pwi, i=1,?,n妓雾，在攝像頭參考坐標(biāo)系中的坐標(biāo)為pci, i=1,?,n娶吞。4個控制點在世界坐標(biāo)系中的坐標(biāo)為cwj, j=1,?,4，在攝像頭參考坐標(biāo)系中的坐標(biāo)為ccj?, j=1,?,4君珠。需要指出寝志，在本文中娇斑，pwi策添，pci，cwj毫缆，ccj均非齊次坐標(biāo)唯竹。（Markdown怎么寫公式呢？苦丁？浸颓？？）
EPnP算法將參考點的坐標(biāo)表示為控制點坐標(biāo)的加權(quán)和：
推導(dǎo)如下：
在上述推導(dǎo)過程中谢鹊，用到了EPnP對權(quán)重αij 加和為1的約束條件算吩，如果沒有這個條件則不成立。
那么問題來了：在一般的情形下佃扼，為什么需要4個控制點偎巢？要知道pwi 是非齊次的3D坐標(biāo)，pwi∈?3 兼耀，為什么3個控制點就不可以呢压昼？
這樣我們就可以得出精確解巩踏。本質(zhì)上就是：3D參考點的齊次坐標(biāo)是控制點齊次坐標(biāo)的線性組合秃诵。從而我們可以得出重心坐標(biāo)系的計算方法：
控制點的選擇
原則上，只要控制點滿足矩陣 C 可逆就可以塞琼，但是論文中給出了一個具體的控制點確定方法菠净。3D參考點集為pwi{i=1,?,n}，選擇3D參考點的重心為第一個控制點：
求解控制點在攝像機坐標(biāo)下的坐標(biāo)
設(shè)K 是攝像頭的內(nèi)參矩陣毅往，可以通過標(biāo)定獲得{ui} i=1....n 是參考點{pi} i=1....n 的2D投影，那么
上式中另凌，v[i]k 是特征向量vk 的第i個3×1 sub-vector。我們可以計算MTM 的特征向量得到vi 戒幔，但還需要求出{βi} i=1,?,N吠谢。
這是一個關(guān)于{βk} k=1,?,N的二次方程栅组，這個方程的特點是沒有關(guān)于{βk} k=1,?,N的一次項。如果將這些二次項βiβj變量替換為βij枢析，那么該方程是{βij } i,j=1,?,N的線性方程了玉掸。對于4個控制點，可以得到C24 =6個這樣的方程醒叁。
如果不挖掘任何附加條件司浪，N NN取不同值的時候，新的線性未知數(shù)的個數(shù)分別為：
- N=1 , 線性未知數(shù)的個數(shù)為1;
- N=2 把沼，線性未知數(shù)的個數(shù)為3;
- N=3 啊易，線性未知數(shù)的個數(shù)為6;
- N=4 ，線性未知數(shù)的個數(shù)為10;
N=4 的時候饮睬，未知數(shù)的個數(shù)多于方程的個數(shù)了租谈。注意到βabβcd=βaβbβcβd=βa′b′βc′d′ ，其中{a′,b′,c′,d′}是{a,b,c,d}的一個排列捆愁，我們可以減少未知數(shù)的個數(shù)割去。舉例，如果我們求出了β11昼丑，β12呻逆，β13，那么我們可以得到β23=β12β13/β11 菩帝。這樣咖城，即使對于N=4，我們也可以求解出{βij} i,j=1,?,N了呼奢。
最優(yōu)化
優(yōu)化的目標(biāo)函數(shù)就很明確了：
一個簡單的非線性最優(yōu)化問題
計算攝像頭的位姿
-
計算控制點在攝像頭參考坐標(biāo)下的坐標(biāo)：
-
計算3D參考點在攝像頭參考坐標(biāo)系下的坐標(biāo)：
-
計算{pwi} i=1,?,n的重心pw0和矩陣A：
-
計算{pci} i=1,?,n的重心pc0和矩陣B：
-
計算H:
-
計算位姿中的旋轉(zhuǎn)R：
如果∣R∣<0宜雀，那么R(2,:)=?R(2,:)
-
計算位姿中的平移t：
至此，EPNP 算法描述完畢控妻，感謝Jesse 的總結(jié)
PNP 問題 P3P解法詳解
P3P只需要使用3對匹配點州袒。
可以看出式子中有3個未知數(shù)亥至，所以可以解除對應(yīng)的3個點的解，然后就變成3D-3D的問題了贱迟，使用ICP就可以獲得最后的相機的姿態(tài)了姐扮。
OpenCV中SolvePNP使用效果[代碼]
#include <iostream>
#include <opencv2/opencv.hpp>
void find_rt(cv::Mat iamge ,cv::Mat matrix ,cv::Mat dist_coeffs ,cv::Mat& rvec_mat,cv::Mat& tvec_mat ){
zz::CalibrationMonocular cm; //封裝的一個標(biāo)定類
std::vector<cv::Point2f> image_points;
cm.find_circle(iamge,image_points,cv::Size(4,11),1); //找到圖像中的特征點
std::vector<cv::Point3f> image_points_in3d;
cm.init_cicle_opencv_3dpoints(cv::Size(4,11),image_points_in3d,1,1); // 生成圖像中的特征點對應(yīng)的3D空間中的點，以第一個點為坐標(biāo)原點
cv::solvePnP(image_points_in3d,image_points,matrix,dist_coeffs,rvec_mat,tvec_mat); //調(diào)用opencv的solvePnP求解衣吠，默認(rèn)是迭代法求解
}
int main(int argc, char *argv[])
{
cv::Mat image;
image = cv::imread(argv[1]);
cv::Mat intrinsic = cv::Mat(3,3,CV_64FC1,cv::Scalar::all(0));
cv::Mat distortion = cv::Mat(1,5,CV_64FC1,cv::Scalar::all(0));
intrinsic.at<double>(0,0) = 2469.453065156600600;
intrinsic.at<double>(1,1) = 2470.233140630011300;
intrinsic.at<double>(0,2) = 1991.786856389101300;
intrinsic.at<double>(1,2) = 1500.165893152338400;
intrinsic.at<double>(2,2) = 1;
distortion.at<double>(0,0) = 0.031219512203754 ;
distortion.at<double>(0,1) = -0.063477515747468 ;
distortion.at<double>(0,2) = 0.000016372054794 ;
distortion.at<double>(0,3) = -0.000635253902664 ;
distortion.at<double>(0,4) = 0.000000000000000;
cv::Mat l_rvec_mat,l_tvec_mat;
cv::Mat cover_left_image;
cover_left_image = image.clone();
cv::rectangle(cover_left_image,cv::Point(0,0),cv::Point(2000,3000),cv::Scalar(0,0,255),-1,8,0);
find_rt(cover_left_image,intrinsic,distortion,l_rvec_mat,l_tvec_mat);
std::cout<<"rotate vertor:"<<l_rvec_mat.t()<<std::endl<<"transfer vertor:"<<l_tvec_mat.t()<<std::endl;
return 0;
}
結(jié)果
標(biāo)定板坐標(biāo)原點相對于相機坐標(biāo)系的旋轉(zhuǎn)向量 [0.01777733651588947, 0.01795601519257841, 0.01169115601267934]
標(biāo)定板坐標(biāo)原點相對于相機坐標(biāo)系的旋轉(zhuǎn)向量[0.0213250098434544, -0.06994240284869389, 0.2992679424216718]
和我當(dāng)時拍攝的效果應(yīng)該差不多茶敏。
重要的事情說三遍：
如果您看到我的文章對您有所幫助缚俏，那就點個贊唄 ( * ^ __ ^ * )
如果您看到我的文章對您有所幫助惊搏，那就點個贊唄( * ^ __ ^ * )
如果您看到我的文章對您有所幫助，那就點個贊唄( * ^ __ ^ * )
傳統(tǒng)2D計算機視覺學(xué)習(xí)筆記目錄傳送門
傳統(tǒng)3D計算機視覺學(xué)習(xí)筆記目錄傳送門
任何人或團體忧换、機構(gòu)全部轉(zhuǎn)載或者部分轉(zhuǎn)載恬惯、摘錄，請保留本博客鏈接或標(biāo)注來源亚茬。博客地址：開飛機的喬巴
作者簡介：開飛機的喬巴（WeChat：zhangzheng-thu）酪耳，現(xiàn)主要從事機器人抓取視覺系統(tǒng)以及三維重建等3D視覺相關(guān)方面，另外對slam以及深度學(xué)習(xí)技術(shù)也頗感興趣刹缝，歡迎加我微信或留言交流相關(guān)工作葡兑。
