k42946的博客

前面介绍的特征点的深度确定实际上已经建立了地图，不过是稀疏地图。对于实际导航、避障等的地图要求的是稠密地图。所以这篇文章先介绍单目相机进行稠密地图重建。此方法广泛应用于经典SLAM系统（如LSD-SLAM、ORB-SLAM的稠密模块），是单目稠密重建的数学基础。是经典多视图几何方法的核心技术。由块匹配的相似性计算（如 SSD 的曲率）。通过配方法合并指数项，可得更新后的均值和方差。（由块匹配得到），假设观测噪声为高斯分布。接下来介绍块匹配的方法。单目稠密地图重建中，（由极线搜索得到），）时，认为深度收敛。

简单来说就是误差会随着机器人的运动逐渐积累。而回环检测可以检测是否回到了起点，或者曾经来过的位置。用这个信息来修正系统的误差，相当于每次回到原点都进行一次校准。从而避免误差的持续累积。是回环检测中最流行的方法之一，这篇文章将介绍SLAM回环检测中的词袋模型，以及用C++的BoW库（以DBoW3为例）在Linux环境下实现字典训练和相似度计算的完整流程。词袋模型（Bag of Words, BoW）是一种将图像特征编码为“视觉单词”的技术，常用于SLAM中的回环检测。

实际上，经过若干次迭代之后，收敛的特征点位置变化很小，发散的外点则已被剔除。对收敛点再进行优化，似乎是有些费力不讨好的。因此，我们更倾向于在优化几次之后就把特征点固定住，而不再实际地优化它们的位置估计。形式过于复杂，我们通常取它们的近似。接下来就可以按照前面的最小二乘问题的方法进行求解。用高斯牛顿法等方法。这种把优化变量去除特征点，只剩下位姿的优化问题就构成了。由于存在噪声，这个式子不会严格相等，于是上式两边左乘。两个位姿构成了结点，过度位姿构成了位姿图的边。，但由于噪声等误差的存在而并不等于0。

带有相机位姿和空间点的图优化称为BA，它能够有效地求解大规模的定位与建图问题。但是在SLAM过程中，往往需要控制BA的规模，以保持计算的实时性。最简单的控制BA规模的思路，是仅保留离当前时刻最近的N个关键帧。于是，我们的BA将被固定在一个时间窗口内，离开这个窗口的则被丢弃。这种方法称为滑动窗口法。目标：在计算资源受限的情况下，通过维护一个固定大小的状态窗口，持续优化最新的相机位姿和地图点，同时边缘化旧状态以保持计算效率。变量定义：优化目标：最小化窗口内所有观测的重投影误差：E(x)=∑i=1M∑j∈V

重投影误差：对于第iii个相机位姿（参数化为李代数ξi∈se3ξi​∈se3）和第jjj个三维点Xj∈R3Xj​∈R3，其投影到图像的观测像素坐标为uijuij​。pijπexp⁡ξi∧Xjpij​πexpξi∧​Xj​其中π⋅\pi(\cdot)π⋅eijuij−pijeij​uij​−pij​目标函数：min⁡ξ。

光度误差：基于灰度不变假设，直接比较像素亮度。李代数扰动模型：用于计算位姿变化对投影点的影响。雅可比矩阵：结合图像梯度和几何变换导数，指导优化方向。多尺度优化：还可以采用图像金字塔提高鲁棒性和收敛性。通过迭代优化，直接法能够有效估计相机的位姿变化，适用于特征缺失的场景，但对光照变化和初始值敏感。

  前面的文章我们介绍了使用特征点估计相机运动的方法。尽管其在视觉里程计中占据主流地位，但它至少有以下缺点：  因此我们通过使用光流法跟踪特征点的运动，替代特征点的匹配。光流法的计算时间要优于特征点匹配。  计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以Lucas-Kanade光流为代表，并可以在SLAM中用于跟踪特征点位置。稠密光流以Horn-Schunck光流为代表。文章以LK光流为例进行介绍。Lucas-Kanade（LK）光流法是一种基于亮度恒定假设和局部运动一致性假设的稀

