import numpy as np
def dlt_homography(src, dst):
"""DLT homography estimate. src,dst: shape (n,2). Returns H: (3,3)."""
n = len(src)
A = np.zeros((2*n, 9))
for i, ((x,y),(u,v)) in enumerate(zip(src, dst)):
A[2*i ] = [-x,-y,-1, 0, 0, 0, u*x, u*y, u]
A[2*i+1] = [ 0, 0, 0,-x,-y,-1, v*x, v*y, v]
_, _, Vt = np.linalg.svd(A)
H = Vt[-1].reshape(3, 3)
return H / H[2, 2]
def reproj_errors(H, src, dst):
"""Per-point reprojection errors. Returns shape (n,)."""
n = len(src)
src_h = np.column_stack([src, np.ones(n)]) # shape (n, 3)
proj = (H @ src_h.T).T # shape (n, 3)
pred = proj[:, :2] / proj[:, 2:3] # shape (n, 2)
return np.linalg.norm(pred - dst, axis=1) # shape (n,)
def ransac_homography(src, dst, max_iter=500, threshold=3.0, rng=None):
"""
RANSAC homography estimation.
src, dst: shape (n, 2)
threshold: inlier reprojection error in pixels
Returns (H_best, inlier_mask): H shape (3,3), mask shape (n,) bool.
"""
if rng is None:
rng = np.random.default_rng(0)
n = len(src)
best_inliers = np.zeros(n, dtype=bool)
best_count = 0
for _ in range(max_iter):
idx = rng.choice(n, size=4, replace=False)
H_try = dlt_homography(src[idx], dst[idx]) # shape (3, 3)
errs = reproj_errors(H_try, src, dst) # shape (n,)
mask = errs < threshold # shape (n,) bool
if mask.sum() > best_count:
best_count = mask.sum()
best_inliers = mask
# Refit on all inliers
H_final = dlt_homography(src[best_inliers], dst[best_inliers])
return H_final, best_inliers
# --- Simulation: 50 matches, 40% outliers ---
rng = np.random.default_rng(42)
# Ground-truth H
H_true = np.array([[ 1.2, 0.3, 50.],
[-0.1, 1.1, 30.],
[ 0.001, 0.0005, 1.]]) # shape (3, 3)
H_true /= H_true[2, 2]
# Generate 50 source points (random pixel locations)
n_pts = 50
src_all = rng.uniform([50., 50.], [590., 430.], (n_pts, 2)) # shape (50, 2)
# True destination points
src_h = np.column_stack([src_all, np.ones(n_pts)])
proj = (H_true @ src_h.T).T
dst_all = proj[:, :2] / proj[:, 2:3] # shape (50, 2)
# Add small inlier noise
inlier_noise = rng.normal(0., 1.0, (n_pts, 2)) # shape (50, 2)
dst_all += inlier_noise
# Corrupt 40% of matches with random outliers
n_outliers = int(0.4 * n_pts)
outlier_idx = rng.choice(n_pts, size=n_outliers, replace=False)
dst_all[outlier_idx] = rng.uniform([0., 0.], [640., 480.], (n_outliers, 2))
# Run plain DLT (no RANSAC) and RANSAC
H_dlt, _ = np.linalg.svd( # DLT on all points
np.vstack([[-x,-y,-1,0,0,0,u*x,u*y,u] for (x,y),(u,v) in zip(src_all, dst_all)]
+ [[0,0,0,-x,-y,-1,v*x,v*y,v] for (x,y),(u,v) in zip(src_all, dst_all)])), 2
A = np.zeros((2*n_pts, 9))
for i, ((x,y),(u,v)) in enumerate(zip(src_all, dst_all)):
A[2*i ] = [-x,-y,-1, 0, 0, 0, u*x, u*y, u]
A[2*i+1] = [ 0, 0, 0,-x,-y,-1, v*x, v*y, v]
_, _, Vt = np.linalg.svd(A)
H_dlt = Vt[-1].reshape(3, 3); H_dlt /= H_dlt[2,2]
H_ransac, inlier_mask = ransac_homography(src_all, dst_all, max_iter=500,
threshold=3.0, rng=rng)
def rms_on_inliers(H, src, dst, mask):
errs = reproj_errors(H, src[mask], dst[mask])
return np.sqrt(np.mean(errs**2))
print(f"Points: {n_pts} | Outliers: {n_outliers} ({100*n_outliers/n_pts:.0f}%)")
print(f"Plain DLT — RMS error (inliers only): "
f"{rms_on_inliers(H_dlt, src_all, dst_all, ~np.isin(np.arange(n_pts), outlier_idx)):.2f} px")
print(f"RANSAC — inliers found: {inlier_mask.sum()}/{n_pts}")
print(f" — RMS error (inliers only): "
f"{rms_on_inliers(H_ransac, src_all, dst_all, ~np.isin(np.arange(n_pts), outlier_idx)):.2f} px")
