视觉SLAM十四讲(第2版)总结

最近看完了《视觉SLAM十四讲(第2版):从理论到实践》(高翔等著),原书分两部分,先介绍了数学基础,然后介绍了具体的SLAM实践,非常适合零基础开始学习SLAM。

作为总结,这里并不对书中的内容做太多的重复说明,本人的水平也不足以用聊聊数字概括原书精华,因而这里采用这样一种思路:提出问题,分析求解,实践应用——来说说自己的理解。

本文从研究背景出发,引出SLAM问题的数学模型;运用数学工具分析求解模型(主要是直接求导与扰动模型);最后,对原书第13讲的代码组织进行整理,修改并注释第6讲的g2o代码以便理解和回顾。

一、问题的提法

SLAM,即同步定位与建图,顾名思义,我们需要同时估计自身的位置(定位)和环境中物体(路标)的位置(建图)。例如,在自动驾驶汽车中,传感器(摄像头,毫米波雷达,激光雷达等)测量汽车前方的道路情况和障碍物情况,构建地图(考虑到路上的行人,车辆等时刻在变化,至少得是局部地图)以及汽车在该地图中的方位,用于规划汽车的局部行驶路径以躲避障碍物,这就可以运用到SLAM技术。另外,不考虑建图时,SLAM中的视觉里程计也可以帮助我们估计汽车的运动情况。

在SLAM中,我们用“位姿”来表示“位置”,因为“位姿”不仅包括物体的空间位置还包括其朝向,通常用矩阵来表示;用空间点的集合来表示地图(点云地图,在此基础上可构建其他符合需求的地图)。典型的SLAM问题可写成如下形式的优化问题:

                          视觉SLAM十四讲(第2版)总结

其中,视觉SLAM十四讲(第2版)总结 表示位姿;视觉SLAM十四讲(第2版)总结 表示空间点坐标;视觉SLAM十四讲(第2版)总结 表示在位姿 视觉SLAM十四讲(第2版)总结 下对 视觉SLAM十四讲(第2版)总结 的观测结果,在视觉SLAM中,通常是图像像素点位置;函数 视觉SLAM十四讲(第2版)总结 表示观测方程;函数 视觉SLAM十四讲(第2版)总结 表示核函数,用于避免由于错误匹配等导致的二范数增长过快,提高系统的鲁棒性。

针对上述优化问题,我们主要需要解决以下两个问题:

1、如何构建该模型?主要涉及 视觉SLAM十四讲(第2版)总结 和 视觉SLAM十四讲(第2版)总结 的匹配问题,即那个空间点被那个姿态的相机“看到”?

2、如何求解该模型?考虑到 视觉SLAM十四讲(第2版)总结 通常是矩阵,如何求雅可比矩阵?

二、分析求解

1、问题1:如何构建模型

在原书第13讲,作者给出的做法是:

1) 初始化。检测第一帧图像的特征点并计算对应的空间点(称之为“路标”)。此时,检测到的“路标”便被第一帧对应的位姿 视觉SLAM十四讲(第2版)总结 看到。

2) 匹配相邻两帧图像的特征点,并统计数量。如果匹配的数量足够多,则不增加新的特征点(也就不增加新的路标),因此 视觉SLAM十四讲(第2版)总结 观测到的路标包含于 视觉SLAM十四讲(第2版)总结 观测到的路标集合之中,具体哪个路标被 视觉SLAM十四讲(第2版)总结 看到由匹配情况确定。

3) 如果匹配的数量太少,则重新计算特征点,增加相应的路标,即 视觉SLAM十四讲(第2版)总结 观测到了新增加的所有路标。

2、问题2:如何优化

首先指出,对于一般优化问题,待优化变量为向量,此时我们可以通过矩阵求导法则得到雅可比矩阵(相当于导数),然后运用梯度下降法或者高斯牛顿法等优化方法,求得最优解。在优化过程,关键是找到待优化变量每次迭代时的增量。(原书第6讲)

针对问题2,我们有两种选择:1)用某种向量表示位姿,这样在优化过程中便不存在对矩阵求导的问题;2)利用某种方式求位姿(变换矩阵)的增量。

