Compare monocular and stereo visual odometry

What you will do

You will use the Visual Odometry Simulator to reproduce scale error, then implement a monocular-versus-stereo scale experiment in Python, and explain the result with the epipolar geometry of lesson M2-T10 and the depth law of lesson M2-T12.

Before you start

• Lessons M2-T10 (visual odometry) and M2-T12 (stereo visual odometry).
• The Visual Odometry Simulator page and Python 3 with NumPy.

Part 1: scale in the simulator

1. Open the Visual Odometry Simulator. Note the final error and drift with default settings (assumed height error 0%).
2. Set “assumed height error” to +20%, then −20%. Record the final error and how the path changes shape and size.
3. Explain in two sentences why the shape is preserved and the size is not.
4. Set the true height from 10 m to 20 m with the assumed error at 0%, and verify the drift percentage does not change much: the pipeline “knows” the height. What would a real monocular camera not know?

Part 2: scale from a stereo baseline (code)

Starter: triangulate matched points in two views and estimate the translation; the monocular case returns a unit vector only. Fill the TODOs.

import numpy as np
rng = np.random.default_rng(1)
f, B = 500.0, 0.2                                    # focal length (px) and stereo baseline (m)
X = np.stack([rng.uniform(-3, 3, 100), rng.uniform(-2, 2, 100), rng.uniform(3, 9, 100)], axis=1)   # scene points (m)
t_true = np.array([0.4, 0.0, 0.8])                   # true motion between frames (m)

def project(P): return np.stack([f * P[:, 0] / P[:, 2], f * P[:, 1] / P[:, 2]], axis=1)

# TODO 1 (monocular): given only project(X) and project(X + t_true shifted) estimate t.
#   Show that scaling X and t_true by 2 gives identical image measurements, so the length of t cannot be recovered.
# TODO 2 (stereo): triangulate X from disparity d = f * B / Z, then recover t by aligning the two 3-D point sets.
# TODO 3: report the length error of t for monocular and stereo with 0.3 px disparity noise.

import numpy as np
rng = np.random.default_rng(1)
f, B = 500.0, 0.2
X = np.stack([rng.uniform(-3, 3, 100), rng.uniform(-2, 2, 100), rng.uniform(3, 9, 100)], axis=1)
t_true = np.array([0.4, 0.0, 0.8])
def project(P): return np.stack([f * P[:, 0] / P[:, 2], f * P[:, 1] / P[:, 2]], axis=1)

# 1) monocular: a world scaled by 2 (points and motion) produces the same images
X2 = X + t_true
assert np.allclose(project(X), project(2 * X)) and np.allclose(project(X + t_true), project(2 * X + 2 * t_true))   # scale ambiguity

# 2) stereo: depth from disparity (with noise), then 3-D to 3-D translation (pure translation here: the mean difference)
def triangulate(P, sigma_d=0.3):
    uv = project(P); d = f * B / P[:, 2] + sigma_d * rng.standard_normal(len(P)); Z = f * B / d
    return np.stack([uv[:, 0] * Z / f, uv[:, 1] * Z / f, Z], axis=1)
P1, P2 = triangulate(X), triangulate(X + t_true)
t_est = (P2 - P1).mean(axis=0)
err_len = abs(np.linalg.norm(t_est) - np.linalg.norm(t_true))
print(f"stereo translation estimate {np.round(t_est, 3)}, length error {err_len * 100:.1f} cm; the monocular length is unobservable (any scale fits)")
assert err_len < 0.05 and np.linalg.norm(t_est - t_true) < 0.08

Questions to answer

• At what range does the stereo depth error reach 10% for your numbers? Compare with the formula $Z_{\max}=\varepsilon \,f\,B/\sigma _{d}$.
• How would an IMU give monocular VO its scale (lesson M2-T11)? What motion does it need?
• Which would you choose for a vehicle flying 40 m above ground, and why?

How to know it worked

• You recorded final error and drift for the height-error cases and explained them.
• The reference solution runs and its assertions pass.
• You can state the stereo range limit for your parameters.

If it does not work

References

• OpenCV: depth map from stereo images
• $R$. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, 2nd ed., Cambridge University Press, 2004.

Record your progress