ENPM818Z - Assignment 2: SLAM Frontend

SLAM frontend implementation for ROS2 using KITTI dataset.

👉 Full Documentation

---

Quick Setup (ROS 2 Jazzy Docker)

Minimal setup using the Jazzy-based Docker image:

# 1. Navigate to workspace
cd ~/ENPM818Z-RWA2           # Change to your workspace path

# 2. Allow Docker to access X11 (once per session, on host)
xhost +local:docker

# 3. Build Jazzy image
docker compose build

# 4. Start container (drops you into /workspace)
docker compose run --rm slam_frontend

Runtime Pipeline (3 terminals inside the container)

Terminal 1 – Launch SLAM nodes

ros2 launch slam_frontend rwa2_full.launch.py  # or rwa2_dev.launch.py

Terminal 2 – Play KITTI bag with ROS 2 CLI

ros2 bag play /workspace/rwa2_starter/data/kitti_00_full -r 1.0
# replace kitti_00_full with kitti_00_dev for the dev bag

Terminal 3 – Run verification

cd /workspace/rwa2_starter/slam_frontend/slam_frontend
python3 verification.py

Bag files in rwa2_starter/data/ are mounted read-only from the host, so ensure those directories exist before launching the container.

---

Option 2: Native Installation (Optional)

# 1. Navigate to workspace
cd ~/ENPM818Z-RWA2  # Change to your workspace path

# 2. Create and activate virtual environment
python3 -m venv ~/ros2_venv
source ~/ros2_venv/bin/activate

# 3. Install dependencies
pip install empy==3.3.4 catkin_pkg lark
pip install -r rwa2_starter/slam_frontend/requirements.txt

# 4. Build workspace (replace jazzy with your distro if different)
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja --event-handlers console_cohesion+
source install/setup.bash

# 5. Run
# Dev dataset
ros2 launch slam_frontend rwa2_dev.launch.py

# Full dataset
ros2 launch slam_frontend rwa2_full.launch.py

---

System-Specific Settings

Update these for your system:

• ROS2 Distro: Replace jazzy with your distro (e.g., foxy, iron) in:

  • /opt/ros/jazzy/setup.bash → /opt/ros/YOUR_DISTRO/setup.bash

• Workspace Path: Update in bash function below (line with WS=) to match your location

---

Optional: Bash Helper Function

Add to ~/.bashrc for easy workspace setup:

function slam_frontend_ws {
   VENV="${HOME}/ros2_venv"
   WS="/home/kunj13/ENPM818Z-RWA2"  # Change to your workspace path
   ROS2_DISTRO="jazzy"   # Change to your ROS2 distro

   if [[ -z "$VIRTUAL_ENV" ]]; then
      [ ! -d "${VENV}" ] && python3 -m venv "${VENV}"
      source "${VENV}/bin/activate"
   fi

   [ -f "/opt/ros/${ROS2_DISTRO}/setup.bash" ] && source "/opt/ros/${ROS2_DISTRO}/setup.bash"
   [ -f "${WS}/install/setup.bash" ] && source "${WS}/install/setup.bash"

   cd "${WS}"
}

Then run: slam_frontend_ws

---

Bag Files

Place KITTI bag files in rwa2_starter/data/:

• kitti_00_dev/ - Dev dataset
• kitti_00_full/ - Full dataset

Note: Bag files are not in the repository (too large).

---

Usage

Launch SLAM System

# Dev dataset
ros2 launch slam_frontend rwa2_dev.launch.py

# Full dataset
ros2 launch slam_frontend rwa2_full.launch.py

# Options
ros2 launch slam_frontend rwa2_dev.launch.py rate:=2.0  # 2x speed

Verification Node

In a separate terminal (after launching the system):

cd rwa2_starter/slam_frontend/slam_frontend
python3 verification.py

---

Parameter Tuning and Values

Tuning

Edit rwa2_starter/slam_frontend/config/params.yaml:

• keyframe_distance_threshold: Distance (m) to create keyframe
• keyframe_rotation_threshold: Rotation (deg) to create keyframe
• voxel_size: Point cloud downsampling size
• max_keyframes: Number of keyframes in local map

See the file for detailed tuning guidelines.

Tuned Values

odometry_estimator_node:

keyframe_distance_threshold: 4.0
keyframe_rotation_threshold: 30.0
voxel_size: 0.25

Local Map Manager Parameters

max_keyframes: 20
voxel_size: 0.25

• KITTI sequences involve high-speed, outdoor driving with long, mostly straight trajectories. A distance threshold of 4 meters avoids generating redundant keyframes when the vehicle moves quickly in open spaces, reducing computation while still capturing enough geometric change for stable odometry.

• KITTI vehicle motion has relatively small yaw changes except at intersections. A 30° rotation threshold ensures a new keyframe is created only when a meaningful viewpoint change occurs, improving robustness of scan-to-scan alignment without over-populating the map.

• A 25 cm voxel size is appropriate for KITTI LiDAR point density. It preserves adequate geometric detail for accurate scan matching while downsampling enough to maintain real-time performance.

• Keeping 20 recent keyframes in the local map balances map richness and runtime efficiency.

• Matching the odometry voxel size ensures consistent resolution across keyframe scans. This improves ICP/registration stability and avoids introducing mismatches due to differing point cloud granularities.

Results Summary

Dataset	Processing Rate(Hz)	# of Keyframes	Max Observed Drift(m)	Max Z-drift(m)
Full	15	739	1.8	3.2

Implementation Notes

Challenges faced

• The original KITTI ROS bags did not publish a /clock topic, leading to TF synchronization errors in ROS 2. This was resolved by post-processing the bag and extracting timestamps from the point cloud data to generate a proper /clock stream.

• Initial runs produced too many keyframes and significant Z-axis drift. Achieving stable odometry required careful tuning of distance/rotation thresholds, voxel sizes, and map parameters to balance accuracy and computational load.

• Changing voxel size, thresholds, or ICP parameters often affects each other, making tuning a slow iterative process.

• The provided ROS 2 bag did not play correctly on the Humble distribution, causing message and metadata incompatibilities that required conversion and troubleshooting before development could continue.

Key lessons learned

• learned how keyframes reduce redundant computation, maintain long-term consistency, and allow SLAM systems to scale to large environments.
• Choosing thresholds for distance, rotation, and map growth directly affects odometry stability and computational load.
• Unlike scan-to-scan matching, aligning each incoming scan to a richer local map reduces drift and handles fast motion better.
• Maintaining a sliding window of recent keyframes produces a dense, reliable map for alignment while keeping memory bounded.
• Tuning voxel sizes, thresholds, and map sizes significantly affects drift, stability, and runtime behavior.
• Observed how drift accumulates, how poor tuning can degrade performance, and where odometry or mapping may fail in dynamic or feature-poor environments.

---

Notes

• empy version: 3.3.4 is required for ROS2 Humble compatibility
• Bag file compatibility: Some bags may show yaml-cpp warnings but should still work
• Virtual environment: Always activate before building/running

---

License

MIT License - See LICENSE file.