SGBM 半全局块匹配算法流程详解

3.3 参数影响

• preFilterCap = 31: 适用于室内、光照均匀场景
• preFilterCap = 63: 适用于室外、光照变化大的场景
• 过小 → 梯度信息不足；过大 → 噪声增加

4. 步骤 2: 代价计算 (Cost Computation)

4.1 代价体构建

构建 3D 代价体 (Cost Volume): C : R^(W×H×D) where D = numDisparities

对于每个像素 (x, y) 和视差 d，计算左右图像的匹配代价。

4.2 方法 1: BT (Birchfield-Tomasi) 距离

核心思想: 考虑亚像素采样，减少离散化误差。

公式: C_BT(x, y, d) = (1/N) * Σ_(i,j)∈W ρ(I_L(x+i, y+j), I_R(x+i-d, y+j))

其中 ρ 为鲁棒代价函数： ρ(a, b) = min(|a - b|, |a - b^-|, |a - b^+|) b^- = (I_R(x-d) + I_R(x-d-1))/2, b^+ = (I_R(x-d) + I_R(x-d+1))/2

伪代码:

float computeBT(int x, int y, int d, const Mat& left, const Mat& right) {
    float cost = 0;
    int halfBlock = blockSize / 2;
    for (int dy = -halfBlock; dy <= halfBlock; dy++) {
        for (int dx = -halfBlock; dx <= halfBlock; dx++) {
            int xl = x + dx;
            int yl = y + dy;
            int xr = xl - d;
            if (xr < 0 || xr >= right.cols) continue;
            float IL = left.at<uchar>(yl, xl);
            float IR = right.at<uchar>(yl, xr);
            float IR_minus = (right.at<uchar>(yl, xr) + right.at<uchar>(yl, xr - 1)) / 2.0;
            float IR_plus = (right.at<uchar>(yl, xr) + right.at<uchar>(yl, xr + 1)) / 2.0;
            float diff = std::min({std::abs(IL - IR), std::abs(IL - IR_minus), std::abs(IL - IR_plus)});
            cost += diff;
        }
    }
    return cost / ((blockSize * blockSize));
}

4.3 方法 2: Census 变换

更鲁棒于光照变化，OpenCV 在某些模式下使用。

步骤:

1. 对每个像素的邻域进行二值化编码
2. 使用 Hamming 距离比较左右图像的 Census 串

公式: ξ(x, y) = ⊗_(i,j)∈W 1_I(x+i, y+j) > I(x, y) C_Census(x, y, d) = Hamming(ξ_L(x, y), ξ_R(x-d, y))

示例 (9×9 窗口):

uint64_t computeCensus(int x, int y, const Mat& img) {
    uint64_t census = 0;
    uchar center = img.at<uchar>(y, x);
    for (int dy = -4; dy <= 4; dy++) {
        for (int dx = -4; dx <= 4; dx++) {
            if (dx == 0 && dy == 0) continue;
            uchar neighbor = img.at<uchar>(y + dy, x + dx);
            census <<= 1;
            if (neighbor > center) census |= 1;
        }
    }
    return census;
}

int hammingDistance(uint64_t a, uint64_t b) {
    return __builtin_popcountll(a ^ b); // 硬件加速
}

4.4 代价体示意图

代价体 C[x, y, d]: d (视差) ↓ 0 1 2 ... 63 x=0 [12, 45, 78, ..., 23] x=1 [34, 12, 56, ..., 89] ...

每个 C(x, y, d) 存储像素 (x, y) 在视差 d 下的匹配代价。

5. 步骤 3: 代价聚合 (Cost Aggregation)

5.1 核心思想

这是 SGBM 的灵魂步骤。通过多个方向的路径进行动态规划，将局部代价聚合成考虑全局约束的代价。

5.2 单路径动态规划公式

对于路径方向 r，递推计算累积代价： L_r(p, d): 沿路径 r 到像素 p 视差为 d 的最小累积代价 p-r: 路径 r 方向上的前一个像素 C(p, d): 当前像素的原始匹配代价 P1: 视差变化 ±1 的惩罚（平滑项） P2: 视差大幅变化的惩罚（断裂项） 最后的归一化项防止数值溢出

5.3 路径方向

OpenCV SGBM 支持多种模式：

8 方向示意图: 135° 90° 45° ↖ ↑ ↗ 180° ← [p] → 0° ↙ ↓ ↘ 225° 270° 315°

5.4 动态规划详细过程

以水平方向 (0°) 为例:

