图优化理论现在是SLAM领域的主流优化方式，原理知识很多，这里不能一一介绍记录。简单的梳理以下什么是G2O。
个人理解，图优化把优化问题考虑成图的模式。带优化的变量称为顶点，对于待优量的限制条件，设定为边的概念，就构成了我们理解上的图。
在这里我们把限制条件理解成损失函数，这个函数是优化的关键，我们要通过不断的迭代获取最小的损失，而这又引出了最小二乘法的概念。
上图就是一个单元图，只有一个优化顶点，通过迭代所有边，求最小二乘。
具体的g2o概念后续会在SLAM的优化中一点点记录。
我们现在看看最简单的应用g2o实现优化求解的方法。
我们来拟合优化一个二次方程：
如果想要拟合这个二次方程，必须求出系数(1,2,1)。这个三位向量就是我们所需要的顶点，那么限制又是什么呢？就是观测值和实际值的差，我们可以用一个最小二乘来表示：
我们先来构造出我们需要的观测数据,x 和 f(x)。我们在这里增加了误差，来表示现实世界中的情况。
// 真实参数,假设曲线是f(x)=a*x^2+b*x+c,真实情况下应该是f(x)=x^2+2*x+1
double a = 1.0,b=2.0,c =1.0;
int N = 100;// 数据点的数量
double w_sigma = 1.0;// 噪声的方差Sigma
//cv::RNG rng;// opencv随机数产生器,如果要使用opencv的高斯0均值随机数发生器,请在commonBased引入[将注释取消即可]
double abc[3] = {0,0,0};// abc参数的估计值
std::vector<double> x_data,y_data;//保存随机数的值,即模拟真实点的情况
std::cout << "\t\t\t\t generating data:" << std::endl;
std::cout << "====================================================" << std::endl;
for(auto i=0;i<N;i++)
{
double x = i/100.0;
x_data.push_back(x);
y_data.push_back(
//exp(a*x*x+b*x+c)+rng.gaussian(w_sigma)
exp(a*pow(x,2)+b*x+c)+g2o::Sampler::gaussRand(0,w_sigma)// 采用G2O的随机发生器
);
std::cout << x_data[i] << " " << y_data[i] << std::endl;
}
接下来就是创建边和顶点。
顶点头文件：
class curveVetex: public g2o::BaseVertex<3,Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl();
virtual void oplusImpl(const double* update);
virtual bool read(std::istream &is);
virtual bool write(std::ostream &os) const;
};源文件：
void curveVetex::setToOriginImpl()
{
_estimate << 0,0,0;
}
void curveVetex::oplusImpl(const double* update)
{
_estimate += Eigen::Vector3d(update);
}
bool curveVetex::read(std::istream &is)
{
is >> _estimate[0] >> _estimate[1] >> _estimate[2];
return true;
}
bool curveVetex::write(std::ostream &os) const
{
os << _estimate[0] << _estimate[1] << _estimate[2];
return os.good();
}边的头文件：
class curveEdge : public g2o::BaseUnaryEdge<1,double,curveVetex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
curveEdge (double x):g2o::BaseUnaryEdge<1,double,curveVetex>(),_x (x) {}
// 计算曲线模型误差
void computeError();
//存取
bool read(std::istream& is);
bool write(std::ostream& os) const;
public:
double _x;//x值,y值为_measurement
};源文件：
void curveEdge::computeError()
{
// 一元超边,超边存储的节点为VertexContainer类型,你可以把它当成一个数组,取第一个就是0,对于一元边来说,这就是它的
// 连接的边
const curveVetex *v = static_cast<const curveVetex*>(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
// _error是传入时定义的误差ErrorVector,维度D是构建curveEdge已经定义,必然是个向量,只不过是Eigen::Matrix的_Cols为1
_error(0,0) = _measurement-std::exp(abc[0]*pow(_x,2)+abc[1]*_x+abc[2]);// y-f(x)=error
}
bool curveEdge::read(std::istream &is)
{
is >> _x;
return true;
}
bool curveEdge::write(std::ostream &os) const
{
os << _x;
return os.good();
}代码已经添加了注释，注意看
最后对100个数据进行迭代求最小二乘损失函数。看下整体的代码，包括添加顶点和边到图中后执行优化。
#include <iostream>
#include <g2o/stuff/sampler.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/sparse_optimizer.h>
#include <chrono>
#include "curveVetex.h"
#include "curveEdge.h"
using namespace std;
int main(int argc,char **argv)
{
std::cout << "====================================================" << std::endl;
std::cout << "\t\t\t\t这是关于曲线拟合的例子" << std::endl;
std::cout << "====================================================" << std::endl;
std::cout << "\t\t\t\t anthor:YikaiMao" << std::endl;
std::cout << "====================================================" << std::endl;
std::cout << "\t\t\t\treference:gaoxiang" << std::endl;
std::cout << "====================================================" << std::endl;
// 真实参数,假设曲线是f(x)=a*x^2+b*x+c,真实情况下应该是f(x)=x^2+2*x+1
double a = 1.0,b=2.0,c =1.0;
int N = 100;// 数据点的数量
double w_sigma = 1.0;// 噪声的方差Sigma
//cv::RNG rng;// opencv随机数产生器,如果要使用opencv的高斯0均值随机数发生器,请在commonBased引入[将注释取消即可]
double abc[3] = {0,0,0};// abc参数的估计值
std::vector<double> x_data,y_data;//保存随机数的值,即模拟真实点的情况
std::cout << "\t\t\t\t generating data:" << std::endl;
std::cout << "====================================================" << std::endl;
for(auto i=0;i<N;i++)
{
double x = i/100.0;
x_data.push_back(x);
y_data.push_back(
//exp(a*x*x+b*x+c)+rng.gaussian(w_sigma)
exp(a*pow(x,2)+b*x+c)+g2o::Sampler::gaussRand(0,w_sigma)// 采用G2O的随机发生器
);
std::cout << x_data[i] << " " << y_data[i] << std::endl;
}
std::cout << "====================================================" << std::endl;
std::cout << "\t\t\t\t 构建图:" << std::endl;
//H矩阵块
using BlockSolverType = g2o::BlockSolverPL<3, 1>;
using LinearSolverType = g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>;
//LM算法
auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);// 设置求解器
optimizer.setVerbose(true);// 打开调试输出
curveVetex* v = new curveVetex();
v->setEstimate(Eigen::Vector3d(0,0,0));
v->setId(0);
optimizer.addVertex(v);
for(int i = 0 ; i < N; i++)
{
curveEdge* edge = new curveEdge(x_data[i]);
edge->setId(i);// 边的编号
edge->setVertex(0,v);//设置超边的第几个顶点是什么顶点,目前只有一个顶点
edge->setMeasurement(y_data[i]);//设置观测值
edge->setInformation(Eigen::Matrix<double ,1,1>::Identity()/**1/(w_sigma*w_sigma)*/);//信息矩阵是协方差的逆,1/sigma^2
optimizer.addEdge(edge);// 图中加入顶点
}
std::cout << "\t\t\t\t 构建完成" << std::endl;
std::cout << "====================================================" << std::endl;
std::cout << "\t\t\t\t start optimization" << std::endl;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>> (t2-t1);
std::cout << "====================================================" << std::endl;
std::cout << "\t\t solve time cost = "<< time_used.count() << "seconds" <<std::endl;
std::cout << "====================================================" << std::endl;
Eigen::Vector3d abc_estimate = v->estimate();
std::cout << "\t\t estimated model:" << abc_estimate.transpose() << std::endl;
return 0;
}结果如下：
可以看到最后的结果接近（1，2，1）。
评论（0）
您还未登录，请登录后发表或查看评论