制作一个静态网站源码,网站建设背景和目标,重庆网站开发哪家专业,长沙如何优化排名视觉SLAM的后端主要解决状态估计问题#xff0c;它是优化相机轨迹和地图点的过程#xff0c;从数学上看属于非线性优化问题。后端的目标是结合传感器数据#xff0c;通过最优估计获取系统的状态#xff08;包括相机位姿和场景结构#xff09;#xff0c;在状态估计过程中…视觉SLAM的后端主要解决状态估计问题它是优化相机轨迹和地图点的过程从数学上看属于非线性优化问题。后端的目标是结合传感器数据通过最优估计获取系统的状态包括相机位姿和场景结构在状态估计过程中通常引入概率模型以最大化后验概率或最小化代价函数的方式求解。以下从概述、BABundle Adjustment捆绑调整与图优化的角度全面介绍视觉SLAM的后端实现。
概述
状态估计的概率解释
SLAM问题本质上是一个状态估计问题常用贝叶斯滤波进行建模 p ( x t ∣ z 1 : t , u 1 : t ) ∝ p ( z t ∣ x t ) ∫ p ( x t ∣ x t − 1 , u t ) p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t − 1 ) d x t − 1 p(\mathbf{x}_t|\mathbf{z}_{1:t},\mathbf{u}_{1:t})\propto p(\mathbf{z}_t|\mathbf{x}_t)\int p(\mathbf{x}_t|\mathbf{x}_{t-1},\mathbf{u}_t)p(\mathbf{x}_{t-1}|\mathbf{z}_{1:t-1},\mathbf{u}_{1:t-1})d\mathbf{x}_{t-1} p(xt∣z1:t,u1:t)∝p(zt∣xt)∫p(xt∣xt−1,ut)p(xt−1∣z1:t−1,u1:t−1)dxt−1 x t \mathbf{x}_t xt 系统状态如相机位姿。 z t \mathbf{z}_t zt 观测如图像特征点。 u t \mathbf{u}_t ut 控制量或运动模型。
该公式表明状态估计是基于当前观测、运动模型和历史状态的一种递归估计。
线性系统和卡尔曼滤波KF
线性系统模型卡尔曼滤波假设状态转移方程和观测方程是线性关系 x t A t x t − 1 B t u t w t \mathbf{x}_t\mathbf{A}_t\mathbf{x}_{t-1}\mathbf{B}_t\mathbf{u}_t\mathbf{w}_t xtAtxt−1Btutwt z t H t x t v t \mathbf{z}_t\mathbf{H}_t\mathbf{x}_t\mathbf{v}_t ztHtxtvt 其中 w t \mathbf{w}_t wt和 v t \mathbf{v}_t vt是噪声通常服从高斯分布。卡尔曼滤波提供了最优状态估计 预测基于运动模型预测当前状态。更新结合观测数据修正状态。
卡尔曼滤波具有很高的计算效率但只能处理线性系统。
非线性系统和扩展卡尔曼滤波EKF
在实际SLAM中状态转移方程和观测方程通常是非线性的 x t f ( x t − 1 , u t ) w t \mathbf{x}_tf(\mathbf{x}_{t-1},\mathbf{u}_t)\mathbf{w}_t xtf(xt−1,ut)wt z t h ( x t ) v t \mathbf{z}_th(\mathbf{x}_t)\mathbf{v}_t zth(xt)vtEKF通过对非线性函数进行一阶线性化将非线性问题近似为线性问题 使用雅可比矩阵线性化 (⋅) 和 ℎ(⋅)在每一步更新中使用卡尔曼滤波进行递归估计。
EKF的讨论
优点 EKF可以处理非线性系统适用于SLAM问题。 局限性 一阶线性化导致近似误差特别是在非线性程度较高时效果较差。计算复杂度较高不适合大规模SLAM系统。
因此在实际的视觉SLAM中后端更多使用非线性优化方法如捆绑调整BA与图优化。
BA与图优化
投影模型与BA代价函数
投影模型 在视觉SLAM中2D观测点 z i \mathbf{z}_i zi 与3D地图点 P j \mathbf{P}_j Pj 的关系由投影模型描述 z i π ( K [ R ∣ t ] P j ) \mathbf{z}_i\pi(\mathbf{K}[\mathbf{R}|\mathbf{t}]\mathbf{P}_j) ziπ(K[R∣t]Pj) K相机内参矩阵。 [ R ∣ t ] [\mathbf{R}|\mathbf{t}] [R∣t]相机位姿。π(⋅)将3D点投影到图像平面。 BA代价函数 BA的目标是最小化重投影误差代价函数为 E ∑ i , j ρ ( ∥ z i − π ( K [ R i ∣ t i ] P j ) ∥ 2 ) E\sum_{i,j}\rho\left(\|\mathbf{z}_i-\pi(\mathbf{K}[\mathbf{R}_i|\mathbf{t}_i]\mathbf{P}_j)\|^2\right) Ei,j∑ρ(∥zi−π(K[Ri∣ti]Pj)∥2) ρ ( ⋅ ) \rho(\cdot) ρ(⋅)鲁棒核函数用于减小异常值的影响。
BA的求解
BA是一个非线性最小二乘优化问题通常使用高斯-牛顿法或列文伯格-马夸尔特LM法求解
高斯-牛顿法直接使用二阶近似求解非线性问题收敛快但对初值敏感。LM方法在梯度下降和高斯-牛顿之间平衡鲁棒性更好。
具体流程
初始化相机位姿和地图点坐标。计算雅可比矩阵并构造稀疏的线性方程。迭代更新位姿和地图点直至误差收敛。
稀疏性与边缘化
稀疏性 BA问题的雅可比矩阵具有稀疏性因为每个观测点仅依赖于特定的相机位姿和地图点。利用稀疏矩阵计算可以显著提高优化效率。边缘化 在SLAM中为了减小计算量会将旧的状态变量边缘化 将不再需要优化的变量如历史关键帧边缘化。通过边缘化保持稀疏结构并降低优化问题的维度。
鲁棒核函数
实际观测中常有异常值outliers如错误匹配的特征点。为减小异常值对优化的影响BA中引入鲁棒核函数
常见核函数有Huber核、Cauchy核等。核函数通过降低异常值的权重使得优化结果更加鲁棒。
总结
视觉SLAM后端通过概率模型和非线性优化方法实现状态估计和地图构建。其核心任务是通过最大化后验概率或最小化重投影误差优化相机轨迹和地图点位置。
状态估计从卡尔曼滤波KF到扩展卡尔曼滤波EKF提供递归的状态更新方案。BA与图优化通过非线性最小二乘优化如BA实现全局优化。鲁棒性和效率利用稀疏矩阵计算、边缘化和鲁棒核函数提升系统的计算效率和鲁棒性。
这种多层次的优化体系是视觉SLAM后端的核心也是其能在复杂环境中实现鲁棒性能的关键。
