Data

Tools

Indexes

Applications

Experiments

Awesome

An open API service indexing awesome lists of open source software.

https://github.com/abhishekprajapatt/slamcore

🗺️ slamcore - A lightweight, production-grade simultaneous localization and mapping (SLAM) system designed for ROS2 (Humble/Iron) with comprehensive support for multi-sensor fusion, loop closure detection, and real-time map building.
https://github.com/abhishekprajapatt/slamcore

cmake cpp python shell

Last synced: about 2 months ago
JSON representation

🗺️ slamcore - A lightweight, production-grade simultaneous localization and mapping (SLAM) system designed for ROS2 (Humble/Iron) with comprehensive support for multi-sensor fusion, loop closure detection, and real-time map building.

Awesome Lists containing this project

README 

          
# Slamcore (ROS2): Multi-Sensor SLAM System



A lightweight, production-grade simultaneous localization and mapping (SLAM) system designed for ROS2 (Humble/Iron) with comprehensive support for multi-sensor fusion, loop closure detection, and real-time map building.



## Overview



Slamcore implements a complete SLAM pipeline combining LiDAR odometry, visual odometry, and IMU preintegration through a loosely-coupled multi-sensor architecture. The system performs real-time pose graph optimization to maintain consistency and detect loop closures for trajectory refinement. Designed for simulation-first validation, it provides benchmarking tools for evaluation against public datasets like KITTI and TUM RGB-D.



## Architecture



### Front-End Processing



- **LiDAR Odometry**: Scan-to-scan registration via iterative closest point (ICP) with configurable convergence parameters

- **Visual Odometry**: Feature detection, matching, and motion estimation using essential matrix decomposition

- **IMU Preintegration**: Continuous acceleration and angular velocity integration with bias estimation



### Back-End Optimization



- **Pose Graph Construction**: Incremental frame nodes with odometry constraints

- **Loop Closure Detection**: Descriptor-based similarity matching with configurable thresholds

- **Optimization**: g2o-based pose graph optimization with local bundle adjustment

- **Robust Kernels**: Huber kernel weighting for loop closure constraints



### Mapping & Visualization



- **2D Occupancy Grid**: Voxel-based environment representation

- **3D Point Cloud**: Accumulation and downsampling for sparse map representation

- **Real-Time Publishing**: Continuous map updates at configurable frequencies

- **RViz2 Integration**: Full visualization support for trajectories, maps, and constraints



## System Requirements



### Build Dependencies



- **OS**: Ubuntu 20.04 (Focal) or later

- **ROS2**: Humble or Iron

- **Compiler**: GCC 9+ or Clang 10+ with C++20 support

- **Build System**: CMake 3.20+, colcon



### Runtime Dependencies



- Gazebo Harmonic (for simulation)

- OpenCV 4.5+

- PCL 1.12+

- Eigen3

- g2o

- yaml-cpp

- TF2



### Hardware (Minimal)



- 4GB RAM

- 2-core CPU (real-time performance on 8+ cores)

- No GPU required (optional for acceleration)



## Installation & Build



### 1. Clone Repository



```bash

mkdir -p ~/slamcore_ws/src

cd ~/slamcore_ws/src

git clone https://github.com/abhishekprajapatt/slamcore.git

cd slamcore

```



### 2. Install Dependencies



```bash

sudo apt update

sudo apt install -y \

  ros-humble-desktop \

  ros-humble-gazebo-ros-pkgs \

  ros-humble-image-transport \

  libopencv-dev \

  libpcl-dev \

  libeigen3-dev \

  libyaml-cpp-dev \

  libopenscenegraph-dev

```



For g2o:



```bash

sudo apt install -y libeigen3-dev libsuitesparse-dev qtdeps

git clone https://github.com/RainerKuemmerl/g2o.git

cd g2o && mkdir build && cd build

cmake .. && make -j4

sudo make install

```



### 3. Build Slamcore



```bash

cd ~/slamcore_ws

colcon build --symlink-install

source install/setup.bash

```



Or use the convenience script:



```bash

cd ~/slamcore_ws/src/slamcore

bash scripts/build.sh

source ../../install/setup.bash

```



## Quick Start: Gazebo Simulation



### 1. Launch Simulation Environment



```bash

ros2 launch slamcore gazebo_slam.launch.py gui:=true

```



This launches:



- Gazebo with slam_world environment (bounded room with obstacles)

- Differential-drive robot with LiDAR, camera, and IMU

- All SLAM processing nodes (frontend, backend, mapping)



### 2. Launch Visualization



In another terminal:



```bash

source install/setup.bash

ros2 launch slamcore rviz.launch.py

```



### 3. Command Robot Motion



Publish velocity commands to drive the robot and observe SLAM in action:



```bash

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \

  '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}'

```



### 4. Monitor Output



- **RViz2**: Displays map, trajectory, odometry, and point cloud

- **Terminal**: Node logs show frontend processing, loop closures, optimization iterations