第一种方法中,我们可以采用 [旋转向量;平移向量] 或者 [单位四元数;平移向量] 的形式表示位姿。此时,观测方程 视觉SLAM十四讲(第2版)总结 的形式比较复杂,也不容易写出其对待优化变量的导数(雅可比矩阵)。但是,幸运的是,求导可借助 Ceres, g2o 等优化库提供的自动求导工具,我们只需要计算 视觉SLAM十四讲(第2版)总结 即可。原书作者在第9讲《后端1》中即采用了这种做法。

第二种方法中,我们引入李群李代数,先将矩阵(李群)映射到向量(李代数),从而实现求导,然后再更新位姿。

通常,我们有:

                                                    视觉SLAM十四讲(第2版)总结

则:

                              视觉SLAM十四讲(第2版)总结

(上述公式涉及齐次坐标和非齐次坐标的转换;采用非正式记号,因为并不能对矩阵直接求导)

                                            视觉SLAM十四讲(第2版)总结  , 视觉SLAM十四讲(第2版)总结 是向量,此项符合求导规则

因此,问题的关键在与求:

                                                                   视觉SLAM十四讲(第2版)总结

引入李代数 视觉SLAM十四讲(第2版)总结 ,我们有:

                                                              视觉SLAM十四讲(第2版)总结

则:

                                                     视觉SLAM十四讲(第2版)总结

这里,我们指出:

                                            视觉SLAM十四讲(第2版)总结

1)直接求导

我们将 视觉SLAM十四讲(第2版)总结 在 视觉SLAM十四讲(第2版)总结 处做一阶泰勒展开 :

                                         视觉SLAM十四讲(第2版)总结

其中 视觉SLAM十四讲(第2版)总结 为 视觉SLAM十四讲(第2版)总结 在 视觉SLAM十四讲(第2版)总结 处的导数。根据原书6.2.2节,在一次迭代中求解:

                                                 视觉SLAM十四讲(第2版)总结

即可求得本次迭代的最优增量  视觉SLAM十四讲(第2版)总结 。

视觉SLAM十四讲(第2版)总结的计算如下:

                                            视觉SLAM十四讲(第2版)总结

根据原书第四讲《李群与李代数》,可得:

视觉SLAM十四讲(第2版)总结

可见,这里计算 视觉SLAM十四讲(第2版)总结 的形式十分复杂。同时,我们强调,用这里的 视觉SLAM十四讲(第2版)总结 求得的增量 视觉SLAM十四讲(第2版)总结 可直接叠加用于计算李代数上的更新值 视觉SLAM十四讲(第2版)总结,但:

                                                               视觉SLAM十四讲(第2版)总结

我们可以直接得到李群上的更新值:

                                                      视觉SLAM十四讲(第2版)总结

我们可以考虑始终用李代数表示位姿,这实际上就相当于第一种方法(但不等同,因为  视觉SLAM十四讲(第2版)总结  才表示平移向量)。

2)扰动模型

我们令:

                                             视觉SLAM十四讲(第2版)总结

在已知本次迭代的初始值 视觉SLAM十四讲(第2版)总结 时,上式可看作视觉SLAM十四讲(第2版)总结 的函数。因 视觉SLAM十四讲(第2版)总结 是小量,我们将其在 0 处一阶泰勒展开:

                             视觉SLAM十四讲(第2版)总结

其中 视觉SLAM十四讲(第2版)总结 为 视觉SLAM十四讲(第2版)总结 在 视觉SLAM十四讲(第2版)总结 处的导数。视觉SLAM十四讲(第2版)总结的计算如下(参考第4讲):

                                   视觉SLAM十四讲(第2版)总结

                                                视觉SLAM十四讲(第2版)总结

                                                视觉SLAM十四讲(第2版)总结

用这里的 视觉SLAM十四讲(第2版)总结求得的增量 视觉SLAM十四讲(第2版)总结 不可直接叠加用于计算李代数上的更新值,即 视觉SLAM十四讲(第2版)总结,但我们有李群上的增量:

                                                             视觉SLAM十四讲(第2版)总结

从而李群上的更新值:

                                                          视觉SLAM十四讲(第2版)总结

跟直接求导相比,扰动模型更简单,同时也方便使用矩阵来表示位姿。

