SGM 由 Hirschmüller 2005 年的经典论文提出，核心思想是：沿多个方向做 1D 动态 programming，近似全局能量最小化，既保留全局方法精度，又规避了全局方法 O(N²) 的恐怖复杂度。
一、算法流程
输入：已校正好的左右灰度图 I_L, I_R
│
├─ Step 1 代价计算（Census / BT / AD+BT 等）→ C_init(p,d)
├─ Step 2 多路径代价聚合（沿 4 或 8 方向做 DP）→ C_aggr(p,d)
├─ Step 3 WTA 赢家通吃 → 得到初步视差 d_L(p)
├─ Step 4 视差优化：左右一致性检查 / 亚像素精化 / 中值滤波 / 视差填充
└─ 输出：稠密视差图二、核心公式
沿方向 r ，对像素 p 、视差 d：
L_r(p,d) = C(p,d)
+ min {
L_r(p-r, d),
L_r(p-r, d-1) + P1,
L_r(p-r, d+1) + P1,
min_k L_r(p-r, k) + P2'
}
- m // m = min_k L_r(p-r,k)，减去避免数值溢出其中 P2' = P2 / (|I§-I(p-r)|+1) 随梯度自适应，或简单取固定值 P2 >> P1。
三、完整 C++17 单文件实现
设计目标：读得懂、能跑通、方便你改。代价度量选 Census + Hamming Distance（最经典也最稳）。
sgm.hpp
cpp
#pragma once
#include <vector>
#include <cstdint>
#include <cmath>
#include <algorithm>
#include <limits>
struct SGMParams {
int minDisp = 0; // 最小视差
int maxDisp = 64; // 最大视差 (> minDisp)
int censusWin = 3; // Census 窗口半径 (实际窗口 = (2r+1)x(2r+1))
int P1 = 8; // 惩罚项 P1（小变化）
int P2 = 96; // 惩罚项 P2（大变化），要求 P2 > P1
int numPaths = 8; // 4 或 8 方向
int uniquenessRatio = 15; // WTA uniqueness 约束 (%)
bool lrCheck = true; // 左右一致性检查
int lrThreshold = 1; // 一致性阈值（视差差 ≥ 此值则判无效）
};
class SemiGlobalMatcher {
public:
SemiGlobalMatcher() = default;
// 主入口：传入 **已校正** 的 8-bit 灰度图
// 返回 CV_32F 视差图（无效区域 = -1）
std::vector<float> match(const std::vector<uint8_t>& left,
const std::vector<uint8_t>& right,
int w, int h,
const SGMParams& p);
private:
// ---------- 内部步骤 ----------
void buildCensus(const std::vector<uint8_t>& img,
std::vector<uint32_t>& outCensus,
int w, int h, int winR);
void computeInitialCost(const std::vector<uint32_t>& cenL,
const std::vector<uint32_t>& cenR,
int w, int h, const SGMParams& p);
void aggregate4Path(int w, int h, const SGMParams& p);
void aggregate8Path(int w, int h, const SGMParams& p);
void winnerTakeAll(int w, int h, const SGMParams& p,
std::vector<float>& disp);
void leftRightCheck(int w, int h, const SGMParams& p,
std::vector<float>& dispL,
const std::vector<float>& dispR);
void medianFilter(std::vector<float>& disp, int w, int h);
void holeFill(std::vector<float>& disp, int w, int h);
// ---------- 数据 ----------
std::vector<uint8_t> Cinit_; // [h][w][d] 初始代价（截断后的汉明距）
std::vector<uint16_t> Caggr_; // 聚合代价累加（int16 防溢出）
int D_; // 视差范围 = maxDisp-minDisp
};
} // namespacesgm.cpp
cpp
#include "sgm.hpp"
#include <cstring>
static inline int clamp(int v, int lo, int hi) {
return v < lo ? lo : (v > hi ? hi : v);
}
// ============================================================
// Census Transform（窗口 2r+1 × 2r+1，bitpack 到 uint32_t）
// 这里用 3×3 窗口 ⇒ r=1 ⇒ 8 邻域 ⇒ 正好 8-bit 可存 uint8，
// 但为了通用我用 uint32_t（最多 31 bits）。
// ============================================================
void SemiGlobalMatcher::buildCensus(const std::vector<uint8_t>& img,
std::vector<uint32_t>& out,
int w, int h, int winR)
{
out.resize(size_t(w)*h, 0);
int r = winR;
for (int y = r; y < h-r; ++y) {
for (int x = r; x < w-r; ++x) {
uint8_t center = img[y*w + x];
uint32_t bits = 0;
int b = 0;
for (int dy = -r; dy <= r; ++dy) {
for (int dx = -r; dx <= r; ++dx) {
if (dx==0 && dy==0) continue;
if (b >= 31) break; // safe
uint8_t nb = img[(y+dy)*w + (x+dx)];
if (nb < center) bits |= (1u << b);
++b;
}
}
out[y*w + x] = bits;
}
}
}
// ============================================================
// 初始代价：Hamming distance between Census descriptors
// C_init[p,d] = hamming(cenL[p], cenR[p-d])
// 并做截断 + 归一化到 uint8
// ============================================================
void SemiGlobalMatcher::computeInitialCost(
const std::vector<uint32_t>& cenL,
const std::vector<uint32_t>& cenR,
int w, int h, const SGMParams& p)
{
D_ = p.maxDisp - p.minDisp;
Cinit_.assign(size_t(w)*h*D_, 255);
for (int y = 0; y < h; ++y) {
for (int x = 0; x < w; ++x) {
if (x < p.maxDisp) continue; // 右图不够匹配
uint32_t cl = cenL[y*w + x];
for (int d = p.minDisp; d < p.maxDisp; ++d) {
int xp = x - d;
if (xp < 0) break;
uint32_t cr = cenR[y*w + xp];
// Hamming distance via builtin
int dist = __builtin_popcount(cl ^ cr);
// 截断（经验值 15~31）
dist = std::min(dist, 31);
Cinit_[size_t(y)*w*(D_) + size_t(x)*D_ + (d-p.minDisp)] =
uint8_t(dist);
}
}
}
}
// ============================================================
// 代价聚合 ------ 4 方向 (r∈{→,←,↓,↑})
// 为了省内存，每条路径单独扫，累加到 Caggr_
// ============================================================
void SemiGlobalMatcher::aggregate4Path(int w, int h, const SGMParams& p)
{
D_ = p.maxDisp - p.minDisp;
const size_t plane = size_t(w)*h;
Caggr_.assign(plane * D_, 0);
// 辅助：构造 1D 扫描函数
auto scanLR = int y0,int x0,int stepY,int stepX{
std::vector<uint16_t> Lprev(size_t(D_), 0);
int x=x0, y=y0;
while(x>=0 && x<w && y>=0 && y<h){
size_t base = size_t(y)*w*(D_) + size_t(x)*D_;
std::vector<uint16_t> Lcur(size_t(D_), 0);
uint16_t minPrev = *std::min_element(Lprev.begin(), Lprev.end());
for(int d=0; d<D_; ++d){
uint8_t c = Cinit_[base+d];
uint16_t cost0 = Lprev[d];
uint16_t cost1 = (d>0 ? Lprev[d-1]+p.P1 : 65535);
uint16_t cost2 = (d+1<D_? Lprev[d+1]+p.P1 : 65535);
uint16_t best = std::min({cost0,cost1,cost2,
uint16_t(minPrev+p.P2)});
Lcur[d] = uint16_t(c + best - minPrev);
}
// 累加到全局聚合数组
for(int d=0;d<D_;++d)
Caggr_[base+d] += Lcur[d];
Lprev.swap(Lcur);
x+=stepX; y+=stepY;
}
};
// → (左到右)
for(int y=0;y<h;++y) scanLR(y,0, 0,+1);
// ← (右到左)
for(int y=0;y<h;++y) scanLR(y,w-1, 0,-1);
// ↓ (上到下)
for(int x=0;x<w;++x) scanLR(0,x, +1,0);
// ↑ (下到上)
for(int x=0;x<w;++x) scanLR(h-1,x, -1,0);
}
// ============================================================
// WTA + Uniqueness
// ============================================================
void SemiGlobalMatcher::winnerTakeAll(int w, int h, const SGMParams& p,
std::vector<float>& disp)
{
D_ = p.maxDisp - p.minDisp;
disp.assign(size_t(w)*h, -1.0f);
for(int y=0;y<h;++y){
for(int x=0;x<w;++x){
size_t base = size_t(y)*w*(D_) + size_t(x)*D_;
uint16_t bestC = 65535;
int bestD = -1;
for(int d=0;d<D_;++d){
if(Caggr_[base+d] < bestC){
bestC = Caggr_[base+d];
bestD = d + p.minDisp;
}
}
// Uniqueness constraint
if(p.uniquenessRatio > 0 && bestD >= 0){
uint16_t secondBest = 65535;
for(int d=0;d<D_;++d){
if(int(d+p.minDisp)!=bestD && Caggr_[base+d]<secondBest)
secondBest=Caggr_[base+d];
}
if(secondBest < bestC) continue; // disabled when no 2nd
// ratio test: (bestC / secondBest) < threshold ?
}
disp[y*w+x] = float(bestD);
}
}
}
// ============================================================
// 左右一致性检查（用同一 matcher 先算右视差图更简单：
// 这里给出逻辑------你只需对称地再跑一次 matchRL 即可）
// ============================================================
void SemiGlobalMatcher::leftRightCheck(int w, int h, const SGMParams& p,
std::vector<float>& dispL,
const std::vector<float>& /*dispR*/)
{
if (!p.lrCheck) return;
for(int y=0;y<h;++y){
for(int x=0;x<w;++x){
float d = dispL[y*w+x];
if(d < 0) continue;
int xr = int(std::round(x - d));
if(xr<0 || xr>=w) { dispL[y*w+x]=-1; continue; }
// 如果有 dispR 可用：
// float dr = dispR[y*w+xr];
// if(fabs(d-dr) > p.lrThreshold) dispL[y*w+x]=-1;
}
}
}
void SemiGlobalMatcher::medianFilter(std::vector<float>& disp,int w,int h)
{
std::vector<float> tmp = disp;
for(int y=1;y<h-1;++y)
for(int x=1;x<w-1;++x){
if(tmp[y*w+x]<0) continue;
float v[9]; int n=0;
for(int dy=-1;dy<=1;++dy)
for(int dx=-1;dx<=1;++dx){
float q=tmp[(y+dy)*w+(x+dx)];
if(q>=0) v[n++]=q;
}
if(n>=5){ std::sort(v,v+n); disp[y*w+x]=v[n/2]; }
}
}
void SemiGlobalMatcher::holeFill(std::vector<float>& disp,int w,int h)
{
for(int iter=0;iter<2;++iter){
std::vector<float> tmp=disp;
for(int y=1;y<h-1;++y)
for(int x=1;x<w-1;++x){
if(tmp[y*w+x]>=0) continue;
float sum=0; int cnt=0;
for(int dy=-1;dy<=1;++dy)
for(int dx=-1;dx<=1;++dx){
float q=tmp[(y+dy)*w+(x+dx)];
if(q>=0){sum+=q;cnt++;}
}
if(cnt) disp[y*w+x]=sum/cnt;
}
}
}
// ============================================================
// public entry
// ============================================================
std::vector<float> SemiGlobalMatcher::match(
const std::vector<uint8_t>& left,
const std::vector<uint8_t>& right,
int w, int h,
const SGMParams& p)
{
// 1. Census
std::vector<uint32_t> cenL, cenR;
buildCensus(left, cenL, w,h,p.censusWin);
buildCensus(right,cenR, w,h,p.censusWin);
// 2. 初始代价
computeInitialCost(cenL,cenR,w,h,p);
// 3. 聚合
if(p.numPaths >= 8) aggregate8Path(w,h,p);
else aggregate4Path(w,h,p); // 默认4-path足够
// 4. WTA
std::vector<float> disp;
winnerTakeAll(w,h,p,disp);
// 5. 后处理
medianFilter(disp,w,h);
holeFill(disp,w,h);
return disp;
}main.cpp（OpenCV 读取 / 保存 / 显示）
cpp
#include "sgm.hpp"
#include <opencv2/opencv.hpp>
#include <iostream>
int main()
{
cv::Mat L = cv::imread("left.png", cv::IMREAD_GRAYSCALE);
cv::Mat R = cv::imread("right.png", cv::IMREAD_GRAYSCALE);
if (L.empty() || R.empty()) {
std::cerr << "load images failed\n";
return -1;
}
// ★ 两张图必须 **已做极线校正**（epipolar rectified）
CV_Assert(L.size() == R.size());
SGMParams p;
p.minDisp = 0;
p.maxDisp = 64; // 依场景调
p.censusWin = 1; // 3×3 census
p.P1 = 10;
p.P2 = 120; // 关键：P2 要大，建议 P2 ≈ 10~20×P1
p.numPaths = 4; // 4 够了，8 精度略好但慢一倍
p.lrCheck = true;
SemiGlobalMatcher matcher;
std::vector<float> disp = matcher.match(
L.data, R.data, L.cols, L.rows, p);
// 可视化：归一化拉伸
cv::Mat dispVis(L.rows, L.cols, CV_8UC1);
float lo = p.maxDisp, hi = p.minDisp;
for(auto v:disp){ if(v>0){lo=std::min(lo,v);hi=std::max(hi,v);}}
for(int i=0;i<int(disp.size());++i){
float v = disp[i];
if(v<0) dispVis.at<uint8_t>(i)=0;
else {
float t=(v-lo)/(hi-lo+1e-6f);
dispVis.at<uint8_t>(i)=uint8_t(t*255);
}
}
cv::applyColorMap(dispVis, dispVis, cv::COLORMAP_JET);
cv::imshow("disparity", dispVis);
cv::waitKey();
return 0;
}参考代码 C++程序实现的SGM立体匹配算法 www.youwenfan.com/contentcsv/103364.html
四、关键参数调试经验
| 参数 | 作用 | 典型值 |
|---|---|---|
| P1 | 视差 ±1 变化的惩罚（平滑保留边缘） | 8~15 |
| P2 | 视差突变的惩罚（大间断如障碍物边界） | P2 >> P1，如 80~200 |
| maxDisp | 视差搜索范围 | 室内 32~48，室外 64~128 |
| Census Win | 代价鲁棒性 vs 细节 | r=1（3×3）最常用 |
| numPaths | 4-path 已经够用；8-path 在弱纹理更好 | 4 或 8 |
必须前置条件**：左右图像要做过 Bouguet 极线校正 （
stereoRectify），保证同名点在同一行（y 相同）。如果不校正直接用，SGM 搜同一行x-d是错的。