SLAM 面试题汇总

本文总结面试SLAM中可能遇到的各种问题，共分为十一个章节。

零 数学基础

一 李群和李代数

1. 李群和李代数的关系

2. 雅克比求导

3. 双线性差值如何去做，写公式

4. 为什么要引入李群李代数

旋转矩阵自身是带有约束的，正交且行列式为1，他们作为优化变量时，会引入额外的约束，导致优化变的困难，通过李群李代数的转换关系，把位姿估计变成无约束的优化问题。优化过程求极值，由求导数的定义式，可以知道矩阵对于加法不封闭，对于乘法封闭；我们可以看错把李群就是变换矩阵的集合，但是用李代数的形式去表示变换矩阵，与李群的变换对应关系恰好是与指数相乘的关系，用求导的定义式子就可以求解优化问题了。

• 在SLAM 中，除了要对三维世界中刚体运动进行旋转表示之外，还要对它们进行估计和优化。在SLAM 中位姿往往是未知的，而我们需要解决在什么样的相机位姿下最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的 $R,t$ 使得误差最小化。

• 而旋转矩阵自身是带有约束的（正交且行列式为1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群–李代数间的转换关系，我们可以把位姿估计变成无约束的优化问题，简化求解方式。

二 刚体运动的表示

1. 说一下3D空间的位姿如何去表达?

描述3D空间位姿态需要描述旋转和平移；

旋转可用：旋转向量（即李代数）、欧拉角、四元数、RPY表示；

平移可以：向量描述

结合起来可以李代数矩阵描述。

2. 机器人学中表示旋转的方式有哪些？区别是什么？

表示旋转的方式有4种：旋转矩阵、旋转向量、欧拉角、四元数；

区别

旋转矩阵：用9个变量描述3个自由度，具有冗余，不够紧凑

旋转向量：3维，使用向量描述旋转，向量的方向与旋转轴一致、向量长度等于旋转角，即李代数。

欧拉角：3维，比较直观，将一个旋转拆分成了绕3个坐标轴的旋转，如RPY指横滚、俯仰、偏航角度，指绕固定轴XYZ、绕旋转之后的轴：ZYX。

四元数：4维，旋转向量与欧拉角虽然紧凑但具有奇异性，四元数紧凑、没有奇异性；使用1个实部、3个虚部表示。

3. 四元数

假设某个旋转是绕单位向量$n = [n_x; n_y; n_z]^T $进行了角度为$\theta$ 的旋转，那么这个旋转的四元数形式为：

$$

$$

三 坐标系变换

世界坐标系（world） 相机坐标（camera） 像素坐标（pixel）

• world —— camera：$P_C=T_{cw}∗P_w$
• camera —— world： $P_w=T_{cw}^{−1}P_c$
• pixel —— camera：$u=f_x * X_c / Z_c+C_x ; \quad v=f_y * Y_c / Z_c+C_y$
• camera —— pixel：$X_{c}=(u-C_x) * Z_c / f_x ; \quad Y c=(v-C y) * Z_c / f_y$

一 相机模型

零 图像

1. 图像遍历

1. 一个H高W宽的图像或者matrix，如何去访问每一个元素，先访问行还是列？

提示：跟缓存还有关系~

矩阵在存储中需要区别是行优先还是列优先，Eigen中默认为列优先，因此需要按列访问。这样访问连续的存储能够加快矩阵的访问速度。

#include <Eigen/Dense>
Matrix<double, 3, 3> A;               // Fixed rows and cols. Same as Matrix3d.
Matrix<double, 3, Dynamic> B;         // Fixed rows, dynamic cols.
Matrix<double, 3, 3, RowMajor> E;     // Row major; default is column-major.

图像在opencv中一般是按行存储；一般来说图像行与行之间往往存储是不连续的，但是有些图像可以是连续的，Mat提供了一个检测图像是否连续的函数isContinuous()。当图像连通时，我们就可以把图像完全展开，看成是一行。

2. 图像处理

1、直方图均衡化

直方图均衡化（Histogram equalization）就是一种常用的灰度变换方法。

目的：通过拉伸像素强度分布范围来增强图像的对比度

操作：直接以图像的像素操作为基础，主要分为灰度变换和空域滤波两大类，

原理

• 直方图是图像中像素强度分布的图形表达方式.
• 它统计了每一个强度值所具有的像素个数.

应用：VINS中，如果图片太亮或者太暗，就要进行直方图的均衡化处理。

//如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
if (EQUALIZE)
{
    //自适应直方图均衡
    //createCLAHE(double clipLimit, Size tileGridSize)
    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
    TicToc t_c;
    clahe->apply(_img, img);
    ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}

2、图像滤波方法

在线：图像滤波方法

线性滤波方法：方框滤波(box filter) 、均值滤波(median filter)、 高斯滤波（Gaussian 滤波）

方框滤波

1. 盒式滤波（方框滤波）是一种线性滤波技术，它的实现借鉴了积分图像的原理思想，在快速积分图像求解中，将计算某个矩阵像素间的和值运算，转化为求矩阵对应边角点的求和差值运算。
2. 盒式滤波的实现最关键的步骤就是初始化数组S，数组S的每个值是存放像素邻域内的像素和值，在求解某矩形块中的像素和时，只需要索引对应区域的位置存放的和值就可以完成计算

均值滤波

1. 均值滤波，是最简单的一种线性滤波操作，输出图像的每一个像素是核窗口内输入图像对应像素的像素的平均值( 所有像素加权系数相等)，其实说白了它就是归一化后的方框滤波。
2. 均值滤波算法比较简单，计算速度快，但是均值滤波本身存在着固有的缺陷，即它不能很好地保护图像细节，在图像去噪的同时，也破坏了图像的细节部分，从而使图像变得模糊，不能很好地去除噪声点。但均值滤波对周期性的干扰噪声有很好的抑制作用

高斯滤波

1. 对高斯函数进行离散化，以离散点上的高斯函数值为权值，对我们采集到的灰度矩阵的每个像素点做一定范围邻域内的加权平均，即可有效消除高斯噪声

2. 高斯平滑滤波器对于抑制服从正态分布的噪声非常有效

3. opencv函数

   void GaussianBlur(InputArray src,OutputArray dst, Size ksize,
   					double sigmaX, double sigmaY = 0,
   					int borderType = BORDER_DEFAULT);

非线性滤波：中值滤波、双边滤波

中值滤波

1. 中值滤波法是一种非线性平滑技术，将图像的每个像素用邻域 (以当前像素为中心的正方形区域)像素的中值代替 ，常用于消除图像中的椒盐噪声

2. opencv函数

   void medianBlur( InputArray src, OutputArray dst, int ksize );

双边滤波

1. 双边滤波的改进就在于在采样时不仅考虑像素在空间距离上的关系，同时加入了像素间的相似程度考虑。双边滤波器比高斯滤波多了一个高斯方差sigma－d，它是基于空间分布的高斯滤波函数，所以在边缘附近，离的较远的像素不会太多影响到边缘上的像素值，这样就保证了边缘附近像素值的保存。但是由于保存了过多的高频信息，对于彩色图像里的高频噪声，双边滤波器不能够干净的滤掉，只能够对于低频信息进行较好的滤波。对于脉冲噪声，双边滤波会把它当成边缘从而不能去除。

3. 常用边缘检测算子和优缺点

边缘检测一般分为三步，分别是滤波、增强、检测。

基本原理都是用高斯滤波器进行去噪，之后在用卷积内核寻找像素梯度。常用的三种算法：canny算子，sobel算子，laplacian算子。

算子	方法
scanny 算子	一种完善的边缘检测算法，抗噪能力强，用高斯滤波平滑图像，用一阶偏导的有限差分计算梯度的幅值和方向， 对梯度幅值进行非极大值抑制，采用双阈值检测和连接边缘。
sobel 算子	一阶导数算子，引入局部平均运算，对噪声具有平滑作用，抗噪声能力强，计算量较大， 但定位精度不高，得到的边缘比较粗，适用于精度要求不高的场合。
laplacian 算子	二阶微分算子，具有旋转不变性，容易受噪声影响，不能检测边缘的方向， 一般不直接用于检测边缘，而是判断明暗变化。

一 相机

1 单目相机

常用型号：有非常多的种类可以选择	单目相机
1、应用最广，成本可以做到非常低。 2、体积小，标定简单，硬件搭建也简单。 3、可以用于室内和室外（有适当光照条件下）。	优点
1、具有纯视觉传感器的通病：在光照变化较大，纹理特征缺失、快速运动导致模糊的情况下无法使用（睁眼瞎）。 2、SLAM过程使用单目相机有尺度不确定性，需要专门初始化。 3、必须通过运动才能估计深度（帧间匹配三角化）	缺点

2 双目相机

双目相机	常用型号：Indemind，小觅，ZED等
优点	1、相比于单目，在静止时就能够根据左右相机视差图计算深度。 2、可测量距离可以根据基线调节。基线距离越大，测量距离越远。 3、可以用于室内和室外（有适当光照条件下）。
缺点	1、双目相机标定相对复杂 2、用视差计算深度比较消耗资源 3、具有纯视觉传感器的通病：在光照变化较大，纹理特征缺失、快速运动导致模糊的情况下无法使用（睁眼瞎）。

3 RGB-D相机

RGBD相机	常用型号：Kinect系列、Realsense系列、Orbbec、Pico等
优点	1、使用物理测距方法测量深度，所以避免了纯视觉传感器的通病， 在没有光照的情况下、快速运动的情况下都可以测距。这是非常大的优势。 2、相对双目，输出帧率较高，更适合运动场景。 3、输出深度值比较准，结合RGB信息，容易实现手势识别、人体姿态估计等应用。
缺点	1、测量范围窄，易受日光干扰，通常只能用于室内场景 2、在遇到透射材料、反光表面、黑色物体情况下表现不好，造成深度图缺失 3、通常分辨率无法做到很高，目前主流分辨率VGA（640x480） 4、标定比较复杂。

A. RGB-D相机我们知道可以直接输出 RGB + depth两张图比如我们常见的Kinect 是结构光原理，包括一个彩色相机，一个红外发射器，一个红外接收器。另外，Intel的Realsense系列RGB-D相机也非常常用，比如下面Realsense D415，官网说是Active IR stereo，也就是双目深度相机，这个双目和我们平时说的双目有何不同？为什么有如下四个孔？

4. 相机对比

相机	特点
单目	成本低，搭建简单，单目相机有尺度不确定性，需要专门初始化。
双目	不需要专门初始化，能够计算深度，基线距离越大，测量距离越远，可以用于室内和室外，标定较为复杂，视差计算比较消耗资源。
深度	测量范围窄，噪声大，易受日光干扰，无法测量透射材料，主要用于室内。

1 全局快门相机与卷帘快门相机的异同

快门
定义	快门是照相机用来控制感光片有效曝光时间的机构，是照相机的一个重要组成部分，它的结构、形式及功能是衡量照相机档次的一个重要因素。一般而言快门的时间范围越大越好。秒数低适合拍运动中的物体，某款相机就强调快门最快能到1/16000秒，可轻松抓住急速移动的目标。不过当你要拍的是夜晚的车水马龙，快门时间就要拉长，常见照片中丝绢般的水流效果也要用慢速快门才能拍出来
速度	快门速度单位是”秒”。常见的快门速度有:1 1/2 1/4 1/8 1/15 1/30 1/60 1/125 1/250 1/500 1/1000 1/2000等。相邻两级的快门速度的曝光量相差一倍。如1/60秒比1/125秒的曝光量多一倍，即1/60秒比1/125秒速度慢一级或称低一级。
全局快门（global shutter）	全局快门是通过整幅图片在同一时间曝光实现的。传感器的所有像素点同时收集光线，同时曝光。当预设的曝光时间到了，传感器停止收集光线，并将曝光图像转成电子图像。在这个过程中，并没有实际意义上的快门存在。在曝光开始的时候，传感器开始收集光线，在曝光结束的时候，光线收集电路被切断，传感器读出值即为一幅图片。
卷帘快门（rolling shutter）	卷帘快门通过控制芯片逐行曝光。卷帘快门也没有实际意义上的快门，而是通过断电控制传感器，使其不同部分在不同时间下对光的敏感度不同，逐行进行曝光，直至所有像素点都被曝光。所有的动作在很短的时间内完成。一般情况为1/48至1/60秒。

