Stereo Vision, 3D Imaging, and Sensor Fusion

Introduction

A conventional camera records a two-dimensional projection of a three-dimensional scene. Machine vision systems used in robotics, inspection, autonomous vehicles, and metrology often require more: the distance to an object, its surface shape, its pose, or a geometrically consistent map of the environment. Stereo vision, active 3D imaging, LiDAR, and sensor fusion provide these capabilities.

This chapter develops the geometric and computational foundations of 3D machine vision. It begins with epipolar geometry and stereo correspondence, then examines triangulation, structured light, time-of-flight cameras, LiDAR, point clouds, registration, and fusion with encoders and inertial sensors. The emphasis is on assumptions, error sources, calibration, and engineering decisions rather than on algorithms in isolation.

Learning Objectives

By the end of this chapter, a student should be able to:

1. Explain camera poses, epipolar geometry, and the essential and fundamental matrices.
2. Relate disparity to depth in a calibrated stereo camera.
3. Compare passive stereo, structured light, time-of-flight, and LiDAR.
4. Describe point-cloud generation, filtering, segmentation, and registration.
5. Formulate sensor fusion using coordinate transformations and uncertainty models.
6. Select an appropriate 3D sensing architecture for a mechatronic application.
7. Identify calibration, correspondence, synchronization, and safety limitations.

1. From Image Measurements to 3D Geometry

A pinhole camera maps a scene point to an image point. In camera coordinates, let a point be P = (X,Y,Z)^T, where Z > 0 is its distance along the optical axis. With focal length f and principal point (c_x,c_y), the ideal projection is

Here (u,v) are pixel coordinates, and f_x,f_y are focal lengths expressed in pixels. The equation assumes a pinhole model and omits lens distortion. Real cameras require radial and tangential distortion correction.

A single image does not generally determine X,Y,Z, because many spatial points lie on the same viewing ray. Depth becomes observable when an additional constraint is available: a second viewpoint, a projected pattern, a measured light-travel time, or a known geometric model.

A rigid transformation between coordinate frames is written

where R_AB is a 3 × 3 rotation matrix and t_AB is a translation vector. Both frames must be explicitly defined. Confusing the direction of a transform is a common source of incorrect 3D results.

2. Epipolar Geometry

Consider two cameras observing the same point. The two camera centers and the point define an epipolar plane. Its intersections with the image planes are epipolar lines. The corresponding image point in the second camera must lie on the epipolar line associated with the first image point.

Important terms are:

• Baseline: the vector between camera centers; its magnitude is B.
• Epipole: the image of one camera center in the other camera.
• Epipolar plane: the plane containing both camera centers and a scene point.
• Epipolar line: the intersection of an epipolar plane with an image plane.
• Correspondence: two image measurements generated by the same physical scene point.

If normalized image coordinates are represented by homogeneous vectors x₁ and x₂, the epipolar constraint is

where E is the essential matrix. If the relative pose of camera 2 with respect to camera 1 is (R,t), then

where [t]_× is the skew-symmetric matrix representing the cross product with t. The essential matrix applies to calibrated cameras and normalized coordinates.

For pixel coordinates, the corresponding relation is

where F is the fundamental matrix. If camera calibration matrices are K₁ and K₂, then, under consistent coordinate conventions,

The fundamental matrix applies to unnormalized pixel coordinates and incorporates camera intrinsics. In practice, it is estimated from matched points using methods such as the normalized eight-point algorithm with robust outlier rejection. The estimated matrix should be checked using reprojection or epipolar-line errors, not merely accepted because a numerical solver returned a result.

Rectification

Stereo rectification applies image transformations so that corresponding epipolar lines become horizontal, or nearly so. Correspondence then becomes a one-dimensional search along a row. Rectification does not improve the physical accuracy of the cameras; it simplifies matching and reduces computational cost. Residual vertical disparity indicates calibration error, lens-model error, rolling-shutter effects, or synchronization problems.

3. Stereo Correspondence and Depth

Stereo correspondence seeks the image location in the second camera that represents the same scene point as a selected pixel in the first camera. A useful matching cost may combine intensity differences, gradients, census signatures, or learned features. Local methods are computationally efficient but struggle with textureless regions, repeated patterns, and occlusions. Global and semi-global methods impose smoothness assumptions, which improve weak-texture regions but may blur depth discontinuities.

For a rectified, parallel stereo pair with equal focal length f and baseline B, disparity is

and depth is

