一、g2o图优化（General Graph Optimization）
g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：
先看上半部分。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Δx=−b
这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。
综上所述，在g2o中选择优化方法一共需要三个步骤：
选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
选择一个迭代策略，从GN, LM, Doglog中选。
总结一下做图优化的流程。
选择你想要的图里的节点与边的类型，确定它们的参数化形式；
往图里加入实际的节点和边；
选择初值，开始迭代；
每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
求解稀疏线性方程HkΔx=−bk，得到梯度方向；
继续用GN或LM进行迭代。如果迭代结束，返回优化值。
二、代码解析
g2o中派生出的图优化顶点和边两个派生类
顶点CurvFittingVertex
//继承的g2o::BaseVertex基类 <优化变量维度,数据类型>
class CurveFittingVertex :public g2o::BaseVertex < 3, Eigen::Vector3d >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//1. 重置
virtual void setToOriginImpl()
{
_estimare << 0, 0, 0;
}
//2. 更新X(k+1)=X(k)+x;
virtual void oplusImpl(const double* update)
{
_estimate += Eigen::Vector3d(update);
}
//存盘和读盘：留空
virtual bool read(istream& in){}
virtual bool write(ostream& out){}
};
派生类中的虚函数
1、顶点的更新oplusImpl
2、顶点的重置函数setToOriginImpl
3、边的误差计算函数computeError
边CurveFittingEdge
//继承的g2o::BaseUnaryEdge基类 <观测值维度，类型，连接顶点类型>
class CurveFittingEdge :public g2o::BaseUnaryEdge < 1, double, CurveFittingVertex >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) :BaseUnaryEdge(), _x(x){}//构造函数
double _x, _measurement;
//1. 计算误差
void computeError()
{
//初始化顶点v
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
//abc是边所连接顶点的当前估计值
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){}
};
主框架函数
//主函数
int main()
{
double a = 1, b = 2, c = 3;//abc参数真实值
int N = 100;
double sigma = 1;
cv::RNG rng;？？？
double abc[3] = { 0, 0, 0 };//abc参数估计值
//1. 产生顶点和观测值共100个数据点
vector<double> xdata, ydata;
for (int i = 0; i < N; i++)
{
double x = i / 100;
xdata.push_back(x);
ydata.push_back(exp(a*x*x+b*x+c)+rng.gaussian(sigma);
}
//2. 构建图优化，设定线性方程求解器Blocksolver和迭代算法.
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
Block* solver_ptr = new Block(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//图模型，并设置求解器
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
//3. 图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(0, 0, 0));
v->setId(0);
optimizer.addVertex(v);
//4. 图中增加边
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<double, 1, 1>::Identity() * 1 / (sigma*sigma));//协方差矩阵逆
optimizer.addEdge(edge);
}
//5. 执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);
Eigen::Vector3d abc_estimate = v->estimate();
}
注解1：cv产生随机数RNG
cv::RNG rng
rng.gaussian(sigma);
注解2：g2o中派生出CureFittingVertex和CureFittingEdge.
