ORB-SLAM3 源码分析

标签：ORB 地图 SLAM3 源码位姿优化 cv

一、ORB-SLAM3介绍

ORB-SLAM3是一个先进的同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）系统，实现了基于视觉惯导紧耦合，同时能够对多地图进行复用；另外支持单目/双目/RGB-D作为输入，支持针孔以及鱼眼相机模型。是目前种类最齐全、工程化最好、精度和鲁棒性整体最佳的一个工程框架。

二、视觉SLAM的基本架构

ORB-SLAM3的系统架构如下图所示：

以下是对ORB-SLAM3系统架构的介绍：

跟踪（Tracking）
- 负责从图像中提取特征点，并通过匹配这些特征点来估计相机的位姿。
局部地图（Local Mapping）
- 负责维护和优化局部地图，包括添加新的关键帧和地图点，以及进行局部BA（Bundle Adjustment）优化。
闭环检测（Loop Closing）
- 负责检测回环，并通过回环校正来减少累计误差。
地图管理（Map Management）
- 负责管理多个地图和任务，并进行全局优化。

ORB-SLAM3的系统运行流程图如下图所示：

三、ORB-SLAM3的关键特性

视觉-惯性SLAM
- 集成IMU传感器，提供更高精度的定位。
多地图系统
- 支持同时处理多个地图，适应更复杂的场景。
优化的算法
- 增强了跟踪和优化的鲁棒性和效率。
广泛的传感器支持
- 支持单目、双目、RGB-D相机以及鱼眼镜头。

四、ORB-SLAM3的模块分析

1. 项目结构

ORB-SLAM3的源码结构如下：

ORB_SLAM3
├── CMakeLists.txt
├── Examples
├── Thirdparty
├── include
├── src
└── Vocabulary

CMakeLists.txt：CMake配置文件，用于构建项目。
Examples：包含了各种使用ORB-SLAM3的示例代码。
Thirdparty：第三方依赖库，如DBoW2和g2o。
include：头文件目录。
src：源代码目录。
Vocabulary：字典文件，用于ORB特征匹配。

2. 主要模块

ORB-SLAM3 的架构主要包括以下几个模块：

（1）跟踪（Tracking）

跟踪模块负责从图像中提取特征点，并通过匹配这些特征点来估计相机的位姿。以下是跟踪模块的核心代码：

// src/Tracking.cc
bool Tracking::TrackWithMotionModel() {
    // 获取上一帧的位姿
    cv::Mat currentPose = mLastFrame.mTcw.clone();

    // 通过运动模型预测当前帧的位姿
    mCurrentFrame.mTcw = mpMotionModel->Predict(currentPose);

    // 特征匹配
    ORBmatcher matcher(0.9, true);
    vector<MapPoint*> vpMatches;
    int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, vpMatches);

    // 位姿优化
    if (nmatches < 10)
        return false;

    // 使用PnP优化
    Optimizer::PoseOptimization(&mCurrentFrame);
    return true;
}

（2）局部地图（Local Mapping）

局部地图模块负责维护和优化局部地图，包括添加新的关键帧和地图点，以及进行局部BA（Bundle Adjustment）优化。

// src/LocalMapping.cc
void LocalMapping::Run() {
    while (true) {
        // 插入新的关键帧
        if (CheckNewKeyFrames()) {
            ProcessNewKeyFrame();
        }

        // 局部BA优化
        if (!CheckNewKeyFrames()) {
            OptimizeLocalMap();
        }

        // 其他逻辑
        ...
    }
}

void LocalMapping::ProcessNewKeyFrame() {
    // 添加新关键帧到地图
    mpMap->AddKeyFrame(mpCurrentKeyFrame);

    // 创建新的地图点
    CreateNewMapPoints();

    // 局部BA优化
    OptimizeLocalMap();
}

（3）闭环检测（Loop Closing）

闭环检测模块负责检测回环，并通过回环校正来减少累计误差。以下是闭环检测模块的核心代码：

// src/LoopClosing.cc
void LoopClosing::Run() {
    while (true) {
        // 检测回环
        KeyFrame* pKF = DetectLoop();
        if (pKF) {
            CorrectLoop(pKF);
        }

        // 其他逻辑
        ...
    }
}

KeyFrame* LoopClosing::DetectLoop() {
    // 词袋模型匹配
    DBoW2::BowVector bowVec;
    ComputeBoW(pKF, bowVec);
    vector<KeyFrame*> candidates = mpKeyFrameDB->DetectLoopCandidates(pKF, bowVec);

    // 检测闭环
    for (KeyFrame* candidate : candidates) {
        if (CheckLoop(pKF, candidate)) {
            return candidate;
        }
    }

    return nullptr;
}

五、ORB-SLAM3的详细技术分析

1. 系统初始化

在初始化阶段，系统会加载配置文件、字典文件，并创建SLAM系统对象。代码如下：

// main.cc
int main(int argc, char **argv) {
    // 加载配置文件
    cv::FileStorage fsSettings(argv[1], cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "Failed to open settings file at: " << argv[1] << endl;
        return -1;
    }

    // 加载词袋模型
    ORB_SLAM3::System SLAM(argv[2], argv[3], ORB_SLAM3::System::MONOCULAR);

    // 其他初始化代码
    ...
}

2. ORB特征提取