根据上述过程,扰动模型相当于在李群上施加了一个扰动 视觉SLAM十四讲(第2版)总结 并在李代数上求扰动结果关于 视觉SLAM十四讲(第2版)总结 的在 视觉SLAM十四讲(第2版)总结 处的导数。

三、设计框架

至此,我们知道如何建模:

                                 视觉SLAM十四讲(第2版)总结

以及可以求解该模型了。

具体实现上,参考作者在第13讲给出的设计框架和代码,整理SLAM模块组成如下图所示。实际工程肯定比这个复杂和细致的多,这里仅作理解用。整个模块可按照接口,功能,组件,支持分为四层,将每一层包含的元素分别定义成类,编写成头文件并加以引用;上层元素类的成员通常包括下层元素类,每个元素类的方法根据算法确定。最后,作者定义了一个Config类用于链接数据。  视觉SLAM十四讲(第2版)总结

该讲的实现效果如下所示:

                    视觉SLAM十四讲(第2版)总结

考虑到实际工程中,有不同的前端、后端及回环检测和校正策略,这里仅对图优化库 g2o 的代码进行注释,以便更好地掌握该通用工具。该代码取自第6讲,但是做了修改,采用二元边而不是一元边(应用时没有必要这么做)以便注释。代码如下:

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

//****** 利用 g2o 拟合曲线方程:y = exp(ax^2+bx+c) + w, w为高斯噪声 **************

/* g2o 使用步骤:
 * 1. 自定义顶点和边:通过继承的方式;或使用库中预定义的类型
 * 2. 配置求解器
 * 3. 添加顶点和边
 * 4. 求解
*/

// 1. 通过继承 g2o::BaseVertex 和 g2o::BaseBinaryEdge 自定义边

/// 设计两个顶点类,其中 [a;b] 为一个二维向量顶点类,c 为另一个double顶点类                                        
class CurveFittingVertex1: public g2o::BaseVertex<2, Eigen::Vector2d> // 二维向量顶点类
                                             < 顶点维度,数据类型 > 
{ 
public :
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW 结构体包含eigen成员必须进行宏定义 EIGEN_MAKE_ALIGNED_OPERATOR_NEW, 保证内存对齐
  
  virtual void setToOriginImpl() { _estimate << 0, 0; } //重置

  virtual void oplusImpl(const double *update) { _estimate += Eigen::Vector2d(update); }  顶点更新函数,利用增量更新顶点
  
  virtual bool read(istream &in) {}  无读写操作,留空
  virtual bool write(ostream &out) const {}
};

class CurveFittingVertex2 : public g2o::BaseVertex<1,double>  double顶点类
{
public:
  virtual void setToOriginImpl() { _estimate = 0.0; }
  virtual void oplusImpl(const double *update) { _estimate += update[0]; }
  virtual bool read(istream &in) {}  无读写操作,留空
  virtual bool write(ostream &out) const {}
};

