SLAM十四讲 高翔

清华大学 2016年冬天

CH1

运动模型，地图模型。环境的鲜艳信息，地图的信息。

基于视觉的机器学习。相机只知道灰度。

书：

The Bible: Multiple View Geomery in Computer Vision

State Estimation for Robotics: A Maxtrix-Lie-Group Approach

Probabilistic Robotics

第一部分 数学基础

三维空间刚体运动 Eigen

李群和李代数 Sophus

非线性优化 Ceres g2o

第二部分 视觉SLAM

后端优化 Ceres g2o gtsam

第三部分 视觉SLAM

回环检测 DBoW3

地图构建 PCL Octomap

RGB-SLAM2  VINS-mono（香港科技大学）

How to do SLAM ?   Sensor

安装环境上：

  二维码Marker

  GPS

  轨道、磁条

机器人上：

  IMU

  激光

  相机

“地图来自定位，定位需要地图。”

各类相机的区别：

单目 无景深，通过移动相机产生深度 Moving View Stereo

双目 通过视差计算深度Stereo

RGBD 通过物理方法测量深度

计算相机运动和场景结构 Motion & Structure

前端 VO 后端 Optimization

回环检测 Loop Closing  建图 Mapping

视觉里程计 Visual Odometry

  相邻图像估计相机运动

  基本形式：通过两张图像计算节后

  不可避免 漂移

方法： 特征点法 第七讲   直接法 第八讲

SLAM的数学描述 ：

  需要把它们看成随机变量，服从概率分布。

  Xk = f(Xk-1 ,.Uk ,Wk)  上一时刻，输入，噪声

  Zk,j = h(Xk ,Yj ,Vk,j)

Linux下的C++编译IDE， KDevelop

Ch3  3D space Rigid Body Motion

刚体运动

向量外积：叉乘。

a^ 是一个向量a 的反对称矩阵。 aT = a-1

一个基本问题： 坐标系（世界）之间如何变化？

答案：两个坐标系之间的 旋转 + 平移。

其实就是转置矩阵。E1 X A1 = E2 X A2     E2-1XE1 X A1  = A2    ,把E2-1XE1 成为 R 旋转矩阵。

如果加上平移？ B = R1a + t1  C = R2 b + t2 ，所以 c = R2(R1 a + t1) + t2

用四个数字表示三维向量的做法，叫做 齐次坐标

旋转和平移放到一个矩阵中去，成为变换矩阵

实际编程中，可使用T或者T-1来描述机器人的位姿

Eigen

对比matlab需要配置类型int float double还有矩阵大小。

旋转向量、欧拉角

角轴Angle-Axis或者旋转向量Rotation Vector

罗德里格斯公式

Yaw-oitch-roll  偏航-俯仰-滚转

*幻象锁（转几圈又回起始）的原因，欧拉角实际使用中的。

四元数

一种扩展复数，一个实部加三个虚部。表示三维空间的旋转。

角轴和四元数的关系

四元数对比角轴、欧拉角的优势：紧凑、无奇异性

Ch4 李群 李代数 Lie Group and Lie Algebra

李群李代数的数理基础

三维旋转矩阵 特殊正交群 SO(3) = {R3}

三维变换矩阵 特殊欧氏群 SE（3） =

群：封闭性、结合律、幺元（单位元）、逆

李群（Lie Group）：光滑连续  流行 直观上看适合刚体 SO(3) SE(3) 良好的乘法加法。  *魔方转回自己原始的样子

*如果多个集合运算，那么需要看环论、域论。

*李群能乘法，加法后不是李群

R(t)R(t)T = I

R(t)R(t)T + R(t)R(t)T = 0

R(t)R(t)T = -(R(t)R(t)T)T

可以看出是一个反对称矩阵，

R(t)R(t)T = &（t）

R(t) = &(t) R(t)

可以看到，对R求导后，左侧多出一个&(t)

上式子看错微分方程，泰勒展开一阶：

