淺談PNP的BA求解

本文轉載自查看原文 2019-11-20 22:00 423 SLAM

注：這是我在知乎寫的文章，現搬運至此。原文鏈接：https://zhuanlan.zhihu.com/p/51363371

本文實現的是兩幀間pnp問題的BA求解。為了實現GPU上的BA，對BA過程的透徹理解必不可少，而兩幀間BA優化正是大規模后端優化的基礎.為方便期間,本文求解使用高斯牛頓法。需要額外指出的是，本文並未利H矩陣的稀疏性，直接使用Eigen自帶的最小二乘求解。

本代碼編寫於2018年，如有疏漏，歡迎批評指正。本文代碼地址：https://link.zhihu.com/?target=https%3A//github.com/LiuSQ123/pnp_by_BA

0.PNP和BA簡介

PNP(perspective-n-point)是通過一組匹配好的3D點和2D點來求解兩幀圖像之間運動的一種算法。PNP的求解有DLT(直接線性變換)、P3P、EPNP和BA優化等方式。而BA優化是SLAM中的最核心算法，通過BA求解PNP和SLAM系統后端優化中的BA原理相同。其差別只在於，PNP問題僅僅包含兩幀圖像的位姿，而后端中的BA優化則包含多個圖像的位姿。

BA(bundle adjustment)指的是同時調整相機姿態和特征點位置，以便從每個特征點反射出的光線(bundles of light rays)，通過調整(adjustment)最后都能通過相機光心。故也有人翻譯為光束平差法。BA通常構建為一個最小二乘問題，通過使重投影誤差最小化來同時調整相機的位姿和特征點的坐標。

首先解釋什么是重投影誤差？分為兩步來解釋：

1.重投影：重投影顧名思義就是把 3d空間中的點重新投影到圖像平面上。用數學公式表達就是相機的成像過程，即:

$u=h(\xi,P) \tag{1}$

其中, $h$ 表示相機的成像過程, $\xi$ 為相機在世界坐標系下的位姿(李代數表示)， $P$ 為3d點在世界坐標系下的坐標， $u$ 為該3d點在圖像上像素坐標的理論值。一般情況下，姿態 $\xi$ 和點坐標 $P$ 都為優化變量(優化變量的意思是，在優化開始前我們對 $\xi，P$ 的值只有一個猜測值，而我們要求的正是它們的准確值)。當然，你也可以只優化 $\xi$ 或者 $P$ ，甚至你還可以只優化位姿中的旋轉 $R$ 而固定位移 $t$ ，這一點會在下文中說明。

2.誤差：上面說到， $\xi$ 和 $P$ 都為優化變量，在BA開始之前我們對它無從知曉(正如上文所說，我們對優化變量只有一個猜測值，一般是根據先驗或者傳感器直接測量取得。值得一提的是，針對於BA求解PNP問題，這個初值並不能隨便給定，否則可能會陷入局部極小。而在已知匹配的ICP問題中，初始值可以任意給定，算法總會收斂到全局極小)。那么對應到重投影的公式，再加上噪聲和傳感器誤差的影響，我們根據公式(1)求得的3d點理論像素坐標和我們的觀測到的像素坐標必然不同，而這個理論值和觀測值的差就是重投影誤差，我們定義重投影誤差為：

$e_{r} =(z-u)^2 = ( z-h(\xi,P_{i}))^2 \tag{2}$

其中， $z$ 為觀測值(即相機圖像上特征點的像素坐標), $u$ 為理論值(或者說是我們的預測值)。針對於 $e_{r}$ 我們可以構建最小二乘問題，通過調整 $P,\xi$ 來使重投影誤差的平方和(因為誤差有正有負，所以取平方) 最小化。我們需要做的就是求解出誤差平方和的梯度，然后按梯度下降方向迭代，直至收斂。BA優化的原理就是這么簡單！

1.求解BA

