G2O学习使用
g2o全称是General Graph Optimization，也就是图优化，我们在做SLAM后端或者更加常见的任何优化问题（曲线拟合）都可以使用G2O进行处理。
就经验而言，solvers给人的感觉是大同小异，而 types 的选取，则是 g2o 用户主要关心的内容。然后 core 下面的内容，我们要争取弄的比较熟悉，才能确保使用中出现错误可以正确地应对。
那么，g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：
从代码层面来说，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下就没必要多讲了。
对应的处理流程如下所示：
图的核心
SparseOptimizer是整个图的核心， is-a 是实心箭头，表示这个SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）
定点和边
has-many 表示这个超图（HyperGraph）包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge
is-a:相当于一个等号
has-a:表示包含一个，这个包含的的属于必备的组件
has-many:和has-a差不多，至少要有一个
边的继承关系上图所示，对应的文件为：
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。
一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点
参数
主要就是 几个参数：D, E, VertexXi, VertexXj，他们的分别代表：
D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型
比如我们用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是空间点位置 VertexSBAPointXYZ，和李代数位姿VertexSE3Expmap
除了输入参数外，定义边我们通常需要复写一些重要的成员函数，顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl
成员函数：
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
- read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
- computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
- linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：
_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆
定义g2o的边
class myEdge: public g2o::BaseBinaryEdge
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){}
virtual bool read(istream& in) {}
virtual bool write(ostream& out) const {}
virtual void computeError() override
{
// ...
_error = _measurement - Something;
}
virtual void linearizeOplus() override
{
_jacobianOplusXi(pos, pos) = something;
// ...
/*
_jocobianOplusXj(pos, pos) = something;
...
*/
}
private:
// data
}
一元边，主要是定义误差函数
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
二元边：3D-2D点的PnP 问题，也就是最小化重投影误差问题，。地址：g2o/types/sba/types_six_dof_expmap.h
//继承了BaseBinaryEdge类，观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//1. 默认初始化
EdgeProjectXYZ2UV();
//2. 计算误差
void computeError() {
//李群相机位姿v1
const VertexSE3Expmap* v1 = static_cast(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);
//相机参数
const CameraParameters * cam
= static_cast(parameter(0));
//误差计算，测量值减去估计值，也就是重投影误差obs-cam
//估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。
Vector2D obs(_measurement);
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}
//3. 线性增量函数，也就是雅克比矩阵J的计算方法
virtual void linearizeOplus();
//4. 相机参数
CameraParameters * _cam;
bool read(std::istream& is);
bool write(std::ostream& os) const;
};
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace g2o;
class CurveFittingVertex : public g2o::BaseVertex<3,Eigen::vertor3d>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
virtual void setToOriginImpl() override{
_estimate << 0 , 0, 0;
}
virtual void oplusImpl(const double * update) override{
_estimate += Eigen::Vector3d(update);
}
// 存取或者读取数据
virtual bool read(istream & in){}
virtual bool write(ostream & out) const{}
};
// 误差 ,就是边；模板的参数：观测值维度，类型，连接定点的类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1,double ,CurveFittingVertex>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){}
// 计算对应的误差
virtual void computeError() override
{
const CurveFittingVertex* v = static_cast(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
virtual void linearizeOplus() override
{
const CurveFittingVertex* v = static_cast(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream& in){}
virtual bool write(ostream& out) const{}
public:
double _x;
};
int main(int argc ,char argv[][]){
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 0.0, be = 0.0, ce = 0.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng;
vector x_data, y_data; // 数据
for (int i = 0; i < N; i++)
{
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 构建图优化，先设定g2o
// 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver> BlockSolverType;
// 线性求解器类型
typedef g2o::LinearSolverDense LinearSolverType;
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique(g2o::make_unique()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);
// 往图中增加边
for (int i = 0; i < N; i++)
{
CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);
edge->setVertex(0, v); // 设置连接的顶点
edge->setMeasurement(y_data[i]); // 观测数值
// 信息矩阵：协方差矩阵之逆
edge->setInformation(
Eigen::Matrix::Identity() * 1 / (w_sigma * w_sigma));
optimizer.addEdge(edge);
}
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
} class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() override
{
_estimate = Sophus::SE3d();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override
{
Eigen::Matrix update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
};
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d& pos, const Eigen::Matrix3d& K) : _pos3d(pos), _K(K) {}
virtual void computeError() override
{
const VertexPose* v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
virtual void linearizeOplus() override
{
const VertexPose* v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi
<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
private:
Eigen::Vector3d _pos3d;
Eigen::Matrix3d _K;
};
void bundleAdjustmentG2O(const VecVector3d& points_3d, const VecVector2d& points_2d, const Mat& K, Sophus::SE3d& pose)
{
// 构建图优化，先设定g2o
typedef g2o::BlockSolver> BlockSolverType; // pose is 6, landmark is 2
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose* vertex_pose = new VertexPose(); // camera vertex_pose
vertex_pose->setId(0);
vertex_pose->setEstimate(Sophus::SE3d());
optimizer.addVertex(vertex_pose);
// K
Eigen::Matrix3d K_eigen;
K_eigen <<
K.at(0, 0), K.at(0, 1), K.at(0, 2),
K.at(1, 0), K.at(1, 1), K.at(1, 2),
K.at(2, 0), K.at(2, 1), K.at(2, 2);
// edges
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i)
{
auto p2d = points_2d[i];
auto p3d = points_3d[i];
EdgeProjection* edge = new EdgeProjection(p3d, K_eigen);
edge->setId(index);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(p2d);
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
pose = vertex_pose->estimate();
}