深入理解图优化与g2o：g2o篇

内容提要

　　讲完了优化的基本知识，我们来看一下g2o的结构。本篇将讨论g2o的代码结构，并带着大家一起写一个简单的双视图bundle adjustment：从两张图像中估计相机运动和特征点位置。你可以把它看成一个基于稀疏特征点的单目VO。

g2o的结构

　　g2o全称是什么？来跟我大声说一遍：General Graph Optimization！你可以叫它g土o，g二o，g方o，总之我也不知道该怎么叫它……

　　所谓的通用图优化。

　　为何叫通用呢？g2o的核里带有各种各样的求解器，而它的顶点、边的类型则多种多样。通过自定义顶点和边，事实上，只要一个优化问题能够表达成图，那么就可以用g2o去求解它。常见的，比如bundle adjustment，ICP，数据拟合，都可以用g2o来做。甚至我还在想神经网络能不能写成图优化的形式呢……

　　从代码层面来说，g2o是一个c++编写的项目，用cmake构建。它的github地址在：https://github.com/RainerKuemmerle/g2o 

　　它是一个重度模板类的c++项目，其中矩阵数据结构多来自Eigen。首先我们来扫一眼它的目录下面都有什么吧：

　　如你所见，g2o项目中含有若干文件夹。刨开那些gitignore之类的零碎文件，主要有以下几个：

• EXTERNAL　　三方库，有ceres, csparse, freeglut，可以选择性地编译；
• cmake_modules　　给cmake用来寻找库的文件。我们用g2o时也会用它里头的东西，例如FindG2O.cmake
• doc　　　　　文档。包括g2o自带的说明书（难度挺大的一个说明文档）。
• g2o　　　　　　最重要的源代码都在这里！
• script　　　　在android等其他系统编译用的脚本，由于我们在ubuntu下就没必要多讲了。

 　　综上所述，最重要的就是g2o的源代码文件啦！所以我们要进一步展开看一看！

　　 我们同样地介绍一下各文件夹的内容：

• apps　　　　一些应用程序。好用的g2o_viewer就在这里。其他还有一些不常用的命令行工具等。
• core　　　　核心组件，很重要！基本的顶点、边、图结构的定义，算法的定义，求解器接口的定义在这里。
• examples　　一些例程，可以参照着这里的东西来写。不过注释不太多。
• solvers　　　　求解器的实现。主要来自choldmod, csparse。在使用g2o时要先选择其中一种。
• stuff　　　　对用户来讲可有可无的一些工具函数。
• types　　　　各种顶点和边，很重要！我们用户在构建图优化问题时，先要想好自己的顶点和边是否已经提供了定义。如果没有，要自己实现。如果有，就用g2o提供的即可。

　　就经验而言，solvers给人的感觉是大同小异，而 types 的选取，则是 g2o 用户主要关心的内容。然后 core 下面的内容，我们要争取弄的比较熟悉，才能确保使用中出现错误可以正确地应对。

　　那么，g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：

　　这个图第一次看，可能觉得有些混乱。但是随着g2o越用越多，你会发现越来越喜欢这个图……现在请读者跟着我的顺序来看这个图。

　　先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

　　在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步 $$H \Delta x = -b $$ 这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

　　综上所述，在g2o中选择优化方法一共需要三个步骤：

1. 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
2. 选择一个 BlockSolver 。
3. 选择一个迭代策略，从GN, LM, Doglog中选。

　　这样一来，读者是否对g2o就更清楚的认识了呢？

　　小萝卜：师兄你慢点，我已经晕了……

双视图bundle adjustment：

　　既然小萝卜同学已经晕了，想必我们也成功地把读者朋友都绕进去了。既绕之则绕之，下面我们来通过一个实例，更深入地理解 g2o 的用法。这个实例是什么呢？我们来写一个双视图的bundle adjustment吧！

　　代码的git地址：https://github.com/gaoxiang12/g2o_ba_example

　　首先，师兄还是拿出那两张万年不变的老图：

　　我们的目标是估计这两个图之间的运动。虽然我们在《一起做》里讲过这件事怎么做了，但那是在RGBD的条件下。现在，我们没有深度图，只有这两张图像和相机内参，请问如何估计相机的运动？

　　呃，这个问题好像还挺复杂的。我们需要用一点数学来描述它。所以请大家耐心看我推一会儿公式。

　　求解这个问题，当下有两种思路。其一是通过特征点来求，其二是直接通过像素来求。第一种也叫做 sparse 方式，第二种叫做相对的 dense 方式。由于主流仍在用特征点，所以我们例程也用特征点。

