3D-2D：PnP算法原理

• • 1.問(wèn)題背景—— 什么是PnP問(wèn)題 ？
  • 2.PnP問(wèn)題的求解方法
  • • 2.1 P3P
    • • 2.1.1 算法的實(shí)際理解
      • 2.1.2 算法的數(shù)學(xué)推導(dǎo)
      • 2.1.3 算法的缺陷
    • 2.2 直接線性變換(DLT)
  • 3.實(shí)驗(yàn)代碼

根據(jù)高博士的《視覺(jué)SLAM十四講》內(nèi)容和相關(guān)博客的內(nèi)容，對(duì)PnP問(wèn)題有了一點(diǎn)點(diǎn)新的理解。再次記錄一下。
參考博客：
http://www.cnblogs.com/singlex/p/pose_estimation_0.html
http://www.cnblogs.com/singlex/p/pose_estimation_1.html
http://www.cnblogs.com/singlex/p/pose_estimation_3.html

1.問(wèn)題背景—— 什么是PnP問(wèn)題 ？

PnP(Perspective-n-Point)是求解 3D 到 2D 點(diǎn)對(duì)運(yùn)動(dòng)的方法。它描述了當(dāng)我們知道n 個(gè) 3D 空間點(diǎn)以及它們的投影位置時(shí),如何估計(jì)相機(jī)所在的位姿。——《視覺(jué)SLAM十四講》

通俗的講，PnP問(wèn)題就是在已知世界坐標(biāo)系下N個(gè)空間點(diǎn)的真實(shí)坐標(biāo)以及這些空間點(diǎn)在圖像上的投影，如何計(jì)算相機(jī)所在的位姿。羅嗦一句：已知量是空間點(diǎn)的真實(shí)坐標(biāo)和圖像坐標(biāo)，未知量（求解量）是相機(jī)的位姿。

PnP 問(wèn)題有很多種求解方法，例如用三對(duì)點(diǎn)估計(jì)位姿的 P3P 、直接線性變換(DLT)、EPnP。此外,還能用非線性優(yōu)化的方式,構(gòu)建最小二乘問(wèn)題并迭代求解,也就是萬(wàn)金油式的 Bundle Adjustment。下面介紹逐一介紹。

2.PnP問(wèn)題的求解方法

2.1 P3P

2.1.1 算法的實(shí)際理解

PnP問(wèn)題是在已知n 個(gè) 3D 空間點(diǎn)以及它們的投影位置時(shí)估計(jì)相機(jī)所在的位姿。那么 n 最小為多少時(shí)我們才能進(jìn)行估算呢（最少需要幾個(gè)3D-2D點(diǎn)對(duì)）？

我們可以設(shè)想以下場(chǎng)景，設(shè)相機(jī)位于點(diǎn)Oc，P1、P2、P3……為特征點(diǎn)。

場(chǎng)景1：N = 1時(shí)
當(dāng)只有一個(gè)特征點(diǎn)P1，我們假設(shè)它就在圖像的正中央，那么顯然向量OcP1就是相機(jī)坐標(biāo)系中的Z軸，此時(shí)相機(jī)永遠(yuǎn)是面對(duì)P1，于是相機(jī)可能的位置就是在以P1為球心的球面上，此外球的半徑也無(wú)法確定，于是有無(wú)數(shù)個(gè)解。

場(chǎng)景2：N = 2時(shí)
現(xiàn)在多了一個(gè)約束條件，顯然OcP1P2形成一個(gè)三角形，由于P1、P2兩點(diǎn)位置確定，三角形的邊P1P2確定。再加上向量OcP1和OcP2，從Oc點(diǎn)射向特征點(diǎn)的方向角也能確定。于是能夠計(jì)算出OcP1的長(zhǎng)度=r1，OcP2的長(zhǎng)度=r2。于是這種情況下得到兩個(gè)球：以P1為球心，半徑為r1的球A；以P2為球心，半徑為r2的球B。顯然，相機(jī)位于球A，球B的相交處，依舊是無(wú)數(shù)個(gè)解。

