10. SLAM & 3D Reconstruction

KinectFusion, TSDF, ICP pose tracking, loop closure, back-projection, Marching Cubes

1. 3D Data Capturing Challenges

A single sensor frame captures only partial scene data — one viewpoint always leaves occlusions (objects behind other objects are invisible). Therefore the sensor must be carried around the scene — by hand, robot, or UAV — to capture the complete 3D structure.

A 3D scene must be observed from many viewpoints to obtain a complete reconstruction.

The Registration Problem

Each captured frame lives in its own local coordinate system. Registration transforms all frames into one common coordinate system (typically the coordinate system of the first frame). To do this, we need:

1. The sensor pose (position + orientation) at every moment, described by the extrinsic matrix.
2. Accurate timestamps for each sample.

Registration aligns frames from different sensor poses into a shared coordinate system.

After registration, multiple scans combine into a single consistent 3D model.

2. SLAM: Simultaneous Localization and Mapping

SLAM originated in robotics in the 1960s, primarily for sensor localization with 2D floor maps. When adapted to 3D reconstruction, both localization AND mapping accuracy become critical — and typically there is no wheel odometry and no GPS data.

Classic SLAM in robotics: the robot simultaneously builds a 2D floor map (black) while estimating its own trajectory (red).

When SLAM is applied to 3D reconstruction, the result is a detailed 3D model of the scanned environment.

The SLAM Paradox (Chicken-and-Egg Problem)

To build a map we must know our position; to determine our position we need a map. Solution: alternate and jointly optimize between the two steps. Start with a rough estimate from the first frame, then iteratively refine both map and pose.

3. SLAM Types and Landmark Requirements

Feature matching across views: landmarks are detected (green circles) and matched between frames (colored lines). These correspondences drive SLAM localization.

Requirements for SLAM Landmarks

1. Easily re-observable — can be detected again when revisiting the same area.
2. Distinguishable — individual landmarks must be discriminative.
3. Sufficient quantity — hundreds or thousands per image.
4. Stationary — landmarks must not move (dynamic objects are problematic).
5. Low computation cost — real-time performance required.
6. Generality — work across various image types and conditions.

4. The Generic SLAM Pipeline

The SLAM processing loop consists of:

1. Initialize coordinate system at the first scan.
2. Store first sample data into the 3D model.
3. Detect and create landmarks visible in the first scan.
4. Main loop:
   1. Move the sensor; obtain the next scan.
   2. Re-observe landmarks: find already-known landmarks in the new scan.
   3. Compute change in position/orientation from landmark position changes.
   4. Estimate current sensor pose.
   5. Update the 3D model with current sample data.
   6. Check for loop closure.
   7. If loop closure detected: correct poses and model.
   8. Repeat from (a) or finish.

5. Drift and Loop Closure

The Drift Problem

Pose estimation error at each frame is unavoidable due to image noise/blur, insufficient or occluded landmarks, and less than 100% accuracy in finding pairwise correspondences. This error accumulates over time, causing drift: the estimated path diverges from the actual path, and newly captured data integrates into the model at incorrect locations.

Drift in action: the estimated camera path (red) progressively deviates from the actual path (blue dashed).

The resulting 3D model becomes distorted when drift is not corrected — surfaces no longer align properly.

Loop Closure

Loop closure detects when the sensor revisits a previously seen scene (frame $Z$) and corrects accumulated drift.

1. Correct the current sensor pose using the known pose of frame $Z$.
2. Elastic trajectory correction: smoothly correct all intermediate poses from the current frame back to frame $Z$.
3. Re-update the 3D model with correctly positioned 3D data from the corrected poses.

A pose graph with loop closure constraints: when the sensor revisits a known location, connections (red) link the current pose back to the earlier pose, enabling correction.

Before (left) vs. after (right) loop closure correction — the model becomes geometrically consistent.

Elastic correction: when a loop closure is detected, previous poses are smoothly adjusted along the trajectory.

Bundle Adjustment

Bundle adjustment jointly optimizes both the 3D map points and the camera poses, making them mutually consistent with observed sensor data. It minimizes the overall reprojection error across all frames and landmarks simultaneously.

Loop closure in practice: trajectory planning

Scanning trajectories must be carefully planned so that the sensor revisits previously seen scenes to enable loop closure detection. The Enigma project example demonstrates a planned scanning path through a building, ensuring the sensor revisits corridors for reliable loop closure.

Enigma project: the scanning trajectory (green) was carefully planned to ensure corridor revisits for loop closure detection.

6. KinectFusion Overview

KinectFusion is an RGB-D SLAM system for 3D reconstruction using a Microsoft Kinect or Asus xTion depth sensor. It has two interleaved components:

KinectFusion Pipeline Steps

1. At the first Kinect frame, create a coordinate system (world origin = sensor origin of first frame).
2. Initialize a voxel grid (e.g., $3 \times 3 \times 3$ m, $256^3$ voxels, voxel size $\approx 1.2$ cm).
3. Back-project depth data into the voxel grid using TSDF representation.
4. Detect and store landmarks (vertices and normals from TSDF).
5. For each new frame: use ICP to align new depth data to the existing model.
6. Update the TSDF voxel grid with new depth data.
7. Repeat.

Depth Sensor Specifications (Kinect / xTion)

KinectFusion uses a structured light depth sensor with an IR pattern emitter, an IR sensor, and an RGB sensor. An internal ASIC generates depth data from IR sensor correlation against a known speckled light pattern, calibrated for 2048 depth planes.

7. TSDF Voxel Grid Representation

The Truncated Signed Distance Function (TSDF) encodes 3D geometry as a scalar field over a voxel grid. For each voxel at position $g$:

Values transition from +1 (outside/in front of surface) through 0 (on the surface) to -1 (inside/behind surface). The function is truncated (clamped to $[-1, +1]$) within a narrow band $\delta$ around the surface. The zero-crossing (sign change from positive to negative) indicates the surface location.

TSDF function definition: values range from +1 (outside) through 0 (on surface) to -1 (inside).

TSDF along a viewing ray: the signed distance transitions from +1 to -1 across the surface, truncated within the band delta.

Voxel Grid Initialization

A voxel grid discretizes 3D space into a regular array of small cubic cells. Each voxel stores a TSDF value.

• At the first frame, initialize a cube of voxels in front of the sensor.
• Example: $3 \times 3 \times 3$ m volume, $256^3$ voxels, each $\approx 1.2$ cm.
• All voxel values initialized to $-1$.

8. Back-Projection: Depth Pixels to 3D Points

Each valid depth value $D(\mathbf{u})$ at pixel $\mathbf{u} = (x, y)$ provides a 3D point $\mathbf{v}$ in camera coordinates:

where $d$ = depth value at pixel $(x, y)$, $K$ = intrinsic matrix of the IR camera, $(x, y)$ = 2D pixel coordinates, $(X, Y, Z)$ = resulting 3D vertex in camera coordinates.

Perspective projection: 3D points project through the optical center onto the image plane. Back-projection reverses this process.

The back-projection equation: a 2D depth pixel is transformed into a 3D vertex using the inverse intrinsic matrix scaled by depth.

The Intrinsic Matrix $K$

General Case: Camera Not at Origin

When the camera coordinate system is not aligned with the world coordinate system:

where $T_{g,k}$ is the extrinsic matrix (pose) of the camera at time $k$ relative to the global coordinate system $g$.

General case: when the camera has moved, the extrinsic matrix inverse is included to transform depth pixels into world coordinates.

9. ICP Pose Tracking (Iterative Closest Point)

When the sensor moves to a new position, we need to estimate its new pose $T_{w,k}$. ICP aligns the vertices of the new depth frame with the vertices of the previous frame (or the global 3D model).

Pose tracking problem: given the known previous pose $T_{w,k-1}$ and a new depth frame, estimate the current pose $T_{w,k}$ by aligning current vertices to the existing 3D model.

Key Observations

• Every pixel of a depth frame is a 3D vertex in its own coordinate system.
• At 30 fps, there is only small motion between consecutive frames (33 ms apart).
• Small motion implies small distances between corresponding vertices of neighboring frames.
• This makes point-based alignment feasible: closest points correspond.

ICP in action: the white mesh is the target point cloud, the red mesh is the source after one ICP iteration. The algorithm iteratively minimizes the distance between corresponding points.

Camera Pose: The Extrinsic Matrix

where $R_{g,k} \in \mathbb{R}^{3 \times 3}$ is the rotation matrix and $\mathbf{t}_{g,k} \in \mathbb{R}^3$ is the translation vector at time $k$, both relative to the global frame $g$. A point $\mathbf{p}_k$ in camera coordinates transforms to world coordinates as $\mathbf{p}_g = T_{g,k} \cdot \mathbf{p}_k$.

Landmarks in KinectFusion

The TSDF voxel grid defines landmarks as:

1. Vertices: Grid cells with TSDF $\approx 0$ (on the surface).
2. Iso-surfaces: Continuous surface between positive and negative TSDF values.
3. Normals: Surface normal vectors computed via cross product of neighboring surface points: $$\mathbf{n} = (\mathbf{v}_1 - \mathbf{v}_0) \times (\mathbf{v}_2 - \mathbf{v}_0)$$

