EnhancedMarkovSLAM v1.0 🧠 基于马尔可夫的单目SLAM

📖 概述

EnhancedMarkovSLAM 是为Linux设计的单目SLAM框架。它在传统几何SLAM基础上，使用马尔可夫状态预测提高跟踪稳定性。系统包含以下主要模块：

✨ 核心特性

• 完整的单目SLAM流水线 — 初始化、跟踪、局部建图、回环、全局优化。
• 马尔可夫状态预测 — 使用运动模型增强丢失时的鲁棒性。
• 多线程架构 — 跟踪、局部建图、回环、可视化并行运行。
• 基于g2o的高效优化 — 支持局部BA、全局BA、位姿优化、Sim3优化。
• ORB特征 + 快速匹配 — 使用Hamming距离的BFMatcher。
• 实时Pangolin可视化 — 绘制地图点、关键帧、轨迹、当前相机。
• 模块化设计 — 每个组件可独立测试或替换。

⚡ 快速开始

运行demo：从摄像头或视频文件实时SLAM

# 编译后执行
./enhanced_markov_slam [video.mp4]    # 不带参数则打开默认摄像头

# 按键: q 或 ESC 退出

最小示例代码 (main.cpp)：

#include "MarkovSLAM.h"
using namespace MarkovSLAM;

int main() {
    MarkovSLAMSystem slam;
    cv::Mat K = (cv::Mat_<double>(3,3) << 525,0,320, 0,525,240, 0,0,1);
    slam.SetCameraParameters(K, cv::Mat::zeros(4,1,CV_64F));

    cv::VideoCapture cap(0);
    cv::Mat frame;
    while(cap.read(frame)) {
        slam.ProcessFrame(frame, cv::getTickCount()/cv::getTickFrequency());
        if(cv::waitKey(1)=='q') break;
    }
    slam.Shutdown();
    return 0;
}

📦 编译与依赖

Ubuntu 22.04 / Debian 12

sudo apt install build-essential cmake git \
    libopencv-dev libeigen3-dev libpangolin-dev \
    libg2o-dev libsuitesparse-dev libglew-dev

CMake 构建

git clone https://github.com/your/EnhancedMarkovSLAM.git
cd EnhancedMarkovSLAM
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
./enhanced_markov_slam

🏗️ 系统架构

MarkovSLAMSystem 聚合所有模块并管理线程：

• Tracking (主线程) — 处理每一帧，决定关键帧。
• LocalMapping (独立线程) — 处理新关键帧，局部BA。
• LoopClosing (独立线程) — 检测回环并优化。
• Viewer (独立线程) — 绘制地图。

数据流：Frame → Tracking → KeyFrame → LocalMapping / LoopClosing → Map 更新 → Viewer。

🎯 跟踪模块 (Tracking)

核心类 Tracking，负责每帧处理、初始化、跟踪、重定位。

状态机

NOT_INITIALIZED → INITIALIZING → TRACKING ↔ LOST

关键方法

提取ORB特征，根据当前状态执行初始化、运动模型跟踪、参考关键帧跟踪或重定位。

初始化 (单目)

使用两帧匹配，通过 ReconstructWithH 或 ReconstructWithF 恢复位姿并三角化初始地图点。

运动模型

relative_motion_ 预测当前帧位姿，加速匹配。

🗺️ 局部建图 (LocalMapping)

独立线程处理新插入的关键帧：

• ProcessNewKeyFrame — 更新共视图。
• MapPointCulling — 剔除观测少于2的地图点。
• CreateNewMapPoints — 与相邻关键帧三角化新点。
• LocalBundleAdjustment — 优化局部窗口。
• KeyFrameCulling — 剔除冗余关键帧（观测点少于50）。

🔄 回环检测 (LoopClosing)

目前为简化版，接口已预留。实际可扩展为基于词袋或外观的检测。

• DetectLoop() — 待实现。
• ComputeSim3() — 计算相似变换。
• CorrectLoop() — 本质图优化 (调用 Optimizer::OptimizeEssentialGraph)。

