Lesson 28: Structure from Motion¶
This lesson assembles feature matching (Lesson 19), robust estimation (Lesson 22), the essential matrix and pose recovery (Lesson 26), and calibration (Lesson 27) into a complete pipeline that takes 2D image correspondences from two views and recovers both the cameras' relative motion and the 3D positions of the points that were being viewed — Structure from Motion. The one ingredient we haven't yet developed in detail is triangulation: turning a matched 2D point pair, plus known camera poses, into a 3D point. Lesson 21 mentioned it by name (disparity converts to depth via triangulation), but this lesson derives and implements it directly.
import numpy as np
import cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
The classic pipeline, at a glance¶
- Match features between two images (Lesson 19: SIFT + ratio test).
- Estimate the essential matrix $E$ from those correspondences, robustly (Lesson 22: RANSAC), using known intrinsics $K$ (Lesson 27: calibration).
- Recover relative pose $(R, t)$ from $E$ (Lesson 26:
cv2.recoverPose) — up to an unknown scale on $t$. - Triangulate: for every matched pair, intersect the two corresponding rays in 3D to recover a 3D point — developed in detail for the first time in this lesson.
We use two real photos of a bronze sculpture from the BlendedMVS dataset (Yao et al., 2020), which reconstructed each scene's geometry with a full SfM+MVS pipeline and ships every photo's recovered camera pose alongside it — so, unlike a photo pair we'd shoot ourselves, we still have ground truth to check the entire pipeline against, end to end.
Two real photos, with known camera poses¶
BlendedMVS recovered each scene's 3D geometry and camera poses from real photos via SfM+MVS, so every image ships with a ground-truth $(K, R, t)$. We treat the first photo's camera as the world origin and express the second camera's pose relative to it — giving us P1 and P2_true, the two ground-truth projection matrices this whole lesson tries to recover from image correspondences alone.
K = np.array([[583.2225, 0., 255.4034],
[0., 583.2225, 188.8833],
[0., 0., 1.]])
R1, t1 = np.eye(3), np.zeros(3)
R_true = np.array([[ 0.92083152, -0.14714329, 0.36113446],
[ 0.12698365, 0.98874714, 0.07907683],
[-0.36870643, -0.02695794, 0.92915445]])
t_true = np.array([-0.78751177, 0.00981936, 0.03364542])
P1 = K @ np.hstack([R1, t1.reshape(3, 1)])
P2_true = K @ np.hstack([R_true, t_true.reshape(3, 1)])
view1 = cv2.imread('../img/bronzebull00.jpg')
view2 = cv2.imread('../img/bronzebull02.jpg')
fig, axes = plt.subplots(1, 2, figsize=(9, 3.5))
axes[0].imshow(cv2.cvtColor(view1, cv2.COLOR_BGR2RGB))
axes[0].set_title('View 1')
axes[1].imshow(cv2.cvtColor(view2, cv2.COLOR_BGR2RGB))
axes[1].set_title('View 2')
for ax in axes:
ax.axis('off')
plt.tight_layout()
plt.show()
Image source: BlendedMVS (CC BY 4.0)
Step 1: feature matching¶
SIFT + ratio test (Lesson 19), run directly on the two photos.
gray1 = cv2.cvtColor(view1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(view2, cv2.COLOR_BGR2GRAY)
sift = cv2.SIFT_create()
kp1, des1 = sift.detectAndCompute(gray1, None)
kp2, des2 = sift.detectAndCompute(gray2, None)
bf = cv2.BFMatcher()
raw_matches = bf.knnMatch(des1, des2, k=2)
good_matches = [m for m, n in raw_matches if m.distance < 0.75 * n.distance]
x1 = np.float32([kp1[m.queryIdx].pt for m in good_matches])
x2 = np.float32([kp2[m.trainIdx].pt for m in good_matches])
print(f'keypoints: {len(kp1)} (view 1), {len(kp2)} (view 2)')
print(f'good matches after ratio test: {len(good_matches)}')
match_vis = cv2.drawMatches(view1, kp1, view2, kp2, good_matches, None,
flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
plt.figure(figsize=(10, 4))
plt.imshow(cv2.cvtColor(match_vis, cv2.COLOR_BGR2RGB))
plt.title('Feature matches')
plt.axis('off')
plt.show()
Step 2: robust essential matrix and pose recovery¶
Feed the matched points through the same robust pipeline built in Lessons 22 and 26 to recover the essential matrix and the cameras' relative pose.
E, inlier_mask = cv2.findEssentialMat(x1, x2, K, method=cv2.RANSAC, threshold=1.0)
_, R_estimated, t_estimated, _ = cv2.recoverPose(E, x1, x2, K)
print(f'inliers: {int(inlier_mask.sum())} / {len(inlier_mask)}')
print('recovered rotation:\n', np.round(R_estimated, 4))
print('true rotation:\n', np.round(R_true, 4))
print()
print('recovered translation direction:', np.round(t_estimated.ravel(), 4))
print('true translation direction: ', np.round(t_true / np.linalg.norm(t_true), 4))
As in Lesson 26, recoverPose returns a unit-length translation direction — the actual baseline distance between the cameras is fundamentally unrecoverable from image correspondences alone. We'll come back to that.
Step 3: triangulation¶
Given two camera projection matrices $P_1, P_2$ (each $3\times4$, mapping a 3D point to a 2D image point in homogeneous coordinates) and a matched pixel pair $(x_1, x_2)$, we want the 3D point $X$ satisfying both $x_1 \propto P_1 X$ and $x_2 \propto P_2 X$. Each view contributes 2 independent linear equations in $X$'s 4 homogeneous unknowns (cross-multiplying out the unknown scale factor), by the same DLT/SVD recipe used for homographies (Lesson 23) and the fundamental matrix (Lesson 26):
$$A = \begin{bmatrix} u_1 P_1^{(3)} - P_1^{(1)} \\ v_1 P_1^{(3)} - P_1^{(2)} \\ u_2 P_2^{(3)} - P_2^{(1)} \\ v_2 P_2^{(3)} - P_2^{(2)} \end{bmatrix}, \qquad AX = 0$$
where $P^{(i)}$ denotes row $i$ of $P$. The null space of $A$ (smallest-singular-value right singular vector) gives $X$.
def triangulate_dlt(P1, P2, x1, x2):
points = []
for (u1, v1), (u2, v2) in zip(x1, x2):
A = np.array([
u1 * P1[2] - P1[0],
v1 * P1[2] - P1[1],
u2 * P2[2] - P2[0],
v2 * P2[2] - P2[1],
])
_, _, Vt = np.linalg.svd(A)
X = Vt[-1]
points.append(X[:3] / X[3])
return np.array(points)
# validate against the dataset's TRUE camera poses: triangulate the RANSAC inliers with them,
# and check that reprojecting the resulting 3D points lands back on the original pixels
inliers = inlier_mask.ravel().astype(bool)
x1_in, x2_in = x1[inliers], x2[inliers]
recon_true_cameras = triangulate_dlt(P1, P2_true, x1_in, x2_in)
cv_points_4d = cv2.triangulatePoints(P1, P2_true, x1_in.T, x2_in.T)
cv_points_3d = (cv_points_4d[:3] / cv_points_4d[3]).T
print(f'max diff vs. cv2.triangulatePoints: {np.abs(recon_true_cameras - cv_points_3d).max():.2e}')
# cheirality check: a correctly-matched 3D point must be in front of BOTH cameras --
# a real pipeline always applies this, since epipolar-only RANSAC can still admit a bad match
depth1 = recon_true_cameras[:, 2]
depth2 = (R_true @ recon_true_cameras.T + t_true.reshape(3, 1))[2]
valid = (depth1 > 0) & (depth2 > 0)
x1_in, x2_in, recon_true_cameras = x1_in[valid], x2_in[valid], recon_true_cameras[valid]
print(f'kept {valid.sum()} / {len(valid)} inliers after cheirality check')
def reprojection_error(P, X, x):
X_h = np.hstack([X, np.ones((len(X), 1))])
proj = (P @ X_h.T).T
proj = proj[:, :2] / proj[:, 2:3]
return np.linalg.norm(proj - x, axis=1)
err1 = reprojection_error(P1, recon_true_cameras, x1_in)
err2 = reprojection_error(P2_true, recon_true_cameras, x2_in)
all_err = np.concatenate([err1, err2])
print(f'reprojection error using TRUE camera poses: mean {all_err.mean():.3f} px, max {all_err.max():.3f} px')
Putting it together: reconstruction using estimated pose¶
Now the real test: triangulate using the camera matrix built from recoverPose's estimated $(R, t)$, not the dataset's true pose. Because $t$ was only recovered up to scale, so is the reconstruction — every 3D point comes out a fixed factor smaller than reality. Multiplying by the true baseline length (the one piece of information triangulation from image correspondences alone can never supply — here read off from BlendedMVS's ground truth, standing in for whatever real-world scale reference you'd use in practice) should recover the correct metric scene.
P2_estimated = K @ np.hstack([R_estimated, t_estimated.reshape(3, 1)])
reconstruction_unit_scale = triangulate_dlt(P1, P2_estimated, x1_in, x2_in)
true_baseline = np.linalg.norm(t_true)
reconstruction_metric = reconstruction_unit_scale * true_baseline
error = np.linalg.norm(reconstruction_metric - recon_true_cameras, axis=1)
print(f'true baseline: {true_baseline:.3f}')
print(f'mean 3D reconstruction error (vs. triangulation from true poses): {error.mean():.3f}')
print(f'max 3D reconstruction error: {error.max():.3f}')
With real (imperfect) correspondences, the entire pipeline — essential matrix, pose, triangulation — reconstructs the scene to within a small fraction of the camera baseline, using nothing but 2D pixel correspondences and known intrinsics, plus one external number (the true baseline) to fix the scale ambiguity. In practice, that scale reference might come from a known object size in the scene, a second sensor (GPS, IMU, LiDAR), or a calibrated stereo rig (Lesson 21) instead of two arbitrary independent cameras.
Visualizing the reconstruction¶
For a denser, more recognizable point cloud than the strict RANSAC-inlier set used for the numeric check above, we loosen the feature-matching thresholds (SIFT_create(contrastThreshold=0.01), ratio 0.85) to pull in more — slightly noisier — correspondences, then color each 3D point with its actual pixel color from view 1.
sift_dense = cv2.SIFT_create(contrastThreshold=0.01)
kp1d, des1d = sift_dense.detectAndCompute(gray1, None)
kp2d, des2d = sift_dense.detectAndCompute(gray2, None)
raw_dense = bf.knnMatch(des1d, des2d, k=2)
good_dense = [m for m, n in raw_dense if m.distance < 0.85 * n.distance]
x1d = np.float32([kp1d[m.queryIdx].pt for m in good_dense])
x2d = np.float32([kp2d[m.trainIdx].pt for m in good_dense])
_, dense_mask = cv2.findEssentialMat(x1d, x2d, K, method=cv2.RANSAC, threshold=1.0)
dense_inliers = dense_mask.ravel().astype(bool)
x1d, x2d = x1d[dense_inliers], x2d[dense_inliers]
recon_dense_true = triangulate_dlt(P1, P2_true, x1d, x2d)
recon_dense_estimated = triangulate_dlt(P1, P2_estimated, x1d, x2d) * true_baseline
# cheirality check again, same as for the sparse set above
depth1 = recon_dense_true[:, 2]
depth2 = (R_true @ recon_dense_true.T + t_true.reshape(3, 1))[2]
valid = (depth1 > 0) & (depth2 > 0)
x1d, recon_dense_true, recon_dense_estimated = x1d[valid], recon_dense_true[valid], recon_dense_estimated[valid]
rgb1 = cv2.cvtColor(view1, cv2.COLOR_BGR2RGB)
px = np.clip(x1d[:, 0].astype(int), 0, rgb1.shape[1] - 1)
py = np.clip(x1d[:, 1].astype(int), 0, rgb1.shape[0] - 1)
point_colors = rgb1[py, px] / 255.0
print(f'{len(x1d)} points in the denser cloud (vs. {len(x1_in)} used for the numeric check above)')
fig = plt.figure(figsize=(7, 6))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*recon_dense_true.T, c='gray', s=8, alpha=0.3, label='triangulated with true poses')
ax.scatter(*recon_dense_estimated.T, c=point_colors, s=25, label='reconstructed (estimated pose)')
# mark the two camera centers
cam1_center = -R1.T @ t1
cam2_center = -R_true.T @ t_true
ax.scatter(*cam1_center, c='blue', s=80, marker='^', label='camera 1')
ax.scatter(*cam2_center, c='green', s=80, marker='^', label='camera 2')
ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
ax.legend(fontsize=8)
ax.set_title('Structure from motion: recovered 3D points and camera poses')
plt.show()
From sparse to dense¶
Even loosened, feature matching only ever gives a sparse cloud — one point per distinctive keypoint, with large gaps over smooth, low-texture regions. Once SfM has recovered the camera poses, though, every pixel becomes fair game: multi-view stereo (MVS) sweeps a plane-hypothesis or patch (Lesson 21's stereo matching, generalized from a rectified pair to arbitrarily-posed calibrated cameras) through space to estimate a depth for every pixel in every view, then fuses those per-view depth maps — filtering out inconsistent estimates and merging the rest — into the dense colored point cloud (or mesh) that toolkits like COLMAP or, fittingly, BlendedMVS's own reconstruction pipeline produce.
Exercise¶
- Add extra synthetic pixel noise (e.g.
rng.normal(0, 1.0, x1_in.shape)) on top of the already-real detections inx1_in/x2_inbefore triangulating. How much does the reconstruction error grow, and does it grow uniformly, or worse for points farther from the cameras (Lesson 21's disparity-depth relationship: distant points produce smaller, noisier parallax)? - This lesson only used 2 of the scene's 131 available views. Real structure-from-motion pipelines (e.g. COLMAP) add many more views incrementally: each new image is resected against the already-reconstructed 3D points (a problem called Perspective-n-Point, or PnP — given 2D-3D correspondences and $K$, solve for that camera's pose) via
cv2.solvePnP, then its new points are triangulated against the growing reconstruction. Look upcv2.solvePnP's signature and sketch (in words) how you'd extend this notebook to a third view. - This pair (views 0 and 2) has a deliberately generous baseline for reliable matching. Swap in views 0 and 1 instead (a much shorter baseline — you'll need their
.npzcamera parameters from the same BlendedMVS scene). How does the shorter baseline affect the number of RANSAC inliers, and the accuracy of the recovered pose and reconstruction, compared to the wider-baseline pair used above?