求解最小二乘問題有很多種方式(我相信大家都知道！不知道的請翻十四講p109),如一階和二階梯度下降法，G-N法，L-M法,狗腿(DogLeg)法等。其中G-N法雖然有很多缺點，例如使用 $J^{T}J$ 近似的 $H$ 矩陣只能保持半正定性，以及由此帶來的算法不穩定,但是其編程實現簡單，且優於一,二階梯度法，故本文使用G-N法求解BA。

1.1 問題簡介

我們把第一幀固定,設定為世界坐標系原點(即該幀相機的旋轉為單位矩陣 $R=I$ ,位移 $t = [0,0,0]$ ),同時優化3D點的空間位置和第二幀的姿態。針對於兩幀之間的BA，我們要求解目標函數:

$min \frac{1}{2}\sum_{i=1}^{m}{\left| z-h(\xi,P_{i}) \right|_{2}^{2}}\tag{3}$

其中, $\xi$ 為我們優化位姿的李代數表示(因為我們要求解的是兩幀之間的PNP問題，並且我們固定了第一幀的姿態，所以只有一個 $\xi$ 無下標)， $P_{i}$ 為第一幀和第二幀共同觀測到的特征點坐標，我們假設在兩幀圖片上共匹配了 $m$ 個特征點。

使用李代數 $\mathfrak{se}$ (3)表示姿態的原因是，只有單位正交陣才能表示旋轉，如果使用矩陣 $R$ ，那么就會構建出一個帶有約束的優化問題。而通過李群-李代數的轉換關系，正好可以把BA構建成無約束優化，簡化求解。

我們再設定待估計的狀態向量為:

$x=\begin{vmatrix} \xi & P_{0}&P_{1} & ... & P_{m} \end{vmatrix}^{T} \tag{}$

其中 $\xi$ 為李代數表示的相機位姿 , $P_{i}=\begin{vmatrix} x_{i} & y_{i} & z_{i} \end{vmatrix}^{T}$ 為兩幀間匹配的3D點坐標,一共m個。我們記單個誤差項為(注意和上文的 $e_{r}$ 進行區分, $e_{r}$ 為 $e$ 的平方，原因可參考<14講>高斯牛頓法)：

$e=z-h(\xi,P_{i}) \tag{4}$

可以看出來，一個特征點每被觀測一次，會產生一個這樣的2維誤差。我們再記整體誤差為 $f(x)$ ，那么 $f(x)$ 的表達式就為：

$f(x)=\begin{vmatrix} e_{0} \\ e_{1}\\ ... \\ ... \\ e_{m} \end{vmatrix}\tag{5}$

因為每個 $e$ 都是二維的，故 $f(x)$ 為2*m維的向量，請牢記 $f(x)$ 的大小，下文中我們還會用到。這樣，我們整體的目標函數可以寫成：

$min \frac{1}{2}\sum_{i=1}^{m}{\left| z-h(\xi,P_{i}) \right|_{2}^{2}} = min \frac{1}{2}{\left| f(x)\right|_{2}^{2}} \tag{6}$

