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

SGBM 半全局块匹配算法流程详解 | 极客日志

步骤	输入	输出	核心操作	OpenCV 参数
1. 预处理	左右图像	滤波后图像	Sobel 梯度	`preFilterCap`
2. 代价计算	滤波图像	代价体 C(x,y,d)	BT 距离/Census	`blockSize`
3. 代价聚合	代价体	聚合代价 S(x,y,d)	多路径 DP	`P1`, `P2`
4. 视差计算	聚合代价	粗视差图	WTA	`numDisparities`
5. 后处理	粗视差图	精细视差图	滤波	`uniquenessRatio`

void preFilterXSobel(const cv::Mat& src, cv::Mat& dst, int preFilterCap){
    cv::Sobel(src, dst, CV_16S, 1, 0, 3); // dx=1, dy=0, ksize=3
    // 截断到 [-preFilterCap, +preFilterCap]
    for(int y = 0; y < dst.rows; y++){
        for(int x = 0; x < dst.cols; x++){
            short val = dst.at<short>(y, x);
            dst.at<short>(y, x) = cv::saturate_cast<short>( std::max(-preFilterCap, std::min(preFilterCap, (int)val)));
        }
    }
}

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));
}

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); // 硬件加速
}

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

模式	路径数量	方向角度	计算量	质量
`MODE_SGBM`	5	0°, 45°, 90°, 135°, 垂直加强	低	中
`MODE_HH`	8	全方向	高	高
`MODE_SGBM_3WAY`	8	3 方向×3 遍历	中	较高

 135° 90° 45° ↖ ↑ ↗
180° ← [p] → 0°
↙ ↓ ↘
225° 270° 315°

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;
            }
        }
    }
}

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;
    }
}

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;
}

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;
}

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 倍)

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; }
        }
    }
}

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; // 匹配不唯一 }
        }
    }
}

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; }
                    }
                }
            }
        }
    }
}

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

#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;
}

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 路径模式
        );
    }
};

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;
}

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

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

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; // 视差也需缩放

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; // 纹理稀疏 → 大窗口 }
}

问题	可能原因	解决方案
视差图全黑	未进行极线校正	使用 `stereoRectify` + `remap`
大量空洞	`uniquenessRatio` 过小	增大到 10-15
过度平滑	P2 过大	降低到 4 × P1
噪点多	P1 过小	增大到 8 × blockSize²
边缘断裂	未启用自适应 P2	使用 `MODE_HH` 模式
计算慢	路径过多	改用 `MODE_SGBM` (5 路径)

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; // 错误率 %
}

优点	缺点
密集匹配 (90%+ 覆盖率)	计算量大 (实时需优化)
边缘保持好	弱纹理区域易失败
抗噪声能力强	参数敏感
亚像素精度	需要良好的极线校正

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

SGBM 算法流程详解

1. 算法概述

1.1 SGBM 的本质

1.2 算法复杂度

2. 完整算法流程

3. 步骤 1: 预处理 (Pre-processing)

3.1 目的

3.2 Sobel 预滤波

3.3 参数影响

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

4.1 代价体构建

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

4.3 方法 2: Census 变换

4.4 代价体示意图

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

5.1 核心思想

5.2 单路径动态规划公式

5.3 路径方向

5.4 动态规划详细过程

5.5 多路径聚合

5.6 自适应惩罚系数

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

6.1 WTA (Winner-Takes-All) 策略

6.2 亚像素增强 (Subpixel Refinement)

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

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

7.2 唯一性检查 (Uniqueness Ratio Check)

7.3 斑点滤波 (Speckle Filtering)

7.4 中值滤波 (可选)

8. 完整代码示例

8.1 基础调用

8.2 高级参数调优

8.3 视差转深度

9. 工程优化技巧

9.1 性能优化

9.2 参数自适应

9.3 错误诊断

9.4 质量评估

10. 总结

10.1 算法特点

10.2 适用场景

10.3 进阶方向

参考文献

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具