Semantic SLAM

Submap-level pose graph SLAM using nvblox feature voxels as 3D correspondences.

Core Idea

Each submap is built with nvblox (TSDF + DINOv2 feature voxels) in its own local frame (origin = anchor camera pose). Feature voxels are matched across submaps via FAISS to estimate relative poses independent of odometry. Both odometry and feature-based relative pose estimates are fused in a GTSAM factor graph. Loop closure candidates are detected via submap descriptor similarity.

Pipeline

RGB-D + poses
     │
     ▼
extract_features.py              → features/feature_N.bin  (offline, Python)
     │                              (DINOv2 ViT-S/14, PCA 384→128, float16, H×W×128)
     ▼
fuse_replica_submaps             → submap_N.nvblx  +  poses.txt
     │                              (TSDF + Color + FeatureLayer per submap,
     │                               all layers in submap-local frame)
     ▼
extract_features                 → submap_N_features.bin
     │                              (~130k points × (xyz_local + feat128) per submap)
     ▼
register_submaps                 → relative_poses.bin
     │                              (FAISS MNN + TEASER++ per consecutive pair,
     │                               optional descriptor-based loop closure)
     ▼
submap_graph                     → poses_optimized.txt
                                    (GTSAM: odometry + feature BetweenFactors,
                                     inlier-scaled noise model)

Executables

Executable	Description
fuse_replica_submaps	Splits an iMAP Replica sequence into submaps. Integrates depth + color + features in submap-local frame. Saves .nvblx + poses.txt.
extract_features	Loads .nvblx files, iterates FeatureLayer, writes (xyz_local + feat128) binary point clouds.
register_submaps	FAISS GPU MNN correspondences + TEASER++ robust pose estimation per consecutive pair. Optional descriptor-based loop closure detection. Writes relative_poses.bin.
submap_graph	GTSAM pose graph with odometry + feature BetweenFactors. Inlier-scaled noise model. Writes poses_optimized.txt.

Code Structure

include/semantic_slam/
    feature_cloud.h      — FeatureCloud struct, load/save, gatherFeatures()
    registration.h       — registerPair(), featureNoiseModel(),
                           RelativePose, save/loadRelativePoses()
    loop_closure.h       — SubMapDescriptor, computeDescriptor(),
                           descriptorSimilarity(), detectLoopClosures()

src/
    feature_cloud.cpp
    registration.cpp     — FAISS MNN, TEASER++ wrapper, serialisation
    loop_closure.cpp     — descriptor computation + candidate detection

    fuse_replica_submaps.cpp   — executable: submap builder
    extract_features.cpp       — executable: feature point cloud extractor
    register_submaps.cpp       — executable: thin main using registration + loop_closure
    submap_graph.cpp           — executable: GTSAM graph builder + optimiser

    datasets/
        replica_imap.cpp       — iMAP Replica data loader

Coordinate Frames

Each submap is built in its own local frame:

• Origin = camera pose at the first frame of the submap (anchor_pose from poses.txt)
• All voxel xyz coordinates are relative to this origin: p_local = T_world_anchor^{-1} * p_world
• TEASER++ between two local-frame clouds gives the true relative pose between submap frames, directly usable as a GTSAM BetweenFactor measurement

Factor Graph Structure

PriorFactor(X0)                               ← anchors map origin
BetweenFactor(Xi, Xi+1, T_odom,   σ_odom)    ← from poses.txt (odometry)
BetweenFactor(Xi, Xj,   T_feat,   σ_feat(n)) ← from register_submaps
                                                 σ scales with inlier count n:
                                                 σ(n) = σ_base * sqrt(ref/n)

GTSAM optimizes the absolute world poses X(0)...X(N) that best satisfy all constraints weighted by their noise models.

Loop Closure Detection (current — naive)

The current descriptor-based detector:

1. Computes a global descriptor per submap = L2-normalised mean of all feature vectors
2. Finds pairs with cosine similarity above threshold (--lc_similarity_threshold)
3. Runs FAISS + TEASER++ on each candidate to verify geometrically

This is a heuristic. Two submaps in the same room can have similar mean descriptors without actually overlapping. See TODOs for planned improvements.

Dependencies

Dependency	Version	Install
ROS2	Humble	docs.ros.org
GTSAM	4.2.0	Build from source (see below)
FAISS	1.7.4+	Build from source (see below)
TEASER++	latest	Build from source (see below)
nvblox	—	git submodule
CUDA	≥ 11.8	System

GTSAM

git clone https://github.com/borglab/gtsam.git --branch 4.2.0 --depth 1
cd gtsam && mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release \
         -DGTSAM_WITH_TBB=OFF \
         -DGTSAM_BUILD_TESTS=OFF \
         -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
         -DGTSAM_USE_SYSTEM_EIGEN=ON \
         -DGTSAM_BUILD_UNSTABLE=OFF \
         -DCMAKE_POLICY_VERSION_MINIMUM=3.5
make -j$(nproc) && sudo make install && sudo ldconfig

Note: Use -DGTSAM_USE_SYSTEM_EIGEN=ON to avoid Eigen version conflicts with TEASER++.

FAISS (GPU)

git clone https://github.com/facebookresearch/faiss.git --branch v1.7.4 --depth 1
cd faiss && mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release \
         -DFAISS_ENABLE_GPU=ON \
         -DFAISS_ENABLE_PYTHON=OFF \
         -DBUILD_TESTING=OFF \
         -DBUILD_SHARED_LIBS=ON \
         -DCMAKE_CUDA_ARCHITECTURES=89 \
         -DCMAKE_INSTALL_PREFIX=/usr/local