對應到代碼中就是:

 1 /***
 2  * @param x 狀態量
 3  * @param v_Point2D 觀測到特征點的像素坐標
 4  * @return f(x)
 5  */
 6 Eigen::Matrix<double ,Eigen::Dynamic,1> 
 7 findCostFunction(Eigen::MatrixXd x, std::vector<cv::Point2d> v_Point2D)
 8 {
 9     //e=u-K*T*P; u為圖像上的觀測坐標,K為相機內參,T為相機外參，P為3D點坐標;
10     double fx=camMatrix(0,0);
11     double fy=camMatrix(1,1);
12     double cx=camMatrix(0,2);
13     double cy=camMatrix(1,2);
14     Eigen::Matrix<double ,Eigen::Dynamic,1> ans;
15  
16     int size_P=(int)(x.rows()-6)/3;
17 
18     if(size_P!=v_Point2D.size()){
19         std::cout<<"---ERROR---"<<endl;
20         return ans;
21     }
22     //把李代數轉化為矩陣 Pose為變換矩陣
23     Eigen::VectorXd v_temp(6);
24     v_temp=x.block(0,0,6,1);
25     Sophus::SE3 SE3_temp=Sophus::SE3::exp(v_temp);
26     Eigen::Matrix<double,4,4> Pose = SE3_temp.matrix();
27 
28     ans.resize(2*size_P,1);
29     ans.setZero();
30     for(int i=0;i<size_P;i++){
31         Eigen::Matrix<double ,4,1> Point;
32         Point(0,0)=x(6+i*3  ,0);
33         Point(1,0)=x(1+6+i*3,0);
34         Point(2,0)=x(2+6+i*3,0);
35         Point(3,0)=1.0;
36         //計算3D點在相機坐標系下的坐標
37         Eigen::Matrix<double ,4,1>  cam_Point=Pose*Point; 
38         double cam_x=cam_Point(0,0); //相機坐標喜下3D點的坐標
39         double cam_y=cam_Point(1,0);
40         double cam_z=cam_Point(2,0);
41         //計算e
42         ans(2*i,  0) =v_Point2D[i].x-((fx*cam_x)/cam_z)-cx;
43         ans(2*i+1,0) =v_Point2D[i].y-((fy*cam_y)/cam_z)-cy;
44     }
45     return ans;
46 }

1.2 G-N法

本文不再對G-N法做詳細的闡釋，請仔細閱讀 <十四講>第六章。也可參考<狀態估計>4.3.1，同時<狀態估計>p217-p219也闡述了G-N法的另外一種解釋。

無論使用什么方式求解最小二乘問題,歸根結底都要面對增量方程：

$H\Delta x = g\tag{7}$

在G-N法中，增量方程具體表示為：

$J(x)^{T}*J(x)*\Delta x = -J(x)^{T}*f(x) \tag{8}$

其中 $H=J(x)^T*J(x) , g =-J(x)^{T}*f(x)$

要想迭代，首先需要求解出 $\Delta x$ 的值，即每個優化變量的增量。可以看出來，無論 $H$ 和 $g$ 如何取值，增量方程的求解都是一個線性方程組問題。大家都知道，在SLAM系統里，增量方程中的 $H$ 矩陣是一個稀疏矩陣，對其求解有着多種特殊方式，可以加速求解。如果我們想要編寫一個可以實時運行的BA系統，那就不可不考慮H矩陣的稀疏性。但是本文暫不考慮這么多，直接使用Eigen自帶的方程組求解算法求解。

1.3 雅克比矩陣J(x)的獲取

上文提到我們定義優化變量：

$x=\begin{vmatrix} \xi \\ P_{0}\\ P_{1}\\ ... \\ ... \\ P_{m} \end{vmatrix}\\$

那么對應其中一個特征點 $i$ 求解雅克比矩陣(詳見<十四講 p250頁>):

$J_{i}(x)=\left[ F_{i} \ 0 \ 0 \ ... \ E_{i}\ ... \ 0 \ 0 \right] \tag{9}$

其中，我們記 $\frac{\partial e_{i}}{\partial\delta\xi}$ 為 $F_{i}$ ，記 $\frac{\partial e_{i}}{\partial P_{i}}$ 為 $E_{i}$ (與《14講》p247頁記號相同)。其中 $e_{i}$ 為該點的重投影誤差， $E_{i}$ 為某個誤差項關於空間點位置 $P_{i}$ 的導數， $F_{i}$ 為某個誤差項關於姿態擾動 $\delta\xi$ 的導數，至於為什么是關於姿態擾動的導數而不是關於姿態的導數，主要原因是求解 $\frac{\partial e}{\partial \xi}$ 需要計算一個比較復雜的雅克比矩陣 $J_{l}$ (詳見<14講>p75、<狀態估計>p216)。為了避免計算這個矩陣 $J_{l}$ (其實也沒必要計算)，我們使用擾動模型。如果對此還有疑惑，可以參考這篇文章的第二部分：

