05 Multiview geometry Visual SLAM 3D Vision Robotics

Multi-view Geometry & Visual SLAM

From Two Rays to
Cameras, Maps, and Objects

A single image constrains depth to a ray; cross-frame correspondences connect rays to constrain camera motion and triangulate the scene. Visual SLAM organizes these local constraints into a state graph that can keep updating, relocalize, and maintain global consistency.

Open the Three-Part Lab Start with coordinate conventions

Problem Contract

Fix pose direction before defining two-view constraints

This site uses right-handed frames, column vectors, and target-from-source notation. Camera pose is world-from-camera; projecting world points uses its inverse, camera-from-world. Derive relative transforms through one consistent chain rather than guessing from variable names.

World poses → relative camera motion

{}^{C_2}\mathbf T_{C_1}=\left({}^{W}\mathbf T_{C_2}\right)^{-1}{}^{W}\mathbf T_{C_1},\qquad {}^{C_2}\mathbf p=\mathbf R_{21}\,{}^{C_1}\mathbf p+\mathbf t_{21}

R21 and t21 map 3D coordinates from C1 into C2. t21 gives the C1 origin in C2 coordinates; it is not the C2 camera center in the world frame. The lab and all equations below use this direction.

Pixel u₁

Undistort, then apply K₁⁻¹ to obtain bearing x₁

↔

MATCH

The same physical point

Features, photometry, flow, or dense correspondences establish identity

↔

Pixel u₂

Bearing x₂ must use its own K₂ and distortion state

→

GEOMETRY

motion + structure

Model selection, relative pose, cheirality, and triangulation

Geometry does not automatically provide correspondence identities. Epipolar geometry, PnP, and BA explain observations assuming mostly correct identities. Descriptor matching, tracking, semantic association, and object identity belong to data association. RANSAC handles some outliers but cannot recover fully incorrect identities.

Epipolar Geometry

E and F reduce a 2D search to an epipolar line

A 3D point, both camera centers, and both observation rays lie in one plane. Its second-image correspondence therefore lies on the epipolar line determined by the first-image point. The essential matrix encodes calibrated relative rigid motion; the fundamental matrix absorbs intrinsics into a pixel-domain relationship.

Calibrated epipolar constraint

\mathbf x_2^\mathsf T\mathbf E\mathbf x_1=0,\qquad \mathbf E=[\mathbf t_{21}]_\times\mathbf R_{21},\qquad \operatorname{sv}(\mathbf E)=(\sigma,\sigma,0)

E has five degrees of freedom: 3 for relative rotation and 2 for unit translation direction. Scaling t21 by any nonzero factor preserves the constraint, so E does not encode monocular baseline length.

Pixel-domain fundamental matrix

\mathbf u_2^\mathsf T\mathbf F\mathbf u_1=0,\qquad \mathbf F=\mathbf K_2^{-\mathsf T}\mathbf E\mathbf K_1^{-1},\qquad \operatorname{rank}(\mathbf F)=2

If points are undistorted to a new camera matrix, use the K matching them. Applying ideal pinhole F/E constraints directly to distorted pixels produces position-dependent systematic residuals.

Model	Input coordinates	Degrees of freedom	Minimum correspondences	Applications	Main degeneracy
Essential matrix · E	Calibrated normalized coordinates x₁ ↔ x₂	5 DoF: R(3) + t direction(2)	5-point; normalized 8-point is also common	Calibrated relative pose in general 3D scenes	E=0 under pure rotation; ill-conditioned depth at low parallax; weak identifiability in planar scenes
Fundamental matrix · F	Original pixels u₁ ↔ u₂	Rank-2 projective matrix, defined up to scale	7-point / 8-point	Two-view epipolar geometry with unknown or unused intrinsics	Does not directly provide metric pose; coordinate preprocessing and numerical normalization are crucial
Homography · H	Pixels on one plane, or all image pixels under pure rotation	8 DoF, defined up to scale	4 nondegenerate point pairs	Planes, pure rotation, image stitching, degenerate branches	One H cannot explain different depths in a general 3D scene