## Dataset Evaluation



### KITTI Sequence Evaluation



```bash

source install/setup.bash



python3 scripts/benchmark_kitti.py kitti-sequence /path/to/kitti 0 --output-dir results/



ros2 bag play /path/to/kitti/00.bag



python3 scripts/evaluate_trajectory.py results/estimated_trajectory.csv \

  /path/to/kitti/poses/00.txt --output-dir results/benchmarks

```



### TUM RGB-D Evaluation



```bash

ros2 bag play /path/to/tum/rgbd_dataset_freiburg3_long_office_household.bag



python3 scripts/evaluate_trajectory.py results/trajectory.csv \

  /path/to/tum/groundtruth.txt --output-dir results/benchmarks

```



### Format for EVO Tool



```bash

python3 scripts/evaluate_trajectory.py trajectory.csv ground_truth.txt \

  --format-for-evo trajectory_evo.tum

evo_traj tum trajectory_evo.tum --plot

```



## Configuration



Edit `config/params.yaml` to customize system behavior:



```yaml

sensor_fusion:

  lidar_weight: 1.0 # LiDAR contribution weight

  camera_weight: 1.0 # Camera contribution weight

  imu_weight: 0.5 # IMU contribution weight



frontend:

  lidar_max_range: 30.0 # LiDAR maximum range (m)

  icp_max_iterations: 50 # ICP max iterations per scan

  icp_fitness_threshold: 0.01 # ICP convergence tolerance



  camera_enable: true

  optical_flow_max_features: 200



backend:

  optimization_frequency: 5.0 # Pose graph optimization rate (Hz)

  g2o_iterations: 20 # G2O solver iterations



loop_closure:

  enabled: true

  min_score: 0.7 # Loop detection confidence threshold

  min_keyframe_separation: 10 # Minimum frames between loop constraints



mapping:

  grid_resolution: 0.1 # Occupancy grid cell size (m)

  grid_max_range: 20.0 # Maximum range for map updates

  publish_frequency: 2.0 # Map publishing rate (Hz)

```



## ROS2 Topics



### Inputs



| Topic                 | Message Type           | Description                 |

| --------------------- | ---------------------- | --------------------------- |

| `/scan`               | sensor_msgs/LaserScan  | 2D LiDAR scans              |

| `/camera/image_raw`   | sensor_msgs/Image      | RGB/Grayscale camera images |

| `/camera/camera_info` | sensor_msgs/CameraInfo | Camera calibration          |

| `/imu/data`           | sensor_msgs/Imu        | Inertial measurement unit   |



### Outputs



| Topic                 | Message Type            | Description                       |

| --------------------- | ----------------------- | --------------------------------- |

| `/odometry/lidar`     | nav_msgs/Odometry       | Front-end LiDAR odometry          |

| `/odometry/visual`    | nav_msgs/Odometry       | Front-end visual odometry         |

| `/odometry/filtered`  | nav_msgs/Odometry       | Fused odometry estimate           |

| `/odometry/optimized` | nav_msgs/Odometry       | Backend-optimized odometry        |

| `/map`                | nav_msgs/OccupancyGrid  | 2D occupancy grid map             |

| `/map_cloud`          | sensor_msgs/PointCloud2 | 3D point cloud map                |

| `/loop_closure_edges` | geometry_msgs/PoseArray | Detected loop closure constraints |



## Nodes



| Node              | Executable               | Function                        |

| ----------------- | ------------------------ | ------------------------------- |

| Lidar Odometry    | `lidar_odometry_node`    | Scan-to-scan registration       |

| Visual Odometry   | `visual_odometry_node`   | Feature-based motion estimation |

| IMU Integration   | `imu_integration_node`   | Sensor preintegration           |

| Loop Closure      | `loop_closure_node`      | Loop constraint detection       |

| Backend Optimizer | `backend_optimizer_node` | Pose graph optimization         |

| Map Builder       | `map_builder_node`       | Occupancy grid and point cloud  |



## Benchmark Results



### Gazebo Simulation (slam_world)



- **Robot Path**: 50m loop trajectory

- **Processing**: Real-time on 4-core CPU

- **Loop Closure**: Detected at ~90% confidence

- **Map Coverage**: 97% of simulated environment



### KITTI Sequence 00 (Reference)



- **Trajectory Length**: 3.7 km

- **APE (Mean)**: ~0.15 m

- **RPE (Mean)**: ~0.05 m / 100m

- **Runtime**: 0.3x real-time (depends on optimization frequency)



**Note**: Exact results depend on parameter tuning, sensor noise, and hardware. These are representative values.



## Code Structure



