前言
我们来看看十四讲上第六章这个曲线拟合的例子
设曲线方程为
a,b,c为待估计参数,w为噪声。假设有N个关于x,y的观测数据点,构建一个非线性优化问题。可以通过求解下面的最小二乘问题来估计曲线参数
可以将本问题转化成图优化问题。用顶点表示优化变量,边表示误差项,使用g2o进行曲线拟合,g2o的编译安装过程这里不进行赘述。g2o编程主要部分在于顶点和边的定义,此外就是构建图模型与将顶点和边添加到图中,执行优化即可。
1.定义曲线模型的顶点
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
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 {}
};
- CurveFittingVertex继承BaseVertex<优化变量维度,顶点数据类型>
- 成员变量 _estimate 类型为顶点的类型,表示待估计的a,b,c
源码为typedef T EstimateType; EstimateType _estimate;
- 成员函数setToOriginImpl()将_estimate重置为初始值
- 成员函数oplusImpl(const double *update)
源码为virtual void oplusImpl(const number_t* v) = 0;
使用v中的参数来更新节点。实际上,number_t是double类型。将更新量以数组形式表示,返回该数组首地址,我们需要将update数组转化成顶点类型。书中顶点类型(Vector3d类型)可直接由数组初始化,不要被迷惑。 - 成员函数setEstimate()为成员变量_estimate赋值,初始化了顶点值,同时调用updateCache()函数(暂时没有弄明白)
2.定义边
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<const CurveFittingVertex *> (_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<const CurveFittingVertex *> (_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; // x 值, y 值为 _measurement
};
- CurveFittingEdge继承BaseUnaryEdge<观测值维度,观测值类型,顶点数据类型>
- 成员函数computeError()函数,用来计算边的误差。其中,成员变量_vertices源码为
typedef std::vector<Vertex*> VertexContainer; VertexContainer _vertices;
即_vertices是由Vertex* 构成的vector,此处_vertices的类型为BaseVertex*.因此要对 _vertices[0]进行强制类型转换static_cast<const CurveFittingVertex *> (_vertices[0])
将其转成CurveFittingVertex类型 - 成员函数estimate(),返回_estimate的值
- 成员变量_measurement保存了观测值的值(error = measurement - estimate)
- 成员函数setVertex()原型为
void setVertex(size_t i, Vertex* v)
,设置该边要连接的顶点。由于本例只有一个顶点,所有边都连接该顶点。 - 成员函数setMeasurement()为观测值_measurement赋值
- void linearizeOplus()函数,计算Jacobian矩阵,将结果保存至 _jacobianOplusXi中
- setInformation矩阵,源码为
void setInformation(const InformationType& information) { _information = information; }
,定义协方差矩阵,这里为高斯噪声方差的逆(1×1),是为了计算高斯牛顿法的增量方程的
3.其他
剩下的工作就是创建求解器、创建图模型、设置求解器等工作了。之后就需要向图中添加顶点和边。
在分析代码的时候,心中要对高斯牛顿法流程有一个清晰的认识,添加顶点设置了待估计参数的初值,添加N条边,确立了顶点与顶点的连接关系(当然,本例只有一个顶点,就都是unary edge),同时也通过计算每个error对estimate的Jacobian矩阵,列写出了总的高斯牛顿增量方程。于是在执行optimize(10)时,重复求解增量,更新估计值。
// 构建图优化
//先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3,误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
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<double, 1, 1>::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<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;