在t0附近，可以一阶导数，位于正切空间(tangent space)上

假设t0附近，&（0）不变，可视为常数。

综上所述，对任意t，找到一个R和&()对应关系。&()是李群R对应的李代数。&()格式上式的向量转化的矩阵。

[ , ]	称为李括号（Lie Bracket）
Φ^	Φ的逆运算

三维旋转群：

[Φ1, Φ2] = (Φ1Φ2  - Φ2Φ1)^

三维变换群： （6个自由度）

ρ是平移向量，Φ是旋转向量，共6个自由度。

三维旋转、三维变换的指数映射和对数映射

R = exp(Φ^) ,Φ^ 是矩阵，对于矩阵如何定义指数运算？ — — Taylor展开

* 前提知识：对任意向量φ，等于模场乘α以方向向量θ。

Φ = αθ

*处理高阶项的方法，α^ α^   = α αT - I   

α^  α^  α^  =  - α^ 

Taylor展开：

最后的近似结果：

上文就是罗德里格斯公式。so(3)物理意义旋转向量。

反之，给定旋转矩阵，亦能求李代数：

但是，上章介绍实际应用中矩阵到向量的转换关系：

三维旋转矩阵、三维旋转向量 SO(3) so(3)的对应关系，证毕。

对三维变换矩阵 三维变换向量 SE(3) se(3) 的对应关系，同上：

* 左上角表示李代数的平移部分到矩阵的平移部分，相差一个线性变换J （雅可比）

证毕。

李代数求导与扰动模型

实际中，我们经常需要对位姿进行估计，但李群元素只有乘法，没有导数。R1 + R2 ≠ SO(3)

* 能否用李代数上的加法，定理李群元素的导数？

*能否用指数映射和对数映射，完成变换关系？

基本问题：李代数做加法，是否等价于李群上乘法？

exp(Φ1^)exp(Φ2^) = exp((Φ1 + Φ2)^) ???

不成立，因为Φ是矩阵不是标量！

完整形式有BCH(Baker-Campbell-Hausdorff)公式给出：

当A B一个很小值，忽略高阶项，BCH具有线性近似形式：

左乘雅可比

右乘雅可比

Jr(Φ) = Jl(-Φ)

所以李群发生变化，看成近似李代数的变化：

对三维旋转：

直观写法，在李群上左乘小量，李代数上的加法相差左雅可比的逆：

反之：

对三维变换：

*六维讲解比较麻烦，不描述

考虑一个基本问题：旋转后的点关于旋转SO(3)的导数

由于R没有加法，导数无从定义。存在两种可能解决方法：

*对R对应的李代数加小量，求相对小量的变化率（导数模型）

*对R左乘或右乘一个小量，求相对小量的李代数的变化率（扰动模型）

导数模型

第二步BCH 第三步近似泰勒展开

-(Rp)^ JL

扰动模型

扰动模型（左乘小量），另其李代数为零。

-(Rp)^

最终简洁、实用。

*对三维变换矩阵SE(3)的扰动模型：

小结：

• 利用BCH线性近似，推到SO(3) SE(3)上的导数和扰动模型
• 扰动模型比导数模型更使用、效率高

Sophus工具库

开源的C++线性代数库，提供快速的线性代数计算，包括解方程。Eigen库提供集合模块，没有提供李代数的支持。Sophus是较好的李群和李代数库。SO(3) so(3) SE(3) se(3)。

Sophus是 Eigen基础上开发的，继承Eigen库的各个类。

Eigen::Matrix3d	Sophus::Matrix3d
Eigen::Vector3d	Sophus::Vector3d

SO3构造方法

SO3()

SO3(const SO3 & other)

explicit SO3 (const Matrix3d & _R);

explicit SO3 (const Quaterniond & unit_quaternion);

SO3 (double rot_x, double rot_y, double rot_z);

SE3构造方法

SE3 ();