Here d is measured in pixels, f in pixels, and B and Z in the same physical length unit. The formula assumes rectification, parallel optical axes, a pinhole model, and a correct correspondence. Positive disparity convention depends on the camera ordering; the magnitude is normally used for depth.

The reconstructed coordinates can be obtained from the left image by

These equations show why depth precision decreases with distance. Differentiating Z=fB/d gives the approximate relationship

for small disparity error ∆d. Thus a one-pixel error has a much larger effect at long range. Increasing focal length or baseline improves depth sensitivity, but a large baseline increases occlusion and reduces the common field of view.

Worked Example: Stereo Depth

A rectified stereo camera has f=800 pixels and baseline B=0.12 m. A feature has disparity d=48 pixels. Its depth is

If the disparity uncertainty is approximately 0.5 pixel, the first-order depth uncertainty is

Thus the estimated depth is approximately 2.00 m with a local uncertainty of 21 mm, excluding calibration and model errors. At Z=4 m, the same disparity uncertainty would produce about 83 mm uncertainty, illustrating the quadratic dependence on range.

Correspondence Failure Modes

Correspondence is unreliable when surfaces are textureless, reflective, transparent, periodically patterned, or occluded. A pixel visible in one camera may not be visible in the other. Practical systems use left-right consistency checks, uniqueness tests, confidence scores, invalid masks, and edge-aware filtering. Depth discontinuities should not be smoothed across object boundaries without evidence.

4. Active 3D Imaging

Structured Light

A structured-light system projects a known pattern—such as stripes, dots, or coded sequences—onto a scene and observes its deformation with a camera. The projected pattern supplies texture and enables triangulation. For a camera-projector pair, the projector can be modeled as an inverse camera, although projector calibration is more demanding because the projector does not directly measure image coordinates.

Phase-shifting methods project several sinusoidal patterns and estimate phase. Gray-code or binary patterns provide discrete coding. Structured light generally provides high spatial accuracy at short and medium range, making it valuable in industrial inspection and robot bin picking. Limitations include sensitivity to ambient light, object motion during a pattern sequence, shiny or translucent surfaces, and a restricted operating range.

Time of Flight

A time-of-flight sensor estimates range from the travel time of emitted light. For direct pulsed ToF, the round-trip relationship is

where R is range, c is the speed of light, and ∆t is measured round-trip time. The factor 2 arises because the light travels to the object and back. Continuous-wave or indirect ToF systems estimate phase delay rather than directly resolving a short pulse. They can acquire depth at video rates, but multipath reflections, ambient illumination, phase wrapping, and per-pixel bias affect accuracy.

The unambiguous range of a phase-based system depends on modulation frequency and phase interpretation. A system may therefore require multiple modulation frequencies or additional processing to resolve range ambiguities.

LiDAR

LiDAR measures range and angle, often by scanning a laser beam or using a solid-state optical arrangement. Each return can be converted to a 3D point using the sensor’s angular calibration and pose. LiDAR is effective over long distances and outdoors, and it can produce geometrically precise point clouds. Important limitations include sparse sampling, occlusion, incidence-angle dependence, atmospheric effects, reflective-surface behavior, cost, and eye-safety requirements.

5. Point Clouds and 3D Reconstruction

A point cloud is a set of 3D points, optionally carrying colour, intensity, normals, confidence, or timestamp. Stereo produces points by back-projecting valid pixels. LiDAR produces points directly from range and angle. A depth image can be converted into a point cloud using the camera model:

where K is the intrinsic calibration matrix and Z_i is the depth associated with pixel (u_i,v_i). This expression assumes the depth is expressed along the camera optical axis, as in the pinhole model. Some sensors report range along the viewing ray instead; that convention must be verified before conversion.

Typical processing stages are:

1. Remove invalid points and sensor-specific outliers.
2. Apply statistical or radius-based outlier filtering.
3. Downsample using a voxel grid when computational reduction is needed.
4. Estimate normals when surface orientation is required.
5. Segment planes, objects, or clusters.
6. Register multiple views.
7. Fuse measurements into a surface or occupancy representation.

A point cloud is not automatically a surface model. Surface reconstruction may use meshing, signed distance fields, occupancy grids, or volumetric fusion. Each representation makes different assumptions about visibility, resolution, and topology.

6. Registration and Coordinate Frames

Registration estimates the rigid or nonrigid transformation that aligns two datasets. For rigid registration, the objective may be written