劉知：SLAM學習過程中的疑惑及其思考(1)zhuanlan.zhihu.com

我們再來看 $J_{i}(x)$ 的形式,很容易看出來它是一個 $\left[ 2,6+3×m\right]$ 維的矩陣。但是可要注意了，這只是一個誤差項 $e$ 的雅克比，我們一共擁有 $m$ 個 $e$ (因為在兩幀圖像上，我們擁有 $m$ 個特征點匹配)，故整體的 $J(x)$ 的維度為 $\left[ 2×m,6+3×m\right]$ 維。

那么整體的雅克比矩陣 $J(x)$ 就為(其實就是把 $J_{i}(x)$ 按行排列，可以參考<14講>p251)：

$J(x)= \left[ \begin{matrix} F_{0} & E_{0} & 0 & ...& ... & 0 \\ F_{1} & 0 & E_{1} & ...& ... & 0 \\ ... & ... & .. & ...& ... & ... \\ F_{i} & 0 & 0 & E_{i}& ... & 0 \\ ... & ... & .. & ...& ... & ... \\ F_{m} & 0 & 0 & ...& ... & E_{m} \\ \end{matrix} \right] \tag{10}$

《14講》中P164頁推導了 $\frac{\partial e}{\partial\delta\xi}$ 和 $\frac{\partial e}{\partial P}$ 的形式，我把 $E$ 和 $F$ 的具體表達形式寫在下面：

$F= \left[ \begin{matrix} -\frac{f_{x}}{Z^`}& 0 & \frac{f_{x}X^`}{Z^{`2}} & | & \frac{f_{x}X^`Y^`}{Z^{`2}} & -f_{x}-\frac{f_{x}X^{`2}}{Z^{`2}}&\frac{f_{x}Y^{`}}{Z^`} \\ 0& -\frac{f_{y}}{Z^`} & \frac{f_{y}Y^`}{Z^{`2}}&|& f_{y}+\frac{f_{y}Y^{`2}}{Z^{`2}} & -\frac{f_{y}X^`Y^`}{Z^{`2}} & -\frac{f_{y}X^`}{Z^{`}} \\ \end{matrix} \right] \tag{11}$

$E= \left[ \begin{matrix} -\frac{f_{x}}{Z^`}& 0 & \frac{f_{x}X^`}{Z^{`2}} \\ 0& -\frac{f_{y}}{Z^`} & \frac{f_{y}Y^`}{Z^{`2}}\\ \end{matrix} \right] \times R\tag{12}$

我把 $F$ 分為了兩部分(注意第三列后的分割線)，左邊是關於平移t的雅克比，右邊是關於旋轉矩陣R的雅克比，另外注意E表達式中的R編程的時候不要漏掉！

2.具體實施過程

好吧，既然我們取得了 $J(x)$ ,那么我們現在只需要求解增量方程:

$J(x)^{T}*J(x)*\Delta x = -J(x)^{T}*f(x) \tag{13}$

就可以求解出 $\Delta x$ 了。按理說應該利用 $H$ 矩陣的稀疏性來求解的,不過本次我們直接使用Eigen求解。

