视觉SLAM十四讲（第2版）总结

2021-02-14 21:30:34 阅读：204 来源： 互联网

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

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

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

一、问题的提法

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

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

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

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

1、如何构建该模型？主要涉及和的匹配问题，即那个空间点被那个姿态的相机“看到”？

2、如何求解该模型？考虑到通常是矩阵，如何求雅可比矩阵？

二、分析求解

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

在原书第13讲，作者给出的做法是：

1）初始化。检测第一帧图像的特征点并计算对应的空间点（称之为“路标”）。此时，检测到的“路标”便被第一帧对应的位姿看到。

2）匹配相邻两帧图像的特征点，并统计数量。如果匹配的数量足够多，则不增加新的特征点（也就不增加新的路标），因此观测到的路标包含于观测到的路标集合之中，具体哪个路标被看到由匹配情况确定。

3）如果匹配的数量太少，则重新计算特征点，增加相应的路标，即观测到了新增加的所有路标。

2、问题2：如何优化

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

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

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

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

通常，我们有：

则：

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

，是向量，此项符合求导规则

因此，问题的关键在与求：

引入李代数，我们有：

则：

这里，我们指出：

1）直接求导

我们将在处做一阶泰勒展开：

其中为在处的导数。根据原书6.2.2节，在一次迭代中求解：

即可求得本次迭代的最优增量。

的计算如下：

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

可见，这里计算的形式十分复杂。同时，我们强调，用这里的求得的增量可直接叠加用于计算李代数上的更新值，但：

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

我们可以考虑始终用李代数表示位姿，这实际上就相当于第一种方法（但不等同，因为才表示平移向量）。

2）扰动模型

我们令：

在已知本次迭代的初始值时，上式可看作的函数。因是小量，我们将其在 0 处一阶泰勒展开：

其中为在处的导数。的计算如下（参考第4讲）：

用这里的求得的增量不可直接叠加用于计算李代数上的更新值，即，但我们有李群上的增量：

从而李群上的更新值：

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

根据上述过程，扰动模型相当于在李群上施加了一个扰动并在李代数上求扰动结果关于的在处的导数。

三、设计框架

至此，我们知道如何建模：

以及可以求解该模型了。

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

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

考虑到实际工程中，有不同的前端、后端及回环检测和校正策略，这里仅对图优化库 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;  
}

标签：double,矩阵,v2,SLAM,视觉,g2o,十四,顶点
来源： https://blog.csdn.net/weixin_45205765/article/details/113810874

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

视觉SLAM十四讲（第2版）总结

一、问题的提法

二、分析求解

三、设计框架