subject to R^T R=I and det(R)=1. Here p_i and q_i are corresponding points in two frames. The least-squares rigid solution can be found from a singular value decomposition when correspondences are known.

The Iterative Closest Point (ICP) algorithm alternates between finding correspondences and estimating the transformation. Point-to-point ICP minimizes distances between points; point-to-plane ICP minimizes the projection of the error onto a target surface normal and can converge faster on smooth surfaces. ICP is local: it requires a sufficiently good initial alignment and can converge to an incorrect local minimum.

Feature-based registration uses descriptors and robust estimation to obtain an initial alignment. Global registration is useful when the initial pose is unknown, but false feature matches must be rejected. Registration quality should be assessed with residuals, geometric consistency, overlap, and independent reference measurements.

A transformation chain in a robot might be written

The order is meaningful because rigid transformations do not generally commute. Calibration must determine the camera intrinsics, camera-to-tool extrinsics, robot kinematics, and any time offset relevant to moving systems.

7. Fusion with Encoders, IMUs, and Other Sensors

Vision alone may be intermittent or ambiguous. Encoders provide precise joint or wheel increments but accumulate drift in global position. An IMU provides high-rate angular velocity and acceleration but suffers from bias and gravity-related observability issues. Combining sensors can produce a more robust estimate of pose and motion.

A state-space model can be written

where x_k is the state, u_k is a control or inertial input, z_k is a measurement, and w_k and v_k represent process and measurement noise. The functions f and h describe the motion and observation models.

The extended Kalman filter (EKF) linearizes these models about the current estimate. With covariance P, process-noise covariance Q, measurement-noise covariance R, and Jacobians F and H, a simplified prediction-update structure is

The Kalman gain K_k weights the innovation according to uncertainty. These equations assume an approximately Gaussian error model and a sufficiently accurate local linearization. Nonlinear systems with large orientation errors or multimodal hypotheses may require an error-state filter, unscented filter, particle filter, or graph-based optimization.

In a wheeled robot, wheel encoders estimate incremental motion, the IMU supplies angular-rate and acceleration information, and cameras or LiDAR provide external geometric constraints. A factor-graph approach represents poses as variables and sensor observations as factors; optimization then seeks the pose trajectory that best satisfies all measurements. Loop closures in visual or LiDAR mapping can correct accumulated drift, but false loop closures can severely corrupt a map and therefore require geometric verification.

Synchronization and Observability

Fusion is valid only when measurements refer to compatible times and frames. Hardware triggering or accurately estimated timestamps is preferable to assuming that messages arriving together were measured together. A time offset during rapid motion appears as a spatial error.

Observability asks whether the available measurements can determine the desired state. For example, an IMU alone cannot generally determine global position without external references, and a planar scene may provide weak constraints for certain camera motions. Sensor diversity is useful only when the measurements add independent information.

8. Illustrative Case: Vision-Guided Robotic Bin Picking

Background

Consider a six-axis robot that must pick randomly oriented metal parts from a bin. A fixed overhead depth camera observes the bin, while joint encoders report robot configuration. A wrist-mounted camera is available for final verification.

Problem

The system must estimate each part’s 3D pose despite occlusion, variable illumination, reflective surfaces, and robot motion. The robot must avoid collisions and place the part within a tolerance of ±2 mm.

Analysis

A passive stereo camera may fail on textureless metal surfaces, while a structured-light camera may suffer from specular reflections. A LiDAR sensor may provide insufficient point density for small features. The system therefore uses a calibrated active depth camera for coarse geometry and a wrist camera for close-range confirmation.

Approach and Solution

The overhead sensor is calibrated to the robot base using a hand-eye or target-based calibration procedure. The depth image is converted to the camera-frame point cloud, filtered, and transformed into the robot base frame. A plane model identifies the bin floor; points above the plane are clustered into candidate parts. A geometric template or trained pose estimator generates candidate six-degree-of-freedom poses.

Joint encoder measurements are used to compute the robot tool pose through forward kinematics. Collision checking uses the estimated part pose, robot geometry, and a safety margin. Before grasping, the wrist camera verifies the candidate pose. If confidence is low or the two sensors disagree beyond a threshold, the robot pauses and acquires another view.

Outcome

The combined architecture separates fast coarse localization from slower, higher-confidence verification. It reduces dependence on any single depth technology and allows encoder-based prediction during short visual interruptions. Performance must still be evaluated using grasp success rate, pose error, cycle time, false detections, and unsafe-stop frequency.

