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 to 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.