SE3 (const SO3 & so3,const Vector3d & translation);

SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);

SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);

SE3 (const SE3 & other);

Sophus库的安装和使用教程_Dongdong Bai的博客-CSDN博客_sophus库

CH5 Cameras and Images

Zk,j = h(yi, xk ,vk,j) 这个观测方程，看下单目相机。

单目相机的模型、内参、径向畸变

一个空间点投影到相机成像平面

矩阵形式：

传统习惯：

中间矩阵称为内参数，右侧是非齐次坐标。相机出场后确定。

除内参外，相机坐标系与世界坐标系还差一个变换：

R t 或 T 称为外参。外参是SLAM的估计目标。

镜头的畸变：

广角镜头，鱼眼镜头。

桶形失真     枕形失真

畸变可以用归一化坐标的变换来描述

小结：

• 世界——相机——归一化平面——像素
• 世界坐标Pw
• 相机运动R t 或变化矩阵T∈SE(3) ， Pc = RPw + t
• Pc 仍有 X Y Z 三个量，投影到归一化平面Z = 1 Pc = [ X/Z , Y/Z ,1 ]T
• P的归一化坐标，经过内参后，对应像素坐标 Pu,v = K Pc

双目相机

严格水平放置。

双目相机的基线。d称为视差（disparity）。虽然距离公式简单，但是d不容易计算。

RGB-D相机

ToF（飞时）或者结构光两种主要原理。

结构光受室外阳光、同机型的干扰。

图像*

OpenCv的图像存储与表达方式*

通道。RGB、BGR、RBGA

Image.clone()深拷贝

基本的相机标定方法*

项目相机示例pose.txt

-0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042

-0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662

-0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536

-1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178

-1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741

X Y Z 四元数

生成点云：

pcd文件格式。

CH6 Non-Linear Optimization

运动方程 Xk = f(Xk-1 , uk ) + wk

观测方程 Zk,j = h(yj , xk ) + vk,j

噪声 wk vk,j

• 最简单：线性系统，高斯噪声
• 复杂情况：非线性系统，非高斯噪声

最小二乘法的含义和处理方式

历史上用滤波器（马尔科夫性：下一个状态仅依赖上一个状态）；

近年非线性优化成主流；

贝叶斯法则：

P(z|x)叫似然估计  P(x) 叫先验

最大后验估计MAP

X*MAP = arg max P(z|x) P(x)

最大似然估计MLE

X*MLE = arg max P(z|x)

现在要求x，y的最大似然估计，怎么求？

x* = arg min( (zk,j - h(xk , yj))T Q-1k,j(Zk,j - h(xk , yj)) )

这个就是最小二乘法。

已经把状态最大似然估计，变成最小二乘问题

定义误差：

最小化误差的二范数：

直观解释：由于噪声存在，估计轨迹带入地图的SLAM的运动、观测方程，需要调整状态的估计，使误差最小。

问题结构：

• 许多误差平方和（或sigma函数和）
• 总体维度高，单项简单，只关联2个变量
• 如果用李代数处李，那么是无约束问题

非线性最小二乘

求任意函数 min1/2 ||f(x)||22

找极值点。df/dx = 0

实际应用上使用 迭代 方式求解

1. X0
2. 第k次，寻找增量ΔXk，使得||f(xk + Δxk)||22  达到极小值
3. 如果 Δxk足够小，则停止
4. 否则，令xk+1 = xk + Δxk ，返回2

问题：如何确定增量Δx？

答案：即梯度下降策略，一阶或二阶

理解Gauss-Newton Levenburg-Marquadt下降策略

Gauss-Newton 下降

增量方向：

称为最速下降法（Steepest Method）一阶

若保留二阶项：

则 HΔx = -JT

该方法称为牛顿法。（Newton Method）二阶

对G-N的近似H ： J（xk）雅可比公式的值

缺点：

• 最速会遇到zigzag贪婪问题
• 牛顿法迭代次数少，需要复杂的Hessian矩阵计算