卷帘快门 rolling shutter	全局快门 global shutter
rolling shutter是逐行顺序曝光，所以不适合运动物体的拍摄，假如物体或摄像头在拍摄期间处于快速运动状态，拍摄结果就可能出现“倾斜”、“摇摆不定”或“部分曝光”等任意一种情况。	global shutter所有像素在同一时刻曝光，类似于将运动物体冻结了，所以适合拍摄快速运动的物体。但是global shutter可能出现像糊现象。像糊现象出现与否取决于曝光时间的长短，假如曝光时间过长，且物体运动快则会出现像糊；假如曝光时间很短，类似于运动物体在瞬间被冻结了，则少有像糊。

二 相机投影模型

1 单目相机

1. 单目相机的投影模型。

$$
\begin{aligned}
\left[
\begin{array}{ccc}
u\v\1
\end{array}
\right]
=
\frac{1}{Z}
\left[
\begin{array}{ccc}
\frac{1}{f_x}&0&cx\
0 & \frac{1}{f_y}& cy\
0 & 0 & 1
\end{array}
\right]
=
\left[
\begin{array}{ccc}
X\Y\Z
\end{array}
\right]
=KP
\end{aligned}
$$

2 相机内参的物理意义

答：相机内参包括焦距fx，fy，cx，cy，径向畸变系数k1,k2,k3，切向畸变系数p1,p2。其中内参一般来说是不会改变，但是当使用可变焦距镜头时每次改变焦距需要重新标定内参。

内参参数	符号	意义	单位
	fx，fy	fx = a f；fy=beta f	像素
焦距(Focal Length)	f	投影中心（光心）到物理成像平面的距离	米
主点(Principal Point )	cx，cy	主光轴在物理成像平面上的角点	像素
径向畸变( Radial Distortion )	k1,k2	由镜片形状不规则引起的畸变
切向畸变(Tangential Distortion )	q1,q2	由镜片不完全与成像平面平行引起的畸变
分辨率(Resolution)	rows,cols	像素平面图像的精密度	像素

相机外参分为旋转矩阵R和平移矩阵t，旋转矩阵和平移矩阵共同描述了如何把点从世界坐标系转换到摄像机坐标系。

3 单目相机，F和H矩阵有何不同，E和F矩阵有何不同，只旋转不平移能不能求F，只旋转不平移能不能求H

仿射变换、透视变换、欧式变换有什么区别

a) 仿射变换：形状会改变，但直线的平行关系不变，如矩形变成平行四边形。是透视变换的特殊形式。
b) 透视变换（或称射影变换）：是仿射变换更一般的形式，是共面点投影的变换关系，如单应性矩阵。平行的直线变换前后可能不会保持平行。
c) 欧式变换（或称等距变换）：旋转、平移；

2 双目相机

1. 我们知道双目相机两个相机光心的间距我们 称之为 baseline。如果双目相机baseline比较大，我们称之为wide baseline.现在某代码中使用一个单目相机进行SLAM过程，在特征匹配时资料中提到了wide baseline，请问这个wide baseline怎么理解？

3 RGBD相机

1. RGB-D的SLAM和RGB的SLAM有什么区别？

a) RGBD-SLAM与RGB-SLAM使用的相机不同，前者可读出深度图像和彩色图像、后者只能读出彩色图像（单目或双目）；

b) 传感器数据不同，主要造成前端视觉里程计很多不同，如RGBD-SLAM不用初始化、计算3D点云方式不同、可以使用ICP直接计算相机位姿，

参考

三 相机畸变模型

径向畸变是由透镜形状引起的畸变（Distortion，也叫失真）。切向畸变在相机的组装过程中由于不能使透镜和成像面严格平行引入的畸变。

通常使用多项式关系描述畸变，即：
$$
x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)\
y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)
$$
对于切向畸变，可以使用p_1,p_2进行纠正：
$$
x_{distorted} = x+2p_1xy+p_2(r^2+2x^2)\
y_{distorted} = y+p_1(r^2+2y^2)+2p_2xy
$$

1. 如何找到点在像素平面上的正确位置

1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为 [x, y]T。

2. 对归一化平面上的点计算径向畸变和切向畸变。
   $$
   x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\
   y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy
   $$

3. 将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

$$
u = f_xx_{distorted}+c_x\
v = f_yy_{distorted}+c_y
$$

2. 如果把一张图像去畸变，写公式，流程。（如何进行去畸变处理）

去畸变过程主要包括以下步骤：

1. 将图像的像素坐标系通过内参矩阵转换到相机归一化坐标系
   $$
   x = (u-c_x)/f_x\
   y = (v-c_y)/f_y
   $$

2. 在相机坐标系下进行去畸变操作
   $$
   r = \sqrt{x^2+y^2}\
   x’ = x(1+k_1r^2+k_2r^4)+2p_1x*y+p_2(r^2+2x^2)\
   y’ = y(1+k_1r^2+k_2r^4)+2p_2xy+p_1(r^2+2*y^2)\
   $$

3. 去畸变操作结束后，将相机坐标系重新转换到图像像素坐标系
   $$
   u’=x’f_x+c_x\
   v’=y’f_y+c_y
   $$

4. 用源图像的像素值对新图像的像素点进行插值

4. 其它问题

1. 如果一部相机的分辨率变为原来的两倍而其他地方不变，那么他的内参会如何变化？

相机内参fx，fy不会变化，cx，cy变为原来的两倍。

2. 相机的内参有 fx, fy, cx, cy, 畸变参数(只考虑k1, k2)，相对世界坐标原点外参T。如果我们现在对相机拍摄的图片进行2倍的下采样，那么这些参数会如何变化？

与上边问题相同，fx，fy将不会变化，cx,cy变为原来的两倍

k1变为原来的1/2，k2变为1/16；

四 相机标定

1. 相机如何标定

针孔相机一般是通过棋盘格进行标定

• 检测每张图片中的棋盘图案的角点；
• 通过使用线性最小二乘法估算相机投影矩阵P；
• 根据P矩阵就解内参矩阵K和外参矩阵R，t;
• 通过非线性优化，提高K，R，t矩阵的精度。

2. RGB-D相机是如何标定的？需要标定哪些参数

参考链接：https://www.cnblogs.com/cv-pr/p/5769617.html

机器视觉中，3D相机产生的深度图像（depth image）通常需要配准（registration），以生成配准深度图像（registed depth image）。实际上配准的目的就是想让深度图和彩色图重合在一起，即是将深度图像的图像坐标系转换到彩色图像的图像坐标系下。

已知彩色图像的像素表示为$=(uR,vR,zR)⊤$，$uR,vR,zR$分别表示彩色图像的横坐标，纵坐标和相机坐标系下的深度值（z方向上的值，非两点的距离）；同样地，深度图像的像素为$(uL,vL,zL)⊤$，$uL,vL,zL$分别表示深度图像的横坐标，纵坐标和相机坐标系下的深度值（z方向上的值，非两点的距离）。注意为了方便表示，本文中下标的R,L分别表示Right,Left的意思。那么深度图配准到彩色图的过程就是找到如下公式中的变换矩阵W′：
$$
\begin{bmatrix} u_{R}\ v_{R}\ 1 \end{bmatrix} =W^{‘} \begin{bmatrix} u_{L}\ v_{L}\ 1 \end{bmatrix}
$$

3. 如果一个电线杆上有两个摄像头，给定内参的情况下，如果标定外参？

1. 利用摄像头做两个纯视觉的Sfm，然后进行一个对齐就可以得出；
2. 利用标定板得到

二 视觉前端

零 特征点

特征点由关键点和描述子两部分构成。

我们在阅读文献或者代码中误差相关时，经常可以看到一个概念，叫逆深度（inverse depth）。也就是深度的倒数，那么同学们有没有想过，为什么使用逆深度误差而不是深度误差？

1 FAST 特征点

FAST特征点则直接利用了关键点与周围像素点灰度值的关系,提取时间非常短,能够实现实时计算,但是FAST关键点不具备尺度和方向不变性,无法应用与SLAM系统

检测过程

1. 在图像中提取像素p，假设它的亮度为Ip；
2. 设置一个阈值T，比如$I_p$的20%；
3. 以像素p为中心，选取半径为3的圆上的16个像素点。
4. 假如选取的圆上,有连续的 N 个点的亮度大于 Ip + T 或小于 Ip − T ,那么像素 p 可以被认为是特征点 (N 通常取 12,即为 FAST-12。其它常用的 N 取值为 9 和 11, 他们分别被称为 FAST-9,FAST-11)。
5. 循环以上四步,对每一个像素执行相同的操作。

可以使用非极大值抑制在一定区域内保留响应极大值的角点，避免角点集中的问题

优点：速度快

缺点：重复性不强，分布不均匀

2 SIFT 特征点

SIFT特征点具备良好的尺度和方向不变性,而且对光照,抖动等噪声具有较强的鲁棒性.但由于SIFT的计算量比较大,根据orbslam2作者发表的论文“ORB-SLAM a Versatile and Accurate Monocular SLAM System”中可以知道,一张照片提取1000个SIFT特征点的平均时间为300ms,无法实现实时的SLAM系统,虽然后面有学者提出了在GPU加速下能够实现实时,但是毕竟实际的应用场景中,SLAM主要是为上层应用提高感知信息,理应不能占用过多的计算资源.而基于SIFT描述子改进的SURF描述子尽管提高了一个数量级的提取效率，但仍然无法满足SLAM系统的要求.

3 ORB 特征点

ORB特征点的提取是基于图像金字塔的,在不同尺度的图像上面提取Oriented FAST 关键点(增加了方向的FAST关键点)和 BRIEF 描述子,以此来实现尺度和方向的不变性.

而ORB特征点则结合了一种改进的FAST关键点和BRIEF,具备有良好的尺度和方向不变性.提取一张照片的ORB特征点大约需要15ms,既实现了实时性,同时还保证了所提取特征点的可靠性.

提取方法

ORB取名已经反映出其是一个结合了改良后的FAST角点提取和BRIEF描述子的算法，提取ORB特征分为两步：

1. FAST关键点提取：找出图像中的FAST角点，相较于原版的FAST，ORB中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变性；
2. BRIEF描述子：对上一步提取出关键点的周围图像区域进行描述。

提取过程

1. 计算图像块的矩
2. 找到图像块的质心
3. 连接几何中心与质心，得到方向向量，来描述ORB特征点

特征点描述

描述子BRIEF：由图像块中两个随机像素的亮度比较得到

4 特征点相关问题

4.1 人工设计的特征需要考虑什么性质?

可重复性；可取别想；高效率；本地性

一 特征提取

给两组已经匹配好的3D点，计算相对位姿变换。已知匹配的ICP问题，写代码。

什么是Essential，Fundamental矩阵？

计算H矩阵和F矩阵的时候有什么技巧呢？实际上在问归一化的操作。

1. 什么是ORB特征，ORB特征的旋转不变性是如何做的，BRIEF算子是怎么提取的。

ORB特征是对FAST特征点检测方法与BRIEF特征描述子进行结合和改进的特征点检测方法；FAST特征点不具备方向，ORB使用矩表示特征点的主方向，有了主方向就可以解决了BRIEF描述子不具备旋转不变性的问题了。方式：关键在于其建立坐标系的方式，ORB在计算BRIEF描述子时是以关键点为圆心、以关键点和取点区域的形心的连线为X轴建立2维坐标，这种方式保证了在不同的旋转角度下，当以同一个取点模式取出的点是相同的，这就解决了旋转不变性的问题？？？
ORB并未解决尺度不变性，OpenCV上的实现使用了高斯金字塔来保证其尺度不变性。

参考：图像特征点—ORB特征点