Learning Points

The case illustrates that sensor fusion is not simply the averaging of measurements. Each sensor contributes a different observability pattern, error distribution, update rate, and failure mode. Calibration, timing, confidence estimation, and recovery behavior are as important as the nominal recognition algorithm.

9. Challenges, Safety, and Best Practices

Calibration and Uncertainty

Calibration errors produce systematic 3D errors that filtering cannot remove. Intrinsic calibration should cover the operating focus and temperature range. Extrinsic calibration should be verified at multiple poses and distances. Report uncertainty separately for random noise, bias, and registration error where possible.

Dynamic Scenes

Most reconstruction methods assume static geometry during acquisition. Moving objects, rolling shutters, vibration, and delayed timestamps violate this assumption. Use global-shutter cameras when appropriate, hardware synchronization, motion compensation, or temporally consistent tracking.

Reflective and Transparent Objects

Specular surfaces can saturate cameras and create false ToF returns. Transparent surfaces may show background geometry rather than the object boundary. Sensor selection, controlled illumination, polarization, multi-view acquisition, and mechanical fixturing may be more effective than algorithmic post-processing alone.

Safety and Governance

Classified laser products require compliance with applicable laser-safety standards and risk assessment. A robot must not treat a low-confidence point cloud as a guaranteed obstacle-free environment. Systems used near people should define fail-safe states, diagnostic checks, access controls, and logs of sensor health. Training data for learned depth or pose models should be representative and evaluated for systematic failure across materials, lighting, object orientations, and environments.

Engineering Best Practices

• Define every coordinate frame and transformation direction.
• Calibrate the complete measurement chain, not just individual sensors.
• Synchronize clocks and quantify time offsets.
• Preserve confidence, timestamp, and validity information with every 3D measurement.
• Validate with known distances, planes, artifacts, and independent instruments.
• Use uncertainty-aware thresholds rather than arbitrary filtering values.
• Test failure cases deliberately, including occlusion, glare, missing data, and motion.
• Monitor sensor health and provide safe degradation when a sensor fails.

10. Contemporary Developments

Modern systems increasingly combine neural correspondence, learned depth completion, differentiable rendering, and geometric optimization. Learned models can infer useful depth in weak-texture regions, but their predictions may be biased outside the training distribution. A robust industrial architecture often combines learned proposals with geometric checks, calibrated sensors, and physical constraints.

Event cameras, high-dynamic-range sensors, solid-state LiDAR, neural radiance representations, and Gaussian-based scene representations are expanding the design space. These technologies do not eliminate the need for coordinate calibration, observability analysis, uncertainty estimation, and safety validation. The central engineering question remains: which measurements provide reliable information for the required task under the actual operating conditions?

Chapter Summary

Stereo vision recovers depth by observing a scene from multiple viewpoints. Epipolar geometry reduces correspondence to constrained searches, while triangulation converts disparity or angular measurements into 3D position. For a rectified parallel stereo pair, depth is proportional to focal length and baseline and inversely proportional to disparity; depth uncertainty increases approximately with the square of range.

Structured light actively projects patterns for accurate short-range reconstruction. Time-of-flight sensors estimate range from light delay or phase, while LiDAR provides range-angle measurements useful for long-range mapping. All systems have characteristic failure modes involving texture, reflectivity, multipath, occlusion, motion, and calibration.

Point clouds require filtering, segmentation, registration, and often surface reconstruction. Rigid registration depends on correct correspondences, good initialization, and consistent coordinate frames. Encoders, IMUs, cameras, and LiDAR can be fused through probabilistic filters or optimization graphs, provided that timing, uncertainty, and observability are addressed. In professional mechatronic systems, calibration, validation, safety, and graceful failure are as important as the reconstruction algorithm itself.

Key Terms

Epipolar geometry: Geometry describing the relationship between two camera views of the same 3D point.

Baseline: Distance or vector separating two camera centers.

Epipolar line: Image line on which a corresponding point must lie.

Essential matrix: Matrix encoding relative pose between calibrated cameras.

Fundamental matrix: Matrix encoding epipolar geometry in pixel coordinates.

Disparity: Difference between corresponding image coordinates in a stereo pair.

Triangulation: Estimation of a 3D point from intersecting or closest viewing rays.

Rectification: Image transformation that makes corresponding epipolar lines aligned.

Structured light: Active 3D sensing using a projected, known pattern.

Time of flight: Range measurement based on light-travel time or phase delay.

