Visual Odometry — C++ / ROS2

A monocular Visual Odometry pipeline re-implemented in C++17 with a ROS2 Humble interface, evaluated on the KITTI Odometry dataset (Sequence 00).

This project is a C++ port of a Python VO pipeline, demonstrating the same algorithm at systems-programming depth and adding a robotics middleware interface suitable for real-time deployment.

---

Pipeline

/camera/image_raw  (sensor_msgs/Image)
        │
        ▼
  FeatureExtractor     GFTT detector  +  ORB descriptor
        │
        ▼
  FeatureMatcher       BFMatcher  +  Lowe's ratio test (0.75)
        │
        ▼
  PoseEstimator        Essential Matrix (RANSAC)  →  recoverPose
        │              inverted to world frame: R_rel = Rᵀ, t_rel = −Rᵀt
        ▼
  Trajectory           T_global = T_global × T_rel   (4×4 Eigen)
        │
        ▼
/vo/odometry       (nav_msgs/Odometry)

---

Project structure

visual_odometry_ros2/
├── .devcontainer/
│   ├── devcontainer.json      # VSCode Dev Container config
│   └── Dockerfile             # Ubuntu 22.04 + ROS2 Humble + OpenCV + Eigen
├── ros2_ws/
│   └── src/
│       └── vo_cpp/
│           ├── src/
│           │   ├── feature_extraction.cpp   # GFTT + ORB (header-only module)
│           │   ├── matching.cpp             # BFMatcher + ratio test
│           │   ├── pose_estimation.cpp      # Essential matrix + recoverPose
│           │   └── vo_node.cpp              # ROS2 node (entry point)
│           ├── CMakeLists.txt
│           └── package.xml
├── kitti_publisher.py          # Publish KITTI images as ROS2 topic
├── trajectory_viz.py           # Subscribe to /vo/odometry → save plot
└── results/
    └── trajectory.png          # Output trajectory plot

---

Quick start

1 — Prerequisites

• Docker Desktop
• VSCode + Dev Containers extension

2 — Open in container

git clone https://github.com/<your-username>/visual-odometry-ros2.git
cd visual-odometry-ros2
code .
# VSCode prompt → "Reopen in Container"

3 — Build

cd /workspace/ros2_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select vo_cpp
source install/setup.bash

4 — Run (three terminals)

Terminal 1 — VO node

source /opt/ros/humble/setup.bash && source /workspace/ros2_ws/install/setup.bash
ros2 run vo_cpp vo_node

Terminal 2 — Publish KITTI images

source /opt/ros/humble/setup.bash
python3 /workspace/kitti_publisher.py \
    --image_dir /data/kitti/dataset/sequences/00/image_0 \
    --fps 10

Terminal 3 — Visualise trajectory

source /opt/ros/humble/setup.bash
python3 /workspace/trajectory_viz.py
# saves results/trajectory.png when the stream ends

---

Results — KITTI Sequence 00

The estimated trajectory (unit scale, no GT-based scale correction in the ROS2 version) traces the correct shape of the ~3.7 km loop with good directional consistency.

---

Key implementation notes

Topic	Detail
Pose convention fix	cv::recoverPose returns world-to-camera; inverted to camera-to-world via R_rel = Rᵀ, t_rel = −Rᵀt
Degenerate frame rejection	Skipped if matches < 30 or RANSAC inliers < 20
Scale	Unit-norm (‖t‖ = 1). For metric scale: stereo baseline, IMU, or GT inter-frame distance
ROS2 output	nav_msgs/Odometry on /vo/odometry at camera frame rate

---

Dependencies

Library	Version	Purpose
ROS2 Humble	—	Middleware, message types
OpenCV	4.x	Feature detection, matching, geometry
Eigen3	3.4	Matrix math, quaternion conversion
cv_bridge	Humble	ROS Image ↔ cv::Mat

---

Relation to Python version

This repo is a C++ re-implementation of visual-odometry-project, which uses the same GFTT+ORB+Essential Matrix algorithm in Python/OpenCV. The C++ version adds a ROS2 interface and demonstrates the same algorithm at systems-programming depth, closer to real-time deployment requirements.

---

License

MIT