求解出 $\Delta x$ 后，更新 $x$ 之后再次進行迭代，直至收斂。我直接設置為迭代5次。：

 
           1  for(int i=1;i<=MAX_LOOP;i++){ //循環求解BA
       std::cout<<"\033[32m"<<"Doing BA Please wait......"<<std::endl;
       double t = (double)cv::getTickCount(); //計時開始
       Eigen::MatrixXd Jacobian=findWholeJacobian(x);       //求解狀態x的Jacobian
       Eigen::MatrixXd JacobianT=Jacobian.transpose();      //求解Jacobian 的轉置
       Eigen::MatrixXd H=JacobianT*Jacobian;                //求解H矩陣
       //std::cout<<"H = "<<endl<<H<<endl;
       Eigen::MatrixXd fx=findCostFunction(x,v_P2d);        //求解f(x)在狀態x下的值
       Eigen::VectorXd g=-1*JacobianT*fx;                   //求解g,相見<十四講>p247
       //求解delt_x
       //1.Using the SVD decomposition
       //Eigen::MatrixXd delt_x=H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(g);
       //2.Using the QR decomposition
       std::cout<<"Solving ......"<<"\033[37m"<<std::endl;
       Eigen::MatrixXd delt_x=H.colPivHouseholderQr().solve(g);
       ///李代數相加需要添加一些余項，轉化為R再相乘，代替李代數加法,詳見14講 72頁；
       ///把SE3上的李代數轉化為4x4矩陣
       Eigen::Matrix4d Pos_Matrix = Sophus::SE3::exp(x.block(0,0,6,1)).matrix();
       Eigen::Matrix4d Pos_update_Matrix = Sophus::SE3::exp(delt_x.block(0,0,6,1)).matrix();
       ///矩陣更新
       Pos_Matrix = Pos_Matrix * Pos_update_Matrix;
       ///轉化為李代數
       Sophus::SE3 new_Pos_se = 
       Sophus::SE3(Pos_Matrix.block<3,3>(0,0),Pos_Matrix.block<3,1>(0,3));
       ///更新姿態
       x = x + delt_x;
       x.block(0,0,6,1)=new_Pos_se.log();
       printf("BA cost %f ms \n", (1000*(cv::getTickCount() - t) / cv::getTickFrequency()));
       //--------------------在原圖相上畫出觀測和預測的坐標-------------------------------------
       Eigen::VectorXd v_temp(6);
       v_temp=x.block(0,0,6,1);
       Sophus::SE3 SE3_temp=Sophus::SE3::exp(v_temp);
       Eigen::Matrix<double,4,4> Pose = SE3_temp.matrix();
       cout<<"POSE:"<<endl<<Pose<<endl;
       cv::Mat temp_Mat=img.clone();
       /// 投影到圖像上，展現優化效果
       for(int j=0;j<v_P3d.size();j++) {
           double fx=camMatrix(0,0);
           double fy=camMatrix(1,1);
           double cx=camMatrix(0,2);
           double cy=camMatrix(1,2);
           Eigen::Matrix<double, 4, 1> Point;
           Point(0,0)=x(6+3*j,  0);
           Point(1,0)=x(1+6+3*j,  0);
           Point(2,0)=x(2+6+3*j,  0);
           Point(3,0)=1.0;
           Eigen::Matrix<double, 4, 1> cam_Point = Pose * Point; //計算3D點在相機坐標系下的坐標
           cv::Point2d temp_Point2d;
           temp_Point2d.x=(fx*cam_Point(0,0)/cam_Point(2,0))+cx;
           temp_Point2d.y=(fy*cam_Point(1,0)/cam_Point(2,0))+cy;
           cv::circle(temp_Mat,temp_Point2d,3,cv::Scalar(0,0,255),2);
           cv::circle(temp_Mat,v_P2d[j],    2,cv::Scalar(255,0,0),2);
       }
       imshow("REPROJECTION ERROR DISPLAY",temp_Mat);
       cout<<"\033[32m"<<"Iteration： "<<i<<" Finish......"<<"\033[37m"<<endl;
       cout<<"\033[32m"<<"Blue is observation......"<<"\033[37m"<<endl;
       cout<<"\033[32m"<<"Red is reprojection......"<<"\033[37m"<<endl;
       cout<<"\033[32m"<<"Press Any Key to continue......"<<"\033[37m"<<endl;
       cv::waitKey(0);
   } 
         