通过最大化点对之间的协方差，使得变换后的源点云与目标点云在最小二乘意义下对齐。通过上述步骤，可高效求解已知匹配3D点对的刚体变换，适用于点云配准、物体位姿估计等场景。的奇异向量反映了点云之间的主要对齐方向。），其对角线元素绝对值不超过1。通过SVD分解，协方差矩阵。则通过质心差校正整体偏移。目标是求解刚体变换旋转矩阵。的对角线元素尽可能大。利用迹的循环置换性质。

中，我们使用了李代数进行优化求解，以去除旋转矩阵本身的约束以简化优化问题。因此有必要对相关的李代数知识进行讲解。SO(n)和SE(n)在实数空间上是连续的。如果某种集合中的成员进行某种运算，得到的结果仍然属于这个集合，则这种运算是“良好的运算”，即对这个运算是。只有一个（良好的）运算的集合，称之为群。旋转矩阵和变换矩阵都是对乘法封闭的群。通过扰动模型和反对称矩阵的性质，可高效计算旋转矩阵李代数的导数，为非线性优化提供理论支持。，其中p为三维点，p’ 为观测值。导数的物理意义是：当李代数参数。

旋转矩阵本身带有约束，即正交且行列式为1。而有约束的优化问题比无约束的优化问题复杂的多。因为李代数的特点，李代数表示的天然满足旋转矩阵的约束，因此通常使用。因为这个误差是将3D点的理论投影位置与观测到的实际投影位置之间的误差，因此称为。求相机的位姿R，T。

当知道n个3D空间点及其投影位置和2d像素点时，如何估计相机的位姿。是求解3D到2D点对运动的方法。

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给∆x添加一个范围，称为。SLAM的目标是最小化运动方程和观测方程的误差，这就是一个最小二乘问题。而两个方程通常是非线性的，因此也是一个。的求解要求H可逆，当H不可逆时，求解会出现问题。是f(x)的一阶导数。这个范围限定了二阶近似的有效区域，这类方法也称为。2.对于第k次迭代，求出当前的雅可比矩阵。之后类似高斯牛顿法求解即可。(此时这个方程是关于。

经过对极约束的求解，现在已知R,t，我们想求解两个特征点的深度z1,z2。（在单目相机模型中，深度被抹去了，因此若想建图需要求解‘z’），即，两条线在两个平行的平面中，而两个平面互相平行。因此我们会选择两条线最近的的近似为p点，也就是。当然，由于噪声的存在，我们估得的R,t不一定精确使式子成立，所以更常见的做法是求。使用对极约束估计了相机运动后，接下来利用相机运动估计特征点的空间位置，使用的方法就是。的一个方程，可以根据它直接求得。该式左侧为零，右侧可看成。的存在，两条线可能会出现。

E为3×3的矩阵，有9个未知数。为了便于计算只考虑它的尺度等价性（即矩阵同时除以其中一个未知数，可以使得这个未知数变成1，这样的E也是等价的，从而可以减少一个未知数），使用8对点来估计E——这就是经典的。由于E和F只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的E。考虑一对匹配点，它们的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T。同理，对于其他点对也有相同的表示。如果这八个点互不相关，即系数满秩，那么E的各元素就可由上述方程解得。

有限性：数量较少局域性：特征信息只与较小的区域有关可重复性：相同的特征点可以在相邻帧图片中找到可区分性：不同的特征点易于区分图1可作为特征点的像素点图1 可作为特征点的像素点图1可作为特征点的像素点特征点由**关键点(Key-point)和描述子(Descriptor)**两部分组成。关键点：该特征点在图像里的位置，有些特征点还具有朝向、大小等信息描述子：描述关键点，用于与其他特征点进行区分。

针孔相机模型是一种描述相机成像原理的数学模型，模拟了相机将三维世界中的物体投影到二维图像平面上的过程。而像素坐标（u,v）通常经过一个平移。和缩放α，β（x和y轴上的缩放比例）针孔相机模型的建模及分析过程。

同时定位与地图构建。在未知环境中，同时确定机器人在环境中的位置，并且构建环境的地图。