　　特征点方法的观点是：一个图像可以用几百个具有代表性的，比较稳定的点来表示。一旦我们有了这些点，就可以忽略图中的其余部分，而只关注这些点。（dense 思路则反对这一观点，认为它丢弃了图像大部分信息，毕竟一个640x480的图有30万个点，而特征点只有几百个）。

　　采用特征点的思路，那么问题变为：给定$N$个两张图中一一对应的点，记作：$${z_1} = \left\{ {z_1^1,z_1^2, \ldots ,z_1^N} \right\},{z_2} = \left\{ {z_2^1,z_2^2, \ldots ,z_2^N} \right\} $$ 以及相机内参矩阵 $C$，求解两个图中的相机运动$R,t$。

　　注：字符$z$的上标不是几次方的意思，而是第几个点。采用上标的原因是为了避免双下标带来的麻烦。同时，每个点的具体值$z$，是指该点对应的像素坐标：$z_i^j = [u,v]_i^j$，它们是二维的。

　　

 　小萝卜：师兄啊，这图一股浓浓的山寨味啊。

　　不管它，总之，假设相机1的位姿为单位矩阵，对于任意一个特征点，它在三维空间的真实坐标位于 $X^j$，而在两个相机坐标系上看来是 $z_1^j, z_2^j$。根据投影关系，我们有：

\[ \begin{equation} {\lambda _1}\left[ \begin{array}{l}
z_1^j\\
1
\end{array} \right] = C{X^j},\quad {\lambda _2}\left[ \begin{array}{l}
z_2^j\\
1
\end{array} \right] = C\left( {R{X^j} + t} \right) \end{equation}\]

 　　这里的 $\lambda_1, \lambda_2$ 表示两个像素的深度值，说白了也就是相机1坐标下$X^j$的$z$坐标。虽然我们不知道这个实际的$X^j$是什么，但它和$z$之间的关系，是可以列写出来的。

 　　这个问题的传统求解方式，是把两个方程中的$X^j$消去，得到只关于$z, R, t$的关系式，然后进行优化。这条道路通向对极几何和基础矩阵（Essential Matrix），理论上，我们需要大于八个的匹配点就能计算出$R,t$。但这里我们并不这样做，因为我们是在介绍图优化嘛。

　　在图优化中，我们构建一个优化问题，并表示成图去求解。这里的优化问题是什么呢？这可以这样写：

\[ \begin{equation} \mathop {\min }\limits_{{X^j},R,t} {\left\| {\frac{1}{{{\lambda _1}}}C{X^j} - {{\left[ {z_1^j,1} \right]}^T}} \right\|^2} + {\left\| {\frac{1}{{{\lambda _2}}}C\left( {R{X^j} + t} \right) - {{\left[ {z_2^j,1} \right]}^T}} \right\|^2} \end{equation} \] 

　　由于各种噪声的存在，投影关系不能完美满足，所以我们转而优化它们误差的二范数。那么对每一个特征点，我们都能写出这样一个二范数的误差项。对它们进行求和，就得到了整个优化问题：

\[ \begin{equation} \mathop {\min }\limits_{X,R,t} \sum\limits_{j = 1}^N {{{\left\| {\frac{1}{{{\lambda _1}}}C{X^j} - {{\left[ {z_1^j,1} \right]}^T}} \right\|}^2} + {{\left\| {\frac{1}{{{\lambda _2}}}C\left( {R{X^j} + t} \right) - {{\left[ {z_2^j,1} \right]}^T}} \right\|}^2}} \end{equation} \]

　　它叫做最小化重投影误差问题（Minimization of Reprojection error）。当然，它很遗憾地，是个非线性，非凸的优化问题，这意味着我们不一定能求解它，也不一定能找到全局最优的解。在实际操作中，我们实际上是在调整每个$X^j$，使得它们更符合每一次观测$z^j$，也就是使每个误差项都尽量的小。由于这个原因，它也叫做捆集调整（Bundle Adjustment）。

　　BA很容易表述成图优化的形式。在这个双视图BA中，一种有两种结点：

• 相机位姿结点：表达两个相机所在的位置，是一个$SE(3)$里的元素。
• 特征点的空间位置结点：是一个XYZ坐标。

　　相应的，边主要表示空间点到像素坐标的投影关系。也就是

\[\lambda \left[ \begin{array}{l}
{z^j}\\
1
\end{array} \right] = C\left( {R{X^j} + t} \right)\] 这件事情喽。

 实现

　　下面我们来用g2o实现一下BA。选取的结点和边如下：

