slam-algorithms

What this skill does

# slam-algorithms

You are **slam-algorithms** - a specialized skill for SLAM (Simultaneous Localization and Mapping) algorithm selection, configuration, and tuning.

## Overview

This skill enables AI-powered SLAM implementation including:
- Configuring ORB-SLAM3 for monocular, stereo, and RGB-D
- Setting up RTAB-Map for visual and LiDAR SLAM
- Configuring Google Cartographer for 2D and 3D SLAM
- Implementing LIO-SAM and LeGO-LOAM for LiDAR-inertial SLAM
- Tuning feature detection and matching parameters
- Configuring loop closure detection and optimization
- Setting up IMU preintegration and visual-inertial fusion
- Optimizing for real-time performance
- Evaluating SLAM accuracy (ATE, RPE metrics)
- Configuring map saving and loading

## Prerequisites

- ROS/ROS2 with SLAM packages
- Camera calibration (intrinsics and extrinsics)
- IMU calibration (if using VI-SLAM)
- Appropriate compute resources (GPU recommended for visual SLAM)

## Capabilities

### 1. ORB-SLAM3 Configuration

Configure ORB-SLAM3 for different sensor configurations:

```yaml
# orb_slam3_config.yaml
%YAML:1.0

# Camera Parameters (Monocular/Stereo)
Camera.type: "PinHole"
Camera.fx: 458.654
Camera.fy: 457.296
Camera.cx: 367.215
Camera.cy: 248.375
Camera.k1: -0.28340811
Camera.k2: 0.07395907
Camera.p1: 0.00019359
Camera.p2: 1.76187114e-05

# Camera resolution
Camera.width: 752
Camera.height: 480
Camera.fps: 20.0

# Stereo parameters
Camera.bf: 47.90639384423901  # baseline * fx

# RGB-D parameters
DepthMapFactor: 1.0
ThDepth: 35.0  # depth threshold

# ORB Extractor
ORBextractor.nFeatures: 1200
ORBextractor.scaleFactor: 1.2
ORBextractor.nLevels: 8
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

# IMU Parameters (for VI-SLAM)
IMU.NoiseGyro: 1.7e-4
IMU.NoiseAcc: 2.0e-3
IMU.GyroWalk: 1.9e-5
IMU.AccWalk: 3.0e-3
IMU.Frequency: 200

# Viewer parameters
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
```

Launch ORB-SLAM3:
```bash
# Monocular
ros2 run orb_slam3_ros orb_slam3_mono \
  --ros-args -p vocabulary:=/path/to/ORBvoc.txt \
  -p settings:=/path/to/config.yaml \
  -r /camera/image_raw:=/robot/camera/image_raw

# Stereo
ros2 run orb_slam3_ros orb_slam3_stereo \
  --ros-args -p vocabulary:=/path/to/ORBvoc.txt \
  -p settings:=/path/to/stereo_config.yaml

# RGB-D
ros2 run orb_slam3_ros orb_slam3_rgbd \
  --ros-args -p vocabulary:=/path/to/ORBvoc.txt \
  -p settings:=/path/to/rgbd_config.yaml

# Stereo-Inertial
ros2 run orb_slam3_ros orb_slam3_stereo_inertial \
  --ros-args -p vocabulary:=/path/to/ORBvoc.txt \
  -p settings:=/path/to/stereo_inertial_config.yaml
```

### 2. RTAB-Map Configuration

Configure RTAB-Map for RGB-D and LiDAR SLAM:

```yaml
# rtabmap_params.yaml
rtabmap:
  ros__parameters:
    # Database
    database_path: ""

    # Detection
    Rtabmap/DetectionRate: "1.0"
    Rtabmap/TimeThr: "0.0"

    # Memory
    Mem/IncrementalMemory: "true"
    Mem/STMSize: "30"
    Mem/RehearsalSimilarity: "0.6"

    # Visual Features
    Vis/FeatureType: "6"  # ORB
    Vis/MaxFeatures: "500"
    Vis/MinInliers: "20"
    Vis/InlierDistance: "0.1"

    # Loop Closure
    RGBD/LoopClosureReextractFeatures: "true"
    RGBD/OptimizeFromGraphEnd: "false"
    RGBD/ProximityBySpace: "true"

    # ICP for LiDAR
    Reg/Strategy: "1"  # 0=Vis, 1=ICP, 2=VisIcp
    Icp/PointToPlane: "true"
    Icp/Iterations: "30"
    Icp/VoxelSize: "0.05"
    Icp/MaxCorrespondenceDistance: "0.1"

    # Graph Optimization
    Optimizer/Strategy: "1"  # g2o
    Optimizer/Iterations: "20"

    # Mapping
    Grid/CellSize: "0.05"
    Grid/RangeMax: "5.0"
    Grid/RayTracing: "true"
    Grid/3D: "true"

rgbd_odometry:
  ros__parameters:
    frame_id: "base_link"
    odom_frame_id: "odom"
    publish_tf: true
    Odom/Strategy: "0"  # Frame-to-Map
    Odom/ResetCountdown: "1"
    Vis/CorType: "0"  # Features matching
```

Launch RTAB-Map:
```python
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # RGB-D Odometry
        Node(
            package='rtabmap_odom',
            executable='rgbd_odometry',
            output='screen',
            parameters=[{
                'frame_id': 'base_link',
                'odom_frame_id': 'odom',
                'subscribe_rgbd': True,
                'approx_sync': True,
            }],
            remappings=[
                ('rgbd_image', '/camera/rgbd'),
            ]
        ),

        # RTAB-Map SLAM
        Node(
            package='rtabmap_slam',
            executable='rtabmap',
            output='screen',
            parameters=[{
                'subscribe_rgbd': True,
                'subscribe_scan': True,
                'approx_sync': True,
                'frame_id': 'base_link',
                'map_frame_id': 'map',
                'odom_frame_id': 'odom',
                'queue_size': 10,
            }],
            remappings=[
                ('rgbd_image', '/camera/rgbd'),
                ('scan', '/lidar/scan'),
            ]
        ),

        # RViz
        Node(
            package='rtabmap_viz',
            executable='rtabmap_viz',
            output='screen',
            parameters=[{
                'subscribe_rgbd': True,
                'subscribe_scan': True,
            }],
        )
    ])
```

### 3. Google Cartographer Configuration

Configure Cartographer for 2D and 3D SLAM:

```lua
-- cartographer_2d.lua
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.55
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6

return options
```

```lua
-- cartographer_3d.lua
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  loo