Surface normals (blue arrows) computed at each vertex of the surface mesh. These normals serve as landmarks for ICP alignment.

After pose estimation, the normal map (colors encode surface orientation) confirms correct alignment of the new frame with the existing model.

10. Computing the Pose Transform (R, t)

Given current-frame vertices $\{\mathbf{p}_i\}$ and previous-frame vertices $\{\mathbf{q}_i\}$, find $R$ and $\mathbf{t}$ that minimizes:

Centroid-Based Approach (3 point pairs)

Step 1: Compute centroids

Theorem: If $(R, \mathbf{t})$ is optimal, then $\{\mathbf{p}_i\}$ and $\{R\mathbf{q}_i + \mathbf{t}\}$ share the same centroid.

Step 2: Center the point sets

After centering, $\mathbf{t}$ vanishes from the equations, allowing us to solve for $R$ independently.

Step 3: Solve for $R$ — using 3 point pairs: $\mathbf{p}'_i = R \cdot \mathbf{q}'_i$ gives $3 \times 3 = 9$ scalar equations for the 9 entries of $R$.

Step 4: Recover $\mathbf{t}$

11. RANSAC within ICP

RANSAC (Random Sample Consensus) is used within ICP to find the best transformation while being robust to outliers (incorrectly matched point pairs).

1. Find corresponding points by Euclidean distance between current frame and model vertices.
2. Select 4 corresponding point pairs for the current RANSAC iteration.
3. Find rotation and translation $[R|\mathbf{t}]$ for these 4 samples.
4. Apply the found $[R|\mathbf{t}]$ to all point pairs.
5. Compute the total alignment error: $$E = \sum_{i=1}^{N} \|R \cdot \mathbf{q}_i + \mathbf{t} - \mathbf{p}_i\|^2$$
6. If error exceeds the threshold: select another 4 pairs, run another iteration.
7. Re-iterate until the error converges below the threshold. Best $[R|\mathbf{t}]$ (lowest error across all iterations) is the final result.

12. Updating the TSDF Model

After finding the camera pose $T_{g,k}$ via ICP, the new depth frame is integrated into the TSDF voxel grid using a weighted average:

Weighted averaging achieves noise reduction: each new observation refines the surface estimate. It allows incremental integration without storing all individual frames, and more observations of a voxel produce a more accurate surface location.

13. Rendering via Ray-Casting

To render a view of the 3D model from any virtual camera position, ray-casting traverses the TSDF voxel grid:

1. Define a virtual camera position and orientation.
2. For each pixel $(u, v)$ of the output image:
3. Cast a ray from the optical center through pixel $(u, v)$ towards the voxel grid.
4. Traverse voxels along the ray, reading their TSDF values.
5. Detect the first sign change in the TSDF (positive to negative) — this is the surface.
6. Record the depth $d$ (camera-to-surface distance) for that pixel.
7. After all pixels are processed, the depth values form the rendered depth image.

A 2D slice of the TSDF grid: the red line traces the zero-crossing (surface). The camera (right) casts rays to find surface intersections.

A 3D TSDF voxel grid containing a reconstructed scene. The surface is encoded as the zero-level set within the voxel volume.

14. Marching Cubes

Marching Cubes is an algorithm that extracts a triangular mesh (iso-surface) from a scalar field like TSDF.

2D Concept (Marching Squares)

In 2D, each grid cell has corners that are either inside (negative TSDF) or outside (positive TSDF) the surface. The surface boundary passes between positive and negative grid points. The algorithm "marches" through each cell and determines the surface configuration based on the sign pattern of the corners.

Marching Squares in 2D: blue points are inside the surface (negative TSDF), red points are outside (positive TSDF). The surface boundary passes between them at the zero-crossing.

3D Marching Cubes

Each cube has 8 corners, each either inside or outside the surface. This gives $2^8 = 256$ possible configurations, which reduce to 15 unique cases by symmetry.

For each cube in the voxel grid:

1. Classify each of the 8 corners as inside ($-$) or outside ($+$) based on TSDF sign.
2. Look up the corresponding triangle configuration from a precomputed table.
3. Interpolate exact vertex positions along edges where the sign changes.
4. Output the triangles for that cube.

A single marching cube with 8 corner vertices (v1–v8) and 12 edge midpoints (e1–e12). Triangle vertices are interpolated along edges where the sign changes.

The 15 canonical Marching Cubes configurations. Black dots indicate corners inside the surface. Each configuration determines a specific set of triangles that approximate the iso-surface within that cube.

The Stanford bunny: a classic example of a triangle mesh extracted from a scalar field using the Marching Cubes algorithm.

Flashcards