場(chǎng)景3：N = 3時(shí)
這次又多了一個(gè)以P3為球心的球C，相機(jī)這次位于ABC三個(gè)球面的相交處，終于不再是無(wú)數(shù)個(gè)解了，這次應(yīng)該會(huì)有4個(gè)解，其中一個(gè)就是我們需要的真解——即相機(jī)真實(shí)的位姿。

場(chǎng)景4：N > 3時(shí)
N=3時(shí)求出4組解，好像再加一個(gè)點(diǎn)就能解決這個(gè)問(wèn)題了，事實(shí)上也幾乎如此。說(shuō)幾乎是因?yàn)檫€有其他一些特殊情況，這些特殊情況就不再討論了。N>3后，能夠求出正解了，但為了一個(gè)正解就又要多加一個(gè)球D顯然不夠"環(huán)保"，為了更快更節(jié)省計(jì)算機(jī)資源地解決問(wèn)題，先用3個(gè)點(diǎn)計(jì)算出4組解獲得四個(gè)旋轉(zhuǎn)矩陣、平移矩陣。根據(jù)公式：

將第四個(gè)點(diǎn)的世界坐標(biāo)代入公式，獲得其在圖像中的四個(gè)投影（一個(gè)解對(duì)應(yīng)一個(gè)投影），取出其中投影誤差最小的那個(gè)解，就是我們所需要的正解。

2.1.2 算法的數(shù)學(xué)推導(dǎo)

P3P 需要利用給定的三個(gè)點(diǎn)的幾何關(guān)系。它的輸入數(shù)據(jù)為三對(duì) 3D-2D 匹配點(diǎn)。記 3D點(diǎn)為 A, B, C,2D 點(diǎn)為 a, b, c,其中小寫(xiě)字母代表的點(diǎn)為大寫(xiě)字母在相機(jī)成像平面上的投影，如上圖所示。此外，P3P 還需要使用一對(duì)驗(yàn)證點(diǎn),以從可能的解出選出正確的那一個(gè)(類(lèi)似于對(duì)極幾何情形)。記驗(yàn)證點(diǎn)對(duì)為 D ? d,相機(jī)光心為 O。
請(qǐng)注意,我們知道的是A, B, C 在世界坐標(biāo)系中的坐標(biāo),而不是在相機(jī)坐標(biāo)系中的坐標(biāo)。

首先,顯然,三角形之間存在對(duì)應(yīng)關(guān)系:

對(duì)于三角形Oab 和 OAB，利用余弦定理可得：

對(duì)于另外兩組三角形，也有同樣的結(jié)論：

對(duì)上面三式全體除以 OC^2 ，并且記 x = OA/OC， y = OB/OC，得

記 v = AB^2 /OC ^2 , uv = BC^2 /OC^2 , wv = AC^2 /OC^2 ,有:

我們可以把第一個(gè)式子中的 v 放到等式一邊,并代入第 2,3 兩式,得:

由于我們知道 2D 點(diǎn)的圖像位置,三個(gè)余弦角cos ?a, b? , cos ?b, c? , cos ?a, c?是已知的。同時(shí),u = BC^2 /AB^2 , w = AC^2 /AB^2 可以通過(guò)A, B, C 在世界坐標(biāo)系下的坐標(biāo)算出,變換到相機(jī)坐標(biāo)系下之后,并不改變這個(gè)比值，所以也是已知量。該式中的 x, y 是未知的,隨著相機(jī)移動(dòng)會(huì)發(fā)生變化。

因此，P3P問(wèn)題最終可以轉(zhuǎn)換成關(guān)于 x, y 的一個(gè)二元二次方程(多項(xiàng)式方程)

該方程最多可能得到四個(gè)解（有上一小節(jié)也可以得出該結(jié)論）,但我們可以用驗(yàn)證點(diǎn)來(lái)計(jì)算最可能的解，得到 A, B, C 在相機(jī)坐標(biāo)系下的 3D 坐標(biāo)以及相機(jī)的位姿。

2.1.3 算法的缺陷

P3P 也存在著一些問(wèn)題：

1. P3P 只利用三個(gè)點(diǎn)的信息。當(dāng)給定的配對(duì)點(diǎn)多于 3 組時(shí),難以利用更多的信息。
2. 如果 3D 點(diǎn)或 2D 點(diǎn)受噪聲影響,或者存在誤匹配,則算法失效。

