视觉SLAM非线性优化之Ceres solver 和 G2o
“ 如果你受苦了，感谢生活，那是它给你的一份感觉；
如果你受苦了，感谢上帝，说明你还活着。”
——伊索
3月就要结束了~
最近在看视觉SLAM的一些东西，把高博的非线性优化章节理解整理下，对传感器的融合优化很有帮助：=
首先(白话)解释下：
关于SLAM的概念基础就不细说了，可以百度下，经典的SLAM模型主要是由一个状态(转移)方程和一个运动(观测)方程组成，由于状态方程在视觉 SLAM 中没有特殊性，暂且不讨论它，主要讨论观测方程。而通常我们会获得大量带有噪声的观测数据，来推断运动相机的位姿和地图，即演化成一个状态估计问题(也可以说是一个最小二乘问题，可根据情况为线性最小二乘或非线性最小二乘)。在此，针对非线性的最小二乘问题，我们不用EKF来求解，而是用当前主流的非线性的优化方法来求解，比如说梯度下降法、高斯牛顿法(G-N)、列文伯格-马夸尔特方法Levenberg-Marquardt Method(LM）、以及Dog-Leg方法等。那么实际当中，通常使用谷歌的 Ceres 库 以及基于图优化的g2o 库来实现这些求解方法。下面补充两个基础知识：1.经典SLAM中的模型方程；2.最小二乘问题的定义以及解法。
●经典SLAM的模型方程
● 最小二乘问题的定义以及解法
也就是说，所谓的最小二乘问题是指找到测量值和模型预测值之间的最小误差，即：
其中e(x)为模型和样本之间的误差，在SLAM中亦可以看作为观测值和估计值之间的误差。通过求解该问题我们就能够优化我们的模型函数使之更接近与真实的函数模型。
下图的数据拟合可以很好的说明最小二乘问题的结果和效果。
注意，如果上面问题为线性的，即线性最小二乘，即可以直接对目标函数求导，并且令其等于零，以此求得其极值，并通过比较求取全局最小值(Global Minimizer)，并将其最为目标函数的解。但是如果问题为非线性，此时我们通常无法直接写出其导数形式（函数过于复杂），因此我们不再去试图直接找到全局最小值，而是退而求其次通过不停的迭代计算寻找到函数的局部最小值(Local Minimizer)，并认为该局部最小值能够使得我们的目标函数取得最优解（最小值），这就是非线性最小二乘的通常求解思路。常用的非线性最小二乘问题的求解方法有：高斯牛顿法(G-N)、列文伯格-马夸尔特方法Levenberg-Marquardt Method(LM）等，具体见博客：https://www.cnblogs.com/leexiaoming/p/7257198.html
OK，基础知识准备差不多了，
接下来就介绍实际应用中常用的两个经典的库，
通过使用上面介绍的方法来实现非线性最小二乘问题的求解：
▉○▉ Ceres solver
Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。使用Ceres求解非线性优化问题，一共分为三个部分：
1、 第一部分：构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符，具体实现方法后续介绍。
2、 第二部分：通过代价函数构建待求解的优化问题。
3、 第三部分：配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。
例：曲线拟合问题：假设有一条满足以下方程的曲线：
其中 a，b，c 为曲线的参数， w 为高斯噪声。现在，假设我们有 N 个关于 x; y 的观测数据点，想根据这些数据点求
出曲线的参数。其实，这就是一个非线性最小二乘问题，那么，我们可以构造该最小二乘问题的代价函数为：
需要注意的是，在这个问题中，待估计的变量是 a，b，c，而不是 x。接下来使用Ceres 库演示一下，在此，我们先基于模型生成 x，y 的真值，然后在真值中添加高斯分布的噪声。随后，使用 Ceres 从带噪声的数据中拟合参数模型。
/**
* 2019年xx月xx日
* z.h
*/
#include
#include
#include
#include
using namespace std;
// 代价函数的计算模型
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 a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector x_data, y_data; // 数据
cout<<"generating data: "< (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr, // 核函数，这里不使用，为空
abc // 待估计参数
);
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
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 = "<() 方法,
使得对象具有像函数那样的行为。
2. 调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度，我们有若干种选择：
（1）使用 Ceres 的自动求导（Auto Diff）；
（2）使用数值求导（NumericDiff）；
（3）自行推导解析的导数形式，提供给 Ceres。
其中自动求导在编码上是最方便的，于是我们就使用自动求导啦！
3. 自动求导需要指定误差项和优化变量的维度。
这里的误差则是标量，维度为 1；优化的是 a; b; c 三个量，维度为 3。
于是，在自动求导类的模板参数中设定变量维度为 1,3。
4. 设定好问题后，调用 solve 函数进行求解。
你可以在 option 里配置（非常详细的）优化选项。
例如，我们可以选择使用 Line Search 还是 Trust Region，迭代次数，步长等等。
*/ ▉○▉ G2O
G2O（General Graphic Optimization， G2O）。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论。其实只需要记住节点为优化变量,边为误差项即可，步骤如下：
同样的问题，代码实现如下：
/**
* 2019年xx月xx日
* z.h
*/
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast (_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 bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
int main( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector x_data, y_data; // 数据
cout<<"generating data: "< > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); // 线性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; isetId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
// 执行优化
cout<<"start optimization"< time_used = chrono::duration_cast>( t2-t1 );
cout<<"solve time cost = "<estimate();
cout<<"estimated model: "< 以上其实就是两个库的实现方式，Ceres 库提供了基于模板元的自动求导和运行时的数值求导，而 g2o 只提供了运行时数值求导这一种方式。但是，对于大多数问题，如果我们能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。
