1. 前言本文将讨论g2o的代码结构并带着大家一起写一个简单的双视图bundle adjustment从两张图像中估计相机运动和特征点位置。你可以把它看成一个基于稀疏特征点的单目VO。2. g2o的结构所谓的通用图优化为何叫通用呢g2o的核里带有各种各样的求解器而它的顶点、边的类型则多种多样。通过自定义顶点和边事实上只要一个优化问题能够表达成图那么就可以用g2o去求解它。常见的比如bundle adjustmentICP数据拟合都可以用g2o来做。甚至我还在想神经网络能不能写成图优化的形式呢……从代码层面来说g2o是一个C编写的项目用cmake构建。它的github地址在https://github.com/RainerKuemmerle/g2o它是一个重度模板类的C项目其中矩阵数据结构多来自Eigen。首先我们来扫一眼它的目录下面都有什么吧如你所见g2o项目中含有若干文件夹。刨开那些gitignore之类的零碎文件主要有以下几个EXTERNAL三方库有ceres, csparse, freeglut可以选择性地编译cmake_modules给cmake用来寻找库的文件。我们用g2o时也会用它里头的东西例如FindG2O.cmakedoc文档。包括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, Powells dogleg 三者之一我们常用的是GN或LM。同时这个 Optimization Algorithm 拥有一个Solver它含有两个部分。一个是 SparseBlockMatrix用于计算稀疏的雅可比和海塞一个是用于计算迭代过程中最关键的一步这就需要一个线性方程的求解器。而这个求解器可以从 PCG, CSparse, Choldmod 三者选一。综上所述在g2o中选择优化方法一共需要三个步骤选择一个线性方程求解器从 PCG, CSparse, Choldmod 中选实际则来自 g2o/solvers 文件夹中定义的东东。选择一个 BlockSolver。选择一个迭代策略从GN, LM, Doglog中选。3. 双视图bundle adjustment既然小萝卜同学已经晕了想必我们也成功地把读者朋友都绕进去了。既绕之则绕之下面我们来通过一个实例更深入地理解 g2o 的用法。这个实例是什么呢我们来写一个双视图的bundle adjustment吧代码的git地址https://github.com/gaoxiang12/g2o_ba_example首先师兄还是拿出那两张万年不变的老图我们的目标是估计这两个图之间的运动。虽然我们在《一起做》里讲过这件事怎么做了但那是在RGBD的条件下。现在我们没有深度图只有这两张图像和相机内参请问如何估计相机的运动呃这个问题好像还挺复杂的。我们需要用一点数学来描述它。所以请大家耐心看我推一会儿公式。求解这个问题当下有两种思路。其一是通过特征点来求其二是直接通过像素来求。第一种也叫做 sparse 方式第二种叫做相对的 dense 方式。由于主流仍在用特征点所以我们例程也用特征点。特征点方法的观点是一个图像可以用几百个具有代表性的比较稳定的点来表示。一旦我们有了这些点就可以忽略图中的其余部分而只关注这些点。dense 思路则反对这一观点认为它丢弃了图像大部分信息毕竟一个640x480的图有30万个点而特征点只有几百个。采用特征点的思路那么问题变为给定N个两张图中一一对应的点记作以及相机内参矩阵 C求解两个图中的相机运动R, t。注字符z的上标不是几次方的意思而是第几个点。采用上标的原因是为了避免双下标带来的麻烦。同时每个点的具体值z是指该点对应的像素坐标它们是二维的。不管它总之假设相机1的位姿为单位矩阵对于任意一个特征点它在三维空间的真实坐标位于而在两个相机坐标系上看来是。根据投影关系我们有这里的表示两个像素的深度值说白了也就是相机1坐标下的z坐标。虽然我们不知道这个实际的是什么但它和z之间的关系是可以列写出来的。这个问题的传统求解方式是把两个方程中的消去得到只关于z, R, t的关系式然后进行优化。这条道路通向对极几何和基础矩阵 (Essential Matrix)理论上我们需要大于八个的匹配点就能计算出R, t。但这里我们并不这样做因为我们是在介绍图优化嘛。在图优化中我们构建一个优化问题并表示成图去求解。这里的优化问题是什么呢这可以这样写由于各种噪声的存在投影关系不能完美满足所以我们转而优化它们误差的二范数。那么对每一个特征点我们都能写出这样一个二范数的误差项。对它们进行求和就得到了整个优化问题它叫做最小化重投影误差问题 (Minimization of Reprojection error)。当然它很遗憾地是个非线性非凸的优化问题这意味着我们不一定能求解它也不一定能找到全局最优的解。在实际操作中我们实际上是在调整每个使得它们更符合每一次观测也就是使每个误差项都尽量的小。由于这个原因它也叫做捆集调整 (Bundle Adjustment)。BA很容易表述成图优化的形式。在这个双视图BA中一种有两种结点相机位姿结点表达两个相机所在的位置是一个SE(3)里的元素。特征点的空间位置结点是一个XYZ坐标。相应的边主要表示空间点到像素坐标的投影关系。也就是这件事情喽4. g2o使用说明4.1. 节点定义节点时需要继承于基类g2o::BaseVertex需要两个模板参数class VertexGyroBias: public g2o::BaseVertex3, Eigen::Vector3d第一个参数是变量维数第二个参数是变量的类型注意变量的类型并不能决定变量的维度例如四元数在这里的类型应该是Eigen::Vector4d但其维数却是34.2. 一元边定义一元边时需要继承于基类g2o::BaseUnaryEdge需要三个模板参数class EdgePoseLineOnlyLine: public g2o::BaseUnaryEdge2, Eigen::Vector4d, VertexLMLineOrth第一个参数代表残差的维数第二个参数代表测量值的类型第三个参数代表与该边相连的节点的类型4.3. 二元边定义二元边时需要继承于基类g2o::BaseBinaryEdge需要四个模板参数class EdgePoseLine: public g2o::BaseBinaryEdge2, Eigen::Vector4d, VertexLMLineOrth, RigVertexPose第一个参数代表残差的维数第二个参数代表测量值的类型第三、四个参数代表与该边相连的两个节点的类型4.4. 多元边定义多元边时需要继承于基类g2o::MyMultiEdge需要两个模板参数class MyMultiEdge: public g2o::BaseMultiEdge6, Eigen::Isometry3d5. 实现下面我们来用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它连接的两个顶点必须是 VertexSBAPointXYZVertexSE3Expmap。我们还能看到它的 computeError 定义和前面给出的公式是一致的。注意到计算Error时它调用了 g2o::CameraParameters 作为参数所以我们在设置这条边时也需要给定一个相机参数。铺垫了那么多之后给出我们的源码/** * BA Example * Author: Xiang Gao * Date: 2016.3 * Email: gaoxiang12mails.tsinghua.edu.cn * * 在这个程序中我们读取两张图像进行特征匹配。然后根据匹配得到的特征计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment我们用g2o进行优化。 */ // for std #include iostream // for opencv #include opencv2/core/core.hpp #include opencv2/highgui/highgui.hpp #include opencv2/features2d/features2d.hpp #include boost/concept_check.hpp // for g2o #include g2o/core/sparse_optimizer.h #include g2o/core/block_solver.h #include g2o/core/robust_kernel.h #include g2o/core/robust_kernel_impl.h #include g2o/core/optimization_algorithm_levenberg.h #include g2o/solvers/cholmod/linear_solver_cholmod.h #include g2o/types/slam3d/se3quat.h #include g2o/types/sba/types_six_dof_expmap.h using namespace std; // 寻找两个图像中的对应点像素坐标系 // 输入img1, img2 两张图像 // 输出points1, points2, 两组对应的2D点 int findCorrespondingPoints(const cv::Mat img1, const cv::Mat img2, vectorcv::Point2f points1, vectorcv::Point2f points2); // 相机内参 double cx 325.5; double cy 253.5; double fx 518.0; double fy 519.0; int main(int argc, char** argv) { // 调用格式命令 [第一个图] [第二个图] if (argc ! 3) { cout Usage: ba_example img1, img2 endl; exit(1); } // 读取图像 cv::Mat img1 cv::imread(argv[1]); cv::Mat img2 cv::imread(argv[2]); // 找到对应点 vectorcv::Point2f pts1, pts2; if (findCorrespondingPoints(img1, img2, pts1, pts2) false) { cout 匹配点不够 endl; return 0; } cout 找到了 pts1.size() 组对应特征点。 endl; // 构造g2o中的图 // 先构造求解器 g2o::SparseOptimizer optimizer; // 使用Cholmod中的线性方程求解器 g2o::BlockSolver_6_3::LinearSolverType* linearSolver new g2o::LinearSolverCholmodg2o::BlockSolver_6_3::PoseMatrixType (); // 6*3 的参数 g2o::BlockSolver_6_3* block_solver new g2o::BlockSolver_6_3(linearSolver); // L-M 下降 g2o::OptimizationAlgorithmLevenberg* algorithm new g2o::OptimizationAlgorithmLevenberg(block_solver); optimizer.setAlgorithm(algorithm); optimizer.setVerbose(false); // 添加节点 // 两个位姿节点 for (int i0; i2; i) { g2o::VertexSE3Expmap* v new g2o::VertexSE3Expmap(); v-setId(i); if (i 0) v-setFixed(true); // 第一个点固定为零 // 预设值为单位Pose因为我们不知道任何信息 v-setEstimate(g2o::SE3Quat()); optimizer.addVertex(v); } // 很多个特征点的节点 // 以第一帧为准 for (size_t i0; ipts1.size(); i) { g2o::VertexSBAPointXYZ* v new g2o::VertexSBAPointXYZ(); v-setId(2 i); // 由于深度不知道只能把深度设置为1了 double z 1; double x (pts1[i].x - cx) * z / fx; double y (pts1[i].y - cy) * z / fy; v-setMarginalized(true); v-setEstimate(Eigen::Vector3d(x,y,z)); optimizer.addVertex(v); } // 准备相机参数 g2o::CameraParameters* camera new g2o::CameraParameters(fx, Eigen::Vector2d(cx, cy), 0); camera-setId(0); optimizer.addParameter(camera); // 准备边 // 第一帧 vectorg2o::EdgeProjectXYZ2UV* edges; for (size_t i0; ipts1.size(); i) { g2o::EdgeProjectXYZ2UV* edge new g2o::EdgeProjectXYZ2UV(); edge-setVertex(0, dynamic_castg2o::VertexSBAPointXYZ* (optimizer.vertex(i2))); edge-setVertex(1, dynamic_castg2o::VertexSE3Expmap* (optimizer.vertex(0))); edge-setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y)); edge-setInformation(Eigen::Matrix2d::Identity()); edge-setParameterId(0, 0); // 核函数 edge-setRobustKernel(new g2o::RobustKernelHuber()); optimizer.addEdge(edge); edges.push_back(edge); } // 第二帧 for (size_t i0; ipts2.size(); i) { g2o::EdgeProjectXYZ2UV* edge new g2o::EdgeProjectXYZ2UV(); edge-setVertex(0, dynamic_castg2o::VertexSBAPointXYZ* (optimizer.vertex(i2))); edge-setVertex(1, dynamic_castg2o::VertexSE3Expmap* (optimizer.vertex(1))); edge-setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y)); edge-setInformation(Eigen::Matrix2d::Identity()); edge-setParameterId(0,0); // 核函数 edge-setRobustKernel(new g2o::RobustKernelHuber()); optimizer.addEdge(edge); edges.push_back(edge); } cout 开始优化 endl; optimizer.setVerbose(true); optimizer.initializeOptimization(); optimizer.optimize(10); cout 优化完毕 endl; //我们比较关心两帧之间的变换矩阵 g2o::VertexSE3Expmap* v dynamic_castg2o::VertexSE3Expmap*(optimizer.vertex(1)); Eigen::Isometry3d pose v-estimate(); cout Pose endl pose.matrix() endl; // 以及所有特征点的位置 for (size_t i0; ipts1.size(); i) { g2o::VertexSBAPointXYZ* v dynamic_castg2o::VertexSBAPointXYZ* (optimizer.vertex(i2)); cout vertex id i2 , pos ; Eigen::Vector3d pos v-estimate(); cout pos(0) , pos(1) , pos(2) endl; } // 估计inlier的个数 int inliers 0; for (auto e:edges) { e-computeError(); // chi2 就是 error*\Omega*error, 如果这个数很大说明此边的值与其他边很不相符 if (e-chi2() 1) { cout error e-chi2() endl; } else { inliers; } } cout inliers in total points: inliers / pts1.size()pts2.size() endl; optimizer.save(ba.g2o); return 0; } int findCorrespondingPoints(const cv::Mat img1, const cv::Mat img2, vectorcv::Point2f points1, vectorcv::Point2f points2) { cv::ORB orb; vectorcv::KeyPoint kp1, kp2; cv::Mat desp1, desp2; orb(img1, cv::Mat(), kp1, desp1); orb(img2, cv::Mat(), kp2, desp2); cout 分别找到了 kp1.size() 和 kp2.size() 个特征点 endl; cv::Ptrcv::DescriptorMatcher matcher cv::DescriptorMatcher::create(BruteForce-Hamming); double knn_match_ratio0.8; vector vectorcv::DMatch matches_knn; matcher-knnMatch(desp1, desp2, matches_knn, 2); vector cv::DMatch matches; for (size_t i0; imatches_knn.size(); i) { if (matches_knn[i][0].distance knn_match_ratio * matches_knn[i][1].distance) matches.push_back(matches_knn[i][0]); } if (matches.size() 20) //匹配点太少 return false; for (auto m:matches) { points1.push_back(kp1[m.queryIdx].pt); points2.push_back(kp2[m.trainIdx].pt); } return true; }在这个程序中我们从命令行参数读取两个图像所在的位置然后构建一个图估计图像间运动和特征点的空间位置。整个工程的编译方式使用cmake请参考 github 工程进行编译这里就不详细说明了。因为肯定又要提一堆Cmake方面的事情。编译完成后可以运行此程序结果如下我们显示了特征点的数量估计的位姿变换以及各特征点的空间位置。最后还显示了inliers的数量我们把误差太大的边认为是outlier在652条边中有614条边是inlier说明匹配还是挺正确的。6. 尺度不确定性关于单目BA还有一点要说就是 scale 不确定性。由于投影公式中的λ存在我们只能推得一个相对的深度而无法确切的知道特征点离我们有多少距离。如果我们把所有特征点的坐标放大一倍把平移量t也乘以2得到的结果是完全一样的。比方说看奥特曼时我们并不知道这其实是人类演员在模型里打架。这就是单目带来的尺度不确定性。7. 使用问题节点和边用动态指针是否需要手动释放节点和边可以放心地使用动态指针而无需手动释放内存因为优化器在析构的时候会自动释放。下面是基类优化器的析构函数void HyperGraph::clear() { for (VertexIDMap::iterator it_vertices.begin(); it!_vertices.end(); it) delete (it-second); for (EdgeSet::iterator it_edges.begin(); it!_edges.end(); it) delete (*it); _vertices.clear(); _edges.clear(); } HyperGraph::~HyperGraph() { clear(); }参考文献深入理解图优化与g2og2o篇 - 半闲居士 - 博客园