视觉SLAM十四讲学习笔记——第六讲 非线性优化（2）
这一节主要回顾一下Ceres、g20的使用。
1.Ceres、G2o源码安装方法
高博士的书中都有各个库的安装方法，但由于版本变化，个别安装方法可能并不适用。这里简单整理一下两个库的源码安装方法，其他的库之后有时间统一整理一下。
（1）Ceres安装
下载源码，下载地址：https://github.com/ceres-solver/ceres-solver
安装依赖项：
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev
sudo apt install cmake libeigen3-dev 打开源码文件夹安装：
mkdir build
cd build
cmake ..
make -j4
sudo make install（2）g2o安装
下载源码，下载地址：https://github.com/RainerKuemmerle/g2o
安装依赖项：
sudo apt-get install libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev qt5-default libqglviewer-dev-qt5 libcholmod3打开源码文件夹安装：
mkdir build
cd build
cmake ..
make -j4
sudo make install2.使用方法
Ceres库的使用方法较为简单，整体感觉比较简单，一些使用时的注意事项都记录在代码注释里了，相较而言g2o的代码更难看懂，使用起来比较困难。这里推荐"计算机视觉life"的三篇博客：
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码_计算机视觉life-CSDN博客
从零开始一起学习SLAM | 掌握g2o顶点编程套路_计算机视觉life-CSDN博客
从零开始一起学习SLAM | 掌握g2o边的代码套路_计算机视觉life-CSDN博客
3.源码注释
高博士的例程中有两个库用来求解曲线拟合的示例代码，我对两个代码做了更详细的注释，也标记出了一些没有看懂的问题：
（1）/slambook2-master/ch6/ceresCurveFitting.cpp
//
// Created by xiang on 18-11-19.
//
#include
#include
#include
#include
using namespace std;
// 代价函数的计算模型：输入x、y，计算当前参数下的损失函数 y-exp(ax^2+bx+c)
struct CURVE_FITTING_COST {
// 构造函数：传递用于计算误差的测量值
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
// 重载()操作符，用于计算误差
template
bool operator()(const T *const abc, // 模型参数，有3维
T *residual)
const {
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
return true;
}
// 用于存储测量值的成员变量
const double _x, _y; // x,y数据
};
int main(int argc, char **argv) {
//相同的数据创建方式
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
//待估计的参数：a,b,c
double abc[3] = {ae, be, ce};
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; i++)
{
// 向问题中添加误差项
problem.AddResidualBlock(
// AutoDiffCostFunction使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数，这里不使用，为空
abc // 待估计参数
);
}
/*
* ceres的使用：
* Problem::AddResidualBlock()：要用于向Problem类传递残差模块的信息
* 传递参数包括：CostFunction *cost_function：代价函数模块 包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式
* LossFunction *loss_function：损失函数模块 该参数可以取NULL或nullptr，此时损失函数为单位函数
* const vector parameter_blocks：参数模块 待优化的参数，可一次性传入所有参数的指针容器
* 参数模块较为简单;损失函数默认不使用;重点关注“代价函数模块”
* 代价函数CostFunction：常用的包括以下三种
* 自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式
* 数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式
* 解析导数（Analytic Derivatives）：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写
* 这里使用的是自行求导AutoDiffCostFunction
* ceres::AutoDiffCostFunction(CostFunctor* functor);
* 传递参数包括：CostFunctor 仿函数
* int residualDim 残差维数（1）
* paramDim 参数维数（3）
* 接受参数类型：为仿函数指针CostFunctor*
* 后两个变量不难理解，重点关注“仿函数CostFunctor”
* 仿函数的本质为结构体struct或者类class，由于重载了()运算符，使得其能够具有和函数一样的调用行为，因此被称为仿函数
* 主要包括 构造函数、重载操作符() 和 工厂函数三部分
* 详细信息见 示例代码中的定义部分
*/
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解:CHOLESKY：分解
options.minimizer_progress_to_stdout = true; // 输出到cout
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
} （2）/slambook2-master/ch6/g2oCurveFitting.cpp
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
// 自定义顶点，模板参数：
// int D: 优化变量维度，3
// T ：数据类型,Vector3d
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// setToOriginImpl()：顶点重置函数，设定被优化变量的原始值为<0,0,0>
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// oplusImpl()：顶点更新函数。用于优化过程中增量△x如何更新到节点的计算。
// 我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，这里选择直接相加“+=”
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
// 自定义边： 模板参数：观测值维度，类型，连接顶点类型
// 针对曲线拟合问题，使用一元边：BaseUnaryEdge
// 观测值维度：1，估计值和实际值之差
// 测量值数据类型：double
// 节点类型：由于是一元边，因此只有一个参数CurveFittingVertex
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 (_vertices[0]);
// 从节点中读取待优化的变量
const Eigen::Vector3d abc = v->estimate();
// 计算误差：error=y-exp(a*x*x+b*x+c)
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// linearizeOplus():是在当前顶点的值下,该误差对优化变量的偏导数,即计算雅可比矩阵
virtual void linearizeOplus() override {
// 连接的节点
const CurveFittingVertex *v = static_cast (_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 {}
// x 值， y 值为 _measurement
public:
double _x;
};
int main(int argc, char **argv) {
//同样的方式生成含有噪声的数据
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 第一步：构建图优化，先设定g2o：每个误差项优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver> BlockSolverType;
// 第二步：创建一个线性求解器LinearSolver：使用dense cholesky分解法,用于求解HΔx=−b
// 这里存在一个问题：BlockSolverType::PoseMatrixType是根据什么确定的，查找了一些资料都没有提及，大家似乎认为这是理所应当的
typedef g2o::LinearSolverDense LinearSolverType;
// 第三步：创建一个块求解器BlockSolver：用LinearSolver初始化
// 第四步：创建一个总求解器solver：用BlockSolver初始化，选择高斯-牛顿法优化策略
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
// 创建稀疏优化器SparseOptimizer：核心优化算法
g2o::SparseOptimizer optimizer;
// 用solver设置为求解器
optimizer.setAlgorithm(solver);
// 打开优化过程中的调试输出
optimizer.setVerbose(true);
// 向图中添加节点：本示例只有一个节点，包含三个需要优化的参数a,b,c
CurveFittingVertex *v = new CurveFittingVertex();
// 添加节点带优化变量
v->setEstimate(Eigen::Vector3d(ae, be, ce));
// 设置节点编号
v->setId(0);
// 向优化器optimizer中添加节点
optimizer.addVertex(v);
// 向图中添加边：每组数据都是一条边，拥有N组100条
for (int i = 0; i < N; i++) {
// x是边的输入
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
// 设置编号
edge->setId(i);
// 都连接到同一个顶点
edge->setVertex(0, v);
// y为观测数值
edge->setMeasurement(y_data[i]);
// 信息矩阵：协方差矩阵之逆
edge->setInformation(Eigen::Matrix::Identity() * 1 / (w_sigma * w_sigma));
// 在优化器optimizer中添加边
optimizer.addEdge(edge);
}
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
// 初始化
optimizer.initializeOptimization();
// 设置迭代次数：10,开始优化
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出最终结果
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
}