SGBM的思路是：

通过选取每个像素点的disparity，组成一个disparity map，设置一个和disparity map相关的全局能量函数，使这个能量函数最小化，以达到求解每个像素最优disparity的目的。

能量函数形式如下：

D指disparity map。E(D)是该disparity map对应的能量函数。

p, q代表图像中的某个像素

Np 指像素p的相邻像素点（一般认为8连通）

C(p, Dp)指当前像素点disparity为Dp时，该像素点的cost

P1 是一个惩罚系数，它适用于像素p相邻像素中dsparity值与p的dsparity值相差1的那些像素。

P2 是一个惩罚系数，它适用于像素p相邻像素中dsparity值与p的dsparity值相差大于1的那些像素。

I[.]函数返回1如果函数中的参数为真，否则返回0

利用上述函数在一个二维图像中寻找最优解是一个NP-complete问题，耗时过于巨大，因此该问题被近似分解为多个一维问题，即线性问题。而且每个一维问题都可以用动态规划来解决。因为1个像素有8个相邻像素，因此一般分解为8个一维问题。

考虑从左到右这一方向，如下图所示：

则每个像素的disparity只和其左边的像素相关，有如下公式：

r指某个指向当前像素p的方向，在此可以理解为像素p左边的相邻像素。
Lr(p, d) 表示沿着当前方向（即从左向右），当目前像素p的disparity取值为d时，其最小cost值。

这个最小值是从4种可能的候选值中选取的最小值：

1.前一个像素（左相邻像素）disparity取值为d时，其最小的cost值。

2.前一个像素（左相邻像素）disparity取值为d-1时，其最小的cost值+惩罚系数P1。

3.前一个像素（左相邻像素）disparity取值为d+1时，其最小的cost值+惩罚系数P1。

4.前一个像素（左相邻像素）disparity取值为其他时，其最小的cost值+惩罚系数P2。

另外，当前像素p的cost值还需要减去前一个像素取不同disparity值时最小的cost。这是因为Lr(p, d)是会随着当前像素的右移不停增长的，为了防止数值溢出，所以要让它维持在一个较小的数值。

C(p, d)的计算很简单，由如下两个公式计算：

即，当前像素p和移动d之后的像素q之间，经过半个像素插值后，寻找两个像素点灰度或者RGB差值的最小值，作为C(p, d)的值。

具体来说：设像素p的灰度/RGB值为I(p)，先从I(p)，(I(p)+I(p-1))/2,(I(p)+I(p+1))/2三个值中选择出和I(q)差值最小的,即

d(p,p-d)。然后再从I(q)，(I(q)+I(q-1))/2,(I(q)+I(q+1))/2三个值中选择出和I(p)差值最小的,即d(p-d,p)。最后从两个值中选取最小值，就是C(p, d)

上面是从一个方向（从左至右）计算出的像素在取值为某一disparity值时的最小cost值。但是一个像素有8个邻域，所以一共要从8个方向计算（左右，右左，上下，下上，左上右下，右下左上，右上左下，左下右上）这个cost值。

然后把八个方向上的cost值累加，选取累加cost值最小的disparity值作为该像素的最终disparity值。对于每个像素进行该操作后，就形成了整个图像的disparity map。公式表达如下：

SGBM算法遍历每个像素，针对每个像素的操作和disparity的范围有关，故时间复杂度为：

SGM