本文为视觉 SLAM 学习总结。

在给定观测数据 $z_{k,j}$ 时，如何估计状态变量 $x_k,y_j$ ，即同时定位和建图

本讲内容概要

最小二乘法的含义和处理方式
Gauss-Newton，Levenburg-Marquadt 等下降策略
Ceres 库和 g2o 库的基本使用方法

状态估计问题

最大后验与最大似然

回顾 SLAM 方程：
$\begin{cases} x_k=f(x_{k-1},u_k)+w_k& \\ z_{k,j}=h(y_j,x_k)+v_{k,j}& \end{cases}$
其中

相机位姿： $x_k=T_k=exp(ξ_k)$
像素： $sz_{k,j}=Kexp(\hat ξ)y_j$
噪声： $w_k\sim(0,R_k),v_k\sim(0,Q_{k,j})$

最简单的情况是： $f,g$ 都为线性函数，噪声服从高斯分布，这种情况在 CH10 中会讲到，使用 KEF 可以求得无偏的最优解。如果是非线性系统，和非高斯噪声，情况会变得很复杂，要分情况讨论。

以前通常用滤波器求解状态估计，该方法假设该系统具有马尔可夫性——系统下一时刻状态仅依赖于上一个状态。于是我们可以只维护上一个状态，当有新的输入进入时就更新一次状态估计。

近年来非线性优化已成为主流方法。

状态变量：所有待求解的量，即所有时刻的位姿和路标
$x=\{x_1,…,x_N,y_1,…,y_M\}$
状态估计实际上是求解条件分布： $P(x|z,u)$ ，这里的 $z,u$ 是统称。考虑最简单的情况，当只有观测时，该问题类似于求解一个 SfM（Structure from Motion）问题。

由贝叶斯法则有：
$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$
其中 $P(z|x)$ 为似然， $P(x)$ 为先验。

然而， $P(x|z)$ 条件分布很难求解，但是我们可以求解：

最大后验估计（MAP）。因为分母与 $x$ 无关直接省略。
$x^*_{MAP}=\arg \max P(x|z)=\arg \max P(z|x)P(x)$
最大似然估计（MLE）。此时先验 $x$ 未知
$x^*_{MAP}=\arg \max P(z|x)$
其物理意义是“在哪种状态下，最容易产生当前的观测值”。

最小二乘的引出

在某次观测 $z_{k,j}=h(y_j,x_k)+v_{k,j}$ ，由于噪声是高斯的 $v_k\sim(0,Q_{k,j})$ ，于是：
$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$
现在我们要求 $x,y$ 的值，使得该高斯分布的值最大，即求解这个高斯分布的最大似然。因为高斯分布具有简单的性质，该问题易解。

一般的高斯分布：
$P(x)=\frac{1}{\sqrt{(2\pi)^N\det(\Sigma)}}\exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))$
观察到形式较为复杂，我们取负对数进行化简，乘法变为加法，指数变成了常数项：
$-\ln(P(x))=\frac{1}{2}\ln((2\pi)^N\det(\Sigma))+\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)$
此时，求原分布的最大值问题就转换为了求负对数的最小值问题。观察到第一项与 $x$ 无关，则我们只需最小化后面的二次型。将二次型带入 SLAM 观测模型中有：
$x^*=\arg \min((z_{k,j}-h(x_k,y_j))^TQ_{k,j}^{-1}(z_{k,j}-h(x_k,y_j)))$
观察上式，我们相当于在最小化噪声项，在这里我们就定义误差：
$e_{v,k}=x_{k}-f(x_{k-1},u_k)$

$e_{y,j,k}=z_{k,j}-h(x_k,y_j)$

则误差的平方和为：
$J(x)=\sum\limits_{k}e_{v,k}^TR_k^{-1}e_{v,k}+\sum\limits_{k}\sum\limits_{j}e_{y,k,j}^TQ_{k,j}^{-1}e_{y,k,j}$

直观的解释是，由于噪声的存在，我们估计的轨迹带入 SLAM 的方程中不能很好地成立，我们需要调整状态的估计，使得误差最小化。

该问题的结构：

由多个误差的平方和组成
虽然整体维度高，但每个误差项简单，仅与一两个状态变量有关
如果用李代数表达位姿，是无约束优化问题

至此，我们将一个最大似然问题转换成了非线性最小二乘问题。

非线性最小二乘

我们首先考虑一个最简单的最小二乘问题：
$\min\limits_{x}\frac{1}{2}||f(x)||^2_2$
如果 $f(x)$ 形式十分简单，我们可以考虑对 $f(x)$ 求导：
$\frac{\mathrm{d}f}{\mathrm{d}x}=0$
得到导数为 0 的点，可能是极值点或鞍点，只需要逐个比较大小即可。而当 $f(x)$ 不便于求导或求导后不便于解出极值点时，我们通常用迭代的方法逼近最优解：

