本篇文章将向大家介绍在SLAM中使用的相关优化算法及其在ORB-SLAM3中相关模块中的具体应用案例。
首先，SLAM系统的核心功能之一为视觉里程计（Visual Odometry, VO，也通常称为前端，记录智能体随时间变化在场景中运动的姿势方向和位移，由于前端长时间运行会出现累积漂移和误差，因此也还有回环和地图合并检测等后端模块来进行误差矫正和消除）.
在系统初始化或tracking跟踪丢失创新新地图时，地图初始化时的第一帧的位姿设定为单位矩阵，第二帧的位姿以及两个连续帧的匹配特征点对应的地图点初始值通过Camera类的ReconstructWithTwoViews函数进行计算实现，具体是通过对极约束和三角测量来分别估计相机运动位姿和求解像素的深度信息和对应的物理世界的地图点坐标）。
MLPnPSolver优化器，该优化器的作用为基于最大似然的PnP（Perspective-n-Point）问题求解器，PnP问题的一种典型应用场景为基于匹配的前后帧的3D坐标和2D图像特征点集合来估计相机运动的位姿（旋转+平移），在Tracking的重定位算法中用于快速找出相机位姿的初始解供后续进一步优化。MLPnPSolver基于最小二乘的高斯牛顿法迭代求解逐步减小重投影误差来优化相机位姿，其中MLE(最大似然估计）是一种基于统计概率的参数估计方法，在SLAM应用场景中，参数为相机位姿，观察值为图像中ORB特征点的像素坐标，通过MLE算法，找到最优的参数解等价于使得重投影误差最小化。在算法的实现过程中，由于图像的位姿如旋转矩阵的参数对于误差函数不方便直接求导，而是通过罗德里格斯公式将旋转矩阵转化为旋转向量，和位移一起构成6维向量根据李代数的扰动模型进行求导，而在进行优化时，误差项e为图像uv平面的二维向量。因此目标函数将表现为向量的形式，基于雅可比的矩阵导数的优化比较直观，在大规模问题场景中（如基于地图的全局位姿和地图点优化），利用雅可比矩阵的稀疏性能更高效求解。在MLPnPSolver中，同时采用了RANSAC（Random Sample Consensus，重采样一致性算法）算法，RANSAC是一种鲁棒估计方法，其也采用了多次迭代求解，每次迭代随机选择一定数量的匹配3D2D点对进行求解，并统计内点（求得的位姿进行匹配点映射后误差在一定小的范围内）的数量，最后选择内点数量最多的为最后的位姿优化结果。
Optimizer类的相关优化函数；（1）、PoseOptimization，基于3D2D匹配点对和已经根据Tracking的过程优化的初始位姿，采用g2o的图优化方法来优化位姿，其中g2o的图优化中，节点有基于位姿的g2o::VertexSE3Expmap，边有ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose（顾名思义，这条边仅仅通过相机位姿SE3将地图点坐标映射到图像像素且仅仅优化位姿Pose），以及设置观测值（像素坐标），核函数等，（2）、LocalBundleAdjustment，基于传入的关键帧及其共视关键帧集合对多帧位姿和地图点进行联合优化，同样基于g2o的图优化算法，这里有一个需要注意的地方在于地图点的观测帧不属于共视关键帧的观测帧将作为位姿固定的节点（不参与位姿优化的节点）加入到优化的graph里。这里用的边为EdgeSE3ProjectXYZ，将同时优化位姿和地图点；（3）、GlobalBundleAdjustemnt和BundleAdjustment将以关键帧数组和地图点数组来构建g2o优化图进行联合位姿和地图点优化。(4)，Optimizer还有一些和IMU相关的联合优化方法实现，具体可以参考文献2中的代码实现[2]。
Sim3Solver：Horn’s 四元数方法（Horn’s Method）是一种用于最优对齐两个三维点集的方法，广泛用于位姿估计、点云配准、结构光扫描等场景。它通过最小化均方误差（Least-Squares Error），利用四元数来高效求解旋转矩阵R，在ORB-SLAM3系统中，其主要用于（1）、回环匹配关键帧和当前关键帧的变换求解；求解的参数有旋转、平移和尺度信息，为七个自由度；（2）、合并匹配关键帧和当前关键帧的变换求解。
References
- [1]、Sim2Solver：Sim3Solver优化方法介绍
- [2]、Optimizer的实现： src/Optimizer.cc
Leave a Reply