1. ORB特征即Oriented FAST and Rotated BRIEF，由FAST关键点和BRIEF描述子两部分组成，先使用FAST提取角点作为特征点，再使用BRIEF对特征点周围区域进行描述，计算描述子；
2. 通过改进FAST特征点获得尺度不变性和旋转不变性：普通FAST角点不具备方向性和尺度不变性，ORB对其进行改进，增加了尺度不变性和特征点的方向信息，所以称为Oriented FAST关键点；尺度不变性通过构建图像金字塔、并在金字塔每一层检测角点实现；特征的方向（旋转）信息由灰度质心法计算图像块的质心、再连接图像块几何中心O与质心C，即可得到特征点的方向向量OC，特征点的方向即定义为theta =arctan(m01/m10)。至此FAST角点具有了尺度与旋转的描述。FAST特征点有了方向信息，在后续计算BRIEF描述子时，即可保证特征点的旋转不变性。
3. FAST角点提取：半径为3的圆上16个像素点，如果连续的N个点的亮度大于Ip+T或小于Ip-T（T为设定的阈值，如0.2*Ip），则认为该点是特征点，N常去12，即FAST-12。
4. BRIEF算子是二进制描述子，其描述向量由许多0和1组成，通过在关键点附近随机取两个像素（如p和q），比较p和q像素值的大小关系，如果p大于q，则取1，反之取0，取128组这样的p、q，即可得到特征点的128维描述子。
5. ORB速度快的原因：相比其他特征点检测算法，FAST只是比较像素亮度大小；BRIEF通过随机选点、编码0和1的方式计算描述子，因此速度快。

2. ORB-SLAM中的特征是如何提取的？如何均匀化的？

特征提取

ORB-SLAM2跟踪运行在主线程，是整个SLAM系统的基础。主程序在初始化SLAM系统后，

// Examples/Monocular/mono_kitti.cc line:53``
// Create SLAM system. It initializes all system threads and gets ready to process frames.``
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,``true``);

就可以将每一帧图像送往跟踪函数，如下是单目SLAM主函数调用跟踪函数的代码：

// Examples/Monocular/mono_kitti.cc line:84``
// Pass the image to the SLAM system``
SLAM.TrackMonocular(im,tframe);

TrackMonocular()函数调用GrabImageMonocular()函数实现跟踪功能：

// System.cc line:260``
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

ORB-SLAM提取ORB特征时采用了8层金字塔，尺寸因子为1.2。对于像素为512384到752480的图片，提取1000个FAST角点，对于更高的分辨率，提取2000个FAST角点就可以了。

至此，得到当前帧ORB特征点mvKeys和描述子mDescriptors，均是Frame类对象mCurrentFrame的成员变量。提取出特征点后，需要对其去失真UndistortKeyPoints();。同时需要将图片分割为64*48大小的栅格，并将关键点按照位置分配到相应栅格中，从而降低匹配时的复杂度，实现函数为AssignFeaturesToGrid(); 。

特征均匀化

二 特征匹配

1 暴力匹配

执行步骤如下：

1. 利用特征提取算法计算关键点和描述符
2. 创建特征匹配器，选择距离计算方法以及计算方式
3. 计算匹配对并进行筛选
4. 绘制匹配对

cv2.BFMatcher参数说明：

• normType：它是用来指定要使用的距离测试类型。默认值为cv2.Norm_L2。这很适合SIFT和SURF等（c2.NORM_L1也可）。对于使用二进制描述符的ORB、BRIEF和BRISK算法等，要使用cv2.NORM_HAMMING，这样就会返回两个测试对象之间的汉明距离。如果ORB算法的参数设置为WTA_K==3或4，normType就应该设置成cv2.NORM_HAMMING2。
• crossCheck：使用交叉匹配的方法来滤除错误匹配，默认值为False。若设置为True，匹配条件就会更加严格，只有到图片A中的第i个特征点与图片B中的第j个特征点距离最近，反过来，还要图片B中的第j个特征点到图片A中的第i个特征点也是最近，才会认为他们是最佳匹配(i,j)并返回，即：这两个特征点要互相匹配才行。（你喜欢我，我也喜欢你，单恋可不行…）

暴力匹配器BFMatcher有两个重要的方法，分别是BFMatcher.match() 和BFMatcher.knnMatch()。送入查询图片A和目标图片B

match() 方法会为图片A从图片B中找到最佳的单个匹配点；

而knnMatch()方法会为图片A从图片B中找到最佳的前k个匹配点，k值由用户指定。当我们需要做一些额外的工作时，这种方法会很有用。

2 快速最近邻算法（FLANN）

Lowe’s算法：为了进一步筛选匹配点，获取优秀的匹配点，即“去粗取精”。为了排除因为图像遮挡和背景混乱而产生的无匹配关系的关键点，SIFT的作者Lowe提出了比较最近邻距离与次近邻距离的SIFT匹配方式：

原理：取一幅图像中的一个SIFT关键点，并找出其与另一幅图像中欧式距离最近的前两个关键点，在这两个关键点中，如果最近的距离除以次近的距离得到的比率ratio少于某个阈值T，则接受这一对匹配点。因为对于错误匹配，由于特征空间的高维性，相似的距离可能有大量其他的错误匹配，从而它的ratio值比较高。显然降低这个比例阈值T，SIFT匹配点数目会减少，但更加稳定，反之亦然。
Lowe推荐ratio的阈值为0.8；

有网友对大量存在任意尺度、旋转和亮度变化的两幅图片进行匹配，结果表明ratio取值在0. 4~0. 6 之间最佳，小于0. 4的很少有匹配点，大于0. 6的则存在大量错误匹配点，所以建议ratio的取值原则如下（仅供参考）:

• ratio=0. 4：对于准确度要求高的匹配；
• ratio=0. 6：对于匹配点数目要求比较多的匹配；
• ratio=0. 5：一般情况下。

Python版本

FLANN，Fast Library for Approximate Nearest Neighbors. 其是针对大规模高维数据集进行快速最近邻搜索的优化算法库.

FLANN Matcher 需要设定两个字典参数，以指定算法和对应的参数，分别为 IndexParams 和 SearchParams.

1. IndexParams

使用 SIFT、SURF等描述子，可以传递如下：

index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)

使用 ORB 描述子是，可以传递如下：

index_params= dict(algorithm = FLANN_INDEX_LSH,
                   table_number = 6, # 12
                   key_size = 12,     # 20
                   multi_probe_level = 1) #2

2. SearchParams

参数指定了索引里的树应该被递归遍历的次数，值越高准确度越高，但计算耗时也更久.

search_params = dict(checks=100)

2 外点去除

A. RANSAC算法

RANSAC算法的基本假设是样本中包含正确数据(inliers，可以被模型描述的数据)，也包含异常数据(outliers，偏离正常范围很远、无法适应数学模型的数据)，即数据集中含有噪声。这些异常数据可能是由于错误的测量、错误的假设、错误的计算等产生的。同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。

优缺点

​ RANSAC算法的优点是能鲁棒的估计模型参数。例如，他能从包含大量局外点的数据集中估计出高精度的参数。缺点是它计算参数的迭代次数没有上限，如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。RANSAC只有一定的概率得到的可信的模型，概率与迭代次数成正比。另一个缺点是它要求设置跟问题相关的阈值，RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。

算法流程

RANSAC是一个迭代算法。在基础版本中，每次迭代包括一下五个步骤：

1. 在原始数据中随机选取一个最小子集作为假设的内点（如果根据数据随机选择一条二维直线，则选择两个点）；
2. 根据假设的内点拟合一个模型（比如根据两个点拟合直线）；
3. 判断生育点的原始数据是否符合拟合的模型，并将其分为内点和外点。如果内点太少，则该次迭代被标记为无效并中止；
4. 根据假设的内点和上一步中划分出的内点重新拟合模型
5. 计算所有内点的残差，根据残差的和重新评估模型。

迭代上述步骤，把具有残差和最小的模型作为最佳模型。

迭代次数的上限

例如，k次才可以保证有概率p可以选到仅有内点组成的子集，假设每个测量为内点的概率均为$w$。
$$
1-p=(1-w^n)^k
$$
其中，n是拟合模型所需要的最少数据个数，k为总的迭代次数。k为
$$
k = \frac{\ln(1-p)}{\ln(1-w^n)}
$$

B. M估计

C. 有哪几种鲁棒核函数？

RANAC和鲁棒核函数都是为了解决出现outlier的问题：RANAC是从数据中选择正确的匹配进行估计，鲁棒核函数则是直接作用在残差上，对残差进行饱和函数运算，限制单个数据点对于误差函数的影响力。等于对最小二乘问题做了包装，通过降低错误匹配的权重，使得观测数据中的outlier影响不到最终的估计结果：

常用核函数：Huber、Cauchy、Turkey;

3 问题

1 如何对匹配好的点做进一步的处理，更好保证匹配效果

（1）确定匹配最大距离，汉明距离小于最小距离的两倍
（2）使用KNN-matching算法，令K=2。则每个match得到两个最接近的descriptor，然后计算最接近距离和次接近距离之间的比值，当比值大于既定值时，才作为最终match。
（3）RANSAC（使用RANSAC找到最佳单应性矩阵。由于这个函数使用的特征点同时包含正确和错误匹配点，因此计算的单应性矩阵依赖于二次投影的准确性）
（4）交叉匹配。针对暴力匹配，可以使用交叉匹配的方法来过滤错误的匹配。交叉过滤的思想很简单，再进行一次匹配，反过来使用被匹配到的点进行匹配，如果匹配到的仍然是第一次匹配的点的话，就认为这是一个正确的匹配。举例来说就是，假如第一次特征点A使用暴力匹配的方法，匹配到的特征点是特征点B；反过来，使用特征点B进行匹配，如果匹配到的仍然是特征点A，则就认为这是一个正确的匹配，否则就是一个错误的匹配。OpenCV中BFMatcher已经封装了该方法，创建BFMatcher的实例时，第二个参数传入true即可，BFMatcher bfMatcher(NORM_HAMMING,true)。

2 如果对于一个3D点，我们在连续帧之间形成了2D特征点之间的匹配，但是这个匹配中可能存在错误的匹配。请问你如何去构建3D点？除了RANSAC之外，还有什么鲁棒估计的方法？

可以使用RANSAC算法去除错误的匹配，M估计、鲁棒核函数。

3 如何优化重投影误差？采用什么方法求解？如果误匹配的点重投影之后误差很大，如何解决它对整个优化问题的影响？

图优化模型将路标点和相机位姿作为两个节点，观测模型作为边，同时优化两个变量，SLAM中常用L-M求解。

如果误匹配误差很大可以考虑用核函数（Huber），核函数可以减小误匹配对整个方法的影响。

4 RANSAC在选择最佳模型的时候用的metric是什么？

5. 描述子距离的匹配方法，除了暴力匹配还有什么？

1. 基于阈值: 如何描述子之间的距离小于某个阈值，则认为他们相互匹配.由于大小阈值的的描述子可能有很多个，因此该方法可能会得到多个与之匹配的描述子。 if |Da-Db|<t then matches else dismatch
2. 基于最近邻 : 如果Db就Da的最近邻，并且 距离小于某个阈值，则认为他们相互匹配。该方法得到唯一的匹配
3. 基于距离比率（distance ratio）的最近邻: 该方法与最近邻类似，不同的是其对最近邻与次近邻间的distance ratio应用阈值处理.即：if ||Da-Db||/||Da-Dc||<T then matched (Db,Dc分别是最近邻与次近邻) 该方法保证与Da匹配的描述子只有一个，其它的与其相差较大。这在描述子评价中有非常重要的作用，如果一种描述子能保证上述条件，那么该描述子优于其它描述子。
4. FLANN近似最近邻
5. 使用词袋模型进行初步筛选再进行匹配
6. 重投影匹配

三 相机运动求解

一 极线约束（2D-2D）

$$
x^T_2t^{\wedge}R x_1 = 0
$$

1. 按照你的理解讲解一下什么是极线约束？这个约束能带来什么好处？