如何规避计算？ 答案 Levenburg-Marquadt下降。

 Levenburg-Marquadt下降

L-M保证H不可逆（非正定）时，也能使用。L-M属于信赖区域方法（Trust Region）

• 近似程度描述 ρ = f(x + Δx) - f(x)  / J(x) Δx
• 若ρ太小，则减小近似范围
• 若ρ太大，则增大近似范围

改进版本G-N：

Trust Region内优化，利用Lgrange乘子转换无约束：

在Levenberg方法中，则D = I则：

(H  + λI) Δx = g

L-M相对G-N，能保证增量方程的正定性。人为近似只在一定范围内成立，如果近似不好则缩小范围。

λ控制一阶二阶间的权重。

实际中，简单情况下用G-N高斯牛顿，复杂情况下用L-M列文伯格。

小结

• 非线性优化是很大的主题，研究者奋斗多年
• 主要方法：最速下降，牛顿，G-N，L-M等
• 与线性规划不同，非线性需要（凹凸，几凹几凸）具体分析
• 跟初始值影响很大，否则陷入局部最优解

学习Ceres库和g2o库的基本使用方法

谷歌的Ceres、SLAM常用的g2o，两个最小二乘的求解的库。

实践Ceres

// 构建最小二乘问题

ceres::Problem problem;

// 代价函数的计算模型

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     // 残差

    {

        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数据

};

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                 // 待估计参数

        );

    }

    // 配置求解器

    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 );  // 开始优化

slambook/main.cpp at master · gaoxiang12/slambook (github.com)

实践g2o

以图模型表达上述最小二乘问题。chrono是辅助时间库。

// 构建图优化，先设定g2o

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器

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; i<N; i++ )

{

CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );

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 );

CH7 Visual Odometry(1)

SLAM正式内容的第一部分。

理解图像特征点，单幅图像提取特征点

路标Landmark 位姿Pose

路标是三维空间中固定不变的点，能在特定位姿下观测到。

• 数量多
• 良好的区分性

特征点（Descripter）：可重复性 可区别性 高效 本地

例子SIFT SURF ORB ： OpenCv feature2d模块

例子 ORB特征检测：Oriented FAST，如下

FAST: 连续N个点的灰度有明显差异 ，1ms内

Oriented FAST：计算旋转 （算灰度的重心、指向角度）

BRIEF （二进制描述制）

BRIEF-128：

附近128次像素比较，是大，还是小？

BRIEF是一种二进制描述，需要用汉明距离度量

特征匹配：

• 描述子Descriptor的差异
• 暴力
• OpenCv加速：快速近邻FLANN

slambook/feature_extraction.cpp at master · gaoxiang12/slambook (github.com)

理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运动

特征点之间的对应关系

• 两个单目图像，2D-2D之间的关系  对极几何
• 帧和地图，3D-2D之间的关系     PnP
• RBGD图，3D-3D之间的关系  ICP

对极几何

两个参考系：

 S1p1 = KP  s2p2 = K(RP + t)

归一化：

X1 = K-1 p1  x2 = K-1 p2

齐次关系：

X2 = Rx1 + t

两边左乘：

t^ x2 = t^ Rx1

再一步左乘：XT2 t^ x2 = xT2 t^ Rx1

对极约束：xT2 t^ Rx1 = 0

另一个形式对极约束：

定义：E= t^R  F = K-TEK-1

Essential矩阵  Fundamental矩阵

xT2 E x1 = pT2 F p1 = 0

两步计算位姿：

• 由X1 X2 计算E
• 由E恢复R t

八点法求E：

E有五个自由度，最少需要5个点计算R t

将E看作3X3的矩阵，去掉因子后剩八个自由度

向量形式：

八对点构成方程组：

奇异值分解，从E计算R,t :

四个可能的解，但只有一个深度为正。

*当特征点位于同一平面，可使用homegrahpy矩阵。

