Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization

C++/ROS 2 implementation of the paper:

Probabilistic Method for Mapping & 3D SLAM of an off-road Terrain with Four Wheeled Robot Kunal Bhujbal, Dr. Mahavir Devmane, Ananya More 2022 5th International Conference on Advances in Science and Technology (ICAST), IEEE DOI: 10.1109/ICAST55766.2022.10039652

Robot-centric elevation mapping with confidence bounds. Measurements from onboard range sensors are fused with proprioceptive state estimation while accounting for pose drift. The map is generated in real-time from a forward-facing stereo camera.

Overview

A ROS 2 package for probabilistic elevation mapping with a four-wheeled mobile robot. Designed for local navigation with robots equipped with pose estimation (IMU & odometry) and a distance sensor (RealSense, LiDAR, stereo camera). The elevation map moves with the robot and explicitly handles drift of the pose estimation.

Key features:

• Robot-centric 2.5D elevation map with per-cell spatial covariance
• Probabilistic upper/lower confidence bounds via map fusion
• Sensor noise models for Intel RealSense D435, Microsoft Kinect v2, Velodyne VLP-16
• Wheeled odometry noise model with slip detection
• Dynamic environment adaptation via visibility checking
• Quantitative evaluation (RMSE, MAE, confidence coverage, drift analysis)
• Gazebo simulation with four test environments

Method

Coordinate Frames

Four coordinate frames: inertial I, robot base B, sensor S, and map M. The map frame is defined relative to B (not I), with z-axis aligned to gravity and yaw matching the robot's heading.

Map Fusion with Confidence Bounds

Height measurements are fused with existing estimates using a 1D Kalman filter. As the robot moves, spatial covariance grows — cells farther from the robot accumulate more uncertainty, causing previously observed regions to "melt" into each other.

For each cell, a weighted empirical CDF is built from neighboring cells within the 2σ confidence ellipse. The 95% confidence bounds are extracted:

Flat regions maintain tight confidence intervals. Edges of objects produce wider bounds — useful for collision avoidance and contact planning.

Dynamic Environment Extension

When obstacles move, ray tracing from the sensor detects cells that violate visibility constraints and removes them. Noise injection accelerates adaptation when new measurements fall below the current estimate.

Results

Simulation

The map fusion approximates the true confidence bounds accurately. Flat regions stay tight; edges widen.

Hardware

Despite odometry drift, the true terrain remains within the confidence bounds.

Platform Comparison: Wheeled vs Legged

Metric	Wheeled	Legged
Position Drift Rate	~0.8 cm/m	~3.1 cm/m
Yaw Drift Rate	~0.03 rad/m	~0.07 rad/m
RMSE	~1.2 cm	~3.4 cm
Coverage Rate	~96.7%	~95.1%
Mean CI Width	~3.5 cm	~8.9 cm

Installation

Dependencies

• ROS 2 Humble (or later)
• Grid Map, PCL, Eigen (>= 3.3)
• Gazebo (optional, for simulation)

Build

cd ~/ros2_ws/src
git clone https://github.com/kbhujbal/probabilistic_SLAM_quadruped_robot.git
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-select probabilistic_terrain_mapping
source install/setup.bash

Tests

colcon test --packages-select probabilistic_terrain_mapping
colcon test-result --verbose

Usage

Gazebo Simulation

ros2 launch probabilistic_terrain_mapping simulation.launch.py world:=stairs
# Options: flat_terrain, stairs, rocky_terrain, dynamic_obstacles

# Drive the robot
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.0}}"

Real Sensor Data

ros2 launch probabilistic_terrain_mapping terrain_mapping.launch.py

Offline Evaluation

ros2 launch probabilistic_terrain_mapping evaluation.launch.py bag_path:=/path/to/rosbag

Nodes

terrain_mapping_node

Real-time data collection. Subscribes to /camera/depth/points ([PointCloud2]), /odom ([Odometry]), /imu/data ([Imu]). Publishes /elevation_map ([GridMap]).

map_fusion_node

Parallel confidence bound computation. Subscribes to /elevation_map. Publishes /fused_map with layers: elevation, lower_bound, upper_bound, confidence_interval.

evaluation_node

Offline benchmarking. Subscribes to /fused_map, /ground_truth_map, /odom, /ground_truth_odom. Publishes /eval_metrics (JSON).

Project Structure

probabilistic_terrain_mapping/
├── config/          # Parameters (sensors, robots)
├── include/         # C++ headers (core, math, sensors, state_estimation, evaluation)
├── src/             # C++ implementations + ROS 2 nodes
├── msg/             # ElevationCell, ElevationMap, EvaluationMetrics
├── launch/          # terrain_mapping, simulation, evaluation
├── test/            # 11 test files, 50+ test cases
├── scripts/         # Python plotting and analysis
├── simulation/      # Gazebo worlds, URDF, robot models
├── rviz/            # Visualization config
└── doc/             # Paper figures

Citing

Probabilistic Method for Mapping & 3D SLAM of an off-road Terrain with Four Wheeled Robot Kunal Bhujbal, Dr. Mahavir Devmane, Ananya More 2022 5th International Conference on Advances in Science and Technology (ICAST), IEEE

@inproceedings{bhujbal2022probabilistic,
  author    = {Bhujbal, Kunal and Devmane, Mahavir and More, Ananya},
  title     = {Probabilistic Method for Mapping \& 3D SLAM of an off-road Terrain with Four Wheeled Robot},
  booktitle = {2022 5th International Conference on Advances in Science and Technology (ICAST)},
  year      = {2022},
  organization = {IEEE}
}

License

BSD-3-Clause. See LICENSE for details.

Bugs & Feature Requests

Issue Tracker