所谓极线约束就是说同一个点在两幅图像上的映射，已知左图映射点p1，那么右图映射点p2一定在相对于p1的极线上，这样可以减少待匹配的点数量。（画图解释）

极线约束的好处：从上面的描述我们可以看到，我们在做特征点匹配时，左图成像点p1的待匹配点p2一定在相对于p1的极线上，那么我们在做搜索时就可以在极线附近（考虑实际可能 会有一点误差）进行搜索，相对暴力匹配极大减少待匹配的点的数量。

可以画图解释，注意成像平面、特征点、极点、极线、极平面、相机光心等概念。

2 本质矩阵E、基本矩阵F、单应性矩阵H区别？

基本矩阵：描述的是不同帧之间同一空间点像素坐标的几何约束关系，将图像归一化坐标替换为像素点坐标，得到基本矩阵约束，基本矩阵描述的约束又称为极线约束。基本矩阵和相机内参，外参都有关系。

本质矩阵：描述空间点在不同帧之间的归一化坐标的约束关系，是相机坐标系层面。本质矩阵和相机外参有关系，和内参无关。本质矩阵则是基本矩阵的一种特殊情况，是在归一化图像坐标下的基本矩阵，可以理解为本质矩阵对应的坐标位于相机坐标系，基础矩阵对应的坐标位于图像平面坐标系。

单应性矩阵：在相机只有旋转而没有平移的情况，此时t为0，E也将为0，导致无法求解R，这时可以使用单应矩阵H求旋转，但仅有旋转，无法三角化求深度。

3 根据基础矩阵E如何求解相机运动R，t?

答：SVD分解

其中的R矩阵怎么定义

//SVD分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd(E,ComputeThinU|ComputeThinV);
Matrix3d V=svd.matrixV(),U=svd.matrixU();
Matrix3d un_S=U.inverse()* E*V.transpose().inverse(); //类型不要搞混

//计算后的Sigma矩阵
//调整奇异值矩阵 调成E的解析格式，否则可能补满足E内在形式
double delta_1=un_S(0,0),delta_2=un_S(1,1);
Matrix3d S = Matrix3d::Zero();
S(0,0)=(delta_1+delta_2)/2;
S(1,1)=(delta_1+delta_2)/2;

// set t1, t2, R1, R2 
Matrix3d t_wedge1, t_wedge2;
Matrix3d R1, R2;
// 中间旋转矩阵的定义
AngleAxisd V1(M_PI / 2, Vector3d(0, 0, 1));
AngleAxisd V2(- M_PI / 2, Vector3d(0, 0, 1));

Matrix3d Rz_pos = V1.toRotationMatrix();
Matrix3d Rz_neg = V2.toRotationMatrix();
t_wedge1 = U*Rz_pos*S*U.transpose();
t_wedge2 = U*Rz_neg*S*U.transpose();
R1 = U*Rz_pos*V.transpose();
R2 = U*Rz_neg*V.transpose();

2 ICP（3D-3D）

迭代最近邻估计

3 PnP（2D-3D)

1. 什么是PnP算法？请用你的语言描述一下原理，它一般用在什么场景，解决什么问题？

Perspective-n-Points, PnP(P3P)提供了一种解决方案，它是一种由3D-2D的位姿求解方式，即需要已知匹配的3D点和图像2D点。目前遇到的场景主要就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿,第一帧图像中的3D点以及对应到第二帧图像中的2D点，通过相机成像模型，将3D点投影到二维平面，通过构建误差目标函数通过优化调整位姿的方法使得误差目标函数达到最小，所以它求得的是当前帧相对于上一帧的位姿变换，都是基于已知3D点和对应的图像2D点求解相机运动的过程。

2. PNP求解最少需要几个点?只有一个点的自由度是多少?两个呢?

• PnP(Perspective-n-point):多点透视成像,描述了当知道n个3D空间点及其投影位置时,如何估计相机的位姿.
• PnP求解最少需要三个点对:P3P
• 只有一个点对的自由度是4
• 两个点对的自由度是2

四 利用三角化求特征点位置

六 2D 光流

七 直接法

直接法估计相机位姿时，并不需要 提取特征点，而是通过优化匹配点的像素值误差（也称光度误差）估计位姿，但也会面临快速运动，光照变化等的挑战，如果让你改善该问题，你会采用哪些方法来提高跟踪质量(精度，速度，鲁棒性等)？

八 方法对比

零 特征点法

特征点法通过特征匹配得出2D-2D匹配点，使用对极几何即可计算相机位姿R、t，进一步通过最小化重投影误差优化R、t，得出最优的相机位姿。不同与直接法的地方在于，特征点法使用对极几何(或PnP、ICP)计算出的R、t作为位姿初值，最小化重投影误差进行优化，而直接法则使用经验值设定的R、t作为初值，使用最小化灰度误差进行优化。

一 特征点法与直接法

特征点法

优点	缺点
（1）精确，直接法属于强假设	（1）关键点提取、描述子、匹配耗时长
（2）运动过大时，只要匹配点在像素内，则不太会引起误匹配，鲁棒性好	（2）特征点丢失场景无法使用
	（3）只能构建稀疏地图

直接法

优点	缺点
（1）省去计算特征点、描述子时间	（1）易受光照和模糊影响
（2）可以用在特征缺失的场合（比如白墙）	（2）运动必须微小，要求相机运动较慢或采样频率较高（可以用图像金字塔改善）
（3）可以构建半稠密乃至稠密地图	（3）非凸性；单个像素没有区分度

简述一下特征点法和直接法的概念，以及对应的优缺点。

特征点法，根据提取、匹配 特征点来估计相机运动，优化的是重投影误差，对光照变化不敏感 ，是比较成熟的方案。

特征点法	比如ORB-SLAM
优点	（1）特征点本身对光照、运动、旋转比较不敏感，所以比较稳定 （2）相机运动较快（相对直接法来说）也能跟踪成功，鲁棒性好一些 （3）研究时间较久，方案比较成熟
缺点	（1）关键点提取、描述子、匹配耗时长 （2）特征点丢失场景无法使用 （3）只能构建稀疏地图

直接法，根据相机的亮度信息估计相机的运动，可以不需要计算关键点和描述子，优化的是光度误差，根据使用像素数量可分为稀疏、半稠密、稠密三种。

直接法	SVO，LSD-SLAM
优点	（1）速度快，可以省去计算特征点、描述子时间 （2）可以用在特征缺失的场合（比如白墙），特征点法在该情况下会急速变差 （3）可以构建半稠密乃至稠密地图
缺点	（1）因为假设了灰度不变，所以易受光照变化影响 （2）要求相机运动较慢或采样频率较高（可以用图像金字塔改善） （3）单个像素或像素块区分度不强，采用的是数量代替质量的策略

特征点法和直接法的BA有何不同

（1） 误差函数不同。特征点法是重投影误差，直接法是光度误差

（2） 雅克比矩阵不同

二 光流法与直接法

光流法

光流法通过光流跟踪图像像素点，得出两帧间角点的匹配关系，当存在一系列匹配点之后，即可使用三角测量、PnP、ICP等方法估计相机位姿。

光流法与直接法的区别

光流法

是基于像素的光度不变性假设，跟踪图像像素点的方法，描述了像素在图像中的运动。当使用光流法跟踪图像特征点得出匹配点对之后，即可以用匹配的特征点，使用ICP、PnP或对极几何估计相机运动。光流法本质上还应该划分到特征点法中，只不过把提取特征点、计算描述子、特征匹配替换成了光流跟踪而已，之后求解R和t的过程是一样的。

直接法

1. 是由光流法演变而来的，也是基于灰度不变性假设，通过最小化光度误差(或称为测量误差)来优化R和t。直接法将获取匹配点对(数据关联关系)与计算相机位姿放到同一个非线性最小二乘问题中，而特征点法是先得出匹配点，再进行非线性优化得出相机位姿，是分步进行的。
2. 区别：光流仅估计了像素间平移，但没有用到相机本身的几何结构、没有考虑到相机的旋转和图像的缩放、对于边界上的点，光流不好追踪，但直接法则考虑了这些信息；
3. 直接法流程：假设有两个帧，运动未知，但有初始估计R,t(通过经验值设定)；第一帧上看到了点P,投影为P1；按照初始估计，将P1转到第二帧，得出P在第二帧上投影P2；通过最小化P1与P2点的灰度误差来优化初始估计R、t。以上是定位过程，建图过程如下：当确定最优的R、t后，即可重投影得出3D点坐标。

光流和直接法有何不同：

答案1：匹配方法不同。
直接法是通过最小光度误差来匹配特征点，而光流法是通过计算
光流仅估计了像素间的平移，但像素梯度以及灰度关于时间的导数来预测像素在下一时刻的位置。

答案2：光流仅估计了像素间的平移，但

（1）没有用相机结构
（2）没有考虑相机的旋转和图像缩放
（3）边界点追踪效果差

特征匹配（稀疏）和稠密匹配区别

特征匹配： （1）速度快，效率高，可以到亚像素级别，精度高 （2）匹配元素为物体的几何特征，对照明变化不敏感

稠密匹配 （1）速度慢，效率低 （2）对无纹理区域匹配效果不理想，对光强条件敏感

三 后端

一 卡尔曼滤波

卡尔曼滤波器主要包含两个步骤

预测：如何从上一时刻的状态，根据输入信息推断当前时刻的状态分布（先验）计算协方差
$$
\hat{x}{k|k-1} = F_k \hat{x}{k-1|k-1}+B_ku_k\
P_{k|k-1} = F_{k}P_{k-1|k-1}F^T_k+Q_k
$$

更新：计算增益Kg，然后计算后验

首先计算以下三个量
$$
\tilde{y}k = z_k - H_k \hat{x{k|k-1}} （测量物质）\
S_k = H_k P_{k|k-1}H^T_k + R_K （测量残差协方差）\
K_k = P_{k|k-1}H^T_kS^{-1}k
$$
用上述三个量来更新滤波器变量x与P
$$
\hat{x}{k|k} = \hat{x}{k|k-1} + K_k \tilde{y}_k\
P{k|k} = (I-K_kH_k)P_{k|k-1}
$$
先更新状态估计，再更新写方法估计

上述方法仅在计算最优卡尔曼增益的时候有效。

拓展卡尔曼滤波器与卡尔曼滤波器的算法结构与步骤；

1. 卡尔曼滤波增益推导

答：https://zhuanlan.zhihu.com/p/165570020

2. 描述（扩展）卡尔曼滤波与粒子滤波，你自己在用卡尔曼滤波时遇到什么问题没有？

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。因为观测数据中包括系统中的噪声和干扰的影响，因此最优估计也可以当作是滤波的过程。（顺便复习粒子滤波！）

3. 常见滤波方法的对比（KF、EKF、IEKF、UKF、PF）

二 非线性优化

最小化重投影误差问题（Minimization of Reprojection error）是个非线性，非凸的优化问题，这意味着我们不一定能求解它，也不一定能找到全局最优的解。在实际操作中，我们实际上是在调整每个$X_j$，使得它们更符合每一次观测$z_j$，也就是使每个误差项都尽量的小。由于这个原因，它也叫做捆集调整（Bundle Adjustment）。

0 重投影误差

1. 重投影误差的表达式，误差关于位姿的偏导数怎么算？误差关于空间点的偏导数怎么计算？

1 最小二乘法

对于一个最小二乘问题：
$$
\min_{x}F(x) = \frac{1}{2} ||f(x)||2_2
$$
最小二乘法即使用迭代的方式，从一个初始值出发，不断更新优化变量，使得目标函数下降。

具体步骤如下：

1. 给定某个初值$x_0$；
2. 对于第k次迭代，寻找一个增量$\Delta x_k$ ,使得$||f(x_k+\Delta x_k)||^2_x$达到极小值；
3. 若$\Delta x_k$ 足够小,则停止‘；
4. 否则，令$x_{k+1} = x_k + \Delta x_k$,返回第二步。

2 增量$\Delta x_k$的确定—一阶二阶梯度法