八点法的讨论：

• 用于单目SLAM的初始化（首次除外，可用PnP）
• 尺度不确定性：E乘任意倍数，也满足对极约束
• 纯旋转问题：t = 0
• 多于八对点时：最小二乘或RANSAC

实践：对极约束求解相机运动

slambook/pose_estimation_2d2d.cpp at master · gaoxiang12/slambook (github.com)

三角法*

一口气解说，没力气继续解说。看书《SLAM十四讲》。

三角测量的实践： slambook/triangulation.cpp at master · gaoxiang12/slambook (github.com)

CH7

理解PNP问题，利用已知三维结构与图像的对应关系，求解摄像机的三维运动

已知3D的世界坐标，已知在平面坐标。

需要怎么估计Pose？需要几个点？怎么解旋转和平移？

• 代数的解法/优化的解法 （鲁棒性差）
• 代数的 DLT（直接线性变换） P3P（三个点去解） EPnP UPnP（变种） （需要良好的初始点）
• 优化的Bundle Adjustment

DLT（直接线性变换）

空间点 P = （X ，Y ，Z ，1）T

投影点 x = （u，v ，1） 归一化坐标

投影关系 sx = [R | t]p

展开

看成关于t的线性方程，求解t。

用最下行消除前两行的s，一个特工点提供两个方程。

为求解12个未知数，需要12/2 = 6对点。

DLT将R t 看成独立的未知量，所以求出结果后，需要将t组成的矩阵投影回SO(3) （通常QR分解）

或者将内参矩阵K带入，但是SLAM一般假设K已知，所以这里步骤没有代入

P3P（利用3对点）

利用三对点求相机外参

利用三个相似角度。

余弦定理：

关于 x = OA/OC y = OB/OC 的二元方程

得到x y 后，利用

解得v，从而解得OC得长度，进而得到各点的距离

缺点：

• 对三个点以上的情况难处理
• 误匹配时算法失效

Bundle Adjustment 捆集调整（PnP的优化解法）

3D-2D 。目前不知相机旋转和平移，也就是求外参？R t

最小化重投影误差 Minimizing a reprojection error

投影关系

定义重投影误差并取最小化

线性化和雅可比

考虑单个投影点误差

线性化

雅可比的形式？ 基于扰动模型

……

对3D点求导：

思路：

误差最小—扰动模型—极值点—导数|微分—简化运算量

实践 pose estimation 3D-2D ：

slambook/pose_estimation_3d2d.cpp at master · gaoxiang12/slambook (github.com)

OpenCv   solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法

理解ICP问题，利用点云的匹配关系，求解摄像机的三维运动

3D-3D ICP

用RBGD相机，拿到两组待匹配的3D点。求这两组点之间的平移和旋转。

可用迭代最近点（Iterative Closest Point）ICP 求解。

误差ei

可以利用g2o构建非线性最小二乘法。

定位质心：

改写目标函数：化简上式最小二成问题的公式，利用正交

先求出R使得左边最小，然后取t，使得最后一项为零

旋转R的求取：

定义去质心坐标

最小化

推导：

最右一项：

SVD解法 （奇异值分解 Singualar Value Decomposition）

在机器学习中广泛应用的算法，降维算法的特征值分解，推荐系统、自然语言处理。机器学习的基石。

NLP中的潜在语义索引（LSI）。

SVD也可用于PCA（主成信息）降维，来做数据压缩和去噪。

Scikit-learn的PCA算法背后真正的实现就是SVD，而不是暴力。且只用右奇异矩阵。

对最右一项SVD解法：

ICP也可以用非线性优化角度，但：

• 已知匹配时，ICP问题存在唯一解或无穷多解。只有唯一解才是全局最优解。
• 所以正常情况下，SVD结果和优化一样，且优化很快收敛

注：

• 在激光情况下，匹配点未知，将指定最近点为匹配点。此时问题非凸，极小值不一定为最小值。
• 利用非线性优化将ICP于PnP结合一起。

