深度剖析 SLAM 算法：从传感器融合到空间重建的完整技术链路

2.2 视觉 - 惯性紧耦合

SLAM 采用紧耦合的视觉 - 惯性融合策略，这种方法相比松耦合具有更高的精度和鲁棒性。

图 2：视觉 - 惯性紧耦合时序图 - 展示传感器数据的实时融合过程

3. 前端特征处理：从像素到语义的转换

3.1 多尺度特征提取策略

SLAM 在特征提取方面采用了改进的 ORB（Oriented FAST and Rotated BRIEF）特征，并结合多尺度金字塔来提高特征的尺度不变性。

class FeatureExtractor {
private:
    int nfeatures; // 特征点数量
    float scaleFactor; // 尺度因子
    int nlevels; // 金字塔层数
    int iniThFAST; // FAST 阈值
    int minThFAST; // 最小 FAST 阈值
public:
    void extractFeatures(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors) {
        // 1. 构建图像金字塔
        computeImagePyramid(image);
        // 2. 在每层提取 FAST 角点
        std::vector<std::vector<cv::KeyPoint>> allKeypoints(nlevels);
#pragma omp parallel for // OpenMP 并行加速
        for(int level = 0; level < nlevels; level++) {
            extractFASTFeatures(imagePyramid[level], allKeypoints[level], level);
        }
        // 3. 分布均匀化处理
        distributeKeypoints(allKeypoints, keypoints);
        // 4. 计算描述子方向
        computeOrientation(keypoints);
        // 5. 计算 BRIEF 描述子
        computeBRIEFDescriptors(keypoints, descriptors);
    }
private:
    void distributeKeypoints(
        const std::vector<std::vector<cv::KeyPoint>>& allKeypoints,
        std::vector<cv::KeyPoint>& keypoints)
    {
        // 使用四叉树进行特征点分布均匀化
        for(int level = 0; level < nlevels; level++) {
            std::vector<cv::KeyPoint> vToDistribute = allKeypoints[level];
            if(vToDistribute.empty()) continue;
            const int N = vToDistribute.size();
            const int W = 30; // 网格宽度
            const int H = 30; // 网格高度
            // 计算每个网格应该保留的特征点数
            const int nIni = round(static_cast<float>(nfeatures) / (nlevels * W * H));
            const float hX = static_cast<float>(imagePyramid[level].cols) / W;
            const float hY = static_cast<float>(imagePyramid[level].rows) / H;
            // 使用响应值排序选择最佳特征点
            for(int i = 0; i < H; i++) {
                for(int j = 0; j < W; j++) {
                    std::vector<cv::KeyPoint> vCell;
                    // 收集当前网格内的特征点
                    for(size_t k = 0; k < vToDistribute.size(); k++) {
                        if(vToDistribute[k].pt.x >= j*hX && vToDistribute[k].pt.x <= (j+1)*hX && vToDistribute[k].pt.y >= i*hY && vToDistribute[k].pt.y <= (i+1)*hY) {
                            vCell.push_back(vToDistribute[k]);
                        }
                    }
                    // ... 后续逻辑省略
                }
            }
        }
    }
};