ORB-SLAM3 使用 ORB 特征进行图像匹配和位姿估计。ORB 特征结合了 FAST 角点检测和 BRIEF 描述子，通过在不同的尺度空间检测特征点并计算描述子，确保了特征点的尺度不变性和旋转不变性。ORB特征提取是ORB-SLAM3的核心步骤之一，以下是特征提取的关键代码：

// src/ORBextractor.cc
void ORBextractor::operator()(InputArray _image, vector<KeyPoint>& _keypoints, OutputArray _descriptors) {
    // 图像预处理
    Mat image = _image.getMat();
    assert(image.type() == CV_8UC1);

    // 检测FAST角点
    vector<KeyPoint> keypoints;
    FAST(image, keypoints, mnFASTThreshold, true);

    // 计算ORB描述子
    Mat descriptors;
    computeDescriptors(image, keypoints, descriptors);

    // 返回结果
    _keypoints = keypoints;
    _descriptors = descriptors.clone();
}

3. 位姿估计

在ORB-SLAM3中，位姿估计和地图构建是系统的核心功能。通过匹配当前帧与参考帧的ORB特征点，使用PnP算法进行位姿估计，并将位姿添加到地图中。以下是Tracking类的TrackWithMotionModel函数源码分析：

// src/Tracking.cc
bool Tracking::TrackWithMotionModel() {
    // 获取上一帧的位姿
    cv::Mat currentPose = mLastFrame.mTcw.clone();

    // 通过运动模型预测当前帧的位姿
    mCurrentFrame.mTcw = mpMotionModel->Predict(currentPose);

    // 特征匹配
    ORBmatcher matcher(0.9, true);
    vector<MapPoint*> vpMatches;
    int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, vpMatches);

    // 如果匹配点数不足，则返回false
    if (nmatches < 10)
        return false;

    // 使用PnP优化
    cv::Mat R, t;
    bool bPnP = Optimizer::PoseOptimizationPnP(mCurrentFrame, R, t);

    if (!bPnP)
        return false;

    mCurrentFrame.SetPose(Optimizer::ConvertRtoT(R, t));

    return true;
}

4. 特征匹配与优化

特征匹配是ORB-SLAM3中的关键步骤，通过ORB特征进行匹配。

//src/ORBmatcher.cc
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, vector<MapPoint*> &vpMatches) {
    // 投影上一帧的地图点到当前帧
    for (int i = 0; i < LastFrame.N; i++) {
        MapPoint* pMP = LastFrame.mvpMapPoints[i];

        if (pMP) {
            // 计算投影点
            cv::Point2f uv = CurrentFrame.project(pMP->GetWorldPos());
            if (CurrentFrame.isInFrame(uv)) {
                // 进行匹配
                int idx = CurrentFrame.SearchORBDescriptor(uv, pMP->GetDescriptor());
                if (idx >= 0) {
                    vpMatches[idx] = pMP;
                }
            }
        }
    }
    return vpMatches.size();
}

优化则是通过BA（Bundle Adjustment）进行的。

//src/Optimizer.cc
bool Optimizer::PoseOptimizationPnP(Frame &Frame, cv::Mat &R, cv::Mat &t) {
    // 设置PnP求解器
    cv::Mat inliers;
    cv::solvePnPRansac(Frame.mvKeysUn, Frame.mvMapPoints, Frame.mK, cv::Mat(), R, t, false, 100, 8.0, 0.99, inliers);

    // 判断优化是否成功
    return inliers.rows >= 10;
}

5. 地图优化与维护

ORB-SLAM3 使用非线性优化方法（如 g2o）进行位姿图优化。在地图构建过程中，系统会不断添加关键帧和地图点，并通过图优化算法（如 Levenberg-Marquardt）对位姿进行优化，减少累计误差。

// src/Optimizer.cc
void Optimizer::OptimizePoseGraph(Map* pMap) {
    // 设置优化问题
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // 添加顶点和边
    for (auto& vertex : pMap->GetAllKeyFrames()) {
        optimizer.addVertex(vertex);
    }
    for (auto& edge : pMap->GetAllEdges()) {
        optimizer.addEdge(edge);
    }

    // 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(10);
}

六、ORB-SLAM3与ORB-SLAM2的区别

ORB-SLAM3相较于ORB-SLAM2，进行了多项改进和扩展，主要包括以下几点：

多传感器支持
- ORB-SLAM3 支持单目、双目和RGB-D相机，而ORB-SLAM2仅支持单目和双目相机。
多地图与多任务
- ORB-SLAM3 能够同时处理多个地图和任务，适用于更复杂的场景和应用。
更高的鲁棒性与适应性
- ORB-SLAM3 通过改进的算法和优化方法，在复杂环境中表现出更高的鲁棒性和适应性。
性能优化
- ORB-SLAM3 在多个方面进行了性能优化，提高了系统的运行速度和效率。

七、总结

通过对ORB-SLAM3的详细分析，可以看到其在SLAM系统中的多项改进，包括视觉-惯性融合、多地图支持以及优化算法的提升。这些改进使得ORB-SLAM3在动态和复杂环境中表现更为出色，为机器人自主导航、增强现实等领域提供了强有力的技术支持。结合具体源码的分析，更好地理解了ORB-SLAM3的实现细节和工作原理，为未来的研究和应用提供了重要参考。

标签：ORB,地图,SLAM3,源码,位姿,优化,cv
From： https://www.cnblogs.com/blue666/p/18278363