LiDAR: Active sensing using laser range and angular measurements.

Point cloud: Collection of 3D points with optional attributes.

Registration: Estimation of a transformation aligning spatial datasets.

ICP: Iterative algorithm that aligns point clouds using estimated correspondences.

Extrinsic calibration: Estimation of position and orientation between sensor frames.

Intrinsic calibration: Estimation of camera parameters such as focal length and distortion.

Sensor fusion: Combination of measurements from multiple sensors to estimate a state.

Observability: Ability of measurements to determine the desired system state.

Loop closure: Recognition that a mapping system has returned to a previously observed place.

Review Questions

5 Short Answer Questions

1. Why can a single pinhole-camera image not normally determine the depth of a point?
2. Distinguish the essential matrix from the fundamental matrix.
3. What is the role of rectification in stereo vision?
4. State two failure modes of time-of-flight depth cameras.
5. Why must sensor timestamps be synchronized in a moving robot?

5 Long Answer Questions

1. Derive the rectified stereo relationship Z=fB/d and explain how baseline and focal length affect depth precision.
2. Compare passive stereo, structured light, ToF, and LiDAR for industrial inspection.
3. Explain the main stages in converting a depth image into a registered point cloud.
4. Describe ICP, its objective, initialization requirement, and local-minimum problem.
5. Explain how encoders, IMUs, and vision can be combined in an EKF or factor graph.

3 Analytical / Application Questions

1. A stereo pair has f=600 pixels, B=0.10 m, and measured disparity d=30 pixels. Calculate depth and explain how a disparity error of 1 pixel affects the estimate approximately.
2. Design a 3D sensing system for a robot inspecting shiny parts moving on a conveyor. Justify the sensor, illumination, calibration, synchronization, and validation strategy.
3. A LiDAR map and a camera-derived point cloud disagree near a transparent panel. Identify possible physical and computational causes and propose diagnostic tests.

Multiple Choice Questions with Answers

Question 1

For a rectified parallel stereo pair, which relationship gives depth?

A. Z=d/(fB)

B. Z=fB/d

C. Z=f+d+B

D. Z=B/(fd)

Correct Answer: B. Z=fB/d

Question 2

The epipolar constraint for calibrated cameras is expressed using which matrix?

A. The essential matrix

B. The confusion matrix

C. The covariance matrix only

D. The homography in every scene

Correct Answer: A. The essential matrix

Question 3

Why does depth uncertainty generally increase rapidly with range in stereo vision?

A. Pixel size increases with distance

B. The approximate uncertainty is proportional to Z²

C. The baseline becomes zero

D. Focal length changes during matching

Correct Answer: B. The approximate uncertainty is proportional to Z²

Question 4

What is a principal purpose of stereo rectification?

A. Increase laser power

B. Make corresponding epipolar lines aligned for simpler matching

C. Remove all sensor noise

D. Convert intensity directly into temperature

Correct Answer: B. Make corresponding epipolar lines aligned for simpler matching

Question 5

A direct pulsed time-of-flight sensor uses which ideal range equation?

A. R=c∆t/2

B. R=2c∆t

C. R=∆t/c

D. R=c+∆t

Correct Answer: A. R=c∆t/2

Question 6

Which statement about ICP is correct?

A. It always finds the global optimum

B. It requires no initial alignment

C. It alternates correspondence estimation and transformation estimation

D. It applies only to 2D images

Correct Answer: C. It alternates correspondence estimation and transformation estimation

Question 7

Which sensor commonly provides high-rate angular velocity and acceleration measurements?

A. IMU

B. Projector only

C. Passive lens

D. Calibration target

Correct Answer: A. IMU

Question 8

What is a major cause of incorrect fusion between a camera and robot encoders?

A. Using explicit coordinate frames

B. Accurate timestamps

C. An incorrect camera-to-robot extrinsic transformation

D. Recording measurement confidence

Correct Answer: C. An incorrect camera-to-robot extrinsic transformation

Question 9

For f=800 pixels, B=0.12 m, and d=48 pixels, the stereo depth is:

A. 0.018 m

B. 0.48 m

C. 2.0 m

D. 38.4 m

Correct Answer: C. 2.0 m

Question 10

Which issue is particularly associated with phase-based ToF sensing?

A. Phase wrapping and multipath interference

B. Requirement for textured surfaces in every case

C. Inability to measure any range

D. Mandatory use of two separated cameras

Correct Answer: A. Phase wrapping and multipath interference