ALGEBRAIC

|x₂ᵀEx₁|

Convenient algebraically, but its value varies with E scale and coordinate scaling; it is unsuitable as a comparable geometric distance.

SAMPSON

First-order geometric error approximation

Normalize the algebraic residual using the first two components of E x₁ and Eᵀx₂. Common in RANSAC, but still an approximation to full nonlinear reprojection error.

REPROJECTION

Project a 3D hypothesis back into both images

Given poses and 3D points, joint two-view or multiview pixel residuals are closest to the final BA objective.

Model Selection & Degeneracy

Choose a model supported by the scene before recovering pose

Five-point, eight-point, and homography methods encode different geometric assumptions. E/F constrain general 3D scenes; H explains a single plane or pure rotation. Reliable initialization compares support, residuals, parallax, and cheirality together.

Plane-induced homography · nᵀX=d convention

\mathbf H=\mathbf K_2\!\left(\mathbf R_{21}+\frac{\mathbf t_{21}\mathbf n^\mathsf T}{d}\right)\!\mathbf K_1^{-1},\qquad \mathbf n^\mathsf T\mathbf X=d

The sign depends on the plane-equation convention. Here n and X are in C1. If t21=0, all depths share the pure-rotation homography H=K2 R21 K1⁻¹. State the plane convention before choosing a sign.

RANSAC handles outliers, not unobservability. Even fully correct correspondences cannot supply a triangulation baseline under pure rotation, remove weak planar branches, or prevent amplified depth variance at low parallax. Implement and log model scores and observability checks separately.

Triangulation

Epipolar constraints test coplanarity; triangulation recovers depth from rays

Noiseless world rays intersect; real observed rays are usually skew, requiring minimization of algebraic, ray-distance, or reprojection errors. No method bypasses conditioning: depth uncertainty grows sharply as parallax approaches zero.

DLT

Linear homogeneous initialization

Construct A X=0 from multiview projection equations and take the smallest singular vector. Fast, but its algebraic objective differs from a pixel-noise model.

MIDPOINT

Midpoint of the closest ray points

Find the closest points on two 3D rays and use their midpoint. Intuitive for teaching, but dependent on the frame and directional-noise model.

NONLINEAR

Reprojection refinement

Initialize linearly, minimize robust reprojection error in two or more views, and check positive depth.

INVERSE DEPTH

Distant points and delayed initialization

Inverse depth ρ=1/Z behaves more smoothly as distant points approach ρ=0; avoid forcing a metric point from insufficient evidence.

Parallax and conditioning intuition

\theta=\cos^{-1}\!\left(\hat{\mathbf d}_1^\mathsf T\hat{\mathbf d}_2\right),\qquad \text{depth sensitivity}\ \propto\ \frac{1}{\sin\theta}

This is not a full covariance formula, but shows the degeneracy trend: as θ→0, nearly parallel rays turn tiny pixel perturbations into large depth changes. Set thresholds using focal length, noise, baseline, and task depth, rather than a fixed angle alone.

Interactive Geometry Lab

One set of conventions connects two views, degeneracies, and SLAM graphs

The first view computes E, Sampson distance, closest-ray triangulation, parallax, and positive depth. The second compares ground-truth E/H residuals in four controlled scenes. The third changes sensors, object states, and loops to show gauge, state dimensions, the Schur reduced system, and drift.

True point P Triangulated P̂ Observation rays noisy ray Top view shows X–Z; calculations use full 3D coordinates

Epipolar

x₂ᵀEx₁

—Sampson —

Parallax

Angle Between World Rays

——

Triangulation

Estimated 3D Point

——

Cheirality

Positive Depth in Both Cameras

——

Translation

What Does E Recover?

direction only—

Essential matrix · normalized scale

E / ‖t₂₁‖

x₂ᵀEx₁ = 0