make -j$(nproc) && sudo make install && sudo ldconfig

Replace 89 with your GPU's compute capability (nvidia-smi to find it).

TEASER++

git clone https://github.com/MIT-SPARK/TEASER-plusplus.git --depth 1
cd TEASER-plusplus && mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release \
         -DBUILD_TEASER_FPFH=OFF \
         -DBUILD_TESTING=OFF \
         -DBUILD_PYTHON_BINDINGS=OFF \
         -DCMAKE_INSTALL_PREFIX=/usr/local
make -j$(nproc) && sudo make install && sudo ldconfig

Build

cd <workspace>
source /opt/ros/humble/setup.bash
pip install catkin_pkg
git submodule update --init --recursive
# Prevent colcon from scanning nvblox's broken Python packages
touch src/semantic_slam/extern/nvblox/COLCON_IGNORE
colcon build --symlink-install --packages-select semantic_slam --executor sequential \
  --cmake-args -DCMAKE_POLICY_VERSION_MINIMUM=3.5
source install/setup.bash

Usage

Step 1 — Extract DINOv2 features

python3 scripts/extract_features.py \
    --dataset_path /path/to/replica/imap/00 \
    --num_pca_frames 50
# Output: imap/00/features/feature_N.bin  (float16, H×W×128)

Step 2 — Build submaps in local frame:

ros2 run semantic_slam fuse_replica_submaps \
    /path/to/replica/imap/00/ \
    /output/submaps/ \
    --frames_per_submap=200 \
    --submap_voxel_size=0.05 \
    --feature_integration_stride=5

Step 3 — Extract feature point clouds:

ros2 run semantic_slam extract_features /output/submaps/
# Output: submap_N_features.bin  (~130k pts × (xyz_local + feat128) per submap)

Step 4 — Register submap pairs:

# Consecutive pairs only
ros2 run semantic_slam register_submaps /output/submaps/ \
    --noise_bound=0.1 --knn_ratio=0.9 --min_inliers=10

# With loop closure detection
ros2 run semantic_slam register_submaps /output/submaps/ \
    --noise_bound=0.1 --knn_ratio=0.9 --min_inliers=10 \
    --enable_loop_closure \
    --lc_similarity_threshold=0.85 \
    --lc_min_index_gap=2
# Output: relative_poses.bin, relative_poses.txt

Step 5 — Optimize pose graph:

# Odometry only (baseline)
ros2 run semantic_slam submap_graph /output/submaps/poses.txt

# With feature constraints
ros2 run semantic_slam submap_graph /output/submaps/poses.txt \
    --relative_poses=/output/submaps/relative_poses.bin
# Output: poses_optimized.txt

Feature Point Cloud Format

Each submap_N_features.bin is a raw binary file:

[int32 num_points] [int32 feat_dim=128]
[float32 x, y, z, f0, f1, ..., f127]  × num_points

xyz coordinates are in the submap's local frame.

Load in Python:

import numpy as np, struct
with open("submap_000_features.bin", "rb") as f:
    n, d = struct.unpack("ii", f.read(8))
data = np.fromfile("submap_000_features.bin", dtype=np.float32)[2:].reshape(n, 3+d)
xyz, feats = data[:, :3], data[:, 3:]

Relative Poses Format

relative_poses.bin written by register_submaps:

[int32 num_pairs]
per pair:
  [int32 id_src] [int32 id_dst] [int32 num_inliers]
  [float64 × 16]  T_src_dst  (row-major 4×4, GTSAM BetweenFactor convention:
                               T = X_src^{-1} * X_dst)

Dataset Format (iMAP Replica)

<sequence>/
  depth/depth_0.png, depth_1.png, ...     (uint16, mm)
  rgb/rgb_0.png,     rgb_1.png,   ...     (uint8 RGB)
  traj_w_c.txt                            (T_world_camera, 4×4, one per line)
  features/feature_0.bin, ...             (float16, H×W×128, generated offline)

Camera intrinsics hardcoded for standard iMAP Replica render: w=1200, h=680, fx=fy=600.0, cx=599.5, cy=339.5

---

TODO

Loop Closure

• Better place recognition descriptor — replace mean-feature descriptor with DINOv2 CLS token averaged over keyframes in each submap. The CLS token is trained for global image similarity and is far more discriminative than the mean of spatial patch features.
• VLAD-style aggregation — cluster feature vectors into a vocabulary and use Fisher vector / VLAD encoding as the submap descriptor for better geometric specificity.

Registration

• Per-pair noise model from registration quality — currently inlier count scales the noise model. A better estimate would also use the inlier residuals from TEASER++ to set the covariance directly.
• Validate registrations with Open3D — write a Python script that loads two local-frame feature clouds, applies the estimated T_src_dst, and visualises the alignment to confirm TEASER++ is finding correct correspondences.
• Non-consecutive window registration — beyond loop closure, register nearby submaps within a fixed window (e.g. --loop_window=2) to add short-range constraints that stiffen the graph without needing full loop closure.

Mapping

• Global mesh stitching — after submap_graph, transform each submap's mesh from local frame to world frame using the optimised poses and merge into a single globally-consistent mesh.
• Incremental / online operation — current pipeline is fully offline batch. Moving to incremental submap building + online graph optimisation would enable real-time operation.