实践ICP：

slambook/pose_estimation_3d3d.cpp at master · gaoxiang12/slambook (github.com)

理解如何通过三角化，获得二位图像的对应点的三维结构 *

小结

介绍特征点相关的视觉里程计VO部分算法：

• 特征点如何提取并匹配
• 如何通过2D-2D的特征点估计相机运动
• 三角化原理
• 3D-2D的PnP问题，线性解法和BA解法
• 3D-3D的ICP问题，线性解法与BA解法

【原创】优秀开源代码盘点：稀疏视觉SLAM/SfM_哔哩哔哩_bilibili

盘点稀疏视觉SLAM

• ENFT-SFM

2015浙大

延长特征点的鲁棒性 消除误差、飘逸

• ORB-SLAM2

西班牙

同时支持单目、双目、RBGD

代码可读性强，二次开发最多，榜一

• OpenVSLAM

2019 日本

兼容相机 鱼眼 球面

Web跨平台查看

• TagSLAM

2019 美国宾夕法尼亚

支持任意数量的相机

CH8 Visual Odometry(2)

回忆特征点法VO：

1. 在图像提取特征点计算特征描述 非常耗时 10mins
2. 在不同图像寻找特征匹配
3. 利用匹配点信息计算相机位姿

不提取特征点计算VO的思路

1. 通过其他方式寻找 光流
2. 无匹配点 其他

理解光流法

像素的流动。

稀疏光以Lucas-Kanade （LK）光流为代表

稠密以Horn-Schunck（HS）光流为代表

本质上是估计像素在不同时刻的运动

LK稀疏光流

灰度不变假设 ： I(x + dx , y + dy , t + dt) ≠= I(x ,y ,t)

Taylor展开一阶

假定一个窗口（W x W）内光度不变

实践LK光流

slambook/useLK.cpp at master · gaoxiang12/slambook (github.com)

理解直接点法原理

增加考虑相机的旋转缩放。

两个Frame的投影关系：

最小光度误差：

待估计的量为相机运动，我们关系误差相对相机的导数：

相机位姿是李代数，所以要一个扰动模型

根据使用图像信息不同，可分为：

• 稀疏直接法  只处理稀疏角点或关键点
• 稠密直接法  使用所有像素
• 半稠密直接发  使用部分梯度明显的像素

RGBD-SLAM的直接光实践：

算六自由度的相机位姿

slambook/direct_sparse.cpp at master · gaoxiang12/slambook (github.com)

CH10 后端（1）

理解EKF为代表的滤波器原理

后端Backend

• 从带噪声的数据估计内在状态  状态估计问题
• Estimated the inner state from noisy data

渐进式Incremental

• 保持当前状态的估计，在加入新信息时，更新已有的估计（滤波）

批量式Batch

• 给定一定规模的数据，计算该数据下的最优估计（优化）

不确定性的直观描述。

Bayes展开：

 在非线性模型、高斯状态分布下，可以在工作点附近线性展开，得到扩展卡尔曼滤波

卡尔曼滤波在高斯白噪声下。

*

理解非线性优化的后端工作原理

Bundle Adjustment 和图优化

使用g2o和Ceres实际操作后端优化

【CAIRDC2020】高翔 - 《SLAM在自动驾驶中的应用和难点》

1. 地图（工业地图 开发地图 GPS地图）
2. 路线路标实践

来自 <https://www.bilibili.com/medialist/play/watchlater/BV1i54y1v7jt>

VIO主流框架&VINS推导解析

课程内容：

1. 分析BA及其与因子图的关系；

2. 分析主流开源VIO（MSCKF、ROVIO、VINS、VI-ORB、ICEBA）框架；

3. 推导VINS预积分和滑窗优化公式及与代码的对应关系。

来自 <https://www.bilibili.com/medialist/play/watchlater/BV1gb411a7bh>