The essential matrix does not encode monocular baseline length. Scaling t₂₁ by any nonzero factor preserves the epipolar constraint. Calibrated monocular two-view geometry therefore recovers rotation and unit translation direction; triangulated depth and the entire map vary by the same scale.

Visual Odometry → SLAM

SLAM requires more than repeatedly applying two-view formulas

A persistent system manages keyframes, landmark lifecycles, local windows, relocalization, loop closure, and map merging. VO mainly estimates consecutive local motion; SfM usually reconstructs cameras and structure offline; SLAM localizes online while maintaining a reusable, globally consistent map.

01

Front-end tracking

Match features, minimize photometric error directly, or use learned correspondences / flow; output cross-frame observations with uncertainty.

→

02

Initialization

Select between models such as E and H, recover relative motion, check cheirality, and triangulate only points with sufficient parallax.

→

03

Keyframes and local maps

Insert keyframes based on motion, overlap, and tracking quality; initialize new points through multiview geometry.

→

04

Local optimization

Jointly optimize recent cameras, landmarks, and optional object states; manage the window with robust kernels, outlier removal, and marginalization.

→

05

Relocalization

After tracking failure, retrieve place candidates and recover camera pose through 2D–3D PnP / geometric verification.

→

06

Loop closure and map merging

Verify nonlocal revisits, optimize a pose graph / essential graph, and perform global BA and map merging as needed.

Front-end family	Main observations / residuals	Strengths	Risks
Feature-based	Keypoints, descriptors, matches, and reprojection residuals	Usually more robust to exposure changes and larger baselines; explicit graph structure	Low texture, repeated texture, and discrete matching
Direct / semi-direct	Photometric residuals from pixel intensity, patches, or gradients	Uses more pixels and can retain continuous signals in weakly textured regions	Depends on brightness constancy, good initialization, and exposure/response modeling
Dense / learned	Depth, optical flow, correspondence fields, or neural-map residuals	Combines semantics and strong priors to cover areas difficult for traditional features	Domain shift, confidence calibration, geometric consistency, computation cost

VISUAL ODOMETRY

Local increments and short-term consistency

Allows long-term drift; often serves control, initialization, or a SLAM front end.

STRUCTURE FROM MOTION

Offline global reconstruction

May use repeated matching, global BA, and incremental/global reconstruction without real-time localization constraints.

VISUAL SLAM

Online localization + reusable map

Requires relocalization, loop closure, map maintenance, and failure recovery; a trajectory alone does not establish SLAM.

Bundle Adjustment

BA jointly explains cameras and 3D points instead of patching poses frame by frame

Camera poses, landmarks, intrinsics, and optional object states jointly affect reprojection. BA iterates on manifolds, using block-sparse Jacobians and Schur complements to eliminate many point variables. Robust kernels reduce outlier influence; data association and gauge still require explicit handling.

Robust bundle-adjustment objective

\min_{\{\mathbf T_{C_iW}\},\,\{\mathbf P_j\}}\ \sum_{(i,j)\in\mathcal O}\rho\!\left(\left\|\mathbf u_{ij}-\pi\!\left(\mathbf K_i,\mathbf T_{C_iW},\mathbf P_j\right)\right\|_{\boldsymbol\Sigma_{ij}^{-1}}^2\right)

O is the visible observation set; Σ whitens pixel noise and confidence, and ρ is a robust loss. Update poses through SE(3) retraction. Local BA, sliding windows, and global BA share the formula but differ in variable scope and prior management.

BLOCK SPARSITY

Each observation connects one camera and one point

The Jacobian is extremely sparse; ignoring that structure and forming dense normal equations wastes orders of magnitude in resources.

SCHUR

Eliminate landmark blocks first

Reduce the main system to camera, intrinsic, and object variables, then back-substitute point increments.

GAUGE

Fix unobservable freedoms first

An unanchored Hessian is singular; first-pose anchors, metric scale, or priors must match sensor observability.

MARGINALIZATION

Compress old states into a prior

Real-time sliding windows retain information at a linearization point. Outliers entering the prior can contaminate subsequent estimates persistently.