```

slamcore/

├── include/slamcore/slam.hpp          # Core class definitions

├── src/

│   ├── frontend_lidar.cpp             # LiDAR odometry

│   ├── frontend_visual.cpp            # Visual odometry

│   ├── imu_integration.cpp            # IMU processing

│   ├── backend_optimizer.cpp          # Pose graph optimization

│   ├── loop_closure.cpp               # Loop closure detection

│   ├── map_builder.cpp                # Map construction

│   ├── frame.cpp                      # Frame data structure

│   └── nodes/                         # ROS2 node implementations

├── launch/

│   ├── gazebo_slam.launch.py          # Full simulation launch

│   └── rviz.launch.py                 # Visualization launch

├── config/

│   ├── params.yaml                    # System parameters

│   └── rviz_config.rviz               # RViz configuration

├── worlds/slam_world.sdf              # Gazebo environment

├── models/slam_robot.urdf.xacro       # Robot URDF

├── scripts/

│   ├── benchmark_kitti.py             # Dataset utilities

│   ├── evaluate_trajectory.py         # Trajectory evaluation

│   ├── build.sh                       # Build script

│   └── run_demo.sh                    # Demo launcher

├── tests/

│   └── test_frame.cpp                 # Unit tests

├── CMakeLists.txt                     # Build configuration

├── package.xml                        # ROS2 package metadata

├── LICENSE                            # MIT License

└── README.md                          # This file

```



## Core Algorithms



### Frontend: Scan Matching



- Algorithm: Iterative Closest Point (ICP) with Euclidean distance

- Convergence: Fitness threshold on point cloud alignment error

- Frequency: On every LiDAR scan (~10 Hz)



### Frontend: Visual Odometry



- Features: ORB descriptors (fast, rotation-invariant)

- Matching: Brute-force descriptor matching with RANSAC

- Motion: Essential matrix decomposition for R,t estimation

- Frequency: On every image (~30 Hz)



### Backend: Pose Graph Optimization



- Solver: g2o with Levenberg-Marquardt optimization

- Variables: SE(3) poses for each keyframe

- Constraints: Odometry edges + loop closure edges

- Robustness: Huber kernel for outlier rejection



### Loop Closure: Descriptor Matching



- Method: Similarity-based candidate selection

- Descriptor: ORB features from camera images

- Verification: Ransac-based pose refinement

- Minimum Separation: Configurable temporal gap



## Limitations & Future Work



### Current Limitations



- **Simulation-Only**: Designed for Gazebo validation; real hardware integration requires sensor drivers

- **Single-Robot**: No multi-robot support

- **No Real-Time Guarantees**: Processing time scales with environment complexity and loop closure frequency

- **Limited Loop Closure**: Descriptor-based approach; no global localization

- **No Relocalization**: Cannot recover from tracking loss



### Planned Features



- 3D LiDAR support (point cloud registration)

- Place recognition with bag-of-words

- IMU-camera-LiDAR tightly-coupled fusion

- Octomap support for 3D volumetric mapping

- ROS2 humble → Iron migration validation

- Distributed multi-robot variants



## Performance Tuning



### For Speed



1. Reduce `icp_max_iterations` (e.g., 20)

2. Increase `icp_fitness_threshold` (e.g., 0.05)

3. Decrease `optimization_frequency` (e.g., 2 Hz)

4. Enable keyframe selection (not yet implemented)



### For Accuracy



1. Increase `icp_max_iterations` (e.g., 100)

2. Decrease `icp_fitness_threshold` (e.g., 0.001)

3. Increase `optimization_frequency` (e.g., 10 Hz)

4. Lower `loop_closure_min_score` (e.g., 0.6)



## Testing



### Unit Tests



```bash

colcon test --packages-select slamcore

```



### Integration Tests (Manual)



1. Launch `gazebo_slam.launch.py`

2. Publish motion commands

3. Verify `/odometry/*` topics output

4. Check `/map` for occupancy grid updates



## Contributing



Contributions are welcome. Please ensure:



- Code compiles cleanly with `-Wall -Wextra -Wpedantic`

- Follow existing naming conventions (snake_case functions, PascalCase classes)

- Add tests for new functionality

- Update parameters in `config/params.yaml` for new tunable parameters



## Citation



If you use Slamcore in research, please cite:



```

@software{slamcore2025,

  title={Slamcore: Multi-Sensor SLAM for ROS2},

  author={Abhishek Prajapatt},

  year={2025},

url={https://github.com/abhishekprajapatt/slamcore}

```



## License



MIT License – See LICENSE file for details.



## Support & Issues



For bug reports and feature requests, visit the [GitHub Issues](https://github.com/abhishekprajapatt/slamcore/issues) page.



## Related Work



- **ORB-SLAM2/3**: Feature-based visual SLAM (closed-source reference)

- **LOAM**: LiDAR-only SLAM with local-to-global optimization

- **iSAM2**: Incremental smoothing and mapping (g2o foundation)

- **Cartographer**: Multi-sensor SLAM by Google

- **LIO-SAM**: Tightly-coupled LiDAR-Inertial odometry



---



Built with ❤️ for the robotics community. Star this repo if it helps your research!