将目标函数在x附近进行泰勒展开
$$
||f(x+\Delta x)||^2_2 \simeq ||f(x)||^2_2 + J(x)\Delta x +\frac{1}{2}\Delta x^T H\Delta x
$$
其中，J表示雅可比矩阵，H为海塞矩阵。

2.1 一阶梯度法（最速下降法）

增量的方向为
$$
\Delta x^* = -J^T(x)
$$
步长$\lambda$需要自己确定

梯度下降法：在寻找目标函数极小值时，是沿着反梯度方向进行寻找的。梯度的定义就是指向标量场增长最快的方向，在寻找极小值时，先随便定初始点$(x_0，y_0)$然后进行迭代不断寻找直到梯度的模达到预设的要求。

缺点：在远离极小值的地方下降很快，而在靠近极小值的地方下降很慢，靠近的时候可能成zig-zag下降。

2.2 二阶梯度法（牛顿法）

保留优化目标展开式的二阶梯度信息
$$
\Delta x^* = \arg \min {||f(x)||^2_2 + J(x)\Delta x + \frac{1}{2}\Delta x^TH\Delta x}
$$
令右侧等式关于$\Delta x$的导数并令其为0，得到了增量的解
$$
H\Delta x = -J^T
$$
缺点：牛顿法需要计算H矩阵，计算量很大，因此需要避免H的计算

最速下降法比较直观，但过于贪心，容易走出锯齿路线，反而增加迭代次数，导致局部收敛速度下降。
牛顿法在最速下降的基础上引入二阶导数，这样就处理了最速下降法一阶导数为0的情况，但是需要计算目标函数的海塞矩阵，二阶求导运算量不小，在问题规模较大时非常困难。

3 高斯牛顿法

高斯牛顿法与LM相比起来更加实用，它将f(x)进行的一阶泰勒展开而不是将目标函数$f^2(x)$

$$
f(x+\Delta x) = f(x) +J(x)\Delta x
$$

最小二程问题转化为线性最小二乘问题
$$
\Delta x^* = \arg \min_{\Delta x} \frac{1}{2}||f(x)+J(x)\Delta x||^2
$$
经过推导，可以得到：
$$
J(x)^TJ(x)\Delta x = -J(x)^Tf(x)
$$
左边定义为H,右边定义为g,可以得到：
$$
H\Delta x = g
$$
优点：对比牛顿法可见，Gauss-Newton 用$J^TJ$ 作为牛顿法中二阶Hessian 矩阵的近似，从而省略了计算H 的过程。求解增量方程是整个优化问题的核心所在。

算法步骤

1. 给定初始值$x_0$。
2. 对于第k 次迭代，求出当前的雅可比矩阵$J(x_k)$ 和误差$f(x_k)$。
3. 求解增量方程：$HΔx_k = g$:
4. 若$Δx_k$ 足够小，则停止。否则，令$x_{k+1} = x_k + Δx_k$，返回2.

缺点：

1. 使用gauss-newton法要求H具有可逆性，但通常$J^TJ$只有半正定性，可能出现$J^TJ$ 为奇异矩阵或者病态(ill condition)
   的情况，此时增量的稳定性较差，导致算法不收敛。
2. 求出的步长$\Delta x$太大，会导致局部近似不够准确，有可能甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。

4 LM算法

高斯牛顿法则对牛顿法进行了简化，避免了二阶求导，但实际中该近似只有半正定性，容易产生病态方程。
列文伯格—马夸尔特算法在高斯牛顿法引入阻尼项，一定程度上避免系数矩阵的非奇异和病态问题，但收敛速度较慢。

核心思想：为GN算法加入一个信赖区域；信赖区域的范围通过近似模型与实际函数之间的差异来确定。
$$
\rho = \frac{f(x+\Delta x) - f(x)}{J(x)\Delta x}
$$
算法步骤

1. 给定初始值$x_0$，以及初始优化半径$\mu_0$。

2. 对于第k次迭代，求解：
   $$
   \min_{Δx_k}\frac{1}{2}
   ∥f (x_k) + J (x_k)Δx_k∥2; s.t:∥DΔx_k∥2\leq\mu;
   $$
   这里$\mu$是信赖区域的半径，D 将在后文说明。

3. 计算$\rho$。

4. 若$\rho> \frac{3}{4}$，则$\rho = 2\rho$；

5. 若$\rho \lt \frac{1}{4}$，则$\rho = 0.5\rho$；

6. 如果$\rho$大于某阈值，认为近似可行。令$x_{k+1} = x_k + Δx_k$。

7. 判断算法是否收敛。如不收敛则返回2，否则结束。

利用拉格朗日乘子法求解迭代式，相当于求解
$$
(H+\lambda D^TD)\Delta x = g
$$
简化形式
$$
(H+\lambda I)\Delta x = g
$$

• 当参数$\lambda$ 比较小时，H 占主要地位，这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于G-N 法。
• 另一方面，当$\lambda$ 比较大时，$\lambda I$ 占据主要地位，L-M更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。

优点：G-N中的H矩阵可能为奇异矩阵或者病态矩阵，导致算法不收敛。而且当步长较大时，也无法保证收敛性，所以采用L-M求解增量方程，但是它的收敛速度可能较慢。L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量Δx。

4.1 LM算法里面那个λ是如何变化的呢？

答：LM算法中$\lambda$ 表示阻尼因子，阻尼因子的变化是根据比例因子来确定的。比例因子根据以下公式确定：
$$
\rho = \frac{F(x)-F(x+\Delta x_{lm})}{L(0)-L(\Delta x_{lm})}
$$

阻尼因子的作用

• $\lambda>0$ 保证 $J^TJ+\lambda I$的正定性。
• $\lambda$非常大的情况下。则$\Delta x_{lm} = -\frac{1}{\mu}J^Tf = -\frac{1}{\lambda}F’(x)^T$，接近最速下降法。
• $\lambda$比较小的情况下，则认为$\Delta x_{lm} \approx \Delta x_{gn}$，最进阶高斯牛顿法。

基本思想

首先比例因子分母始终大于 0,如果:

• ρ < 0, 则 F (x) ↑ ,应该 μ ↑→ ∆x ↓, 增大阻尼减小步长。
• 如果 ρ > 0 且比较大,减小 μ, 让 LM 接近 Gauss-Newton 使得系统更快收敛。
• 反之,如果是比较小的正数,则增大阻尼 μ,缩小迭代步长。

具体的阻尼策略(Marquardt)

• 当$\rho>\frac{3}{4}$,则设置$\lambda = 2\lambda$；
• 当$\rho < \frac{1}{4}$ ，则设置$\lambda = 0.5 \lambda$

4.2 一阶梯度下降，G-N和L-M三种方法的关系

(H+λI)△x=b

• 当λ= 0时，L-M等于G-N；
• 当λ= ∞时，L-M等于一阶梯度下降。

L-M的好处就在于：如果下降的太快，使用较小的λ，如果下降的太慢，使用较大的λ

5 Dog-Leg算法

5.1. DogLeg算法基本介绍

L-M法是G-N法与最速下降法的混合形式，通过调整阻尼因子来在这两种方法之间切换。

狗腿法则通过改变信赖域来实现的。主要引申出以下问题：

1. 如何判断是使用G-N法还是最速下降法的增量？
   1.
2. 如果确定了是使用哪个方法，那么它对应的增量是多少？
   1.
3. 狗腿法的增量为多少？

5.2 DogLeg算法与GN 和LM 有何异同

相关材料

dogleg算法推导与解析

三 对比

1. SLAM后端一般有两种方法：滤波方法和非线性优化方法，这两种方法有什么优缺点？

slam的后端一般分为两种处理方法，一种是EKF（扩展卡尔曼滤波）为代表的滤波方法，一种是以图优化为代表的非线性优化（BA）方法。主流方法还是图优化。

优缺点

（1）优点：在资源受限、待估计量比较简单的情况下，EKF比较有效，经常用在激光slam中。
（2）缺点：存储量和状态量是平方增长关系（存储的是协方差矩阵）。不适合大型场景。现阶段，基于视觉的slam中特征点数据大，滤波方法效率非常低。
图优化一般分为两个任务：构建图。机器人位姿作为顶点，位姿间的关系作为边；优化图。调整机器人的位姿来尽量满足边的约束，使得误差最小。

2. 简述EKF和BA的区别

（1） EKF假设了马尔科夫性，认为k时刻的状态只与k-1时刻有关；非线性优化使用所有的历史数据，做全体的SLAM
（2） EKF做了线性化处理，在工作点处用一阶泰勒展开式近似整个函数，但在工作点较远处不一定成立。非线性优化每迭代一次，状态估计发生改变，我们会重新对新的估计点做 泰勒展开

总而言之，可以把EKF看做只有一次迭代的BA

3 Gauss-Netwon法 和 LM算法以及两者的区别。

为什么用这两种算法：在SLAM中求解相机位姿时，常构建待优化位姿的误差函数（光度误差或重投影误差），通过使得这个误差函数取最小值，得出最优的相机位姿。如果这个误差函数是线性的，则可以直接求导数等于0处的极值即可，但该误差函数通常是关于待优化位姿的非线性多元函数，这实际上是一个非线性无约束最优化问题，常用的解法是G-N、LM算法。
两者均是非线性最小二乘求解方法， 都是通过泰勒展开进行线性化，并利用导数确定最优迭代方向，进行逐步迭代求出最优解的优化算法。

高斯牛顿法：其利用了目标函数的泰勒展开式把非线性函数的最小二乘化问题化为每次迭代的线性函数的最小二乘化问题。高斯牛顿法的缺点在于：若初始点距离极小值点过远，迭代步长过大会导致迭代下一代的函数值不一定小于上一代的函数值。
LM算法：在高斯牛顿法中加入了因子μ，当μ大时相当于梯度下降法，μ小时相当于高斯牛顿法。在使用Levenberg-Marquart时，先设置一个比较小的μ值，当发现目标函数反而增大时，将μ增大使用梯度下降法快速寻找，然后再将μ减小使用牛顿法进行寻找。
LM算法在高斯牛顿法中加入了因子μ，当μ大时相当于梯度下降法，μ小时相当于高斯牛顿法。在使用Levenberg-Marquart时，先设置一个比较小的μ值，当发现目标函数反而增大时，将μ增大使用梯度下降法快速寻找，然后再将μ减小使用牛顿法进行寻找。

4 为什么SLAM中常用LM算法而不是高斯牛顿求解优化问题？

1. GN：线搜索 将f（x）进行一节泰勒展开，最后求解线性方程H△x=b；用JT*J近似H矩阵，H的计算过程根据特定问题特殊分析；该方法特点是：稳定性差，可能不收敛；
2. LM：信赖区域； 求解线性方程(H+λI)△x=b；通过调整拉格朗日乘子，可以判断是H占据主导地位(二阶近似较好)，还是λI占据主导地位(一阶近似较好)，
3. 避免非奇异和病态问题，提供更稳定，更准确的增量。

四 图优化理论

1. 做图优化时，对比采用四元数法和李代数法在数学直观性、计算量上的差异性

2. 画后端优化因子图

3. 边缘化的过程全面分析，示意图，公式推导，优缺点，哪些矩阵块有改变

4. 10个相机同时看到100个路标点，问BA优化的雅克比矩阵和信息多少维？

雅克比矩阵维度=误差项数*误差变量数 （2000*360）

信息矩阵维度=误差变量数*误差变量数（360*360）

6. 什么是边缘化？First Estimate Jacobian？一致性？可观性？

对于VIO系统，边缘化的目的是把旧的状态量从状态估计窗口中移除，保证运行效率；同时，需要把移除的状态量的信息保留下来，作为当下窗口的先验，尽可能避免信息丢失。

7. 相比VSLAM，加入IMU后，哪些状态可观？

a. 单目SLAM7个自由度不可观：6个自由度+尺度；
b. 单目+IMU4个自由度不可观：偏航角（yaw）+3自由度不可观；翻滚角（roll）、俯仰角（pitch）由于重力存在而可观，尺度因子由于加速度计的存在而可观；

五 编程问题