void aggregateCostPath(const Mat& cost, Mat& L, int direction, int P1, int P2) {
    int rows = cost.rows;
    int cols = cost.cols;
    int dispRange = cost.size[2]; // 视差范围
    // 方向向量
    int dx = direction_dx[direction]; // 例如：0° → dx=1, dy=0
    int dy = direction_dy[direction];
    // 从路径起点开始扫描
    for (int y = 0; y < rows; y++) {
        for (int x = 0; x < cols; x++) {
            int prev_x = x - dx;
            int prev_y = y - dy;
            if (prev_x < 0 || prev_x >= cols || prev_y < 0 || prev_y >= rows) {
                // 路径起点，直接复制代价
                for (int d = 0; d < dispRange; d++) {
                    L.at<float>(y, x, d) = cost.at<float>(y, x, d);
                }
                continue;
            }
            // 找到前一个像素的最小累积代价
            float prev_min = FLT_MAX;
            for (int d = 0; d < dispRange; d++) {
                prev_min = std::min(prev_min, L.at<float>(prev_y, prev_x, d));
            }
            // 对当前像素的每个视差进行 DP
            for (int d = 0; d < dispRange; d++) {
                float C_curr = cost.at<float>(y, x, d);
                // 4 种情况的最小值
                float cost0 = L.at<float>(prev_y, prev_x, d); // 无变化
                float cost1 = (d > 0) ? L.at<float>(prev_y, prev_x, d - 1) + P1 : FLT_MAX; // +1
                float cost2 = (d < dispRange - 1) ? L.at<float>(prev_y, prev_x, d + 1) + P1 : FLT_MAX; // -1
                float cost3 = prev_min + P2; // 大变化
                float min_cost = std::min({cost0, cost1, cost2, cost3});
                // 归一化
                L.at<float>(y, x, d) = C_curr + min_cost - prev_min;
            }
        }
    }
}

5.5 多路径聚合

将所有路径的累积代价求和： S(p, d) = Σ_r=1^P L_r(p, d)

void aggregateAllPaths(const Mat& cost, Mat& S, int P1, int P2) {
    int numPaths = 8; // 或 5
    // 初始化聚合代价为 0
    S = Mat::zeros(cost.size(), CV_32F);
    // 对每个方向进行聚合
    for (int r = 0; r < numPaths; r++) {
        Mat L_r;
        aggregateCostPath(cost, L_r, r, P1, P2);
        // 累加到总代价
        S += L_r;
    }
}

5.6 自适应惩罚系数

在图像边缘处降低 P2，允许视差断裂： P2_adaptive(p, p-r) = P2 / max(|∇I_p|, |∇I_{p-r}|, ε)

float getAdaptiveP2(int x, int y, int prev_x, int prev_y, const Mat& gradX, const Mat& gradY, float P2_base) {
    float grad1 = std::abs(gradX.at<short>(y, x)) + std::abs(gradY.at<short>(y, x));
    float grad2 = std::abs(gradX.at<short>(prev_y, prev_x)) + std::abs(gradY.at<short>(prev_y, prev_x));
    float maxGrad = std::max({grad1, grad2, 1.0f});
    return P2_base / maxGrad;
}

6. 步骤 4: 视差计算 (Disparity Computation)

6.1 WTA (Winner-Takes-All) 策略

选择累积代价最小的视差作为最终视差： D(x, y) = argmin_d∈[0, D_max] S(x, y, d)

cv::Mat computeDisparityWTA(const Mat& S) {
    int rows = S.rows;
    int cols = S.cols;
    int dispRange = S.size[2];
    Mat disparity(rows, cols, CV_16S);
    for (int y = 0; y < rows; y++) {
        for (int x = 0; x < cols; x++) {
            float minCost = FLT_MAX;
            int bestDisp = 0;
            for (int d = 0; d < dispRange; d++) {
                float cost = S.at<float>(y, x, d);
                if (cost < minCost) {
                    minCost = cost;
                    bestDisp = d;
                }
            }
            disparity.at<short>(y, x) = bestDisp * 16; // OpenCV 用 16 倍存储
        }
    }
    return disparity;
}

6.2 亚像素增强 (Subpixel Refinement)

通过二次曲线拟合提升精度到 1/16 像素。

数学原理: 假设代价函数在最优视差附近呈二次曲线： S(d) ≈ a(d - d_0)^2 + b

通过三点 (d-1, S_{d-1}), (d, S_d), (d+1, S_{d+1}) 拟合，得到： d_sub = d - (S(d-1) - S(d+1)) / (2(S(d-1) - 2S(d) + S(d+1)))

代码实现:

float subpixelInterpolation(float cost_prev, float cost_curr, float cost_next) {
    float numerator = cost_prev - cost_next;
    float denominator = 2.0 * (cost_prev - 2.0 * cost_curr + cost_next);
    if (std::abs(denominator) < 1e-6) {
        return 0.0f; // 避免除零
    }
    float delta = numerator / denominator;
    // 限制在 [-1, 1] 范围内
    return std::max(-1.0f, std::min(1.0f, delta));
}

void refineDisparity(Mat& disparity, const Mat& S) {
    int dispRange = S.size[2];
    for (int y = 0; y < disparity.rows; y++) {
        for (int x = 0; x < disparity.cols; x++) {
            short d16 = disparity.at<short>(y, x);
            int d = d16 / 16;
            if (d <= 0 || d >= dispRange - 1) continue;
            float cost_prev = S.at<float>(y, x, d - 1);
            float cost_curr = S.at<float>(y, x, d);
            float cost_next = S.at<float>(y, x, d + 1);
            float delta = subpixelInterpolation(cost_prev, cost_curr, cost_next);
            // 更新为亚像素视差 (16 倍精度)
            disparity.at<short>(y, x) = (d + delta) * 16;
        }
    }
}

效果对比: 像素级：d = 10 → 深度误差 ±5cm (假设基线 100mm, 焦距 500px) 亚像素：d = 10.3125 → 深度误差 ±0.3cm (提升 16 倍)

7. 步骤 5: 后处理 (Post-processing)

7.1 左右一致性检查 (Left-Right Consistency Check)

目的: 检测遮挡区域和误匹配。

原理:

• 计算右视差图 D_R (将左右图像交换再计算)
• 对于左图像素 (x, y)，其对应右图像素为 (x - D_L(x, y), y)
• 若两者视差不一致，则标记为无效

公式: Valid = { 1 if |D_L(x, y) - D_R(x - D_L(x, y), y)| ≤ disp12MaxDiff 0 otherwise }

代码实现:

void leftRightCheck(Mat& dispLeft, const Mat& dispRight, int disp12MaxDiff) {
    for (int y = 0; y < dispLeft.rows; y++) {
        for (int x = 0; x < dispLeft.cols; x++) {
            short dL = dispLeft.at<short>(y, x);
            if (dL == -16) continue; // 已标记为无效
            int d = dL / 16;
            int x_right = x - d;
            if (x_right < 0 || x_right >= dispRight.cols) {
                dispLeft.at<short>(y, x) = -16; // 无效视差
                continue;
            }
            short dR = dispRight.at<short>(y, x_right);
            // 检查一致性
            if (std::abs(dL - dR) > disp12MaxDiff * 16) {
                dispLeft.at<short>(y, x) = -16;
            }
        }
    }
}

参数建议:

• disp12MaxDiff = 1: 严格模式，适用于高质量标定
• disp12MaxDiff = 2: 宽松模式，适用于有畸变的场景
• disp12MaxDiff = -1: 禁用检查（不推荐）

7.2 唯一性检查 (Uniqueness Ratio Check)

目的: 过滤匹配不唯一的像素（多个视差代价接近）。

公式: Uniqueness = (S_2ndmin - S_min) / S_min × 100

若 Uniqueness < uniquenessRatio，则标记为无效。

void uniquenessCheck(Mat& disparity, const Mat& S, int uniquenessRatio) {
    int dispRange = S.size[2];
    for (int y = 0; y < disparity.rows; y++) {
        for (int x = 0; x < disparity.cols; x++) {
            short d16 = disparity.at<short>(y, x);
            if (d16 == -16) continue;
            int d = d16 / 16;
            float minCost = S.at<float>(y, x, d);
            // 找第二小代价
            float secondMinCost = FLT_MAX;
            for (int d2 = 0; d2 < dispRange; d2++) {
                if (d2 == d) continue;
                float cost = S.at<float>(y, x, d2);
                if (cost < secondMinCost) {
                    secondMinCost = cost;
                }
            }
            // 计算唯一性比率
            float ratio = (secondMinCost - minCost) / minCost * 100.0;
            if (ratio < uniquenessRatio) {
                disparity.at<short>(y, x) = -16; // 匹配不唯一
            }
        }
    }
}

参数影响:

• uniquenessRatio = 5: 非常严格，会产生较多空洞
• uniquenessRatio = 10: 平衡模式（推荐）
• uniquenessRatio = 15: 宽松模式，保留更多匹配

7.3 斑点滤波 (Speckle Filtering)

目的: 去除孤立的噪声点和小连通区域。

算法:

