浅谈PNP的BA求解

注：这是我在知乎写的文章，现搬运至此。原文链接：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次。：

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);
   }