深⼊理解图优化与g2o：g2o篇
内容提要
讲完了优化的基本知识，我们来看⼀下g2o的结构。本篇将讨论g2o的代码结构，并带着⼤家⼀起写⼀个简单的双视图bundle adjustment：从两张图像中估计相机运动和特征点位置。你可以把它看成⼀个基于稀疏特征点的单⽬VO。
g2o的结构
g2o全称是什么？来跟我⼤声说⼀遍：General Graph Optimization！你可以叫它g⼟o，g⼆o，g⽅o，总之我也不知道该怎么叫它……
所谓的通⽤图优化。
为何叫通⽤呢？g2o的核⾥带有各种各样的求解器，⽽它的顶点、边的类型则多种多样。通过⾃定义顶点和边，事实上，只要⼀个优化问题能够表达成图，那么就可以⽤g2o去求解它。常见的，⽐如bundle adjustment，ICP，数据拟合，都可以⽤g2o来做。甚⾄我还在想神经⽹络能不能写成图优化的形式呢……
它是⼀个重度模板类的c++项⽬，其中矩阵数据结构多来⾃Eigen。⾸先我们来扫⼀眼它的⽬录下⾯都有什么吧：
如你所见，g2o项⽬中含有若⼲⽂件夹。刨开那些gitignore之类的零碎⽂件，主要有以下⼏个：
EXTERNAL 三⽅库，有ceres, csparse, freeglut，可以选择性地编译；
cmake_modules 给cmake⽤来寻库的⽂件。我们⽤g2o时也会⽤它⾥头的东西，例如ake
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吧！
⾸先，师兄还是拿出那两张万年不变的⽼图：
我们的⽬标是估计这两个图之间的运动。虽然我们在《⼀起做》⾥讲过这件事怎么做了，但那是在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: gaoxiang12@mails.tsinghua.edu
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
28using namespace std;
29
30// 寻两个图像中的对应点，像素坐标系
31// 输⼊：img1, img2 两张图像
32// 输出：points1, points2, 两组对应的2D点
33int findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );
34
35// 相机内参
36double cx = 325.5;
37double cy = 253.5;
38double fx = 518.0;
39double fy = 519.0;
40
41int main( int argc, char** argv )
42 {
43// 调⽤格式：命令 [第⼀个图] [第⼆个图]
44if (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;
56if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
57 {
58 cout<<"匹配点不够！"<<endl;
59return0;
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// 两个位姿节点
77for ( int i=0; i<2; i++ )
78 {
79 g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
80 v->setId(i);
81if ( i == 0)
82 v->setFixed( true ); // 第⼀个点固定为零
83// 预设值为单位Pose，因为我们不知道任何信息
84 v->setEstimate( g2o::SE3Quat() );
85 optimizer.addVertex( v );
86 }
87// 很多个特征点的节点
88// 以第⼀帧为准
89for ( size_t i=0; i<pts1.size(); i++ )
90 {
91 g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
92 v->setId( 2 + i );
93// 由于深度不知道，只能把深度设置为1了
94double z = 1;
95double x = ( pts1[i].x - cx ) * z / fx;
96double 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;
110for ( 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// 第⼆帧
124for ( 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// 以及所有特征点的位置
150for ( 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的个数
159int inliers = 0;
160for ( auto e:edges )
161 {
162 e->computeError();
163// chi2 就是 error*\Omega*error, 如果这个数很⼤，说明此边的值与其他边很不相符
164if ( e->chi2() > 1 )
165 {
166 cout<<"error = "<<e->chi2()<<endl;
167 }
168else
169 {
170 inliers++;
171 }
172 }
173
174 cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
175 optimizer.save("ba.g2o");
176return0;
177 }
178
为什么现在都用cmake179
180int 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
191double 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;
195for ( size_t i=0; i<matches_knn.size(); i++ )
196 {
197if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
198 matches.push_back( matches_knn[i][0] );
199 }
200
201if (matches.size() <= 20) //匹配点太少
202return false;
203
204for ( auto m:matches )
205 {
206 points1.push_back( kp1[m.queryIdx].pt );
207 points2.push_back( ainIdx].pt );
208 }
209
210return true;
211 }
在这个程序中，我们从命令⾏参数读取两个图像所在的位置，然后构建⼀个图估计图像间运动和特征点的空间位置。
整个⼯程的编译⽅式使⽤cmake，请参考 github ⼯程进⾏编译，这⾥就不详细说明了。（因为肯定⼜要提⼀堆Cmake⽅⾯的事情。） 编译完成后，可以运⾏此程序，结果如下：
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论