Loop factors and gauge are distinct. Loop constraints relate revisited poses and redistribute accumulated drift; they do not create monocular metric scale. Monocular submaps often merge in Sim(3) because their scales may differ.

Scale & Observability

Sensors determine observable freedoms in the same SLAM graph

Optimization and observability differ. An optimizer can assign numbers to any variable, but without independent information there may be gauge freedom, strong correlations, or extreme variance. Fixing a variable chooses a reference; it does not create a physical observation.

Modality	Direct observations	Scale implications	Alignment / engineering notes
Monocular	bearing / projective geometry	Global scale is unobservable; map and translations share one unknown scale	Evaluation often needs Sim(3) alignment; fixing the first frame alone leaves scale gauge
Calibrated stereo	Left/right pixels + known baseline	The baseline converts disparity into metric depth	Use SE(3) with correct calibration; baseline error directly produces scale bias
RGB-D	Color pixels + depth / points	Valid depth provides metric scale	Handle depth scale, invalid values, RGB–Depth extrinsics, and time synchronization
Visual–Inertial	Visual bearings + IMU specific force / angular rate	Scale and gravity direction become recoverable with sufficient excitation and correct initialization	Stationary or particular motions weaken observability; time offsets, biases, and extrinsics also require calibration

Gauge-equivalent monocular reconstruction

\mathbf P_j' = s\,\mathbf P_j,\qquad \mathbf t_i'=s\,\mathbf t_i,\qquad \pi(\mathbf R_i\mathbf P_j+\mathbf t_i)=\pi(\mathbf R_i\mathbf P_j'+\mathbf t_i')

For any s>0, perspective division yields the same pixels. Rotation stays unchanged while camera translations, 3D points, and object positions scale together. Known-size objects, camera height, or wheel odometry can supply scale priors, but their errors directly enter the map.

Object-level & Dynamic SLAM

6D object pose can be both an output and a map state

Point maps store 3D positions without semantics; object maps explicitly store object frames, categories, sizes, symmetry, and uncertainty. Static objects can serve as large landmarks for localization; dynamic objects require time-indexed states, motion models, and cross-frame identity association.

Static object observation factor

{}^{C_i}\mathbf T_{O_j}={}^{C_i}\mathbf T_W\,{}^{W}\mathbf T_{O_j},\qquad \mathbf r_{ij}=\operatorname{Log}\!\left(\widehat{\mathbf T}_{C_iO_j}^{-1}\,{}^{C_i}\mathbf T_W\,{}^{W}\mathbf T_{O_j}\right)

A detector or 6D pose estimator supplies camera–object observations; the SLAM graph jointly optimizes camera poses and world object poses. Define residual direction, Log order, symmetry equivalence classes, and covariance frames together.

Dynamic object state

\mathcal X=\left\{{}^{W}\mathbf T_{C_i},\ {}^{W}\mathbf T_{O_j,i},\ \mathbf P_k,\ \text{identity}_j\right\},\qquad {}^{W}\mathbf T_{O_j,i+1}\approx f\!\left({}^{W}\mathbf T_{O_j,i},\mathbf v_{j,i}\right)

In dynamic scenes, pixel motion combines camera motion, object motion, and depth. Without static background, independent depth, or motion priors, they are strongly coupled. Incorrect data association can force unrelated object trajectories together.

REJECT DYNAMIC

Treat dynamic pixels as outliers

Suitable when camera localization is the priority and static background is sufficient; rejected objects do not receive usable trajectories.

STATIC OBJECT MAP

Objects as persistent landmarks

Instance recognition and 6D pose provide strong semantic constraints, while repeated instances, symmetry, and model errors require handling.

DYNAMIC OBJECT SLAM

Joint camera and object motion

Requires per-object trajectories, motion priors, occlusion lifecycles, and probabilistic association, greatly enlarging the state.

3D VISION

Multiview geometry provides pose, depth, reconstruction, and cross-frame correspondences.

VISUAL SLAM