1 介绍一下Ceres优化库

Ceres是一个广泛使用的最小二成求解库。用户只需要按照一定步骤定义待解的优化问题。Ceres求解的最小二成问题一般如下形式：
$$
\min_x\ \frac{1}{2}\sum_{i}p_i(||f_i(x_{i_1},…,x_{i_n})||^2)\
s.t\quad l_j\leq x_j \leq u_j
$$
一般求解过程：

1. 定义参数块

2. 定义残差块的计算方式

3. 定义残差快计算雅克比矩阵的方式；

   三种方式可以使用：（1）自动求导（需要指定误差项和优化变量的维度）（AutoDiff）（2）使用数值求导(Numeric Diff)；（3）自行推到解析函数的形式。

4. 在option里配置优化选项，比如使用Line search还是trust Regin；迭代次数、步长等

5. 传入problem对象，调用solve函数求解。

2 介绍一下g2o库以及使用的基本方法

g2o库是一个SLAM领域广为使用的优化库。它主要基于图优化。图优化就是把优化问题表现成图的一种优化方法。图有多个顶点以及链接这些顶点的边组成。用顶点表示优化变量，用边表示误差项。

g2o的基本使用步骤：

1. 定义顶点和边的类型；
2. 构建图；
3. 选择优化算法；
4. 调用g2o进行优化，返回结果；

3 g2o 与 ceres的区别与联系，

在使用过程中：

1. g2o提供了大量现成的边和顶点，非常便于相机的位姿态估计问题；
2. ceres则需要手动实现每一个cost function。

• 其它问法：用g2o和ceres库都能用来进行BA优化，这两者在使用过程中有什么不同？

4. 优化求解过程中，g2o或者ceres的内部实现过程，有哪些加速计算的处理

5. 还有别的什么优化库吗

除了Ceres库和g2o库，还有NLopt库、liblbfgs、slam++库等等。

其它优化问题

5. 给你m相机n个点的bundle adjustment。当我们在仿真的时候，在迭代的时候，相机的位姿会很快的接近真值。而地图点却不能很快的收敛这是为什么呢？

五 闭环检测与重定位

一 闭环检测

1 什么是闭环检测？

在视觉SLAM问题中，位姿的估计往往是由上一帧位姿解算当前帧位姿，这么递增求解，因此相邻两帧之间的误差就会产生累计。如我们在求解第五帧位姿的时候，一般是根据第四帧计算的，但是如果我们发现第5帧还可以由第2帧计算出来，就减少了误差的累计。这种与之前的某一帧（非相邻帧）建立位姿约束关系就叫做回环。找到可以建立这种位姿约束的历史帧，就是回环检测。

a 什么要进行回环检测？

回环通过减少约束数，起到了减小累计误差的作用。

2 闭环检测有哪些方法? 最常用的方法有哪些？

朴素方法

1. 对任意两张图像都做一遍特征匹配，根据正确匹配的数量确定哪两个图像存在关联。（这是 O(N2) 的复杂度，随着轨迹变长增长太快，在实时系统中不实用）
2. 随机抽取历史数据并进行回环检测，比如说在n 帧当中随机抽 5 帧与当前帧比较。（盲目试探方法在帧数 N 增长时，抽到回环的几率又大幅下降，使得检测效率不高）

基于里程计的几何关系

当我们发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系

基于外观的检测方法

它和前端后端的估计都无关，仅根据两张图像的相似性确定回环检测关系。核心问题是如何计算图像间的相似性。

方法：特征匹配，提取当前帧与过去所有帧的特征，并进行匹配，这种方式假设了过去所有帧都有可能出现回环，匹配十分耗时、计算量大。基于词袋模型，词袋模型就是把特征看成是一个个单词，通过比较两张图片中单词的一致性，来判断两张图片是否属于同一场景。词袋模型需要训练字典(K-means聚类)，但通常字典内单词数量巨大，在确定某个特征时需要与字典内每个单词进行匹配，效率低下。为提高匹配效率，字典在训练的过程中构建了一个有k个分支，深度为d的树(K叉树)，类似于层次聚类，可容纳$k^d$个单词，保证了对数级别的查找效率。

3 闭环检测的评价方法

表 12-1 回环检测的结果分类

算法 \ 事实	是回环	不是回环
是回环	真阳性(True Positive)	假阳性(False Positive)
不是回环	假阴性(False Negative)	真阴性(True Negative)

准确率 (False Positive)

准确率描述的是,算法提取的所有回环中,确实是真实回环的概率。
$$
Precision = T P /(T P + F P ),
$$

召回率 (Precision & Recall)

召回率则是说,在所有真实回环中,被正确检测出来的概率
$$
Recall = T P /(T P + F N ).
$$
在 SLAM 中,我们对准确率要求更高,而对召回率则相对宽容一些。

PR曲线

测试回环检测在各种配置下的 P 和 R 值,然后做出一条Precision-Recall 曲线。当用召回率为横轴,用准确率为纵轴时,我们会关心整条曲线偏向右上方的程度、100% 准确率下的召回率,或者 50% 召回率时候的准确率,作为评价算法的指标。

4 词袋模型

词袋,也就是 Bag-of-Words(BoW) ,目的是用“图像上有哪几种特征”来描述一个图像。

词袋模型的简明流程：

1. 提取单词，组成字典；
2. 用单词描述图片，即构成一个向量(对图片的描述)
3. 比较向量的相似程度

a 字典构建

字典生成问题类似于一个聚类(Clustering)问题。

b K-means聚类

1. 随机选取 k 个中心点:c1 , . . . , ck ;
2. 对每一个样本,计算与每个中心点之间的距离,取最小的作为它的归类;
3. 重新计算每个类的中心点。
4. 如果每个中心点都变化很小,则算法收敛,退出;否则返回 1。

c 字典的表示

使用一种 k 叉树来表达字典。它的思路很简单,类似于层次聚类,是 k-means 的直接扩展。假定我们有 N 个特征点,希望构建一个深度为 d,每次分叉为 k 的
树,那么做法如下(见图 12.3.1 ):

1. 在根节点,用 k-means 把所有样本聚成 k 类(实际中为保证聚类均匀性会使用k-means++) 。这样得到了第一层。
2. 对第一层的每个节点,把属于该节点的样本再聚成 k 类,得到下一层。
3. 依此类推,最后得到叶子层。叶子层即为所谓的 Words。

d 相似度的计算

TF-IDF(Term Frequency–Inverse Document Frequency)[100, 101],或译频率-逆文档频率­。TF 部分的思想是,某单词在一个图像中经常出现,它的区分度就高。另一方面,IDF 的思想是,某单词在字典中出现的频率越低,则分类图像时区分度越高。

在词袋模型中,在建立字典时可以考虑 IDF 部分。我们统计某个叶子节点 wi 中的特征数量相对于所有特征数量的比例,作为 IDF 部分。另一方面,TF 部分则是指某个特征在单个图像中出现的频率。
$$
IDF_i = \log \frac{n}{n_i}\
TF_i = \frac{n_i}{n}\
\eta_i = TF_i \times IDF_i
$$
考虑权重以后,对于某个图像 A,它的特征点可对应到许多个单词,组成它的 Bag-of-Words:
$$
A = {(w1 , η1 ), (w2 , η2), . . . , (wN , ηN )} = ∆ v_A
$$
给定 vA 和 vB ,如何计算它们的差异呢?这个问题和范数定义的方式一样,存在若干种解决方式,比如 [102] 中提到的 L1 范数形式:
$$
s (v_A − v_B ) =2∑^N_{o=1} |v_{A_i} | + |v_{B_i} | − |v_{A_i} − v_{B_i}|.
$$

e 相似性评分的处理

考虑到这种情况,我们会取一个先验相似度 s (vt , vt−∆t ),它表示某时刻关键帧图像与上一时刻的关键帧的相似性。然后,其他的分值都参照这个值进行归一化:
$$
s（v_t, v_{t_j})’ = s(v_t , v_{t_j}) /s (v_t , v_{(t−∆t)}) .
$$
:如果当前帧与之前某关键帧的相似度,超过当前帧与上一
个关键帧相似度的 3 倍,就认为可能存在回环。

好处：这个步骤避免了引入绝对的相似性阈值,使得算法能够适应更多的环境。

f 关键帧的处理

检测回环关键帧的选取。如果关键帧选得太近,那么导致两个关键帧之间的相似性过高, 相比之下不容易检测出历史数据中的回环。比如检测结果经常是第 n 帧和第 n − 2 帧、n − 3 帧最为相似,这种结果似乎太平凡了,意义不大。所以从实践上说,用于回环检测的帧最好是稀疏一些,彼此之间不太相同,又能涵盖整个环境。

e 为什么检测到回环之后还要验证 如何验证？

词袋的回环检测算法完全依赖于外观而没有利用任何的几何信息,这导致外观相似的图像容易被当成回环。并且,由于词袋不在乎单词顺序,只在意单词有无的表达方式,更容易引发感知偏差。所以,在回环检测之后,我们通常还会有一个验证步骤 [80, 103]。

验证的方法

1. 时间上的一致性检测。设立回环的缓存机制,认为单次检测到的回环并不足以构成良好的约束,而在一段时间中一直检测到的回环,才认为是正确的回环。
2. 空间上的一致性检测。即是对回环检测到的两个帧进行特征匹配,估计相机的运动。然后,再把运动放到之前的 Pose Graph 中,检查与之前的估计是否有很大的出入。总之,验证部分通常是必须的,但如何实现却是见仁见智的问题。

6. 闭环检测成功之后有什么操作？

1. 根据PnP等算法计算运动关系

2. 根据重投影关系验证回环是否成立

3. 利用全局BA或Pose Graph进行优化

二 重定位

绑架问题就是重定位，是指机器人在缺少之前位置信息的情况下，或跟踪丢失的情况下，如何进行重新定位、确定当前位姿。例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，在这种情况下如何重新确定自己的位置。

我们在看SLAM相关论文的时候，会遇到一个词“kidnap”， 直译过来就是“绑架”，不了解的同学可能感觉怪怪的。你知道这个“绑架”是什么意思吗？可以用哪些方法解决这样的问题？

三 对比

1 重定位与闭环检测的区别

重定位 指的是当跟丢之后重新找回当前的姿态，通过当前帧与关键帧的特征匹配，定位当前帧的相机位姿。顾名思义，这是“ 重新”定位，当前图像因为和最近的图像或者局部地图之间缺乏足够的匹配，导致机器人无法确定自己的位姿，此时处于当前状态的机器人不再知道其在地图中的位置，也叫做机器人被“绑架”，就说的是人质被蒙上双眼带到未知地方，蒙罩去掉后完全不知道自己在哪里，这时候就需要充分利用之前建好的地图或者存好的数据库。此时机器人需要观察周围环境，并且从已有地图中寻找可靠的匹配关系（一般是关键帧信息），这样就可以根据已有信息“ 重新 ”估计机器人的姿态。

回环检测 是为了解决解决位置估计随时间漂移的问题。主要是通过识别曾经到过的场景，将其与当前帧对应，优化整个地图信息，包括3D路标点、及相机位姿、相对尺度信息.
回环的主要目的是降低机器人随时间增加，轨迹中累积的漂移，一般发生在建图过程中。这是因为基于运动传感器或者视觉信息的里程计容易出错，使估计的轨迹偏离其实际真实的情况。通过回环，优化整个地图信息，包括3D路标点、及相机位姿、相对尺度信息。回环检测提供了回环帧与所有历史帧的关系，可以极大减小误差。

联系：回环主要是纠正机器人/相机轨迹，而重新定位在从未知状态找回姿态。两者都需要当前图像预先访问过之前的位置附近，本质上都是一个图像识别问题。