1. 使用连通域标记找到所有连通区域
2. 计算每个区域的大小和内部视差方差
3. 若区域大小 < speckleWindowSize 且方差 > speckleRange，则标记为噪声

void speckleFilter(Mat& disparity, int speckleWindowSize, int speckleRange) {
    if (speckleWindowSize == 0) return;
    Mat labels, stats, centroids;
    int numComponents = cv::connectedComponentsWithStats(disparity > 0, // 有效视差掩码
                                                          labels, stats, centroids, 8);
    for (int i = 1; i < numComponents; i++) {
        // 跳过背景 (label=0)
        int area = stats.at<int>(i, cv::CC_STAT_AREA);
        if (area < speckleWindowSize) {
            // 计算该连通区域的视差方差
            std::vector<short> disparities;
            for (int y = 0; y < disparity.rows; y++) {
                for (int x = 0; x < disparity.cols; x++) {
                    if (labels.at<int>(y, x) == i) {
                        disparities.push_back(disparity.at<short>(y, x) / 16);
                    }
                }
            }
            // 计算标准差
            float mean = std::accumulate(disparities.begin(), disparities.end(), 0.0) / disparities.size();
            float variance = 0;
            for (auto d : disparities) {
                variance += (d - mean) * (d - mean);
            }
            float stddev = std::sqrt(variance / disparities.size());
            // 若区域小且视差变化大，视为噪声
            if (stddev > speckleRange) {
                for (int y = 0; y < disparity.rows; y++) {
                    for (int x = 0; x < disparity.cols; x++) {
                        if (labels.at<int>(y, x) == i) {
                            disparity.at<short>(y, x) = -16;
                        }
                    }
                }
            }
        }
    }
}

参数建议:

• speckleWindowSize = 100: 过滤小于 100 像素的连通区域
• speckleRange = 2: 区域内视差变化超过 2 则视为噪声
• 设为 0 可禁用斑点滤波（提升速度）

7.4 中值滤波 (可选)

对最终视差图进行 3×3 或 5×5 中值滤波平滑：

cv::medianBlur(disparity, disparity, 5);

8. 完整代码示例

8.1 基础调用

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>

int main() {
    // 1. 读取校正后的立体图像对
    cv::Mat leftRect = cv::imread("left_rect.png", cv::IMREAD_GRAYSCALE);
    cv::Mat rightRect = cv::imread("right_rect.png", cv::IMREAD_GRAYSCALE);

    // 2. 创建 SGBM 对象
    int minDisparity = 0;
    int numDisparities = 128; // 必须是 16 的倍数
    int blockSize = 5;
    int P1 = 8 * blockSize * blockSize;
    int P2 = 32 * blockSize * blockSize;
    
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        minDisparity, numDisparities, blockSize, P1, P2, 1, // disp12MaxDiff
        63, // preFilterCap
        10, // uniquenessRatio
        100, // speckleWindowSize
        32, // speckleRange
        cv::StereoSGBM::MODE_SGBM_3WAY
    );

    // 3. 计算视差图
    cv::Mat disparity16S;
    sgbm->compute(leftRect, rightRect, disparity16S);

    // 4. 转换为浮点视差 (除以 16)
    cv::Mat disparity32F;
    disparity16S.convertTo(disparity32F, CV_32F, 1.0 / 16.0);

    // 5. 处理无效视差
    cv::Mat validMask = disparity16S > 0;
    disparity32F.setTo(0, ~validMask);

    // 6. 可视化
    cv::Mat disparityVis;
    cv::normalize(disparity32F, disparityVis, 0, 255, cv::NORM_MINMAX, CV_8U);
    cv::applyColorMap(disparityVis, disparityVis, cv::COLORMAP_JET);
    cv::imshow("Disparity Map", disparityVis);
    cv::waitKey(0);

    // 7. 转换为深度图
    float baseline = 0.120; // 120mm
    float focal = 718.856; // 像素
    cv::Mat depth = (baseline * focal) / disparity32F;

    return 0;
}

8.2 高级参数调优

class SGBMTuner {
public:
    static cv::Ptr<cv::StereoSGBM> createForIndoor() {
        return cv::StereoSGBM::create(
            0, 128, 3, 72, // P1 = 8*1*3²
            288, // P2 = 4*P1
            1, // 严格一致性
            31, // 小预滤波
            10, // 中等唯一性
            50, // 小斑点窗口
            1, // 严格斑点过滤
            cv::StereoSGBM::MODE_HH // 高质量模式
        );
    }
    static cv::Ptr<cv::StereoSGBM> createForOutdoor() {
        return cv::StereoSGBM::create(
            0, 192, 7, 392, // P1 = 8*1*7²
            1568, // P2 = 4*P1
            2, // 宽松一致性
            63, // 大预滤波（应对光照）
            15, // 宽松唯一性
            200, // 大斑点窗口
            2, // 宽松斑点过滤
            cv::StereoSGBM::MODE_SGBM_3WAY
        );
    }
    static cv::Ptr<cv::StereoSGBM> createForRealtime() {
        return cv::StereoSGBM::create(
            0, 64, 5, 200, 800, -1, // 禁用左右检查
            63, 5, 0, // 禁用斑点过滤
            0,
            cv::StereoSGBM::MODE_SGBM // 5 路径模式
        );
    }
};