/// 二元边,连接二维向量顶点和double顶点
class CurveFittingEdge: public g2o::BaseBinaryEdge<1, double, CurveFittingVertex1,CurveFittingVertex2>
                                              < 观测值维度,观测值类型,连接的顶点类1,连接的顶点类2 > 
{
public :
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  CurveFittingEdge(double x) : BaseBinaryEdge(), _x(x) {} CurveFittingEdge类的构造函数
  
  void computeError() 计算边所代表的误差
  {
   const CurveFittingVertex1 *v1 = static_cast<const CurveFittingVertex1 *>( _vertices[0] );  static_cast 类型转换,把父类的_vertice 转换为子类的CurveFittingVertex
   const CurveFittingVertex2 *v2 = static_cast<const CurveFittingVertex2 *>( _vertices[1] );
    _vertices[] 为边的成员,存储顶点信息;对二元边,_vertices[] 的大小为2
   
   const Eigen::Vector2d ab = v1->estimate();  获取v1顶点的当前值
   const double c = v2->estimate();  获取v2顶点的当前值
   
  // _error(0,0) = _measurement - std::exp( ab(0,0)*_x*_x + ab(1,0)*_x + c );  也可以采用下述方式赋值
   _error << _measurement - exp(ab[0]*_x*_x + ab[1]*_x + c);  _error 的类型为 Eigen::Matrix<double, D, 1, Eigen::ColMajor>
                                                                _measurement 的类型为声明明边类型时的观测值类型,这里是 double
  }
  
  virtual void linearizeOplus() override 计算雅可比矩阵J,用于计算海塞矩阵 H=J^T*J 和 误差 b=-J^T*_error
  {
    const CurveFittingVertex1 *v1 = static_cast<const CurveFittingVertex1 *>(_vertices[0]);
    const CurveFittingVertex2 *v2 = static_cast<const CurveFittingVertex2 *>(_vertices[1]);
    
    const Eigen::Vector2d ab = v1->estimate();
    const double c = v2->estimate();
    
    double y = exp(ab[0]*_x*_x + ab[1]*_x + c);
    _jacobianOplusXi[0] = -_x*_x*y;  _error 对 ab 的导数,为 1x2 的矩阵
    _jacobianOplusXi[1] = - _x*y;
    _jacobianOplusXj[0] = -y;  _error 对 c 的导数,为 1x1 的矩阵
    
    //** J = [_jacobianOplusXi, _jacobianOplusXj]    
  }
  
  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随机数产生器
 
  vector<double> x_data, y_data;    /// 数据序列
 
  cout << "generating data: " << endl;
  for (int i=0; i<N; i++)
  {
    double x = i/100.0; // i 是int型,要除以100.0 保证结果为double
    x_data.push_back(x);
    y_data.push_back( exp(a*x*x + b*x +c) + rng.gaussian(w_sigma) );
    cout << x_data[i] << "\t" << y_data[i] << endl;
  }
  
  // 2. 配置求解器
  /// ref: https://www.jianshu.com/p/36f2eac54d2c
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<2,1>> BlockSolverType;
  /// 定义块求解器,将用于生成 H 和 b 的结构;       pose,landmark:都表示变量的维度
  /// 将生成 H 和 b 结构 :
  ///                       H = Hpp Hpl   b = bp
  ///                           Hlp Hll       bl
  BlockSolverType::LinearSolverType *linearSolverType = new g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>(); 
  ///定义线性求解器; 这里用到了 Schur 消元,参见第9讲,线性求解器的结构与 Hpp 相同,即 ::PoseMatrixType
    
  BlockSolverType *solver_ptr = new BlockSolverType(linearSolverType); //生成块求解器对象
   
  g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); /// 配置算法
  g2o::SparseOptimizer optimizer; /// 声明图模型(优化器);
  optimizer.setAlgorithm(solver); /// 把(求解器+算法)导入模型
  optimizer.setVerbose(true); /// 输出到调试
  
  // 3. 添加顶点和边
  CurveFittingVertex1 *v1 = new CurveFittingVertex1();
  v1->setEstimate( Eigen::Vector2d(0,0) ); /// 顶点赋初值
  v1->setId(0);
  optimizer.addVertex(v1); /// 添加顶点,Id为0
   
  CurveFittingVertex2 *v2 = new CurveFittingVertex2();
  v2->setEstimate(0); /// 顶点赋初值
  v2->setId(1);
  v2->setMarginalized(true); /// ! ! ! ! 这里必须要设置边缘化,因为 v2 对应 landmark, 在配置求解器时被消元了
  optimizer.addVertex(v2); /// 添加顶点,Id为1
   
  for (int i=0; i<N; i++) /// 100组数据,100条边
  {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]); ///构造CurveFittingEdge类的对象edge
    edge->setId(i);
    edge->setVertex(0,v1); /// edge连接顶点 v1 和 v2
    edge->setVertex(1,v2);
    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();
  // 4. 求解
  optimizer.initializeOptimization();
  optimizer.optimize(100); /// 最大迭代次数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::Vector2d ab_estimate = v1->estimate();
  double c_estimate = v2->estimate();
  cout << "True value of model: a = 1.0, b = 2.0, c = 1.0\n";
  cout << "Estimated model: \n" << "a_estimated = " << ab_estimate[0] 
                                << "\nb_estimated = " << ab_estimate[1]
                                << "\nc_estimated =" << c_estimate << endl;
   
  return 0;  
}

 

上一篇:【论文翻译】Bessel-Fourier moment-based robust image zero-watermarking


下一篇:sql的报错注入总结