Organizes local geometry into camera trajectories and reusable maps.

ROBOTICS

Connects camera poses to base, hand–eye, and tool frames.

GRASPING

Continuously updates object poses, velocities, and uncertainty for predictive grasping and closed-loop manipulation.

Evaluation & Debugging

Separate association, degeneracy, calibration, and graph optimization before judging one score

ATE, RPE, and reprojection error answer different questions. Sim(3) alignment absorbs monocular scale error; applying it indiscriminately to stereo / RGB-D hides metric-scale errors those systems should resolve. Report alignment type, coordinate frames, timestamp association, and tracking success with every trajectory result.

ATE

Global trajectory error

Measures overall positional consistency; specify SE(3) or Sim(3) alignment.

RPE

Relative error at fixed time / distance intervals

Reflects local drift more directly, but does not replace global map quality after loop closure.

TRACKING

Success rate and recovery

Report lost frames, relocalization time, and initialization success, rather than successful segments alone.

MAP / OBJECT

Reconstruction, object pose, and identity

Also evaluate coverage, surface error, symmetry-aware pose error, and association metrics.

Failure modes	Failure mechanism	Response / validation
Pure rotation / zero baseline	E degenerates to zero; translation direction and depth cannot be recovered	Use a rotation homography; wait for translation before creating points
Single or approximately planar scene	Both H and E may have low residuals, leaving initialization branches ambiguous	Compare both models, retain candidates, and verify with nonplanar points and new views
Low parallax / distant points	Small epipolar residuals but large triangulation variance	Delay point creation, use inverse depth, increase keyframe baseline
Incorrect K / distortion state	Systematically curved epipolar traces or persistent relative-pose bias	Check calibration on static multiview data; keep coordinates consistent with K/D
Rolling shutter / time misalignment	No single rigid camera pose describes the whole frame	Use RS / continuous-time models and calibrate camera–IMU/depth timing
Motion blur / low texture	Sparse correspondences, weak photometric gradients, sudden tracking loss	Exposure/motion strategies, additional modalities, quality gating, reliable relocalization
Repeated texture / perceptual aliasing	Mismatches and false loops can also produce large consensus sets	Local geometry + temporal checks + PnP/Sim(3) verification; gate loop closures strictly
Dominant dynamic objects	Foreground motion is interpreted as camera motion, violating the static-world assumption	Semantic/motion segmentation, static-background estimation, or explicit joint object-track optimization
Monocular scale drift	Submaps have inconsistent scales and cannot be joined by SE(3) alone	Use Sim(3) loop closure / map merging, or add metric observations
Incorrect frame / transform direction	Smooth-looking trajectories but reversed object, map, or robot transform chains	Fix the world-from-camera convention and verify every composition and inverse
False loop closure	One strong constraint can distort the entire map	Place retrieval is only a proposal; add geometric verification, switchable constraints, and rollback
Marginalization and outlier contamination	Old errors become embedded in the prior and difficult to recover from	Remove outliers before marginalization, monitor prior consistency, and rebuild the local map if needed

01
Fix frames and the camera model
Log each frame's ^WT_C, K/D, image size, timestamp, and units; verify two-view reprojection with ground-truth points.
02
Inspect correspondences before pose
Plot match tracks, epipolar distances, spatial coverage, and track lifetimes; changing solvers does not fix incorrect identities.
03
Log E, H, and observability separately
Report both models' support, median parallax, cheirality, triangulation angles, and depth variance.
04
Disable loop closure and object states
Stabilize minimal VO / local BA, then add loops, IMU, and object factors in stages to identify which factor type disrupts the graph.
05
Inspect every BA residual family
Break down by frame, landmark, image region, dynamic label, and sensor so means do not hide structured bias.
06
Test recoverable failures
Inject blur, occlusions, false loops, and brief tracking loss to verify quality gating, relocalization, and map rollback.

Classic Texts, Original Papers, and Official APIsExpand multiview geometry, BA, visual SLAM, and Object SLAM sources