目录
1.EKF的步骤
一、问题定义与模型建立
二、线性化处理
三、应用卡尔曼滤波
四、迭代与收敛
五、结果评估与优化
注意事项
2.BA问题的步骤
一、问题定义与数据准备
二、构建优化模型
三、选择优化算法
四、执行优化过程
五、结果评估与优化
六、应用与验证
1.BA的定义
2.BA的基本原理
(1) 数学建模
(2) 误差函数构建
(3) 非线性优化
3.VSLAM中的优化
1.EKF的步骤
EKF，即扩展卡尔曼滤波（Extended Kalman Filter），是一种应用于非线性系统的状态估计方法。它通过泰勒级数展开将非线性系统线性化，然后使用标准的卡尔曼滤波算法进行状态估计。以下是EKF问题解决的一般步骤：
一、问题定义与模型建立
明确目标：确定要估计的状态变量，如位置、速度
建立模型：
- 状态方程：描述系统状态如何随时间变化的非线性方程。
- 观测方程：描述系统状态与观测值之间的非线性关系。
二、线性化处理
- 泰勒级数展开：对状态方程和观测方程中的非线性函数进行泰勒级数展开，并忽略二阶及以上项，得到近似的线性化模型。
- 计算雅克比矩阵：线性化后的模型需要计算状态转移矩阵F和观测矩阵H的雅克比矩阵，这些矩阵描述了线性化后的状态变化率和观测变化率。
三、应用卡尔曼滤波
初始化：设置初始状态估计值、初始状态估计误差协方差矩阵等。
预测步骤：
- 状态预测：使用线性化后的状态方程预测当前时刻的状态。
- 协方差预测：使用状态转移矩阵F和初始状态估计误差协方差矩阵预测当前时刻的协方差矩阵。
更新步骤：
- 计算卡尔曼增益：根据观测矩阵H、观测噪声协方差矩阵R和预测协方差矩阵计算卡尔曼增益。
- 状态更新：使用观测值和卡尔曼增益更新状态估计值。
- 协方差更新：使用卡尔曼增益和观测噪声协方差矩阵更新协方差矩阵。
四、迭代与收敛
- 迭代：重复预测和更新步骤，直到满足终止条件（如达到最大迭代次数或状态估计误差小于某个阈值）。
- 收敛性判断：检查状态估计值和协方差矩阵是否收敛，即是否在某个范围内稳定波动。
五、结果评估与优化
- 结果评估：通过对比实际观测值和估计值，评估EKF算法的性能。
- 优化：根据评估结果，调整模型参数、噪声协方差矩阵等，以提高EKF算法的准确性和稳定性。
注意事项
- 线性化假设：EKF依赖于局部线性化假设，当系统非线性较强时，线性化误差可能较大，导致算法性能下降。
- 噪声假设：EKF假设过程噪声和观测噪声都是高斯白噪声，且噪声统计特性已知。在实际应用中，这些假设可能不成立，需要进行适当的调整。
- 计算复杂度：EKF需要计算雅克比矩阵和进行矩阵运算，计算复杂度较高。在实际应用中，需要考虑计算资源的限制。
通过以上步骤，可以解决非线性系统中的状态估计问题，并应用EKF算法进行实时状态估计。
2.BA问题的步骤
BA问题，即束调整（Bundle Adjustment）问题，是计算机视觉和摄影测量中的一个关键问题。它本质上是一个优化问题，旨在通过最小化重投影误差来优化三维结构和相机参数。以下是BA问题解决的一般步骤：
一、问题定义与数据准备
- 明确目标：确定要优化的参数，包括三维点的位置、相机的姿态（旋转和平移）、内部校准参数（如焦距、主点等）以及可能的径向畸变参数。
- 收集数据：获取多张包含同一场景的照片以及对应的相机参数（如焦距、曝光时间等）和图像特征点（如角点、边缘等）的匹配信息。
二、构建优化模型
- 建立重投影误差模型：对于每个图像特征点，计算其在三维空间中的位置，并将其投影到对应的图像平面上，与实际观测到的位置进行比较，形成重投影误差。
- 构建非线性最小二乘问题：将重投影误差的平方和作为目标函数，形成一个非线性最小二乘问题。
三、选择优化算法
- Levenberg-Marquardt算法：BA问题通常使用Levenberg-Marquardt（LM）算法进行求解，因为它在处理非线性最小二乘问题时具有较好的收敛性和鲁棒性。
- 稀疏性利用：由于BA问题的线性系统具有稀疏块结构，可以利用稀疏性来减少计算量，提高求解效率。
四、执行优化过程
- 初始化参数：为待优化的参数设置初始值，这些初始值可以通过其他方法（如SFM、ICP等）获得或随机生成。
- 迭代优化：使用LM算法进行迭代优化，每次迭代都更新参数值，并计算新的重投影误差。
- 检查收敛性：在每次迭代后，检查目标函数的值是否满足收敛条件（如变化量小于某个阈值），如果满足，则停止迭代；否则，继续迭代。
五、结果评估与优化
- 评估结果：检查优化后的参数是否满足预期要求，如三维点的位置是否合理、相机的姿态是否准确等。
- 优化调整：如果评估结果不理想，可以调整优化算法的参数（如LM算法的阻尼因子、迭代次数等），或重新收集数据并重新执行优化过程。
六、应用与验证
- 应用优化结果：将优化后的参数应用于实际场景，如三维重建、相机定位等。
- 验证准确性：通过与其他方法（如激光扫描、GPS等）的结果进行比较，验证BA问题的优化结果的准确性。
通过以上步骤，可以解决BA问题，并优化三维结构和相机参数，为计算机视觉和摄影测量的应用提供准确的基础数据。
3.0滑窗
在视觉SLAM（Simultaneous Localization and Mapping）中，滑动窗口法是一种用于控制后端束调整（Bundle Adjustment，BA）规模的有效方法。以下是视觉SLAM中滑动窗口法的解决步骤：
一、滑动窗口法引入
在SLAM过程中，随着关键帧和路标点的增多，后端BA的计算效率会不断下降。为了避免这种情况，需要使用滑动窗口法将待优化的关键帧限制在一定的数量内，从而控制BA的规模。
二、滑动窗口法操作
滑动窗口法的核心在于如何有效地添加和删除关键帧，同时保持优化问题的稀疏性和一致性。
新增关键帧：
- 当新的关键帧被添加到窗口中时，需要将其及其观测到的路标点一起加入到BA优化问题中。
- 这通常涉及构建新的误差项，并更新整体的Hessian矩阵和残差向量。
删除旧的关键帧：
- 为了保持窗口的大小固定，需要定期删除窗口中的旧关键帧。
- 然而，直接删除关键帧会丢失信息，并破坏相邻关键帧之间的约束关系。因此，实际上采用的是边缘化的方法。
三、边缘化过程
边缘化是一种将某些变量从优化问题中移除，同时保留其信息的方法。在滑动窗口法中，边缘化通常用于处理被删除的关键帧及其观测到的路标点。
选择被边缘化的变量：
- 确定要删除的关键帧，并找出其观测到的所有路标点。
构建边缘化方程：
- 利用BA优化问题的Hessian矩阵和残差向量，构建与被边缘化变量相关的边缘化方程。
执行边缘化操作：
- 通过舒尔补（Schur complement）等方法，将被边缘化的变量从Hessian矩阵中消去，同时更新残差向量。
- 这将导致与被边缘化变量相关的变量在新的线性化点上计算雅可比矩阵，从而引入先验信息。
保持稀疏性：
- 为了保持优化问题的稀疏性，需要采取一些策略来减少Fill-in现象。例如，在边缘化旧的关键帧时，可以同时边缘化其观测到的路标点，从而将路标点的信息转换成剩下关键帧之间的共视信息。
四、滑动窗口法的迭代优化
在滑动窗口法中，随着窗口的滑动，需要不断迭代地更新Hessian矩阵和残差向量，并重新进行BA优化。这通常涉及以下步骤：
更新窗口：
- 根据一定的策略（如时间、空间等）选择新的关键帧添加到窗口中，并删除最旧的关键帧。
构建新的优化问题：
- 根据更新后的窗口内容，构建新的BA优化问题。
执行BA优化：
- 利用现有的优化算法（如Gauss-Newton、Levenberg-Marquardt等）对新的优化问题进行求解。
更新状态估计：
- 根据优化结果更新相机位姿和路标点位置等状态变量。
五、注意事项
一致性问题：
- 在滑动窗口法中，由于边缘化操作会引入先验信息，并导致与被边缘化变量相关的变量在不同的线性化点上计算雅可比矩阵，因此可能会破坏系统的一致性。为了缓解这一问题，可以采取一些策略来减少Fill-in现象，并保持优化问题的稀疏性。
滑动窗口法适用性：
- 滑动窗口法比较适合视觉里程计（Visual Odometry，VO）系统，因为它能够实时地处理新的关键帧并丢弃旧的关键帧。然而，对于大规模建图的系统来说，滑动窗口法可能无法提供足够的全局一致性信息。在这种情况下，可以考虑使用其他方法（如位姿图优化）来补充全局信息。
综上所述，视觉SLAM中的滑动窗口法是一种有效的控制后端BA规模的方法。通过合理地添加和删除关键帧，并保持优化问题的稀疏性和一致性，可以实现实时的位姿估计和地图构建。
1.BA的定义
BA的中文译为“光束法平差”,也有翻译为“束调整”、“捆绑调整”等,是一种用于计算机视觉和机器人领域的优化技术,主要用于精确优化相机参数(包括内参数和外参数)和三维空间中特征点的位置。BA的目标是通过最小化重投影误差来提高三维重建的精度和一致性。重投影误差是指实际观测到的图像点和通过估计的三维点投影到图像平面上的点之间的距离。重投影误差的示意图如图3所示,其中p1 、p2 和p3 是通过特征匹配知道的同一个空间点P的投影,但是相机的位姿是未知的。初值中P的投影p'1 与实际的p1 之间有距离e,此时可以 调整相机的位姿使之变小。由于这个调整要考虑到很多 个点,而每个点的误差不会为0,因此最终的效果应是整体误差的缩小,这种把相机和三维点放在一起进行最小化的问题统称为BA
2.BA的基本原理
BA的基本原理可以概括为以下几个步骤:
(1) 数学建模
假设有n个三维点和m个相机位置,定义一个误差函 数,该函数计算所有图像观测值与重投影值之间的误差和。 考虑空间中的第i个3D路标点坐标为Pi =[Xi ,Yi ,Zi ]T, 其投影的2D像素坐标为pi ,希望计算的相机位姿的李代 数用ξ表述,每个匹配好的特征点相应的深度为s,相机的内参矩阵为K,包含fx 、fy 、Cx 、Cy 四个参数,Pi 变换到相机坐标系下的空间点坐标为P'i ,则3D路标点到2D 特征点的转换关系如下:
(2) 误差函数构建
由于未知的相机位姿和观测中的噪声,这个等式存在 误差,无法找到一个准确的解。因此,将误差求和构造一 个最小二乘问题,然后找到最佳相机位姿以及特征点的位 置进行最小化,方程如下:
(3) 非线性优化
常用的解非线性最小二乘问题的方法有最速下降法、 高斯牛顿(GaussianNewton,GN)法、列文伯格马尔夸特(LevenbergMarquardt,LM)法等。然而,最速下降法 在计算时容易走出锯齿路线，反而增加了迭代次数，GN 法采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果。
LM法可以看作是GN法和最速下降法的结合。它 通过引入一个调整参数μ来平衡GN法和最速下降法,从 而提高算法的鲁棒性和收敛速度。LM法比GN法有更好的全局收敛性,对于不良条件化问题或远离最优解的初始估计,LM法表现更好。但是,LM方法的计算成本更高。通过增量求解方程,可以求解当前变量的一个改进, LM算法的增量方程如下:
在特征点优化的过程中,式(4)是一个三维线性方程组,在位姿估计过程中,式(4)是六维线性方程组。I为一 个单位矩阵,f(x)是在x处的目标函数值。当在FPGA上部署LM算法时,为了避免重复的λ更新操作,设置缩 放因子,并在每次迭代后,检查系统的误差情况,若误差减 小,则说明优化情况良好,λ除缩放因子,若误差增大,则λ乘缩放因子。
在进行矩阵求解时,针对矩阵的规模和类型,目前最 新的成果中有FSFICholesky、FIIterative和PCG等方 法。FSFICholesky是基于Cholesky分解的直接解法, 适用于稠密矩阵,通过将矩阵分解为下三角矩阵及其转置 来解矩阵。FSFICholesky优化了传统Cholesky分解中 的除法和平方根运算,因此在处理小规模矩阵时表现良 好,尤其在硬件实现中效率更高。FIIterative是基于迭 代法的求解方法,适合大规模矩阵,尤其是稀疏矩阵。它 通过迭代计算来逐步逼近线性方程组的解,计算复杂度较 低,适合高并行度的硬件实现。PCG也是一种迭代法,适 用于大规模稀疏对称正定矩阵。它通过引入预条件器来 加速收敛,特别适合解决复杂度较高的问题,在大规模矩 阵求解中常用。
3.VSLAM中的优化
VSLAM系统中的前端和后端均涉及BA优化,它 们各自承担不同的任务,并相互配合,最终完成机器人对 环境的建图和自我定位。前端优化主要处理图像数据,提 取视觉特征并建立初步的位姿估计;后端优化是全局优 化,主要对整个系统中的地图和位姿信息进行全局一致性 的调整和优化。
前端优化算法主要分为3类:对极几何法、迭代最近点算法(Iterative Closest Point, ICP)和透视n点算法(PerspectivenPoint, PnP)。对极几何法由于主要应用于单目相机,无法获取深度信息,因此具有一定的局限性。ICP算法通过旋转和平移操作来最小化两点集之间的距离,但其精度相对较低。PnP算法通过构建非线性优化问题, 采用最小二乘法进行求解，即通常所称的BA,它是最为关键的位姿估计方法之一。在机器人应用中，只有在获得物体的三维位置和方向后，机器人才能实现高效且精确的运动。在AR应用中，系统利用真实场景中的二维或三维对象作为标记,将虚拟信息与真实场景对齐，这一过程也与相机的姿态密切相关，从而为用户带来真实的体验。因此BA的应用范围更加广泛，合性能较为优异。
在后端优化中,BA是一种全局非线性优化方法,它优化的不仅是相机的位姿,还包括3D地图点的位置。BA是一个高度复杂的非线性最小二乘问题,涉及大量的计算资源。BA 的核心目标是最小化所有帧的重投影误差,从而实现整个系统的全局一致性