所以后續(xù)人們還提出了許多別的方法，如 EPnP、 UPnP 等。它們利用更多的信息，而且用迭代的方式對(duì)相機(jī)位姿進(jìn)行優(yōu)化,以盡可能地消除噪聲的影響。

2.2 直接線性變換(DLT)

考慮某個(gè)空間點(diǎn) P ,它的齊次坐標(biāo)為 P = (X, Y, Z, 1)^T 。在圖像 I 中,投影到特征點(diǎn) x 1 = (u 1 , v 1 , 1)^T (以歸一化平面齊次坐標(biāo)表示)。此時(shí)相機(jī)的位姿 R, t 是未知的。與單應(yīng)矩陣的求解類(lèi)似,我們定義增廣矩陣 [R|t] 為一個(gè) 3 × 4 的矩陣,包含了旋轉(zhuǎn)與平移信息 。我們把它的展開(kāi)形式列寫(xiě)如下:

用最后一行把 s 消去,得到兩個(gè)約束:

為了簡(jiǎn)化表示,定義 T 的行向量:

于是有:

和

請(qǐng)注意 t 是待求的變量,可以看到每個(gè)特征點(diǎn)提供了兩個(gè)關(guān)于 t 的線性約束。假設(shè)一共有 N 個(gè)特征點(diǎn),可以列出線性方程組:

由于 T 一共有 12 維，因此最少通過(guò)六對(duì)匹配點(diǎn)，即可實(shí)現(xiàn)矩陣 T 的線性求解，這種方法(也)稱(chēng)為直接線性變換(Direct Linear Transform, DLT)。當(dāng)匹配點(diǎn)大于六對(duì)時(shí)，可以使用 SVD 等方法對(duì)超定方程求最小二乘解。

在 DLT 求解中，我們直接將 T 矩陣看成了 12 個(gè)未知數(shù)，忽略了它們之間的聯(lián)系。因?yàn)樾D(zhuǎn)矩陣 R ∈ SO(3)，用 DLT 求出的解不一定滿足該約束，它是一個(gè)一般矩陣。平移向量比較好辦，它屬于向量空間。對(duì)于旋轉(zhuǎn)矩陣 R，我們必須針對(duì) DLT 估計(jì)的 T 的左邊3 × 3 的矩陣塊，尋找一個(gè)最好的旋轉(zhuǎn)矩陣對(duì)它進(jìn)行近似。這可以由 QR 分解完成 [3, 48]，相當(dāng)于把結(jié)果從矩陣空間重新投影到 SE(3) 流形上,轉(zhuǎn)換成旋轉(zhuǎn)和平移兩部分。

需要解釋的是，我們這里的 x 1 使用了歸一化平面坐標(biāo)，去掉了內(nèi)參矩陣 K 的影響——這是因?yàn)閮?nèi)參 K 在 SLAM 中通常假設(shè)為已知。如果內(nèi)參未知，那么我們也能用 PnP去估計(jì) K, R, t 三個(gè)量。然而由于未知量的增多，效果會(huì)差一些。

3.實(shí)驗(yàn)代碼

視覺(jué)slam十四講中的代碼運(yùn)行結(jié)果如下：