给定某个初始值 $x_0$
对于第 $k$ 次迭代，寻找增量 $\Delta x_k$ ，使得 $||f(x_k+\Delta x_k)||^2_2$ 达到极小值
若 $\Delta x_k$ 足够小则停止
否则更新 $\Delta x_k$ ，令 $x_{k+1}=x_k+\Delta x_k$ ，返回 2

其中最关键的问题是 $\Delta x_k$ 如何确定？

一阶和二阶梯度法

我们将目标函数在 $x$ 附近进行泰勒展开：
$||f(x+\Delta x)||^2_2≈||f(x)||^2_2+J(x)\Delta x+\frac{1}{2}\Delta x^TH\Delta x$
我们可以选择保留一阶项或二阶项，其分别对应了一阶和二阶梯度法。

如果保留一阶项，令增量方程导数为 0，增量的解为：
$\Delta x^*=-J^T(x)$
我们只需要沿着 $x$ 的反梯度方向前进就可下降，若要下降地最快，还需要计算该方向上的步长 $\lambda$ ，这种方法称为最速下降法。

如果保留二阶项，令增量方程导数为 0，增量的解为：
$H\Delta x=-J^T$
该方法称为牛顿法。

一阶和二阶梯度下降法十分直观，但各自都有缺点：

最速下降法过于贪心，走出锯齿线路，反而增加了迭代次数
牛顿法要计算 $H$ 矩阵，在问题规模较大时难以求解

高斯牛顿法

高斯牛顿法的思想是对 $f(x)$ 进行一阶泰勒展开，推导过程比较复杂，这里不做介绍，直接给出增量方程：
$J(x)^TJ(x)\Delta x=-J(x)^Tf(x)$
将左边系数定义为 $H$ ，右边残差定义为 $g$ ，则上式变为：
$H\Delta x=g$
对比牛顿法，高斯牛顿法将 $J^TJ$ 来近似牛顿法中的 $H$ 矩阵，省略了计算 $H$ 。我们在求解增量时只需求解当前的雅可比矩阵 $J(x_k)$ 和误差 $f(x_k)$ 。但 $J^TJ$ 会出现不可逆的情况。

高斯牛顿法属于线搜索方法：先找方向，后确定步长。

裂纹伯格—马夸尔特方法

裂纹伯格—马夸尔特方法属于信赖区域方法：在信赖区域中，认为近似是有效的。

使用近似模型和实际函数的差异来进行近似程度的描述：
$\rho=\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$
其中分子为实际下降，分母为近似模型的下降。

当 $\rho$ 接近 1 时，近似是好的
$\rho$ 太小时，近似较差，要缩小近似范围
$\rho$ 太大时，实际下降更大，可以放大近似范围

该方法的框架为：

给定初始值 $x_0$ 以及初始优化半径 $\mu$
对于第 $k$ 次迭代，求解：
$\min\limits_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2, s.t.||D\Delta x_k||\le\mu$
其中 $\mu$ 为信赖区域半径， $D$ 通常取 $I$ ，相当于把 $\Delta x$ 约束在球中，也可取非负数对角阵，约束在椭球中。
计算 $\rho$
若 $\rho>\frac{3}{4}$ ，则 $\mu=2\mu$ ；若 $\rho>\frac{1}{4}$ ，则 $\mu=0.5\mu$ ；为经验值。
如果 $\rho$ 大于某阈值，则认为近似可行，更新 $x_{k+1}$
判断算法是否收敛。如果不收敛返回 2，否则结束

在求解增量方程时，牛顿法是直接对 $\Delta x$ 求导，但这里我们有信赖区域的约束，因此需要用拉格朗日乘子将其转换为一个无约束的问题。我们直接写出增量方程：
$(H+\lambda D^TD)\Delta x=g$
在裂纹伯格—马夸尔特方法中， $D$ 取 $I$ ，相当于求解：
$(H+\lambda I)\Delta x=g$
通过观察上式发现， $\lambda$ 实际为一二阶间的权重，增强了正定性。

小结

非线性优化的主要方法有：最速下降、牛顿、G-N、L-M、DogLeg 等。

与线性规划不同，非线性需要针对具体问题具体分析。

当问题非凸时，即目标函数有多个局部最优解，对初值敏感，如果初值离最优解过远可能会陷入局部最优解。

下面对两个实验进行讲解，两个实验求解同一个问题。

实践：Ceres

安装 Ceres

首先安装 Ceres，安装依赖项（如果缺少依赖项，在 cmake 时会有提示，补上重新 cmake 即可）：

sudo apt-get install libcxsparse3.1.4 libsuitesparse-dev libeigen3-dev libgoogle-glog-dev libgtest-dev