• 结点1：相机位姿结点：g2o::VertexSE3Expmap，来自<g2o/types/sba/types_six_dof_expmap.h>；
• 结点2：特征点空间坐标结点：g2o::VertexSBAPointXYZ，来自<g2o/types/sba/types_sba.h>；
• 边：重投影误差：g2o::EdgeProjectXYZ2UV，来自<g2o/types/sba/types_six_dof_expmap.h>；

　　为了给读者更深刻的印象，我们显示一下边的源码（也请读者最好亲自打开g2o下这几个文件看一下顶点和边的定义）：

　　这个是 EdgeProjectXYZ2UV 边的定义。它是一个Binary Edge，后面的模板参数表示，它的数据是2维的，来自Eigen::Vector2D，它连接的两个顶点必须是 VertexSBAPointXYZ， VertexSE3Expmap。 我们还能看到它的 computeError 定义，和前面给出的公式是一致的。注意到计算Error时，它调用了 g2o::CameraParameters 作为参数，所以我们在设置这条边时也需要给定一个相机参数。

　　铺垫了那么多之后，给出我们的源码：

  1 /**
  2  * BA Example 
  3  * Author: Xiang Gao
  4  * Date: 2016.3
  5  * Email: [email protected]
  6  * 
  7  * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。
  8  */
  9 
 10 // for std
 11 #include <iostream>
 12 // for opencv 
 13 #include <opencv2/core/core.hpp>
 14 #include <opencv2/highgui/highgui.hpp>
 15 #include <opencv2/features2d/features2d.hpp>
 16 #include <boost/concept_check.hpp>
 17 // for g2o
 18 #include <g2o/core/sparse_optimizer.h>
 19 #include <g2o/core/block_solver.h>
 20 #include <g2o/core/robust_kernel.h>
 21 #include <g2o/core/robust_kernel_impl.h>
 22 #include <g2o/core/optimization_algorithm_levenberg.h>
 23 #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
 24 #include <g2o/types/slam3d/se3quat.h>
 25 #include <g2o/types/sba/types_six_dof_expmap.h>
 26 
 27 
 28 using namespace std;
 29 
 30 // 寻找两个图像中的对应点，像素坐标系
 31 // 输入：img1, img2 两张图像
 32 // 输出：points1, points2, 两组对应的2D点
 33 int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );
 34 
 35 // 相机内参
 36 double cx = 325.5;
 37 double cy = 253.5;
 38 double fx = 518.0;
 39 double fy = 519.0;
 40 
 41 int main( int argc, char** argv )
 42 {
 43     // 调用格式：命令 [第一个图] [第二个图]
 44     if (argc != 3)
 45     {
 46         cout<<"Usage: ba_example img1, img2"<<endl;
 47         exit(1);
 48     }
 49     
 50     // 读取图像
 51     cv::Mat img1 = cv::imread( argv[1] ); 
 52     cv::Mat img2 = cv::imread( argv[2] ); 
 53     
 54     // 找到对应点
 55     vector<cv::Point2f> pts1, pts2;
 56     if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
 57     {
 58         cout<<"匹配点不够！"<<endl;
 59         return 0;
 60     }
 61     cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
 62     // 构造g2o中的图
 63     // 先构造求解器
 64     g2o::SparseOptimizer    optimizer;
 65     // 使用Cholmod中的线性方程求解器
 66     g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
 67     // 6*3 的参数
 68     g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
 69     // L-M 下降 
 70     g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
 71     
 72     optimizer.setAlgorithm( algorithm );
 73     optimizer.setVerbose( false );
 74     
 75     // 添加节点
 76     // 两个位姿节点
 77     for ( int i=0; i<2; i++ )
 78     {
 79         g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
 80         v->setId(i);
 81         if ( i == 0)
 82             v->setFixed( true ); // 第一个点固定为零
 83         // 预设值为单位Pose，因为我们不知道任何信息
 84         v->setEstimate( g2o::SE3Quat() );
 85         optimizer.addVertex( v );
 86     }
 87     // 很多个特征点的节点
 88     // 以第一帧为准
 89     for ( size_t i=0; i<pts1.size(); i++ )
 90     {
 91         g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
 92         v->setId( 2 + i );
 93         // 由于深度不知道，只能把深度设置为1了
 94         double z = 1;
 95         double x = ( pts1[i].x - cx ) * z / fx; 
 96         double y = ( pts1[i].y - cy ) * z / fy; 
 97         v->setMarginalized(true);
 98         v->setEstimate( Eigen::Vector3d(x,y,z) );
 99         optimizer.addVertex( v );
100     }
101     
102     // 准备相机参数
103     g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
104     camera->setId(0);
105     optimizer.addParameter( camera );
106     
107     // 准备边
108     // 第一帧
109     vector<g2o::EdgeProjectXYZ2UV*> edges;
110     for ( size_t i=0; i<pts1.size(); i++ )
111     {
112         g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
113         edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
114         edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
115         edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
116         edge->setInformation( Eigen::Matrix2d::Identity() );
117         edge->setParameterId(0, 0);
118         // 核函数
119         edge->setRobustKernel( new g2o::RobustKernelHuber() );
120         optimizer.addEdge( edge );
121         edges.push_back(edge);
122     }
123     // 第二帧
124     for ( size_t i=0; i<pts2.size(); i++ )
125     {
126         g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
127         edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
128         edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
129         edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
130         edge->setInformation( Eigen::Matrix2d::Identity() );
131         edge->setParameterId(0,0);
132         // 核函数
133         edge->setRobustKernel( new g2o::RobustKernelHuber() );
134         optimizer.addEdge( edge );
135         edges.push_back(edge);
136     }
137     
138     cout<<"开始优化"<<endl;
139     optimizer.setVerbose(true);
140     optimizer.initializeOptimization();
141     optimizer.optimize(10);
142     cout<<"优化完毕"<<endl;
143     
144     //我们比较关心两帧之间的变换矩阵
145     g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
146     Eigen::Isometry3d pose = v->estimate();
147     cout<<"Pose="<<endl<<pose.matrix()<<endl;
148     
149     // 以及所有特征点的位置
150     for ( size_t i=0; i<pts1.size(); i++ )
151     {
152         g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
153         cout<<"vertex id "<<i+2<<", pos = ";
154         Eigen::Vector3d pos = v->estimate();
155         cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
156     }
157     
158     // 估计inlier的个数
159     int inliers = 0;
160     for ( auto e:edges )
161     {
162         e->computeError();
163         // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
164         if ( e->chi2() > 1 )
165         {
166             cout<<"error = "<<e->chi2()<<endl;
167         }
168         else 
169         {
170             inliers++;
171         }
172     }
173     
174     cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
175     optimizer.save("ba.g2o");
176     return 0;
177 }
178 
179 
180 int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
181 {
182     cv::ORB orb;
183     vector<cv::KeyPoint> kp1, kp2;
184     cv::Mat desp1, desp2;
185     orb( img1, cv::Mat(), kp1, desp1 );
186     orb( img2, cv::Mat(), kp2, desp2 );
187     cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;
188     
189     cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");
190     
191     double knn_match_ratio=0.8;
192     vector< vector<cv::DMatch> > matches_knn;
193     matcher->knnMatch( desp1, desp2, matches_knn, 2 );
194     vector< cv::DMatch > matches;
195     for ( size_t i=0; i<matches_knn.size(); i++ )
196     {
197         if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
198             matches.push_back( matches_knn[i][0] );
199     }
200     
201     if (matches.size() <= 20) //匹配点太少
202         return false;
203     
204     for ( auto m:matches )
205     {
206         points1.push_back( kp1[m.queryIdx].pt );
207         points2.push_back( kp2[m.trainIdx].pt );
208     }
209     
210     return true;
211 }

 　　在这个程序中，我们从命令行参数读取两个图像所在的位置，然后构建一个图估计图像间运动和特征点的空间位置。

　　整个工程的编译方式使用cmake，请参考 github 工程进行编译，这里就不详细说明了。（因为肯定又要提一堆Cmake方面的事情。）

　　编译完成后，可以运行此程序，结果如下：

　　我们显示了特征点的数量，估计的位姿变换，以及各特征点的空间位置。最后，还显示了inliers的数量（我们把误差太大的边认为是outlier）：

　　在652条边中有614条边是inlier，说明匹配还是挺正确的。

讨论

　　关于单目BA还有一点要说，就是 scale 不确定性。由于投影公式中的$\lambda$存在，我们只能推得一个相对的深度，而无法确切的知道特征点离我们有多少距离。如果我们把所有特征点的坐标放大一倍，把平移量$t$也乘以二，得到的结果是完全一样的。

　　比方说：看奥特曼时，我们并不知道这其实是人类演员在模型里打架。这就是单目带来的尺度不确定性。

　　小萝卜：师兄你现在才知道吗？我小时候就知道了啊！

小结

　　本节介绍了g2o的大致框架，并提供了一个计算单目双视图Bundle Adjustment的例程供读者练习。

　　

　　如果你觉得我的博客有帮助，可以进行几块钱的小额赞助，帮助我把博客写得更好。