Visual-Inertial Odometry from Scratch (C++)

A from-scratch implementation of visual-inertial odometry (VIO): monocular visual odometry fused with IMU data via a 15-dimensional Error-State Kalman Filter (ESKF), built to learn and demonstrate core SLAM concepts for autonomous vehicles / robotics roles — implemented by hand rather than relying on an existing SLAM library (ORB-SLAM3, VINS-Mono, etc.), which are used only as reference benchmarks.

What's implemented

• Data pipeline: EuRoC MAV dataset (MH_01_easy) parsers for IMU, stereo camera frames, and ground truth; timestamp synchronization between IMU (~200Hz) and camera (~20Hz) streams.
• Monocular visual odometry: ORB feature detection, kNN matching with Lowe's ratio test, RANSAC + essential matrix estimation, pose recovery (cv::recoverPose).
• 15-state Error-State Kalman Filter: position, velocity, orientation (quaternion), accelerometer bias, gyroscope bias — full nonlinear IMU integration with quaternion kinematics, linearized error-state covariance propagation (Jacobians derived from first principles), and Kalman correction using visual measurements.
• Adaptive measurement noise: visual measurement confidence scaled by recoverPose's inlier count, so the filter automatically trusts weak (short-baseline, low-feature) frames less.
• Chi-square outlier gating: statistically inconsistent corrections are rejected rather than applied.
• Stationary initialization: initial gyro bias and gravity-aligned orientation estimated from the first ~1s of IMU data, instead of starting blind at zero.
• Quantitative evaluation: ATE/RPE computed with evo against ground truth (TUM trajectory format), not just visual inspection.

Results and honest limitations

Metric	VO-only baseline	Pure IMU integration	EKF fused (orientation+position)
Full-sequence drift	ATE RMSE 4.71m	~230,000m (unusable)	ATE RMSE 115.8m

The fused result currently underperforms the simple VO-only baseline over the full 184s sequence. This was diagnosed, not just observed:

1. Root cause: scale ambiguity. Monocular recoverPose only gives translation direction, not real-world distance. Without solving visual-inertial scale initialization (a genuinely hard sub-problem that systems like VINS-Mono dedicate significant engineering to), position measurements use a fixed placeholder scale that doesn't match true motion magnitude frame-to-frame.
2. Orientation-only correction works correctly — gyroscope bias is learned and stays bounded. Accelerometer bias, however, is structurally unobservable from orientation alone: the error-state Jacobian has no coupling between ba and δθ, so no orientation measurement can ever inform it. Verified empirically: ba stayed exactly [0,0,0] for the entire run until position correction was added.
3. Adding position correction created ba observability (via the p→v→ba chain in the state transition Jacobian) but introduced a new failure mode: a vicious cycle where early IMU drift (amplified by gravity misprojection before orientation converges) grows faster than the filter's own covariance estimate, causing the chi-square gate to reject nearly all subsequent corrections as "outliers" — even correct ones — permanently locking the filter into open-loop divergence.
4. This mechanism was confirmed empirically, not just theorized: threshold-loosening (7.8 → 100) barely changed the rejection rate, and stationary initialization reduced early-transient error (~2.8x improvement at frame 500) without fixing the long-horizon divergence — both results are consistent with the diagnosed root cause.

What this demonstrates: correct implementation and understanding of EKF/ESKF mechanics, Kalman gain behavior, observability analysis, and outlier rejection — validated through genuine empirical debugging, not just a working demo. The real fix (proper VI scale initialization, or a keyframe-based sliding-window optimizer like VINS-Mono/OKVIS use instead of a pure filter) is scoped as future work, not attempted here given the project timeline.

Project structure

src/ ├── imu_data.h # IMU CSV parser ├── camera_data.h # Camera frame loader ├── sync_data.h # IMU-camera timestamp synchronization ├── camera_calibration.h # Camera intrinsics/distortion parser ├── imu_calibration.h # IMU noise parameter parser ├── ground_truth.h # Ground truth trajectory parser ├── adaptive_noise.h # Confidence-based measurement noise scaling ├── eskf_state.h # Nominal state + nonlinear IMU propagation ├── eskf.h # Full ESKF: covariance propagation, correction, gating ├── stationary_init.h # Initial bias/orientation estimation ├── tum_format.h # TUM trajectory format writer (for evo) ├── orb_test.cpp # ORB detection + matching test ├── vo_test.cpp # Essential matrix + pose recovery test ├── trajectory_test.cpp # Full-sequence VO-only trajectory (baseline) ├── imu_propagation_test.cpp # Pure IMU dead-reckoning test (drift demo) └── eskf_test.cpp # Full ESKF pipeline over the sequence

Dataset

EuRoC MAV dataset (MH_01_easy). Not included in this repo — download separately, place under data/MH_01_easy/.

Build

Requires OpenCV, Eigen3, CMake (macOS: brew install opencv eigen cmake).

mkdir build && cd build
cmake ..
make

Run

./eskf_test ../data/MH_01_easy/mav0/imu0 ../data/MH_01_easy/mav0/cam0
python3 ../plot_trajectory.py   # visualize against ground truth

Evaluate with evo (pip install evo):

evo_ape tum ground_truth.tum eskf_trajectory.tum -a -p
evo_rpe tum ground_truth.tum eskf_trajectory.tum -a -p

Future work

• Proper visual-inertial scale/gravity initialization (align monocular scale with metric IMU scale)
• Replace pure filtering with a sliding-window optimizer (bundle adjustment over recent keyframes)
• IMU pre-integration (Forster et al.) instead of per-sample integration
• Loop closure for global drift correction
• CUDA-accelerated ORB descriptor matching (scoped but not completed in this timeline)