A from-scratch stereo Visual SLAM pipeline implemented progressively across five milestones, from classical monocular geometry to modern learned features, packaged as a ROS2 deployment in M6. Built as a deep-learning portfolio project on KITTI, with the goal of understanding every component end-to-end rather than relying on black-box libraries.
Status: M0–M6 complete.
Real-time visualization of the M5 pipeline (SuperPoint + LightGlue features, BoW loop detection, GTSAM pose graph optimization) running in ROS2 on KITTI sequence 04. Visualized in RViz2 via three ROS2 nodes (KITTI publisher, VO node, visualization node).
This project was built as a flagship for a 3D Vision / SLAM research direction. The driving question: can I understand a full Visual SLAM stack — from feature extraction to pose graph optimization — by building it piece by piece, with every architectural choice explicitly motivated and benchmarked?
Inspired by the "implement-to-understand" tradition (Karpathy's micrograd, nanoGPT), each milestone follows the same loop: read the relevant paper → implement → benchmark → identify limits → motivate the next milestone.
The project also serves as a perception-stack reference for autonomous ground vehicle and rover work (TEKNOFEST İKA, URC, ERC).
visual-slam/
├── src/ # Core SLAM library (M0–M5)
│ ├── dataset.py # KITTI loader
│ ├── features.py # SuperPoint + LightGlue wrappers
│ ├── feature_models/ # Model loaders
│ ├── geometry.py # Triangulation, PnP, essential matrix
│ ├── mapping.py # MapPoint, Keyframe, Map
│ ├── loop_detection.py # BoW + geometric verification
│ ├── pose_graph.py # GTSAM pose graph optimization
│ ├── vo.py # Main VO pipeline
│ ├── metrics.py # ATE, scale alignment
│ └── visualization.py # Trajectory plots
├── scripts/
│ └── run_vo.py # Entry point
├── ros2_ws/ # M6: ROS2 workspace (separate)
│ └── src/visual_slam_ros/
│ ├── visual_slam_ros/
│ │ ├── kitti_publisher_node.py
│ │ ├── vo_node.py
│ │ └── visualization_node.py
│ ├── launch/
│ │ └── visual_slam.launch.py
│ └── rviz/
│ └── visual_slam.rviz
├── data/dataset/ # KITTI sequences (not tracked)
├── assets/ # README assets (demo gif, final plots)
├── results/ # Run outputs (not tracked)
├── logs/ # Run logs (not tracked)
└── requirements.txt
[Stereo Image Pair]
↓
[SuperPoint Feature Extraction] ←→ [LightGlue Matching]
↓
[Stereo Triangulation] → [3D Map Points]
↓
[Frame-to-Map PnP Tracking] ←→ [Persistent Map]
↓
[Keyframe Selection] → [Map Expansion]
↓
[BoW Loop Detection] → [Geometric Verification]
↓
[Pose Graph Optimization (GTSAM)]
↓
[Optimized Trajectory]
Problem. Before any VO logic, the project needed reliable loading of KITTI sequences: stereo image pairs, calibration matrices, and ground-truth trajectories.
Approach. KittiDataset class parses calib.txt (P2, P3 projection matrices), extracts intrinsic K from P2's left 3×3 block, loads ground-truth poses from poses/XX.txt as an (N, 3, 4) array.
Trade-off. Chose the color dataset (image_2 / image_3) over grayscale (image_0 / image_1). This means P2/P3 must be parsed instead of P0/P1 — a common pitfall when adapting code that assumes grayscale KITTI.
Result. Successfully loads sequence 04 (271 frames) and sequence 00 (4541 frames). Ground-truth trajectory plot matches the published KITTI map.
Problem. Recover camera motion (R, t) from consecutive monocular frames.
Theory. Epipolar geometry: for two views of the same 3D point, x₂ᵀ E x₁ = 0. The essential matrix E encodes rotation and translation direction: E = [t]× R. With ≥5 point correspondences and RANSAC, E can be estimated robustly; cheirality testing picks the geometrically valid (R, t) from the four decomposition candidates.
Approach. ORB features (500 keypoints/frame) → BFMatcher (Hamming, cross-check) → cv2.findEssentialMat with RANSAC → cv2.recoverPose. Trajectory built by chaining 4×4 homogeneous transforms.
Trade-off. Monocular's fundamental limitation: translation is recovered only up to scale (‖t‖ = 1). Without external reference, absolute scale is unknowable. Useful for understanding the geometry, but unusable for real-world metric VO.
Result (KITTI 04). Inlier ratio ~57%, det(R) ≈ 1.0, scale-aligned ATE 8.74m over a 394m trajectory. Trajectory shape matches GT, but scale factor 1.4579 confirms the monocular ambiguity.
Lessons.
- The world↔camera convention is the single biggest source of subtle bugs.
recoverPosereturns world-to-camera; the inverse is needed for "camera position in world." - 4×4 homogeneous matrices aren't pedantic — they make pose composition a single
@operation, which matters when you start chaining hundreds of poses.
Problem. Solve the monocular scale ambiguity, and reduce per-frame drift.
Theory. Stereo gives metric depth: Z = (fx · baseline) / disparity. With known baseline (~0.54m for KITTI), triangulation produces 3D points in true metric units. PnP (Perspective-n-Point) then recovers metric camera pose from 3D-2D correspondences.
Approach. Extended dataset to read stereo pairs and P3. Added triangulate and solve_pnp to geometry.py. Rewrote vo.py for frame-to-frame stereo PnP: at each frame, triangulate fresh 3D points from the stereo pair, then solve PnP against the previous frame's points.
Trade-off. Keyframe interval tested at 300 (single static map → similarity decays, scale collapses to 0.007), 5 (drift accumulates, scale ~0.8), and 1 (every frame is a keyframe — pragmatic, less elegant than true keyframe selection, but produces clean results). True keyframe selection (based on motion threshold or shared inlier count) was deferred to M3.
Result (KITTI 04). Median depth 26m (matches typical road-scene distances), scale factor 0.9628, raw ATE 17.35m without any post-hoc scale alignment.
Lessons.
- The first frame-to-frame translation came out at Z = 1.34m — KITTI runs at ~10 m/s @ 10 Hz, so ~1m/frame is expected. Seeing this number was the moment metric scale "clicked."
- Descriptor dtype matters: ORB returns uint8 binary; if it silently casts to float during numpy conversion, BFMatcher (Hamming) breaks. Always verify
descriptors.dtype == uint8.
Problem. M2 wasted information by discarding the map every frame. A 3D point observed across multiple frames carries more information than one triangulated independently each time. Also, drift was still significant (17m).
Theory. A persistent map stores 3D points and keyframes with bi-directional links (each MapPoint knows which keyframes observed it; each Keyframe knows which MapPoints it sees). Tracking becomes frame-to-map: match new frame features against the map's accumulated descriptors. The "active descriptor set" — points observed by the last N keyframes — keeps the search space bounded as the map grows.
Approach. Added mapping.py with MapPoint, Keyframe, and Map classes. Refactored vo.py into _initialize (first keyframe), _track (PnP against active descriptors), and _add_keyframe (expand the map every N frames). Used get_active_descriptors(recent_keyframes=10) to constrain matching to the recent window.
Trade-off. Searching the entire map vs. recent window: the full map causes exponential drift (the system tries to match against geometrically stale points, PnP returns garbage, and the next iteration poisons the map further). The 10-keyframe window is a pragmatic substitute for a proper covisibility graph (as used in ORB-SLAM).
Result (KITTI 04). Scale factor 1.0091, raw ATE 13.65m — a 21% improvement over M2.
Lessons (the most expensive).
- Every "improvement" needs a regression test on the previous working state. Attempting to add ratio test, distance threshold, and local bundle adjustment in sequence — without committing the working state in between — collapsed the pipeline to numerical overflow (translations in the e+47 range). Recovery required
git checkout m2and starting over. - Local BA via
scipy.least_squareswith sparse Jacobian, bounds anchoring, and trust-region tweaks was attempted, but the parameter choices weren't grounded in theory. The result was numerically unstable and could not be debugged without first understanding why each parameter mattered. This experience cemented a personal rule: don't use a parameter you can't explain.
Problem. Drift accumulates inevitably in pure VO (13.65m at the end of KITTI 04). But when the vehicle revisits a place it has seen before, that observation is a strong constraint that can correct the entire past trajectory.
Theory. Two-stage loop detection: (1) appearance-based candidate selection — find past keyframes that visually resemble the current one; (2) geometric verification — confirm with a PnP solve that the candidate and current keyframe really see the same 3D structure. Once confirmed, the loop becomes an additional edge in a pose graph: nodes are keyframes, edges are relative poses (sequential odometry + loop closures). Levenberg-Marquardt optimizes the entire pose graph to satisfy all constraints simultaneously. Note: this is pose-only optimization, not full bundle adjustment (3D points are not optimized — that's M5+ territory in production systems like ORB-SLAM).
Approach. New loop_detection.py with LoopDetector (temporal exclusion to avoid trivial matches with recent keyframes, similarity threshold, geometric verification via PnP). New pose_graph.py using GTSAM (BetweenFactorPose3 for both sequential and loop edges, PriorFactorPose3 to anchor the first keyframe). Integrated into vo.py with a trajectory rebuild step after each successful loop closure.
Trade-off. GTSAM vs. scipy for the optimizer: I initially attempted scipy + manual Jacobian sparsity + bounds, which proved unstable (see M3 lessons). GTSAM's BetweenFactor API and built-in LM optimizer are designed exactly for this problem and turned out to be both more correct and easier to use.
Result.
- KITTI 04: scale 1.014, ATE 13.78m — same band as M3, as expected (no loops in this short, straight sequence). Importantly, no regression: the M4 pipeline runs M3-equivalent behavior when no loops are detected.
- KITTI 00: tracking lost at frame 207. The naive feature pipeline (ORB-based) cannot survive KITTI 00's first sharp turn — the 10-keyframe window doesn't contain enough overlap to maintain tracking through the viewpoint change. M4's loop closure mechanism could not be tested here because tracking failed before reaching the actual loop.
Lessons.
- Numpy 2.x ABI incompatibility with GTSAM 4.2 required pinning
numpy==1.26. Worth pinning inrequirements.txt. - The KITTI 04 ↔ KITTI 00 gap is not a bug — it's a structural limit of the naive feature pipeline. ORB lacks the viewpoint robustness needed for KITTI 00, regardless of pose graph quality. This motivated M5.
Problem. Two limits from M4: (1) ORB features couldn't survive KITTI 00's viewpoint changes (tracking lost at frame 207); (2) loop detection against all past keyframes scales quadratically and becomes infeasible on long sequences.
Theory.
SuperPoint (DeTone et al., CVPR 2018): a single VGG-style CNN that jointly detects keypoints and produces descriptors. Trained via self-supervision: first, a base detector (MagicPoint) is trained on synthetic geometric shapes where keypoints are mathematically defined; then, Homographic Adaptation applies random homographies to real images and accumulates the warped detections into a consensus pseudo-ground-truth. This bootstraps keypoint annotations without manual labels.
LightGlue (Lindenberger et al., 2023): attention-based feature matching. Stacks self-attention (each keypoint sees the geometric context of its own image) and cross-attention (each keypoint sees candidate matches in the other image) over 9 layers, then uses Sinkhorn optimal transport to produce a soft assignment matrix with bidirectional consistency. Apache 2.0 licensed (SuperGlue, the precursor, is non-commercial — important for downstream use).
Bag of Words for loop detection: descriptors from each keyframe are quantized against a learned visual vocabulary (MiniBatchKMeans, 500 words). Each keyframe becomes an L2-normalized histogram. Loop candidates are filtered by cosine similarity of histograms — O(N) instead of O(N²) brute-force matching. Only the top-K candidates go through expensive LightGlue geometric verification.
Approach. New feature_models/ directory wraps SuperPoint and LightGlue (weights loaded via the lightglue package). features.py refactored to maintain backward compatibility with the existing cv2.KeyPoint / cv2.DMatch interface, so the rest of the pipeline (mapping, geometry, vo) didn't need rewriting. mapping.py updated for float32 descriptors instead of uint8. loop_detection.py rewritten with a BoW frontend: train the vocabulary after N keyframes are accumulated, compute histograms retroactively, then use cosine similarity for fast candidate filtering before LightGlue verification.
Trade-off.
- LightGlue over SuperGlue: Apache 2.0 vs. non-commercial license. Same architectural family, comparable or better accuracy, faster inference. For any project that might eventually intersect with industrial or defense use, LightGlue is the safe default.
- GPU cost: ORB runs at ~10ms/frame on CPU; SuperPoint + LightGlue runs at ~50–150ms/frame on an RTX 3050 Ti. The pipeline is noticeably slower per frame, but the gain in feature quality is dramatic enough to make this the right trade.
- BoW via MiniBatchKMeans vs. NetVLAD or DBoW3: KMeans is a pragmatic minimum-viable approach — easy to reason about, no extra dependencies, training in seconds. NetVLAD or learned aggregation would be more robust but require additional setup and paper study; deferred as future work.
Result.
KITTI 04 — scale 0.9727, raw ATE 3.30m — a 76% improvement over M4 (13.78m). The biggest single-milestone gain in the project.
KITTI 00 — tracking now survives through frame 1561 (vs. frame 207 in M4 — 8× longer). One loop closure confirmed: KF 312 ↔ KF 29, BoW similarity 0.616. The estimated trajectory recovers the topology of KITTI 00's figure-8 path, though absolute scale and final ATE remain affected by post-1561 tracking loss.
Lessons.
- Learned features integrate cleanly on top of classical geometry — the architectural refactor was minimal because the interfaces (extract → match → triangulate → PnP) stayed the same.
- BoW collapses loop detection from O(N²) to effectively linear-with-small-constant, making it feasible on long sequences.
- KITTI 00 is still not fully solved. Reaching ORB-SLAM-level performance requires full bundle adjustment, relocalization after tracking loss, and a proper covisibility graph. These are out of scope for this prototype.
Problem. Make the SLAM pipeline deployable as a ROS2 service — the standard interface for robotic perception. Even without real hardware, packaging the pipeline as ROS2 nodes with proper topics, TF tree, and visualization demonstrates readiness for downstream robotic integration (TEKNOFEST İKA, URC-style platforms).
Approach. Three-node architecture:
-
kitti_publisher_node— reads KITTI stereo pairs from disk and publishes them on/camera/left/image_rawand/camera/right/image_rawtopics with shared timestamps. Configurable publish rate (3 Hz default to prevent VO frame drop). -
vo_node— subscribes to stereo pairs withmessage_filters.ApproximateTimeSynchronizerfor sync, runs the M5 pipeline frame-by-frame, publishes:/vo/pose(PoseStamped) — current camera pose/vo/path(Path) — full trajectory history/vo/map_points(PointCloud2) — accumulated 3D map/vo/keyframes(PoseArray) — keyframe positions- TF broadcast:
map→camera_link
-
visualization_node— subscribes to/vo/poseand/vo/keyframes, republishes them as RViz-friendlyMarkerArray(current pose as a green arrow, keyframes as blue spheres).
A visual_slam.launch.py brings up all four (3 nodes + RViz with preloaded config) with a single command.
Trade-off.
- Python (rclpy) vs. C++ (rclcpp): Python chosen for speed of development and consistency with the rest of the codebase. C++ would offer better performance for production but is out of scope for this portfolio version.
- Frame drop handling: VO processing (~150ms/frame on RTX 3050 Ti) is slower than the dataset's native 10 Hz. Publish rate was reduced to 3 Hz to keep the synchronizer queue from dropping frames. In a real system this would be solved with
SensorDataQoSand adaptive processing. - VO pipeline refactor: Added
VisualOdometry.process_frame(frame_idx, img_L, img_R)to expose single-step processing for ROS callbacks while keeping the originalprocess()method for offline benchmarking. Same logic, two entry points.
Result. Single-command demo:
ros2 launch visual_slam_ros visual_slam.launch.pyBrings up the full pipeline + RViz visualization (see demo GIF at top). KITTI 04 runs end-to-end in ROS2 with the same accuracy as the standalone benchmark.
Lessons.
- ROS2 parameter type coercion: launch file string
'04'was parsed as integer 4 by ROS YAML — requiredParameterValue(..., value_type=str)to enforce string type explicitly. nav_msgs.msg.Pathandpathlib.Pathcollision: a classic namespace bug that took longer to debug than it should have. Lesson: always alias one or the other on import.- venv + ROS2 PATH ordering: ROS2's
source setup.bashputs system Python ahead of venv. A workspace-levelsetup_env.shscript that sources venv first, then ROS2, then exportsPYTHONPATHto venv site-packages was needed to makelightglueand other venv-installed packages visible to ROS nodes.
| Milestone | KITTI 04 ATE | Scale Factor | Notes |
|---|---|---|---|
| M1 (Monocular) | 8.74m (scale-aligned) | 1.46 | Scale ambiguity inherent to monocular |
| M2 (Stereo + PnP) | 17.35m (raw) | 0.96 | Metric scale recovered, drift remains |
| M3 (Persistent Map) | 13.65m (raw) | 1.01 | 21% improvement over M2 |
| M4 (Loop Closure) | 13.78m (raw) | 1.01 | No regression; no loops in KITTI 04 |
| M5 (SuperPoint + LightGlue + BoW) | 3.30m (raw) | 0.97 | 76% improvement over M4 |
KITTI 00: M4 tracking lost at frame 207. M5 survived to frame 1561 with one verified loop closure (KF 312 ↔ KF 29). Topological trajectory recovery achieved; full benchmark-quality ATE requires further work.
- KITTI 00 not fully tracked. Tracking loss after frame 1561; system stays at last known pose for the remainder. Requires relocalization to recover.
- No full bundle adjustment. Only pose graph optimization is implemented; 3D map points are not jointly optimized with poses. ORB-SLAM-level accuracy requires full BA.
- No relocalization. When tracking is lost, the system cannot re-localize against the existing map. It freezes at the last known pose.
- Single-machine, not real-time. ~50–150ms per frame with SuperPoint + LightGlue on an RTX 3050 Ti. Acceptable for offline benchmarks; not yet real-time for live robotic deployment.
- Loop detection vocabulary is simple. MiniBatchKMeans on 500 words; NetVLAD or DBoW3 would likely be more robust.
- No stereo rectification check. Assumes KITTI's pre-rectified stereo pairs are correct.
- Language: Python 3.10
- Core: NumPy 1.26 (pinned for GTSAM ABI), OpenCV, SciPy
- 3D / Optimization: GTSAM 4.2 (pose graph), Open3D (visualization)
- Learned features: PyTorch, LightGlue + SuperPoint (via
lightgluepackage, ETH CVG) - Clustering: scikit-learn (MiniBatchKMeans for BoW)
- Robotics: ROS2 Humble, RViz2, cv_bridge, message_filters, tf2_ros
- Testing: pytest
git clone <repo>
cd visual-slam
python3 -m venv venv
source venv/bin/activate
# Install dependencies (numpy is pinned to 1.26 for GTSAM 4.2 compatibility)
pip install -r requirements.txt
# PyTorch with CUDA (adjust CUDA version for your system)
pip install torch torchvision --index-url https://download.pytorch.org/whl/cu130Place KITTI odometry dataset under data/dataset/ (sequences and poses).
Requires ROS2 Humble. After core library setup:
cd ros2_ws
colcon build --symlink-install
source ros2_ws/setup_env.sh # sources venv + ROS2 + PYTHONPATHTo run the full ROS2 pipeline without dealing with local ROS2 Humble installations or Python environment collisions, use the provided Docker environment. Requires NVIDIA Container Toolkit.
# Allow Docker to access your local X11 display (for RViz)
xhost +local:docker
# Build the image (installs all dependencies, LightGlue, and compiles the ROS2 workspace)
docker compose buildpython3 scripts/run_vo.pyConfigure sequence ID and other parameters in the script. Output trajectory plot is saved to results/.
For long-running KITTI 00 benchmarks, redirect output to a log file:
mkdir -p logs
python3 scripts/run_vo.py 2>&1 | tee logs/m5_kitti00.log | grep -i "loop\|first track"source ros2_ws/setup_env.sh
ros2 launch visual_slam_ros visual_slam.launch.pyBrings up KITTI publisher, VO node, visualization node, and RViz2 with preloaded config. Pipeline runs end-to-end with live visualization.
To override defaults:
ros2 launch visual_slam_ros visual_slam.launch.py sequence:=00This is the recommended way to run the real-time ROS2 visualization.
- Start the container in the background:
docker compose up -d- Attach to the container and launch the full pipeline:
docker exec -it visual-slam-ros bash
cd ros2_ws
ros2 launch visual_slam_ros visual_slam.launch.py- When finished, cleanly shut down the container from your host terminal:
docker compose down- Full bundle adjustment (joint 3D point + pose optimization)
- Relocalization for recovery from tracking loss
- NetVLAD or DBoW3 for more robust loop detection
- Docker container for fully reproducible deployment
- C++ (rclcpp) port for performance-critical paths
- Live ROS2 deployment with real stereo cameras (TEKNOFEST İKA platform)
- Geiger, Lenz, Urtasun. Are we ready for autonomous driving? The KITTI vision benchmark suite. CVPR 2012.
- Hartley, Zisserman. Multiple View Geometry in Computer Vision. Cambridge University Press.
- Mur-Artal, Tardós. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras. IEEE TRO 2017.
- DeTone, Malisiewicz, Rabinovich. SuperPoint: Self-Supervised Interest Point Detection and Description. CVPRW 2018.
- Sarlin, DeTone, Malisiewicz, Rabinovich. SuperGlue: Learning Feature Matching with Graph Neural Networks. CVPR 2020.
- Lindenberger, Sarlin, Pollefeys. LightGlue: Local Feature Matching at Light Speed. ICCV 2023.
- Stachniss, Cyrill. Photogrammetry & Robotics lecture series, University of Bonn.
- KITTI dataset (Geiger et al., 2012)
- Magic Leap for the original SuperPoint pretrained weights
- ETH Computer Vision and Geometry Group for LightGlue
- README structure and editing assistance: Anthropic Claude
Apache-2.0