8.3 视差转深度

cv::Mat disparityToDepth(const cv::Mat& disparity, float baseline, float focal, float minDepth = 0.1, float maxDepth = 10.0) {
    cv::Mat depth = cv::Mat::zeros(disparity.size(), CV_32F);
    for (int y = 0; y < disparity.rows; y++) {
        for (int x = 0; x < disparity.cols; x++) {
            float disp = disparity.at<float>(y, x);
            if (disp <= 0) {
                depth.at<float>(y, x) = 0; // 无效深度
                continue;
            }
            float d = (baseline * focal) / disp;
            // 深度范围限制
            if (d < minDepth || d > maxDepth) {
                depth.at<float>(y, x) = 0;
            } else {
                depth.at<float>(y, x) = d;
            }
        }
    }
    return depth;
}

9. 工程优化技巧

9.1 性能优化

1. 多线程加速

sgbm->setNumThreads(8); // OpenCV 4.x+ 支持

2. ROI 限制

cv::Rect roi(100, 100, 800, 600);
cv::Mat dispROI;
sgbm->compute(leftRect(roi), rightRect(roi), dispROI);

3. 降采样

cv::Mat leftSmall, rightSmall;
cv::resize(leftRect, leftSmall, cv::Size(), 0.5, 0.5);
cv::resize(rightRect, rightSmall, cv::Size(), 0.5, 0.5);
// 计算后上采样
cv::resize(disparity, disparity, leftRect.size());
disparity *= 2; // 视差也需缩放

9.2 参数自适应

根据图像纹理密度调整 blockSize:

int computeOptimalBlockSize(const cv::Mat& image) {
    cv::Mat grad;
    cv::Sobel(image, grad, CV_16S, 1, 1);
    cv::Scalar meanGrad = cv::mean(grad);
    if (meanGrad[0] > 50) {
        return 3; // 纹理丰富 → 小窗口
    } else if (meanGrad[0] > 20) {
        return 5;
    } else {
        return 7; // 纹理稀疏 → 大窗口
    }
}

9.3 错误诊断

常见问题及解决:

9.4 质量评估

使用 Middlebury 标准评估:

float evaluateDisparity(const cv::Mat& computed, const cv::Mat& groundTruth, float threshold = 1.0) {
    int badPixels = 0;
    int totalPixels = 0;
    for (int y = 0; y < computed.rows; y++) {
        for (int x = 0; x < computed.cols; x++) {
            float gt = groundTruth.at<float>(y, x);
            if (gt == 0) continue; // 跳过无效区域
            float comp = computed.at<float>(y, x);
            if (std::abs(comp - gt) > threshold) {
                badPixels++;
            }
            totalPixels++;
        }
    }
    return (float)badPixels / totalPixels * 100.0; // 错误率 %
}

10. 总结

10.1 算法特点

10.2 适用场景

• ✅ 自动驾驶: 道路障碍物检测
• ✅ 机器人导航: SLAM 中的深度估计
• ✅ 工业检测: 3D 尺寸测量
• ✅ AR/VR: 实时深度感知
• ❌ 高速运动: 帧率要求 > 60fps (考虑 GPU 加速)
• ❌ 极弱纹理: 如白墙 (需结合结构光)

10.3 进阶方向

1. 深度学习融合: 使用 CNN 计算初始代价体 (如 PSMNet)
2. 时序一致性: 利用视频序列前后帧约束
3. GPU 加速: CUDA 实现路径聚合并行化
4. 自适应参数: 根据场景动态调整 P1, P2

参考文献

1. Hirschmüller, H. (2007). 'Stereo Processing by Semiglobal Matching and Mutual Information'. IEEE TPAMI, 30(2), 328-341.
2. OpenCV Documentation: StereoSGBM Class Reference
3. Birchfield, S., & Tomasi, C. (1998). 'A Pixel Dissimilarity Measure That Is Insensitive to Image Sampling'. IEEE TPAMI, 20(4), 401-406.