
Lecture 14
Computer Science Department, Colorado School of Mines
By the end of this lecture, you should be able to:
StereoBM and StereoSGBM.reprojectImageTo3D, and recognize where stereo fails.
Gray: no reliable match (plain ground, window reflections, areas only one camera sees)
Stereo Reconstruction
Rectify the images to side by side \(\rightarrow\) match each pixel along its row \(\rightarrow\) disparity \(\rightarrow\) depth \(\rightarrow\) 3D points
\[ \bbox[border: 2px solid #c53030; padding: 2px 6px]{Z = \frac{f\,b}{d}}\;\;{\color{#c53030}{\begin{array}{l}\textbf{depth from}\\ \textbf{disparity}\end{array}}} \]
The three points from the stereo setup:
Rig: \(f\) = 500 px, \(b\) = 0.5 m, so \(f\,b\) = 250 px·m.
| Point | \(x_L\) (px) | \(x_R\) (px) | \(d\) (px) | \(f\,b\,/\,d\) (m) | True \(Z\) (m) |
|---|---|---|---|---|---|
| near | 257.5 | 132.5 | 125.0 | 2.00 | 2.00 |
| middle | 461.7 | 378.3 | 83.3 | 3.00 | 3.00 |
| far | 355.0 | 305.0 | 50.0 | 5.00 | 5.00 |



\[\Delta Z \approx \frac{Z^2}{f\,b}\,\Delta d\]
| \(Z\) | \(b\) = 0.5 m | \(b\) = 0.125 m |
|---|---|---|
| 2 m | ± 2 cm | ± 6 cm |
| 5 m | ± 10 cm | ± 40 cm |
| 10 m | ± 40 cm | ± 160 cm |


# R, t: pose of camera 2 relative to camera 1
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
K, dist, K, dist, (w, h), R, t, alpha=1)
map1 = cv2.initUndistortRectifyMap(
K, dist, R1, P1, (w, h), cv2.CV_32FC1)
map2 = cv2.initUndistortRectifyMap(
K, dist, R2, P2, (w, h), cv2.CV_32FC1)
rect1 = cv2.remap(img1, *map1, cv2.INTER_LINEAR)
rect2 = cv2.remap(img2, *map2, cv2.INTER_LINEAR)K: calibration matrix; dist: lens distortion; (w, h): image size; R, t: pose of camera 2 relative to camera 1; alpha=1: keep all pixels (0: crop to valid pixels).R1, R2: rotations; P1, P2: new cameras; Q: for 3D later.map1, map2: for each rectified pixel, where to sample in the original image (computed once, reused for every frame).cv2.stereoRectifyUncalibrated uses \(\mathbf{F}\) and matches instead.import cv2
import numpy as np
from scipy.spatial.transform import Rotation
img1 = cv2.imread("assets/14_stereo/frame1.JPG")
img2 = cv2.imread("assets/14_stereo/frame2.JPG")
h, w = img1.shape[:2]
# Calibration: intrinsics (one K, no lens distortion).
K = np.array([[681.916, 0.0, 623.032],
[ 0.0, 681.888, 412.946],
[ 0.0, 0.0, 1.0 ]])
dist = np.zeros(5)
# World-to-camera poses: quaternion (qw, qx, qy, qz) and translation.
R1w = Rotation.from_quat([0.442627, 0.525687, 0.555147, -0.468558], scalar_first=True).as_matrix()
R2w = Rotation.from_quat([0.407402, 0.484639, 0.591031, -0.499831], scalar_first=True).as_matrix()
t1w = np.array([3.22768, 1.01517, 3.73669])
t2w = np.array([2.63246, 1.09683, 3.22009])
# Pose of camera 2 relative to camera 1: x2 = R x1 + t.
R = R2w @ R1w.T
t = (t2w - R @ t1w).reshape(3, 1)
# 1. Rectifying rotations (R1, R2) and new camera matrices (P1, P2).
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(K, dist, K, dist, (w, h), R, t, alpha=1)
# 2. Warp both images.
map1 = cv2.initUndistortRectifyMap(K, dist, R1, P1, (w, h), cv2.CV_32FC1)
map2 = cv2.initUndistortRectifyMap(K, dist, R2, P2, (w, h), cv2.CV_32FC1)
rect1 = cv2.remap(img1, *map1, cv2.INTER_LINEAR)
rect2 = cv2.remap(img2, *map2, cv2.INTER_LINEAR)
# 3. Check: matched features should now be on the same row.
sift = cv2.SIFT_create()
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
good = [m for m, n in cv2.BFMatcher().knnMatch(des1, des2, k=2) if m.distance < 0.75 * n.distance]
pts1 = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
pts2 = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
pts1_rect = cv2.undistortPoints(pts1, K, dist, R=R1, P=P1).reshape(-1, 2)
pts2_rect = cv2.undistortPoints(pts2, K, dist, R=R2, P=P2).reshape(-1, 2)
print("row difference before:", np.median(np.abs(pts1[:, 0, 1] - pts2[:, 0, 1])), "px")
print("row difference after: ", np.median(np.abs(pts1_rect[:, 1] - pts2_rect[:, 1])), "px")
print("baseline:", -P2[0, 3] / P2[0, 0], "m")
cv2.imwrite("rect1.png", rect1)
cv2.imwrite("rect2.png", rect2)Block Matching
For each pixel \((x, y)\) of the left image: compare its \(W \times W\) window with the windows at \((x - d,\, y)\) in the right image for \(d = 0, \dots, D\), and keep the \(d\) with the lowest cost.

Sums over the window; \(I_R\) is taken at \((x - d,\, y)\).
| Cost | Formula | Idea |
|---|---|---|
| SAD | \(\sum \lvert I_L - I_R \rvert\) | minimize absolute differences |
| SSD | \(\sum (I_L - I_R)^2\) | minimize big differences |
| NCC | \(\dfrac{\sum (I_L - \bar I_L)(I_R - \bar I_R)}{\sqrt{\sum (I_L - \bar I_L)^2 \sum (I_R - \bar I_R)^2}}\) | maximize correlation; brightness/contrast invariant |



| Bad \(d\) estimate (> 2 px) | \(W\) = 5 | \(W\) = 11 | \(W\) = 25 |
|---|---|---|---|
| Flat regions | 46% | 38% | 34% |
| Near object boundaries | 41% | 35% | 39% |

numDisparities: search range \(D\); OpenCV requires a multiple of 16. blockSize: window \(W\) (odd).
Output is an integer equal to 16 × disparity (1/16 px precision): divide by 16.
Gray: no match (flat or unreliable areas).
16 ms for a 1414 × 962 image.

\[ E = \sum_{\mathbf p} C(\mathbf p, d_{\mathbf p}) \;+\; \sum_{\text{neighbors } \mathbf p, \mathbf q} \begin{cases} 0 & d_{\mathbf p} = d_{\mathbf q} \\ P_1 & |d_{\mathbf p} - d_{\mathbf q}| = 1 \\ P_2 & |d_{\mathbf p} - d_{\mathbf q}| > 1 \end{cases} \]
Semi-Global Matching
H. Hirschmüller, Stereo Processing by Semiglobal Matching and Mutual Information, IEEE TPAMI, 2008.


left = cv2.imread("piano_im0.png") # color: 3 channels
right = cv2.imread("piano_im1.png")
W = 5
sgbm = cv2.StereoSGBM_create(
minDisparity=0, numDisparities=128, blockSize=W,
P1=8 * 3 * W**2, P2=32 * 3 * W**2, # penalties
uniquenessRatio=10, speckleWindowSize=100,
speckleRange=2, mode=cv2.STEREO_SGBM_MODE_HH)
disp = sgbm.compute(left, right).astype(np.float32) / 16
disp[disp < 0] = np.nan # no match found| Bad pixels | Runtime | |
|---|---|---|
| StereoBM | 39% | 16 ms |
| StereoSGBM | 26% | 660 ms |
minDisparity, numDisparities: search range \(d = 0, \dots, 127\) px; must cover the nearest and farthest points.P1, P2: the smoothness penalties (\(\times\) channels \(\times W^2\), as suggested by OpenCV).MODE_HH: all 8 paths (the default mode uses 5, faster).uniquenessRatio, speckle…: drop unreliable matches (gray).import time
import cv2
import numpy as np
def read_pfm(path):
# Middlebury ground truth: float32, bottom row first; unknown = inf.
with open(path, "rb") as f:
f.readline() # "Pf": one channel
width, height = map(int, f.readline().split())
scale = float(f.readline()) # negative: little-endian
data = np.fromfile(f, "<f4" if scale < 0 else ">f4").reshape(height, width)
return np.flipud(data)
# A rectified pair, its ground-truth disparity, and the mask of pixels both cameras see.
left = cv2.imread("assets/14_stereo/piano_im0.png")
right = cv2.imread("assets/14_stereo/piano_im1.png")
gt = read_pfm("assets/14_stereo/piano_disp0GT.pfm")
visible = cv2.imread("assets/14_stereo/piano_mask0nocc.png", cv2.IMREAD_GRAYSCALE) == 255
scored = visible & np.isfinite(gt)
def bad_percent(disp):
# % of scored pixels whose disparity is off by more than 2 px (or missing).
return 100 * np.mean(~(np.abs(disp - gt) <= 2)[scored])
# 1. Block matching (grayscale).
bm = cv2.StereoBM_create(numDisparities=128, blockSize=15)
start = time.perf_counter()
disp_bm = bm.compute(cv2.cvtColor(left, cv2.COLOR_BGR2GRAY),
cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)).astype(np.float32) / 16
print(f"StereoBM: {1000 * (time.perf_counter() - start):.0f} ms")
# 2. Semi-global matching (color).
W = 5
sgbm = cv2.StereoSGBM_create(minDisparity=0, numDisparities=128, blockSize=W,
P1=8 * 3 * W**2, P2=32 * 3 * W**2,
uniquenessRatio=10, speckleWindowSize=100, speckleRange=2,
mode=cv2.STEREO_SGBM_MODE_HH)
start = time.perf_counter()
disp_sgbm = sgbm.compute(left, right).astype(np.float32) / 16
print(f"StereoSGBM: {1000 * (time.perf_counter() - start):.0f} ms")
# 3. Score against the ground truth, and save colored disparity maps.
for name, disp in [("bm", disp_bm), ("sgbm", disp_sgbm)]:
disp[disp < 0] = np.nan # no match found
print(f"{name}: {bad_percent(disp):.1f}% bad pixels")
scaled = np.nan_to_num(255 * disp / 128).astype(np.uint8)
colored = cv2.applyColorMap(scaled, cv2.COLORMAP_TURBO)
colored[np.isnan(disp)] = 0 # black: no match
cv2.imwrite(f"disparity_{name}.png", colored)
\[ Z = \frac{f\,b}{d}, \quad X = \frac{(x - c_x)\,Z}{f}, \quad Y = \frac{(y - c_y)\,Z}{f}; \quad \begin{bmatrix} X' & Y' & Z' & W \end{bmatrix}^\top = \mathbf{Q} \begin{bmatrix} x & y & d & 1 \end{bmatrix}^\top \]
cv2.stereoRectify.1,059,037 colored 3D points.
Holes: no match found.
Shadows behind objects: seen by only one camera.