CARLA Visual Odometry: RGB-D & Stereo Ego-Motion Estimation

A visual odometry (VO) pipeline for estimating 6-DoF ego-motion in the CARLA autonomous driving simulator. Three implementations are provided — from a simple stereo baseline to a full RGB-D pipeline — evaluated using Absolute Trajectory Error (ATE).

---

📋 Overview

The project estimates vehicle pose frame-by-frame using onboard camera data, without relying on GPS or ground-truth localization. Each Odometry class implements a common interface: sensors() defines what to spawn in CARLA, and get_pose() returns the current 6-DoF pose [x, y, z, roll, pitch, yaw].

Sensor Suite:

Sensor	Config
Left RGB Camera	1280×720, 100° FOV, stereo left
Right RGB Camera	1280×720, 100° FOV, stereo right (0.8m baseline)
Depth Camera	1280×720, 100° FOV, aligned to Left
LiDAR	32-channel, 50m range, 20 Hz (provisioned for future work)

---

🗂️ Project Structure

.
├── odometry.py              # Full RGB-D VO: ORB + depth-based PnP (main implementation)
├── odometry_simple.py       # Simplified stereo VO: ORB + Essential matrix (learning reference)
├── odometry_solution.py     # Stereo VO with depth: reference solution
├── automatic_control.py     # Ego vehicle autopilot for data collection
├── generate_traffic.py      # Spawns NPC traffic in CARLA
├── agents/
│   ├── navigation/          # Path planning & control agents
│   │   ├── basic_agent.py
│   │   ├── behavior_agent.py
│   │   ├── local_planner.py
│   │   ├── global_route_planner.py
│   │   ├── controller.py
│   │   └── sensor_interface.py
│   └── tools/
│       └── misc.py
└── utils/
    ├── ate.py               # Absolute Trajectory Error (ATE) evaluation
    └── transform.py         # SE(3) coordinate frame utilities

---

🔍 Odometry Implementations

1. odometry.py — RGB-D Visual Odometry (Main)

The primary implementation. Uses ORB feature matching between consecutive Left camera frames, lifts 2D keypoints to 3D using the aligned Depth camera, then solves for camera motion with PnP+RANSAC.

Pipeline:

Left RGB (t) + Left RGB (t-1) + Depth (t-1)
    → ORB keypoint detection (5000 features)
    → KNN matching + Lowe's ratio test (threshold: 0.70)
    → Depth backprojection: pixel (u,v,z) → 3D camera coords
    → PnP-RANSAC (ITERATIVE, reprojErr: 2.0px, conf: 0.9999)
    → SE(3) pose update: T_world = T_world × T_relative
    → Output: [x, y, z, roll, pitch, yaw]

Key parameters:

Parameter	Value	Description
ORB features	5000	Keypoints per frame
Ratio test threshold	0.70	Lowe's ratio filter
Min good matches	20	Minimum to attempt PnP
Depth range	0.2 – 80.0 m	Valid depth window
PnP reprojection error	2.0 px	RANSAC inlier threshold
PnP confidence	0.9999	RANSAC confidence

---

2. odometry_simple.py — Simplified Stereo VO (Learning Reference)

A cleaner, more readable implementation for understanding the fundamentals. Uses only the Left RGB camera and the Essential matrix to recover relative rotation and translation (up to scale).

Pipeline:

Left RGB (t) + Left RGB (t-1)
    → ORB detection (2000 features)
    → Brute-force matching (crossCheck=True)
    → Essential matrix estimation (RANSAC)
    → R, t decomposition
    → Incremental pose integration
    → Output: [x, y, z, roll, pitch, yaw]

⚠️ Translation is scale-ambiguous without depth. Best used as a learning baseline, not for metric ATE evaluation.

---

3. odometry_solution.py — Stereo + Depth Reference Solution

A reference implementation using stereo RGB + depth fusion. Uses ORB with 3000 features and a BFMatcher (crossCheck=False) for richer correspondence sets.

---

📐 Odometry Interface

All three implementations share the same interface:

class Odometry:
    def sensors(self) -> list:
        """Return CARLA sensor specs to spawn."""
        ...

    def get_pose(self, sensor_data: dict, prev_sensor_data: dict) -> np.ndarray:
        """
        Estimate current 6-DoF pose.
        
        Args:
            sensor_data:      Current frame. Keys: 'Left', 'Right', 'Depth', 'LIDAR'
                              Each value: (frame_id, numpy_array)
            prev_sensor_data: Previous frame in the same format.
        
        Returns:
            pose: np.ndarray, shape (6,) — [x, y, z, roll, pitch, yaw]
        """
        ...

---

📊 Evaluation — Absolute Trajectory Error (ATE)

Trajectory accuracy is measured using ATE (utils/ate.py), which aligns the estimated and ground-truth trajectories via SE(3) and computes the RMS positional error.

Input format per pose: [x, y, z, qw, qx, qy, qz] (position + quaternion).

python utils/ate.py

---

🚀 Getting Started

Prerequisites

1. CARLA Simulator (0.9.x)

   • Download: https://github.com/carla-simulator/carla/releases

2. Python 3.7+

3. Install dependencies:

   pip install numpy opencv-python scipy pygame

4. Add CARLA Python API to your path:

   export PYTHONPATH=$PYTHONPATH:~/CARLA_0.9.13/PythonAPI/carla/dist/carla-0.9.13-py3.7-linux-x86_64.egg

---

Running

Step 1 — Start CARLA:

cd ~/CARLA_0.9.13
./CarlaUE4.sh        # Linux
# CarlaUE4.exe       # Windows

Step 2 — (Optional) Spawn traffic:

python generate_traffic.py

Step 3 — Run the RGB-D odometry:

python odometry.py

Step 4 — Evaluate trajectory accuracy:

python utils/ate.py

---

🛠️ Troubleshooting

No module named 'carla' Add the CARLA egg to your PYTHONPATH (see Prerequisites).

Connection refused The CARLA server must be fully loaded before running any client script.

Pose drifts quickly

• Ensure the Depth camera is spawned and aligned to the Left camera
• Tighten the ratio test threshold (try 0.65)
• Increase iterationsCount in solvePnPRansac

Trajectories must have same length (ATE error) The estimator returned early on some frames — check that Depth data is valid and within the 0.2–80m range.

---

📚 References

• ORB Features — Rublee et al., ICCV 2011
• PnP-RANSAC — Fischler & Bolles, CACM 1981
• ATE Metric — Sturm et al., IROS 2012
• ATE implementation inspired by cnspy_trajectory_evaluation
• CARLA Simulator — Dosovitskiy et al., CoRL 2017