因为 Ceres 中有 CMakeLists.txt，因此我们可以直接在新建的 build 文件夹中 cmake：

cmake ..
make -j6 # 加速，6替换为自己的CPU内核数
sudo make install

该过程较慢，耐心等待。安装结束后：

ls /usr/local/include/ceres/ # 头文件在这里
ls /usr/local/lib/libceres.a # 库在这里

Ceres 只有 1 个库。在工程目录下添加 cmake_modules/CeresConfig.cmake.in：

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

还需要在 CMakeLists.txt 中添加以下代码：

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )

Ceres 相关文档写的较为全面完善——http://ceres-solver.org/，原理也较为直观，在 Tutorial 中可进行系统学习。

实验：曲线拟合

设曲线方程为 $y=\exp(ax^2+bx+c)+w$ 。我们得到一些带噪声的样本数据 $x,y$ ，拟合曲线参数 $a,b,c$ 。

Ceres 是一个规范的最小二乘求解器。只需定义出优化函数和优化变量，再选好求解器，可直接求解。Ceres 会自动求导，得到数值导数，但结果不一定是最好的（大部分情况是好的），想要得到最好的结果还是需要自己推出解析的导数。

编程实现

接下来我们编程实现。首先给出 $a,b,c$ 的真实值，产生 100 个样本点，噪声是 0 均值的高斯噪声。在循环中生成样本点：

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<double> x_data, y_data;      // 数据

cout<<"generating data: "<<endl;
for ( int 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 ) // 表达式+噪声
    );
    cout<<x_data[i]<<" "<<y_data[i]<<endl;
}

然后构建最小二乘问题：

ceres::Problem problem;
for ( int i=0; i<N; i++ ){
    problem.AddResidualBlock (     // 向问题中添加误差项
    // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
        new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
            new CURVE_FITTING_COST ( x_data[i], y_data[i] )
        ),
        nullptr,            // 核函数，这里不使用，为空
        abc                 // 待估计参数，求导后变为优化后的值
    );
}

其中，代价函数的计算模型：

struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型参数，有3维
        T* residual ) const     // 残差
    { // 给定abc返回误差
        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数据
};

接下来配置求解器：

ceres::Solver::Options options;     // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解，这里使用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<double> time_used = chrono::duration_cast<chrono::duration<double>>( 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;

编译运行后得到结果：

在这里插入图片描述

优化后代价函数明显减小，最终收敛，得到的结果与实际设定值相差小数点后第一位，我们画出图像：

在这里插入图片描述

可见两条曲线几乎重合，估计准确。

实践：g2o

安装 g2o

安装 g2o 是标准的 cmake 安装流程，十分简单。

首先安装依赖项（如果提示无法定位，就把库名写一半然后用 tab 补全）：

sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libcholmod3.0.6

然后在 github 下载安装包，并解压到本地：

git clone https://github.com/RainerKuemmerle/g2o.git
tar -zxvf g2o-master.zip

g2o 也有 CMakeLists.txt，可直接编译：

cd g2o
mkdir build
cd build
cmake ..
make -j6 # 加速，6改为自己的CPU内核数
sudo make install

安装完毕。

图优化理论

图优化是将优化问题用图表示，其中顶点表示优化变量，边表示误差项。

在这里插入图片描述

使用 g2o 拟合曲线时，待估计的参数构成顶点，观测数据构成边。

在这里插入图片描述

其中顶点为三维向量，边为一元边，即只连接一个顶点。

编程实现

噪声的产生与 Ceres 中相同。

构建图优化，先设定 g2o。选择优化方式相似，可选择 GN, LM, DogLeg：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
//Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );

// 梯度下降方法，从GN, LM, DogLeg 中选
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(unique_ptr<Block>(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 );       // 打开调试输出

我们还要定义顶点和边，从 g2o 提供的类型派生，在里面定义顶点和边的行为：

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
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<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 bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

在图模型中增加顶点和边。共 100 个数据，对应 100 条边，只有一个待优化变量，只有一个顶点：

// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
// 添加前设id和估计值
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );

// 往图中增加边
for ( int i=0; i<N; i++ ){
    CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
    // 添加前设id和估计值
    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(100);
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;

得到结果：

在这里插入图片描述

结果与 Ceres 相差不多。

详细了解两个库还需要参考官方文档进行学习。

【视觉SLAM十四讲】非线性优化（附Ceres和g2o的安装）

本讲内容概要

状态估计问题

最大后验与最大似然

最小二乘的引出

非线性最小二乘

一阶和二阶梯度法

高斯牛顿法

裂纹伯格—马夸尔特方法

小结

实践：Ceres

安装 Ceres

实验：曲线拟合

编程实现

实践：g2o

安装 g2o

图优化理论

编程实现

猜你喜欢