g2o优化
接上一篇RANSAC
1 概述
就像g2o文献[1]中说的一样，它的目的是提供一个能够容易应用到各种问题的一个简单、易用的图优化库。它使用图的形式来描述和执行非线性最小二乘问题的优化。所以整个框架大概分为两部分：一部分是用图来描述，另一部分是优化。图一描述了这个结构。
图一 g2o框架
由上图可以看出整个优化问题可以表述成是一个稀疏优化器（SpareOptimizer）。它由图和优化方法组成。
图由顶点和边组成。具体到一个优化问题时，顶点表示待优化变量，它继承自Base Vertex，在继承时我们需要给定它的变量维度D和变量类型T;边表示当前模型和观测值之间的误差，它分为一元边、二元边和超边，分别连接一个变量、两个变量和多个变量，在继承时需要提供误差的维度D、类型E，如果不是超边的话还需要提供它所连接的顶点类型。
优化方法可以分为下降策略和线性方程组求解器。其中下降策略可以选择高斯牛顿、LM和dogleg。这三种方法对变量（顶点）的更新策略不同，有时适用的条件也不同。而Solver主要是求解增量方程，其中SparseBlockMatrix主要是构建增量方程，LinearSolver是数值求解增量方程使用的方法。
对于一个非线性最小二乘问题（线性最小二乘问题比较简单，直接对误差函数求导，令导函数为零即可求解），由于误差函数求导并另导数为零后转化为一个非线性方程组，不好求解，所以要使用迭代的方法来解决，这时候就可以使用g2o了。
2 g2o使用细节
我们还是按照上面的结构分两个部分来介绍如何使用g2o。
构建图
要构建图我们首先要根据自己的问题来确定顶点和边。
在上一篇文章中我们使用RANSAC估计出了一个直线模型，这里我们在它模型基础上进行优化。这个问题现在变成如何调整模型参数使得所有内点到新模型的距离和最短。
设直线方程为
在继承的过程中我们还要重写纯虚函数（具体的可以看后面的实现）
在顶点的定义里面要重写：
virtual void setToOriginImpl() //重置估计值
virtual void oplusImpl( const double* update )//更新估计值
virtual bool read( std::istream& in )//读函数
virtual bool write( std::ostream& out )//写函数
在边的定义里要重写：
void computeError()//计算误差
virtual void linearizeOplus()//计算雅可比矩阵（可选）
virtual bool read( std::istream& in )//读函数
virtual bool write( std::ostream& out )//写函数
这些定义完成后构建图所需要的顶点和边就准备好了，下面是优化算法。
优化算法
对于优化算法，由图一可以看出它是有包含关系的，所以在构建它的时候可以按照结构，从后往前逐次构建。
using namespace g2o;
//估计这两个参数都是优化变量维度
typedef BlockSolver< BlockSolverTraits<2, 1> > Block;
//选择线性求解器，因为这里求得的H矩阵是稠密的所以使用稠密求解器，当然也可以使用分解方法的求解器
Block::LinearSolverType* linear_solve = new LinearSolverDense<Block::PoseMatrixType>();
Block * solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linear_solve));
//选择优化方法
OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(solver_ptr));
SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
这样整个图所需要的东西就准备好了，下面就是将信息添加进图中。
对于顶点来说大概是这几个操作步骤：
创建新顶点
给优化变量赋初值
给顶点编号
向图中添加顶点
对于边来说：
创建新边
连接顶点
给观测值
设置信息矩阵（类似于权重吧）
向图中添加边
3 实现及效果
（附带生成数据、RANSAC）
还是用
#include <iostream>
#include <vector>
#include <math.h>
#include <stdlib.h> //生成随机点
#include <time.h> //根据系统时间生成随机点种子
#include <random> //高斯噪声生成
#include <fstream>
#include <g2o/core/base_vertex.h> //顶点
#include <g2o/core/base_unary_edge.h> //一元边
#include <g2o/core/block_solver.h> //块求解器
#include <g2o/core/optimization_algorithm_levenberg.h> //下降策略
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h> //线性方程稠密求解器
#include <Eigen/Core>
#define random(a) (rand()%a) //0-a
struct DATA{
int x;
int y;
int flag;
};
class RANSAC{
private:
std::vector<DATA> _data; //输入数据
int _min_size; //最小子集
float _ratio; //数据外点比例，不知道的情况下默认0.5
float _p; //至少一次采样，没有外点的概率,默认0.99
//std::vector<float> infor_entro; //信息熵
std::vector<DATA>minset; //每次采样的最小子集
public:
double sum[2] = {0,0}; //内点误差
int times; //迭代次数
std::vector<std::vector<float> > param; //计算的到的参数
std::vector<int> inliners; //每个模型参数的内点个数
std::vector<std::vector<DATA> > inliners_points; //每个模型的内点，在里面的类型是结构体的时候需要保证里面的空间足够所以需要使用reserve
RANSAC();
RANSAC(std::vector<DATA>& data, int min_size, float p = 0.99, float ratio = 0.5){
_data = data;
_min_size = min_size;
_ratio = ratio;
_p = p;
}
void iterator();
//void compute_infor_entro();
};
/**
* 计算迭代次数，最小集采样，计算模型参数，计算内点个数
* 这里默认当进行距离阈值判断时，点为内点的概率为0.95,并且默认噪声服从均值为0,标准差为1的高斯分布
*/
void RANSAC::iterator() {
times = (int)(log(1-_p)/log(1-pow((1-_ratio),_min_size)));//迭代次数
minset.resize(_min_size);
std::vector<DATA> temdata;
param.resize(times);
inliners_points.reserve(times + 1);
for(int j = 0; j < times; j++)
{
temdata = _data;
srand((unsigned)time(0) + j * 10);
for(int i = 0; i < _min_size; i ++)//选取最小点集
{
int local = random(temdata.size());
minset[i] = temdata[local];
temdata[local] = temdata.back();
temdata.pop_back();
}
//计算模型参数
param[j].push_back((float)(minset[1].y - minset[0].y) / (minset[1].x - minset[0].x));
param[j].push_back(minset[0].y - param[j][0] * minset[0].x) ;
//计算每个参数对应的内点个数
int inlinepoint = 0;
for(int i = 0; i < _data.size(); i ++)
{
if(fabs(param[j][0]) > 100000 || fabs(param[j][1]) > 10000)//防止出现无穷大数
break;
sum[0] = abs(param[j][0] * _data[i].x - _data[i].y + param[j][1])/sqrt(pow(param[j][0],2.0) + 1);
if(sum[0] <= 3.84)//距离阈值判断，按照高斯分布且判断内点概率0.95的卡方检验设置
{
sum[1] += sum[0];
inlinepoint ++;
inliners_points[j].push_back(_data[i]);
}
}
inliners.push_back(inlinepoint);
}
}
//定义的顶点
class LineVertex : public g2o::BaseVertex<2,Eigen::Vector2d>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //对齐Eigen指针
virtual void setToOriginImpl() // 重置估计值
{
_estimate << 0,0;
}
virtual void oplusImpl( const double* update ) // 更新估计值
{
_estimate += Eigen::Vector2d(update);
}
// 存盘和读盘：留空
virtual bool read( std::istream& in ) {return 0;} //纯虚函数需要重新定义一下
virtual bool write( std::ostream& out ) const { return 0;}
};
//定义的边
class LineEdge: public g2o::BaseUnaryEdge<1,double,LineVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
LineEdge( int x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const LineVertex* v = static_cast<const LineVertex*> (_vertices[0]);//类型转换，将向量转化为顶点类型
const Eigen::Vector2d abc = v->estimate(); //获得估计参数
_error(0,0) = abs((abc(0,0) * _x - _measurement + abc(1,0)))/sqrt(pow(abc(0,0),2.0) + 1); //计算误差
//_error(0,0) = abs(_measurement - abc(0,0) * _x - abc(1,0));
}
//误差对变量求导 可以不写，那么g2o会自动数值求导
virtual void linearizeOplus(){
const LineVertex* v = static_cast<const LineVertex*> (_vertices[0]);
const Eigen::Vector2d abc = v->estimate();
double a = abc(0,0);
double c = abc(1,0);
_jacobianOplusXi(0,0) = (2*_x*(a*_x*_measurement+c)*(a*a+1)-2*a*(a*_x-_measurement+c))/(a*a+1)/(a*a+1);
_jacobianOplusXi(0,1) = 2*(a*_x-_measurement+c)/(a*a+1);
}
virtual bool read( std::istream& in ) {return 0;} //纯虚函数，需要重新写
virtual bool write( std::ostream& out ) const {return 0;}
public:
int _x; // x 值 y 值为 _measurement
};
/**
* 根据给定模型生成数据
* @param data 存放生成数据
* @param good 准确的数据个数
* @param bad 加噪声的数据个数
*/
void generatordata(std::vector<DATA>& data, int good = 50, int bad = 50)
{
data.resize(good + bad);
srand((unsigned)time(NULL));
for(int i = 0; i < good; i ++) //生成准确点
{
data[i].x = random(good);
data[i].y = 3 * data[i].x + 4;
data[i].flag = 1;
}
double mean = 0;
double stddev = 1;
std::default_random_engine generator;
std::normal_distribution<double> dist(mean, stddev);
//srand((unsigned)time(0) + good*2);
for(int i = good; i < good + bad; i ++)//噪点
{
data[i].x = random(good);
data[i].y = random(good * 3);
//data[i].y = 3 * data[i].x + 4 + dist(generator);
///如果按照测量误差为均值0 标准差1来生成数据的话，大多数情况下所有点都被判别为内点，这样就和处理大量外点情况不太相符
data[i].flag = 0;
}
}
int main ()
{
std::vector<DATA> data;
generatordata(data);//生成数据
RANSAC it1(data,2);
it1.iterator();//RANSAC迭代找到内点最多的模型
//存放数据
std::ofstream of("Ransac.txt");
of << "数据（x y） 前50个为准确点，后50个为随机生成的噪点: " << std::endl;
for(int i = 0; i < data.size(); i ++)
{
of << data[i].x << " " << data[i].y << std::endl;
}
of << "拟合参数和内点个数（a c 内点个数）： " <<std::endl;
for(int i = 0; i < it1.times; i ++)
{
of << it1.param[i][0] << " " << it1.param[i][1] << " " << it1.inliners[i]<< std::endl;
}
//输出参数及内点个数
int idex = 0, max_inliners = 0;
for(int i = 0; i < it1.times; i ++)
{
if(it1.inliners[i] > max_inliners)
{
max_inliners = it1.inliners[i];
idex = i;
}
}
std::cout << "优化前最优模型： y = " << it1.param[idex][0] << "x + " << it1.param[idex][1]<< std::endl;
std::cout << "优化前内点距离和： " << it1.sum[1] << std::endl;
//g2o优化 根据结构从下往上
using namespace g2o;
//<int _PoseDim, int _LandmarkDim> BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>估计这两个参数都是优化变量维度
typedef BlockSolver< BlockSolverTraits<2, 1> > Block;
//选择线性求解器，因为这里求得的H矩阵是稠密的所以使用稠密求解器，当然也可以使用分解方法的求解器
Block::LinearSolverType* linear_solve = new LinearSolverDense<Block::PoseMatrixType>();
Block * solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linear_solve));
//选择优化方法
//OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(solver_ptr));
//OptimizationAlgorithmDogleg* solver = new OptimizationAlgorithmDogleg(std::unique_ptr<Block>(solver_ptr));
SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
// optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
LineVertex* v = new LineVertex;
v->setEstimate( Eigen::Vector2d(it1.param[idex][0],it1.param[idex][1]) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; i<max_inliners; i++ )
{
LineEdge* edge = new LineEdge( it1.inliners_points[idex][i].x );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( it1.inliners_points[idex][i].y ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity() ); // 信息矩阵如何设置？
optimizer.addEdge( edge );
}
// 执行优化
std::cout<<"开始优化......"<<std::endl;
optimizer.initializeOptimization();//初始化图
optimizer.optimize(100);//最多迭代次数
std::cout<<"优化结束！！！" << std::endl;
// 输出优化值
Eigen::Vector2d abc_estimate = v->estimate();
std::cout<<"优化后模型: " << "y = " << abc_estimate(0,0) << "x + " << abc_estimate(1,0) << std::endl;
it1.sum[1] = 0;
for(int i = 0; i < max_inliners; i ++)
{
it1.sum[0] = abs(abc_estimate(0,0) * it1.inliners_points[idex][i].x - it1.inliners_points[idex][i].y + abc_estimate(1,0))/sqrt(pow(abc_estimate(0,0),2.0) + 1);
it1.sum[1] += it1.sum[0];
}
std::cout << "优化后内点距离和： " << it1.sum[1];
return 0;
}
下面是一次运行结果：
优化前最优模型： y = 3.30435x + -6.04347
优化前内点距离和： 408.583
开始优化......
优化结束！！！
优化后模型: y = 3.30319x + -3.38884
优化后内点距离和： 74.7557
Process finished with exit code 0
4 参考资料
[1] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard. g2o: A General Framework for
Graph Optimization. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA). Shanghai,
China, May 2011.
[2] 《视觉SLAM十四讲从理论到实践》第六讲：非线性优化 P104