📐 优化器 (Optimizer)

封装g2o优化：

全局BA，固定第一帧。

仅优化当前帧位姿，返回内点数量，使用鲁棒核函数。

优化Sim3位姿图，用于回环校正。

📐 三角化 (Triangulator)

纯静态工具类：

• LinearTriangulation — DLT方法。
• MultiViewTriangulation — 选择最大基线对。
• TriangulatePoints — 批量三角化并检查深度/重投影误差。
• ComputeParallaxAngle — 计算视差角(度)。

📌 关键帧 (KeyFrame)

继承自 Frame，增加：

• 地图点关联 (map_points_)
• 共视图 (connected_keyframes_) 与权重
• 简化的词袋向量 (bow_vector_)

重要方法：UpdateConnections() 基于共视地图点更新边。

📍 地图点 (MapPoint)

核心属性：世界坐标、观测关系、代表性描述子、坏点标记。

• AddObservation/RemoveObservation — 维护关键帧观测。
• SetBadFlag() — 从所有观测中移除自己。
• 线程安全 (每个数据成员有自己的mutex)。

👁️ 可视化 (Viewer)

基于Pangolin的3D查看器，独立线程运行。绘制：

• 地图点 — 绿色(高质量)、黄色、红色(低质量)。
• 关键帧 — 蓝色金字塔。
• 当前相机 — 红色金字塔。
• 轨迹 — 蓝色连线 (关键帧中心)。

🧰 工具集 & 全局参数 (Common)

Utils 命名空间提供坐标变换、重投影误差、本质矩阵分解等。

Parameters global_params 可动态调整：

struct Parameters {
    int num_features = 1500;
    float max_reprojection_error = 12.0f;
    float min_parallax = 0.2f;          // 度
    float min_triangulation_dist = 0.05f;
    float max_triangulation_dist = 100.0f;
    // ... 见 Common.h
};

💡 示例：自定义数据集

#include "MarkovSLAM.h"
using namespace MarkovSLAM;

int main() {
    MarkovSLAMSystem slam;
    // TUM 相机参数
    cv::Mat K = (cv::Mat_<double>(3,3) << 535.4,0,320.1, 0,539.2,247.6, 0,0,1);
    slam.SetCameraParameters(K, cv::Mat::zeros(4,1,CV_64F));

    std::vector<std::string> images = loadImagePaths("rgb.txt");
    std::vector<double> timestamps = loadTimestamps("rgb.txt");

    for(size_t i=0; i<images.size(); ++i) {
        cv::Mat frame = cv::imread(images[i]);
        slam.ProcessFrame(frame, timestamps[i]);
        if(slam.GetCurrentPose() != Eigen::Matrix4d::Identity()) {
            auto pos = slam.GetCurrentPose().block<3,1>(0,3);
            printf("Frame %zu: x=%.2f y=%.2f z=%.2f\n", i, pos.x(), pos.y(), pos.z());
        }
        cv::waitKey(20);
    }
    slam.Shutdown();
    return 0;
}

⚙️ 关键配置参数 (Common.h)

🔍 故障排除

📊 性能调优建议

• 根据CPU调整 num_features (500~2000)。
• 局部BA的迭代次数可适当减少 (5-10次)。
• 使用OpenMP加速特征匹配 (当前未用，可添加)。
• 如果不需要实时可视化，注释 viewer_ 线程。
• g2o可使用 LinearSolverEigen 加速 (CMake中已配置)。

📝 更新日志

版本 1.0.0 (2026-03-19)

• 完整单目SLAM流水线：初始化、跟踪、局部建图、回环框架。
• 基于g2o的优化器 (BA、位姿、本质图)。
• Pangolin可视化 (地图点、关键帧、轨迹)。
• 线程安全的Map/KeyFrame/MapPoint。
• 马尔可夫运动预测提升跟踪稳定性。

版本 0.9.0 (2026-02-01)

• 基础框架：特征提取、帧、两视图初始化。

⚖️ 许可证