区别：二者目的不同，重定位主要为了恢复姿态估计，而回环为了解决飘移，提高全局精度。之所以容易混淆，可能是因为重定位通常也需要找到与之前帧的对应关系来解出姿态，而这可以通过回环检测来完成。也就是说，二者在匹配帧上可以共享一些算法。然后是第一个问题。专门研究重定位的论文很多，实际在单目VSLAM算法用的比较多的还是基于BoW的匹配方案（ORB-SLAM，VINS等），也有基于机器学习匹配patch的方法（PTAM）。同时也有一些简单粗暴的解决方案，比如单独开线程暴力匹配之前所有关键帧（LSD），或者只扰动初始值不断与上一个关键帧进行匹配（DSO）。

2 词袋模型可以用于回环检测，也可以用于重定位，有什么区别

词袋模型在SLAM中的应用：当前帧与关键帧的特征匹配、重定位的特征匹配、回环检测的特征匹配；（第一个是后两个的基本原理，后两个是应用场景）。连续帧间特征匹配采用的并不是词袋模型。

a) 重定位：主要是通过当前帧与关键帧的特征匹配，定位当前帧的相机位姿。
b) 回环检测：优化整个地图信息，包括3D路标点、及相机位姿、相对尺度信息。回环检测提供了当前帧与所有历史帧的关系，

六 建图

地图点

3D地图点是怎么存储的？表达方式？

a) 地图点构建：单目：可以通过关键帧匹配构造、也可以通过普通帧构造（临时被Tracking用来追踪的）；双目：立体匹配、块匹配；RGBD：彩色深度图对齐得到深度d，再根据彩色(u，v)坐标根据相机投影公式计算3D点坐标。
b) 3D地图点存储方式：Vector3f
c) 地图主要包含关键帧、3D地图点、BoW向量、共视图、生长树等：关键帧(包括特征点、描述符、当前帧的相机位姿，BoW向量无法保存，可在加载关键帧后重新计算)、3D地图点、共视图、
d) 参考

地图

不同的需求需要什么样的地图

• 定位仅需匹配特征点
• 导航和避障需要稠密障碍物信息
• 交互需要稠密的物体表面信息
• 高层任务需要语义信息

建图方法

RGBD稠密建图

• 点云
• 网格/面片
• TSDF
• 八叉树

给一组点云，从中提取平面。

七 SLAM方案

1. 你认为室内SLAM与自动驾驶SLAM有什么区别？
2. 读Maplab，设计室内服务机器人地图更新的方法、流程。

一 时间戳

工程目录下有GPS保存的坐标文件gps.txt和激光雷达保存的坐标文件laser.txt两个文件，两个文件的第一列为记录当前数据的时间戳，后两列为坐标。由于GPS每隔500时间单位保存一次数据，激光雷达每隔300时间单位保存一次数据，因此，一段时间内激光雷达保存的数据比GPS保存的数据要多。现在想取出两个文件中时间戳最接近的数据，并分别存放在gps2.txt和laser2.txt中，编写程序实现。（不知道哪位大神能讲下这道题。。。）

二 关键帧

关键帧的概念

关键帧目前是一种非常常用的方法，可以减少待优化的帧数，并且可以代表其附近的帧。可以理解为一个学校里有100个班级，每个班的班长就是一个关键帧，他可以代表他班里的人，那么如何选取关键帧呢？

关键帧作用：图像插入频率过高会导致信息冗余度快速增加，而这些冗余的信息对系统的精度提升却十分有限，甚至没有提高，反而消耗了更多的计算资源。关键帧的目的在于，适当地降低信息冗余度，减少计算机资源的损耗，保证系统的平稳运行。

0 为什么要选择关键帧

答：1 后端通常实时性较差,不适合处理所有帧；2 如果相机停止,可能给后端留下无用的优化,甚至导致后端问题退化

1 关键帧选择指标（如何选择关键帧）

1. 跟踪质量：比如当前帧跟踪到的特征点数大于一定阈值，如大于50个点，或关键帧跟踪到的点比参考关键帧少90%。
2. 距离最近关键帧的距离是否足够远（空间）：即当前帧空间位置是否有足够的变换，如在静止不动或移动幅度较小的情况下，当移动角度大于一定程度才认为是关键帧。
3. 距离上一关键帧的帧数是否足够多（时间）：如过了20帧仍没有插入关键帧；
4. 局部地图空闲

2 ORB-SLAM中关键帧之间的连接，共视图（Covisibility Graph）数据结构

1. ORB-SLAM2中关键帧之间的连接是通过共视图（Covisibility Graph）和生成树（Spanning Tree）表达的。
2. 共视图：是一个有权重的无向图，图的结点为一个关键帧，如果两个关键帧能共同观测到一定数量的地图点，那么这两个关键帧之间建立一条边，边的权重为共同观测到的地图数量。
3. 生成树： 生成树是共视图的包含最少边的子图，每次向生成树添加一个关键帧时，将该关键帧与树中共视地图点数量最多的关键帧连接。从生成树中删除一个关键帧时，也要更新受到影响的所有关键帧的连接关系。
4. 参考
   

3. 关键帧在SLAM里应用非常多，很多知名的开源算法都使用了关键帧。请你用自己的语言描述一下关键帧是什么？有什么用？如何选择关键帧？

关键帧目前是一种非常常用的方法，可以减少待优化的帧数，并且可以代表其附近的帧。可以理解为一个学校里有100个班级，每个班的班长就是一个关键帧，他可以代表他班里的人，那么如何选取关键帧呢？

选取的指标主要有：

（1）距离上一关键帧的帧数是否足够多（时间）。比如我每隔固定帧数选择一个关键帧，这样编程简单但效果不好。比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重要的帧。

（2）距离最近关键帧的距离是否足够远（空间）/运动

比如相邻帧我根据pose计算运动的相对大小，可以是位移也可以是旋转或者两个都考虑，运动足够大（超过一定阈值）就新建一个关键帧，这种方法比第一种好。但问题是如果对着同一个物体来回扫就会出现大量相似关键帧。

（3）跟踪质量（主要根据跟踪过程中搜索到的点数和搜索的点数比例）/共视特征点

这种方法就是记录当前视角下的特征点数，或者视角，当相机离开当前场景时才会新建关键帧，避免了第2种方法的问题。缺点是比较复杂

打个比方，关键帧相当于slam的骨架，是在局部一系列普通帧中选出一帧作为局部帧的代表，记录局部信息。举例来说，摄像头放在原处不动，普通帧还是要记录的，但关键帧因为总看到原场景，所以不会增加。

三角化需要一定程度的共视区域，所以普通帧每2帧之间会存在大量的信息冗余，如果所有帧全部参与计算，不仅浪费了算力，对内存也是极大的考验，这一点在前端vo递归处理方式中表现不明显，但在后端优化里是一个大问题，所以关键帧主要作用是面向后端优化的算力与精度的折中。此外，关键帧选择时还会对图片质量、特征点质量等进行考察，一定程度上也发挥了滤波的作用，防止无用的或错误的信息进入优化过程而破坏定位建图的准确性。

选择关键帧主要从关键帧自身和关键帧与其他关键帧的关系2方面来考虑。一方面，关键帧自身质量要好，例如不能是非常模糊的图像、特征点数量要充足、特征点分布要尽量均匀等等；另一方面，关键帧与其他关键帧之间的关系，需要和局部地图中的其他关键帧有少量的共视关系，但大部分特征点是新特征点，以达到既存在约束，又尽量少的信息冗余的效果，例如局部地图点投影到此帧的点数低于一个阈值或前一个关键帧的特征点在此帧里已经有90%观测不到等等。

在关键帧的运用上，我认为orbslam做的非常好，尤其是在回环检测中使用了以关键帧为代表的帧“簇”的概念，回环筛选中有一步将关键帧前后10帧为一组，计算组内总分，以最高分的组的0.75为阈值，滤除一些组，再在剩下的组内各自找最高分的一帧作为备选帧，这个方法非常好地诠释了“关键帧代表局部”的这个理念。

三 初始化

初始化的意义是求取两个图像间的运动和特征点距离，初始化之后的运动都以初始化时的平移作为单位1，这称为单目的不确定性问题（Scale Ambiguity）。且在初始化时，要保证两帧图片之间的运动必须包括平移（不能只旋转），否则将导致求得的本质矩阵E为0，也就无法分解得到相机位姿。

1. ORB-SLAM初始化的时候为什么要同时计算H矩阵和F矩阵？

当特征点共面或相机间发生了纯旋转时，基础矩阵自由度下降，即发生了所谓的退化，此时如果仍采用八点法估算F矩阵，基础矩阵多出来的自由度将会由噪声决定，对结果造成极大误差。为避免退化现象造成的影响，通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的那个作为最终的运动估计矩阵。

2. 纯旋转的图像帧可以实现ORB－SLAM的初始化吗？

可以的.

• ORB的初始化主要分为以下步骤:

  • step1 : 创建单目初始化器
  • step2 : 确保连续两帧的特征点数都大于100,否则重新构建初始化器
  • step3 : 在mInitialFrame与mCurrentFrame中找匹配的特征点对
  • step4 : 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
  • step5 : 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
  • step6 : 初始化成功后，删除那些无法进行三角化的匹配点
  • step7 : 将初始化的第一帧作为世界坐标系 step8 : 创建初始化地图点MapPoints

• 初始化的关键步骤主要在于step5:

  当系统纯旋转时,由于F矩阵$t=t^ R$,所以当t=0时会发生退化,而H矩阵不会受到纯旋转的影响,此时可以依靠单应矩阵初始化系统

四 重定位

绑架问题就是重定位，是指机器人在缺少之前位置信息的情况下，如何去确定当前位姿。例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，在这种情况下如何重新确定自己的位置。

五 ORB-SLAM2

1. ORB-SLAM中的特征是如何提取的？如何均匀化的？

2. ORB-SLAM在估计相机位姿时，为什么同时计算基础矩阵F和单应性矩阵H？

当特征点共面或相机间发生了纯旋转时，基础矩阵自由度下降，即发生了所谓的退化，此时如果仍采用八点法估算F矩阵，基础矩阵多出来的自由度将会由噪声决定，对结果造成极大误差。为避免退化现象造成的影响，通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的那个作为最终的运动估计矩阵。

3. ORBSLAM整体流程

4. ORB与VINS的区别

前端：

ORB-SLAM：构建图像金字塔，对每一层提取Fast特征点，使用四叉树进行均匀化。对每一层图像进行高斯模糊，然后提取BRIEF描述子。再恢复到第0层并使用opencv自带函数进行图像去畸变；初始化过程通过计算H和F矩阵得分恢复初始化位姿和地图点；没有运动模型时先使用关键帧追踪，之后主要使用恒速运动模型追踪

VINS：对图像使用KLT角点检测算法，对角点使用LK光流（在金字塔上逐层进行）追踪。均匀化操作是对检测出的特征点进行排序，以特征点为中心画圈，在圈中不允许再有其他的特征点存在。提取完特征点后要去畸变，去畸变使用了一个trick：逐渐逼近式去畸变。通过对极约束求一个本质矩阵，使用RANSAC选取一个置信度最高的本质矩阵，对outlier（不符合对极约束的点）进行筛查。

另外vins涉及到了IMU，需要进行IMU的预积分和初始化

后端：

ORB：主要基于共视关键帧，优化变量：地图点坐标，关键帧和一级相邻关键帧的位姿，残差：视觉残差

VINS：基于滑动窗口，优化变量为： 相机到IMU的外参，滑窗中地图点的逆深度，滑窗中所有IMU的状态（P,V,Q,ba,bw），残差：边缘化先验残差，IMU残差，视觉残差；对移除滑窗图像帧信息进行边缘化，将丢弃的信息保留下来继续约束剩余变量。

回环： 两个方法的回环都利用了词袋，vins对角点额外计算了BRIEF描述子。

另外VINs对外参3自由度的平移和1自由度的yaw角进行优化。

六 视觉SLAM的问题

1. 视觉SLAM存在的问题

虽然双目和RGBD不存在初始化化和尺度漂移问题，但是视觉SLAM仍然存在很多共性问题。相机运动太快会导致图像模糊、相机视野不够会导致匹配特征点少、计算量太大（特征提取和匹配）、遮挡、特征缺失、动态物体或光源干扰等。

2 尺度漂移问题