int main ( int argc, char** argv )
{if ( argc != 5 ){cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;return 1;}//-- 讀取圖像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );cout<<"一共找到了"<<matches.size() <<"組匹配點(diǎn)"<<endl;// 建立3D點(diǎn)Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度圖為16位無(wú)符號(hào)數(shù)，單通道圖像Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );vector<Point3f> pts_3d;vector<Point2f> pts_2d;for ( DMatch m:matches ){ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];if ( d == 0 )   // bad depthcontinue;float dd = d/5000.0;Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );pts_2d.push_back ( keypoints_2[m.trainIdx].pt );}cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;Mat r, t;solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 調(diào)用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法Mat R;cv::Rodrigues ( r, R ); // r為旋轉(zhuǎn)向量形式，用Rodrigues公式轉(zhuǎn)換為矩陣cout<<"R="<<endl<<R<<endl;cout<<"t="<<endl<<t<<endl;//cout<<"calling bundle adjustment"<<endl;bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

對(duì)于該代碼中的BA部分，編譯會(huì)出現(xiàn)問(wèn)題：

/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp: In function ‘void bundleAdjustment(std::vector<cv::Point3_<float> >, std::vector<cv::Point_<float> >, const cv::Mat&, cv::Mat&, cv::Mat&)’:
/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:154:50: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::BlockSolver(g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType*&)’Block* solver_ptr = new Block ( linearSolver );     // 矩陣塊求解器^
In file included from /usr/local/include/g2o/core/block_solver.h:199:0,from /home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:10:
/usr/local/include/g2o/core/block_solver.hpp:40:1: note: candidate: g2o::BlockSolver<Traits>::BlockSolver(std::unique_ptr<typename Traits::LinearSolverType>) [with Traits = g2o::BlockSolverTraits<6, 3>; typename Traits::LinearSolverType = g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >]BlockSolver<Traits>::BlockSolver(std::unique_ptr<LinearSolverType> linearSolver)^
/usr/local/include/g2o/core/block_solver.hpp:40:1: note:   no known conversion for argument 1 from ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType* {aka g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >*}’ to ‘std::unique_ptr<g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >, std::default_delete<g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> > > >’
/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:155:104: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Block*&)’g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );^
In file included from /home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:11:0:
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note: candidate: g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>)explicit OptimizationAlgorithmLevenberg(std::unique_ptr<Solver> solver);^
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note:   no known conversion for argument 1 from ‘Block* {aka g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >*}’ to ‘std::unique_ptr<g2o::Solver>’
/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:156:42: error: redeclaration of ‘g2o::OptimizationAlgorithmLevenberg* solver’g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));^
/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:155:42: note: ‘g2o::OptimizationAlgorithmLevenberg* solver’ previously declared hereg2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );^
/home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:156:112: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::remove_reference<g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >*&>::type)’g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));^
In file included from /home/Andy/my_workspace/Slam/slambook/ch7/pose_estimation_3d2d.cpp:11:0:
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note: candidate: g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>)explicit OptimizationAlgorithmLevenberg(std::unique_ptr<Solver> solver);^
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note:   no known conversion for argument 1 from ‘std::remove_reference<g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >*&>::type {aka g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >*}’ to ‘std::unique_ptr<g2o::Solver>’
CMakeFiles/pose_estimation_3d2d.dir/build.make:62: recipe for target 'CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o' failed
make[3]: *** [CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o] Error 1
CMakeFiles/Makefile2:72: recipe for target 'CMakeFiles/pose_estimation_3d2d.dir/all' failed
make[2]: *** [CMakeFiles/pose_estimation_3d2d.dir/all] Error 2
CMakeFiles/Makefile2:84: recipe for target 'CMakeFiles/pose_estimation_3d2d.dir/rule' failed
make[1]: *** [CMakeFiles/pose_estimation_3d2d.dir/rule] Error 2
Makefile:118: recipe for target 'pose_estimation_3d2d' failed
make: *** [pose_estimation_3d2d] Error 2

參照g2o官方示例和相關(guān)博客，可以做如下修改：

//源代碼// 初始化g2o/*typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 維度為 6, landmark 維度為 3Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 線性方程求解器Block* solver_ptr = new Block ( linearSolver );     // 矩陣塊求解器g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));*///仿照g2o示例中的代碼進(jìn)行修改——可以運(yùn)行auto linearSolver = g2o::make_unique<LinearSolverCSparse<BlockSolverX::PoseMatrixType>>();auto solver_ptr = g2o::make_unique<BlockSolverX >(std::move(linearSolver));OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(std::move(solver_ptr));//網(wǎng)上的修改意見(jiàn)——可以運(yùn)行/*typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 維度為 6, landmark 維度為 3std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>()); // 線性方程求解器std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));     // 矩陣塊求解器g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));*/g2o::SparseOptimizer optimizer;optimizer.setAlgorithm ( solver );

成功編譯后，結(jié)果如下：

總結(jié)

以上是生活随笔為你收集整理的3D-2D：PnP算法原理的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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