Visual Odometry Project

This repository implements a monocular Visual Odometry pipeline based on Projective ICP and Gauss-Newton optimization.

The goal of the project is to estimate the camera trajectory and incrementally reconstruct a sparse 3D map using only image measurements and appearance descriptors.
The ground truth trajectory and the world map are used only for evaluation.

---

Repository Structure

The project is organized as follows:

• data/
  Contains the dataset provided for the assignment. It includes camera parameters, measurement files, ground truth trajectory, and the world map.

• outputs/
  Contains the final results produced by the pipeline, such as estimated trajectory, reconstructed map, error files, summary statistics, and plots.

• utils.py
  Contains the geometric functions used by the pipeline.

• data.py
  Contains the parsing and data-loading utilities.

• projectiveICP.py
  Core implementation of Projective ICP and Gauss-Newton optimization.

• VisualOdometry.py
  Contains the high-level pipeline used to run, evaluate, and visualize the results.

• tests.ipynb
  Notebook used to test the implementation, debug intermediate steps, and run the final pipeline.

---

Code Structure

1. Data Handling (data.py)

In this file I implemented the full data parsing pipeline needed to read the dataset provided for the assignment:

• camera parameters
• measurement frames
• ground truth trajectory
• world landmarks

Additionally, I implemented an appearance-based matching function that associates points using nearest-neighbor search in descriptor space.

---

2. Geometry & Utilities (utils.py)

This file contains the core geometric operations that are used across initialization, tracking, and optimization:

• conversion from planar pose (x,y,theta) a 4x4 SE(3) transform (v2T)
• SE(3) update from 6D vector dx = [tx, ty, tz, wx, wy, wz] (v2T_delta)
• Projection pipeline: world -> camera -> image
• Triangulation of 3D points from two views
• Utility functions such as the skew-symmetric matrix

---

3. Projective ICP (projectiveICP.py)

This file contains the core of the project, the class ProjectiveICP maintains:

• the estimated trajectory
• the current camera pose
• the reconstructed 3D map
• logs for debugging and evaluation

The pipeline is divided into two main stages:

Initialization

The system is initialized using the first two frames:

• Points are matched using appearance
• The Essential Matrix is estimated with RANSAC
• From the computed Essential Matrix relative pose is recovered (up to scale)
• In this way is possible to find an initial set of 3D points using triangulation

Summarizing this step defines:

• the initial camera poses
• the initial 3D map

Tracking

After the initialization for each new frame, I perform two main operations:

1. Pose Estimation

• The current frame is matched against the existing map
• The camera pose is optimized using Gauss-Newton. The error is defined as pixel reprojection error, I derived and implemented the respective Jacobian and a damped Gauss-Newton method is used for stability. Invalid points (behind camera or outside image) are discarded.

2. Map Expansion

• New correspondences between consecutive frames are found
• New 3D points are triangulated
• The map is incrementally updated

Outputs

The class maintains:

• Estimated trajectory (list of SE(3) poses)
• Incremental 3D map
• Detailed logs

---

4. Visual Odometry Pipeline (VisualOdometry.py)

The goal of this file is to wrap everything into a complete high level pipeline.

• Initialization + frame-by-frame tracking

Evaluation

I implemented a full evaluation pipeline that compares the estimated results with ground truth:

• Alignment to the dataset reference frame
• Rotation error computation
• Translation error computation
• Scale estimation
• Map RMSE after matching estimated points to ground truth

Visualization

• 2D trajectory comparison
• 3D map + trajectory
• Map growth
• Gauss-Newton convergence
• Tracking diagnostics

---

5. Notebook (tests.ipynb)

This notebook was used during development to:

• Validate projection and triangulation
• Debug matching and geometry
• Visualize intermediate results
• Run controlled experiments before integrating into the pipeline

It also serves as a quick way to test the system without running the full pipeline.

---

How to Run

1. Clone this repository
2. Install dependencies: pip install numpy matplotlib opencv-python
3. either run test.ipynb or the pipeline

from VisualOdometry import VisualOdometry

vo = VisualOdometry() vo.run() vo.evaluate()

Results

Runtime Performance

• Processed frames: 121
• Mean time per frame: 0.0327 s
• Total time: 3.90 s

Trajectory Estimation

• Estimated poses: 121

Rotation error:

• Mean: 0.074 rad
• Max: 0.309 rad

Translation error:

• Mean: 0.133
• Max: 0.353

Scale Estimation

• Scale ratio: 0.200
• Std deviation: 0.019

Map Reconstruction

• Estimated map points: 490
• Matched map points: 490
• Map RMSE: 0.388

Conclusions

• The pipeline successfully performs monocular visual odometry with incremental mapping.
• Rotation estimation is relatively stable, while:
  • translation suffers from scale ambiguity (expected due to monocular setup)
• The Gauss-Newton optimization converges reliably:
• stable reprojection error
• good number of inliers per frame
• The map grows consistently over time without major degeneracies.

Notes

The results are not fully optimal. Improvements in:

• initialization
• feature matching
• scaling method

would likely lead to better accuracy.