根本原因：单目slam产生尺度漂移的根本原因是单目相机无法根据一张图片得出图中物体的大小，这是尺度漂移的根源；在使用单目估计相机位姿和3D点坐标时，需要通过对极几何、三角化进行估计，在这个过程中会产生误差（特征点精度误差、计算误差），这些误差经过多帧累积后会变得特别大，进而导致尺度的不一致性，造成尺度漂移。
解决办法：1、视觉与IMU融合，借助IMU测得的高帧率的角速度、加速度对视觉进行修正、补充；后端优化时，把尺度作为一个优化变量进行优化，可以减小尺度漂移问题。
补充：由于初始化时存在尺度不确定性，因此单目相机估计的目标物体距离与真实世界里的距离存在比例上的差异，这个比例被称作尺度。而且，受到噪声的影响，导致这个尺度会逐渐漂移、改变，这被称为单目SLAM的尺度漂移问题。从理论上来说，只靠单目相机（不借助其他传感器）是无法确定这个尺度具体是多少的，比较好的解决方法是使用回环检测，但是要求相机的整个运动过程存在回环。

单目视觉slam中尺寸漂移是怎么产生的

单目相机根据一张图片无法得出一张图片中物体的实际大小，同理也就无法得出运动的尺度大小，这是产生尺度漂移的根源。而在优化过程中，单目相机使用对极几何中的三角测量原理求解深度信息，而三角测量中，极小的角度误差在累积之后深度不确定都会变得很大，从而无法保证尺度一致性。

七 视觉惯性SLAM

什么是紧耦合、松耦合？优缺点。

1. VIO是融合相机和IMU数据实现SLAM的算法，根据融合框架的区别又分为紧耦合和松耦合，松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块，将每个模块的输出结果进行融合，而紧耦合则是使用两个传感器的原始数据共同估计一组变量，传感器噪声也是相互影响的，紧耦合算法上比较复杂，但充分利用了传感器数据，可以实现更好的效果，是目前研究的重点。
2. 按照是否把图像的Feature加入到状态向量区分，也就是松耦合是在视觉和IMU各自求出的位姿的基础上做的耦合，紧耦合是使用图像和IMU耦合后的数据计算相机位姿。

八 IMU

1. 测量原理

加速度计的测量原理：一个简单的质量块+弹簧+指示计

加速度计测量值$a_m$为弹簧拉力对应的加速度
$$
a_m = \frac{f}{m} = a - g
$$
f 弹簧拉力， a物体为物体在惯性系下的加速度，g重力加速度；

MEMS加速度计利用电容或者电阻桥来等原理来测量$a_m$;

2. 测量模型与运动模型

东北天坐标系()

3. IMU的优点

IMU能帮助单目确定尺度，；
IMU能测量快速的运动；
IMU在相机被遮挡时亦能提供短时间的位姿估计；
视觉SLAM相邻两帧之间没有约束关系，优化误差函数是重投影误差；IMU增加了相邻帧之间的约束关系。

4. IMU参数标定

5.IMU与相机之间的联合标定

如何标定IMU与相机之间的外参数？

九 其它传感器

一 GPS

1、给你xx误差的GPS，给你xx误差的惯导你怎么得到一个cm级别的地图。

在给定一些有噪声的GPS信号的时候如何去精准的定位？

二 其它

一 除了视觉传感，还用过其他传感吗？比如GPS，激光雷达。

十 通用库

一 Eigen

矩阵赋值

按块赋值

#include <Eigen/Dense>
Eigen::Matrix<double, 3, 4> P;
P.block(0,0,3,3) = camera_pose[0].Rwc;
// 前面两个数值是起点，后面两个数值是块的大小，此处是将3*3的Rwc赋值给P的前三列

动态矩阵

MatrixXd M(rows,cols);

二 OpenCV

图像操作	函数
直方图均衡化	equalizeHist( src, dst )

三 G2O

概念：g2o全称是什么？g2o是一个通用的图优化框架。g2o的核里带有各种各样的求解器，而它的顶点、边的类型则多种多样。通过自定义顶点和边，事实上，只要一个优化问题能够表达成图，那么就可以用g2o去求解它。常见的，比如bundle adjustment，ICP，数据拟合，都可以用g2o来做。

参考链接：https://www.cnblogs.com/gaoxiang12/p/5304272.html

内容

组件	作用
apps	一些应用程序。好用的g2o_viewer就在这里。其他还有一些不常用的命令行工具等。
core	核心组件，很重要！基本的顶点、边、图结构的定义，算法的定义，求解器接口的定义在这里。
examples	一些例程，可以参照着这里的东西来写。不过注释不太多。
solvers	求解器的实现。主要来自choldmod, csparse。在使用g2o时要先选择其中一种。
types	各种顶点和边，很重要！我们用户在构建图优化问题时，先要想好自己的顶点和边是否已经提供了定义。如果没有，要自己实现。如果有，就用g2o提供的即可。

1 G2O的类结构

​ 先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph（超图）。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

　　在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步
$$
HΔx=−b
$$
这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

2 G2O 优化方法的选择步骤：

1. 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
2. 选择一个 BlockSolver 。
3. 选择一个迭代策略，从GN, LM, Dogleg中选。

3 G2O工程化的注意事项

图优化流程：

1. 选择节点和边，确定参数化形式
2. 加入节点和边
3. 选择初值，开始迭代
4. 计算J和H
5. 解H△x = -b
6. GN/LM
7. g2o需要实现其中的③-⑥

g2o

实现过程 ：选择节点和边
节点：g2o :: VertexSE3Expmap（相机位姿）g2o :: VertexSBApointXYZ（路标）
边：g2o :: EdgeProjectXYZ2UV（重投影误差）

样例代码

/**
 * BA Example
 * Author: Xiang Gao
 * Date: 2016.3
 * Email: gaoxiang12@mails.tsinghua.edu.cn
 *
 * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。
 */

// for std
#include <iostream>
// for opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>
// for g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>


using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main( int argc, char** argv )
{
    // 调用格式：命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] );
    cv::Mat img2 = cv::imread( argv[2] );

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"匹配点不够！"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );

    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx;
        double y = ( pts1[i].y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );

    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;

    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else
        {
            inliers++;
        }
    }

    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}


int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
{
    cv::ORB orb;
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb( img1, cv::Mat(), kp1, desp1 );
    orb( img2, cv::Mat(), kp2, desp2 );
    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for ( auto m:matches )
    {
        points1.push_back( kp1[m.queryIdx].pt );
        points2.push_back( kp2[m.trainIdx].pt );
    }

    return true;
}

四 Ceres

五 ROS

ROS系统的缺点

1. 数据copy次数过多

   1. 节点到用户态内存copy
   2. 发送方到内核态copy
   3. tcp链接后从内核态到接受节点copy
   4. 接受节点反序列化乘结构化消息copy

   改进

   • 共享内存通信
     • 发送节点序列化乘流失数据copy
     • 接受节点直接把共享内存数据反序列化copy

六 DBOW

训练字典

实际 BoW 使用时,字典往往是从更大的数据集中生成的,而且最好是来自目标应该环境类似的地方。我们通常使用较大规模的字典——越大代表字典单词量越丰富,容易找到与当前图像对应的单词,但也不能大到超过我们计算能力和内存。

int main( int argc, char** argv )
{
    // read the image
    cout<<"reading images... "<<endl;
    vector<Mat> images;
    for ( int i=0; i<10; i++ )
    {
    string path = "./data/"+to_string(i+1)+".png";
    images.push_back( imread(path) );
    }
    // detect ORB features
    cout<<"detecting ORB features ... "<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
    vector<KeyPoint> keypoints;
    Mat descriptor;
    detector->detectAndCompute( image, Mat(), keypoints, descriptor );
    descriptors.push_back( descriptor );
    }
    // create vocabulary
    cout<<"creating vocabulary ... "<<endl;
    DBoW3::Vocabulary vocab;
    vocab.create( descriptors );
    cout<<"vocabulary info: "<<vocab<<endl;
    vocab.save( "vocabulary.yml.gz" );
    cout<<"done"<<endl;
}

DBoW3 的使用方式非常容易。我们对十张目标图像提取 ORB 特征并存放至 vector容器中,然后调用 DBoW3 的字典生成接口即可。在 DBoW3::Vocabulary 对象的构造函数中,我们能够指定树的分叉数量以及深度,不过这里使用了默认构造函数,也就是 k =10, d = 5。这是一个小规模的字典,最大能容纳 10000 个单词。对于图像特征,我们亦使用默认参数,即每张图像 500 个特征点。最后我们把字典存储为一个压缩文件。

十一 开放问题

一 自己的项目

1. 你做的工作在本质上有什么不同，贡献，创新本质上在哪里？

1. 给定几个连续帧的带有位姿的帧，如何去测量车道线相对于世界坐标系的坐标。
2. 给一张图片，知道相机与地面之间的相对关系，计算出图的俯视图。

2. 说一个自己熟悉的SLAM算法，Lidar/Visual slam，说优缺点。

3. 说一下VINS-Mono的优缺点

4. 其他

二 设计系统

机器人从超市门口出发，前往3公里外的小区送货。请你设计一个定位系统，包括传感器的配置、算法的流程，用伪代码写出来。

三 其他问题

你还有什么问题？

可以从以下方面进行提问：(1) 部门(2) 业务范围(3) 培养方式(4) 工作模式(5) 后续通知时间

你的缺点

1. 喜欢熬夜

四 SLAM行业相关问题

1. 大家都是SLAM方向的研究者，不管是学生还是已经工作，以后都面临找（换）工作的问题，那么你知道哪些做SLAM技术的公司？

2. 讨论一下SLAM应用场景及落地的问题。大家觉得SLAM技术最适合的应用场景是什么？在哪个场景能够最快技术落地呢？

十二 笔试

选择题

1. 以下哪一种情况，可以认为双目视觉系统内参和双目间的相对旋转标定正确

   A. 计算标定时采集的pattern重投影误差，数值相对较小

   B. 使用双目拍摄远处的雪山，无论怎么移动相机，在recify图像上，雪山在左右成像位置相同，但亮度差异较大

   C. 使用双目拍摄远处的车辆，无论怎么移动相机，在recify图像上，车辆在左右成像尽在x方向有disparity；

   D. 使用双目拍摄远处的彩虹，无论怎么移动相机，在recify图像上，彩虹在左右成像的亮度没有差异

答：C

2. 以下哪些相机模型不适用与FOV约180度鱼眼相机投影关系（r为像高，f为焦距，$\theta$为光线到光心的理想入射角）

   A. 针孔相机模型 像高表示为$r=f\tan(\theta)$

   B. 等距投影模型 像高表示为$r=f\theta$

   C. 等立体角投影 像高表示为$r=2 f\sin(\theta)$

   D. 正交投影模型 像高表示为$r=f\sin(\theta)$

3. 以下哪些做法有助于神经网络过拟合

   A. 使用更深的网络以提升泛华性

   B. 进行数据增强以增加数据的多样性

   C. 加入dropout模块以增加训练过程的随机性

   D. 使用leaky relu 取代relu避免特征损失过多

   答：BCD

4. 以下哪些手段可以提升重复纹理环境的定位感知能力

   A. 调整SGBM的参数，增大平滑项的权重

   B. VIO后端增加robust norm 降低feature误匹配带来的误差

   C. 对原图进行高斯滤波，降低重复纹理的梯度

   D. 对depth增加时域的滤波，降低depth误匹配的影响

   答：获取更多的训练数据; 减小网络容量; 添加权重正则化; 添加dropout.

判断题

1. SIFT是图像的全局特征，具有尺度不变和旋转不变性（错）

2. 训练卷积神经网络时，可以对输入进行旋转、平移、缩放、翻转等预处理提高模型泛化能力（对）

3. Hough变换可以用来在图像中进行形状提取（）

4. 牛顿迭代法的基本思想是使用泰勒技术展开式去近似地代替非线性回归函数，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳系数，最后使原模型的残差平方和达到最小（）

简答题

一：

二： 卡尔曼滤波

三

面试

一 美团

打赏