图优化是非线性最小二乘问题的另一种表达而已,使用g2o求解和Ceres很多地方都相似。
最小二乘问题的关键在于:已知观测数据(输入值,观测值),待估计参数及其初始值,误差项,雅克比矩阵,求解增量更新待估计参数,在不同的方法中去寻找对应,对于方法的理解即可迎刃而解。
定义顶点和边的类
1、构建顶点Vertex类:
顶点就是待优化的参数,重要的成员函数有:
setToOriginImpl() 将节点的值进行重置
oplusImpl()表达顶点的更新(待优化变量的更新)
2、构建边Edge类:
边就是误差项,重要的函数有:
构造函数:用于接收已知数据的输入值,从而在computeError() 中书写
computeError() 计算误差项(其中_measurement用于接收观测值)
linearizeOplus() 求解雅克比矩阵
添加顶点和边(其他构建图优化问题部分略,重点展示添加)
1、添加顶点:
创建顶点(指针)访问成员函数:
setEstimate(type) :设定初始值(迭代初始值)
setId(int) :定义顶点的节点编号
optimizer.addVertex( ):添加顶点
2、添加边:(注意边一般添加很多个,因为已知数据有很多组)
通过构造函数创建边(指针)访问成员函数,(同时传入了已知数据的输入值):
setId(i):定义边的编号,决定在H矩阵中的位置
setVertex(int, vertex):将顶点通过编号传入_vertices[]储存,并用于Class边中计算误差
setMeasurement()传入观测值
setInformation()信息矩阵
optimizer.addEdge( edge ):添加边
例子
slambook2/ch6/ceresCurveFitting.cpp
顶点:
//创建顶点类
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>//普通类继承模板类,顶点维度、类型
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}