sm2mm pipelines

This page describes the file format for sm2mm pipelines and lists some useful ready-to-use examples.

Note

👉 There is an online graphical editor for sm2mm pipelines

1. What does sm2mm mean?

sm2mm stands for the process of transforming a “simple map” file, a “key-frame map” typically generated by a SLAM system and having the .simplemap extension, into a “metric map” file (.mm extension), typically comprising several map layers of different types, semantics, or serving to different purposes.

sm2mm generic diagram

Users may use this through two interfaces: a CLI application sm2mm or the C++ API mp2p_icp_filters::simplemap_to_metricmap().


2. Pipeline file specification

Pipeline configuration files are written in YAML format and define the sequence of operations to be applied to the raw sensor observations stored as map keyframes.

The pipeline can include various filters and generators that process the observations, such as downsampling, noise reduction, or feature extraction. The pipeline file can also specify custom plugins to be loaded, which can define new metric map classes or custom filter algorithms.

Each pipeline file can contain these sections:

  • generators:: A list of generators to create maps from each raw observations in each key-frame. If none is provided, the default mp2p_icp::Generator is used, which generates a point cloud from the observation.

  • filters:: A list of filters to apply to each key-frame observations, after generators have been applied. For example, here one typically removes the robot body, de-skew the scan, downsamples the point cloud, and merges the result into one or several final metric map layers used to accumulate the result.

  • final_filters:: An optional list of filters to apply to the final map layers, after all key-frames have been processed.

Refer to example pipeline files sm2mm_*.yaml under the demos directory. Some of them are explained below with a diagram. Their main parameters can be changed without editing them, through SM2MM_* variables listed in each file header.

Variables

YAML files loaded by sm2mm and the rest of mp2p_icp applications (mm-filter, rawlog-filter, icp-run, sm-cli, mm-georef) may contain variables, expanded before parsing the file:

  • ${VAR|default}: replaced by the value of the environment variable VAR if defined, or by default otherwise. The default may be empty: ${VAR|}.

  • ${VAR}: replaced by the value of the environment variable VAR. Loading the file fails if it is not defined.

  • ${CURRENT_YAML_FILE_PATH}: the directory of the YAML file.

Variables may appear anywhere, including within quoted strings and formulas, e.g. "robot_x - ${RANGE|10.0}", but are ignored within comments. For example, with this in my_pipeline.yaml:

voxel_filter_resolution: ${VOXEL_SIZE|0.20} # [m]

the default voxel size can be changed from the command line:

VOXEL_SIZE=0.10 sm2mm -i map.simplemap -o map.mm -p my_pipeline.yaml

Note

This is a subset of the YAML syntax of MOLA launch files, which also supports $include{} and $(cmd). To use those too, expand the file first with mola-yaml-parser.


3. Example sm2mm pipelines


Pipeline: sm2mm_no_decim_imu_mls_keyframe_map.yaml

Purpose: Build a dense (no downsampling) point cloud, using IMU-based motion compensation, then filter the map using the MLS filter, and store the resulting map into a mola::KeyframePointCloudMap layer named localmap, suitable for MOLA-LIO localization-only mode.

sm2mm diagram

Example result screenshots
sm2mm example result screenshots
Pipeline YAML code
# ---------------------------------------------------------------------------------------------
# Pipeline definition file for sm2mm (simplemap-to-metricmap)
#
# See: https://github.com/MOLAorg/mp2p_icp/tree/develop/mp2p_icp_core/apps/sm2mm
#
# Explanation of this particular pipeline:
#  - Generators: Default generator creating 'raw', another just creating
#                an empty layer "localmap" for GICP MOLA-LIO usage.
#  - Filters per observation: Deskew using IMU, remove close points.
#  - Final filters at end: intensity normalization, MLS filtering, insert into "localmap"
#
# Variables (define them as environment variables to override these defaults):
#  SM2MM_DESKEW_METHOD=MotionCompensationMethod::IMU
#      scan de-skew: MotionCompensationMethod::{None,Linear,IMU}
#  SM2MM_MIN_RANGE=3.0
#      drop points closer than this to the robot (e.g. its body) [m]
#  SM2MM_MLS_SEARCH_RADIUS=0.10
#      MLS neighbor search radius [m]
#  SM2MM_MLS_POLYNOMIAL_ORDER=2
#      MLS polynomial order (1: plane, 2: quadratic)
# ---------------------------------------------------------------------------------------------

# --------------------------------------------------------
# 1) Generator (observation -> local frame metric maps)
# --------------------------------------------------------
generators:
  # This one will convert all incoming lidar scan observations into pointclouds in layer "raw"
  - class_name: mp2p_icp_filters::Generator
    params:
      name: "generator_default"
      filterOutPointsAtZero: true # Remove invalid points in 'organized' LiDAR scans

  # This one will just create the layer "localmap", expected by the GICP LIO algorithm for localization
  - class_name: mp2p_icp_filters::Generator
    params:
      name: "generator_localmap"
      target_layer: "localmap"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: "" # None, do not insert directly, but in the final_filters stage.
      #process_sensor_labels_regex: '.*'

      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/stable/group_mrpt_maps_grp.html
        class: mola::KeyframePointCloudMap
        plugin: "libmola_metric_maps.so" # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          max_search_keyframes: 3
          k_correspondences_for_cov: 20
        insertOpts:
          remove_frames_farther_than: 0 # [m]
        likelihoodOpts: ~ # none required
        renderOpts:
          color.A: 0.25 # [0,1] Use this alpha value for points, RGB from colormap
          colormap: "cmHOT" # cmJET, cmHOT, cmGRAYSCALE
          recolorByPointField: "intensity" # x,y,z,ring, intensity, ambient, etc.
          max_points_per_kf: 100000 # Max number of points to render per keyframe
          max_overall_points: 100000 # Max number of points to render overall (e.g. to avoid FoxGlove WS overflow)
          #point_size: 1.0  # superseded by visualization.local_map_point_size when run inside MOLA-LIO

# --------------------------------------------------------
# 2) Per local frame filtering
# --------------------------------------------------------
filters:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: "raw"
      silently_ignore_no_timestamps: false
      method: "TimestampAdjustMethod::MiddleIsZero"

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: "raw"
      output_pointcloud_layer: "deskewed"
      method: ${SM2MM_DESKEW_METHOD|MotionCompensationMethod::IMU}
      ignore_accelerometer: true # Set to false if using high-quality IMU and in Drones, etc.
      silently_ignore_no_timestamps: false

      output_layer_class: "mrpt::maps::CGenericPointsMap" # Keep all channels: intensity, ring, ...

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx, vy, vz, wx, wy, wz]

      # IMPORTANT: In the context of sm2mm, de-skew happens with points already in global coordinates
      # so we need the robot pose to correct points:
      points_already_global: true
      robot_pose:
        [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "deskewed"
      output_layer_outside: "filtered"
      range_min: 0.0
      range_max: ${SM2MM_MIN_RANGE|3.0}
      metric_l_infinity: true # Faster
      # Measure distances from the moving robot pose:
      center: [robot_x, robot_y, robot_z]

  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      # one or more layers to remove
      pointcloud_layer_to_remove: ["raw", "deskewed"]

# -------------------------------------------------------------------
# 3) Final, overall filter pipeline to apply to the whole metric map
# -------------------------------------------------------------------
final_filters:
  # ----------------------------------------------------------------------
  # Configuration for the Moving Least Squares (MLS) Filter
  # ----------------------------------------------------------------------
  - class_name: mp2p_icp_filters::FilterMLS
    params:
      # The layer containing the point cloud used to compute the MLS surface.
      # This is the 'source' cloud for the surface fitting.
      input_pointcloud_layer: "filtered"

      # The layer where the smoothed/projected points and their normals will be
      # stored. If it doesn't exist, it will be created.
      output_pointcloud_layer: "mls"

      # The class name for output layer if it does not exist and needs to be
      # created. Empty means same class than input.
      output_layer_class: "mrpt::maps::CGenericPointsMap"

      # ====================================================================
      # MLS Core Parameters
      # ====================================================================

      # Search radius (in meters) for finding neighbors around each query point.
      # A larger radius results in a smoother but potentially less detailed surface.
      search_radius: ${SM2MM_MLS_SEARCH_RADIUS|0.10}

      # Order of the polynomial to fit to the local neighborhood.
      # 1: Planar fit (fastest, basically a weighted PCA for normals).
      # 2: Quadratic fit (standard for curvature/better smoothing).
      # 3+: Higher order (can lead to oscillations/overfitting).
      polynomial_order: ${SM2MM_MLS_POLYNOMIAL_ORDER|2}

      # Minimum number of neighbors required to successfully compute a fit.
      # Should be at least 3 for a basic plane (order 1), and more for order 2+.
      min_neighbors_for_fit: 10

      # ====================================================================
      # Smoothing / Projection Method
      # ====================================================================

      # Method for projecting points onto the fitted polynomial surface.
      # SIMPLE: Project point along the normal direction to the polynomial at
      #         the projected (u,v) location. (Fast and robust).
      projection_method: FilterMLS::ProjectionMethod::SIMPLE

      # Method for determining which points to smooth/project.
      # NONE: Smooth the 'input_pointcloud_layer' itself (standard smoothing).
      # DISTINCT_CLOUD: Build the surface from 'input_pointcloud_layer', but
      #                 project the points from 'distinct_cloud_layer' onto it
      #                 (used for upsampling or projection).
      upsampling_method: FilterMLS::UpsamplingMethod::NONE

      # Required only if upsampling_method is DISTINCT_CLOUD.
      # The points in this layer are the ones that will be projected onto the
      # surface computed from 'input_pointcloud_layer'.
      # distinct_cloud_layer: "sparse_scan"

      # ====================================================================
      # Parallelization
      # ====================================================================

      # If TBB is enabled, this is the grain size for parallel execution.
      # Smaller values increase parallelism overhead; larger values may leave
      # cores idle. Default (1024UL) is typically good.
      parallelization_grain_size: 1024

  # Specific map for localization with the GICP pipeline
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: "mls"
      target_layer: "localmap"

  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      # one or more layers to remove
      pointcloud_layer_to_remove: ["filtered", "mls"]


Pipeline: sm2mm_pointcloud_voxelize.yaml

Purpose: Process point clouds by applying voxelization to downsample the data while preserving spatial structure, creating a more memory-efficient representation.

sm2mm voxelization diagram

Example result screenshots

Overview of voxelized map:

sm2mm voxel overview

Detailed view of voxel structure:

sm2mm voxel detail
Pipeline YAML code
# -----------------------------------------------------------------------------
# Pipeline definition file for sm2mm (simplemap-to-metricmap)
#
# See: https://github.com/MOLAorg/mp2p_icp/tree/develop/mp2p_icp_core/apps/sm2mm
#
# Explanation of this particular pipeline:
#  - Generators: default generator is used (creates an 'raw' layer).
#  - Filters: Just one downsampling filter, with an additional removal of close
#             points (e.g. the robot body)
#
# Variables (define them as environment variables to override these defaults):
#  SM2MM_DESKEW_METHOD=MotionCompensationMethod::IMU
#      scan de-skew: MotionCompensationMethod::{None,Linear,IMU}
#  SM2MM_VOXEL_SIZE=0.02
#      voxel size of the per-keyframe downsampling [m]
#  SM2MM_MIN_RANGE=5.0
#      drop points closer than this to the robot (e.g. its body) [m]
# -----------------------------------------------------------------------------

# --------------------------------------------------------
# 1) Generator (observation -> local frame metric maps)
# --------------------------------------------------------
generators:
  - class_name: mp2p_icp_filters::Generator
    params:
      filterOutPointsAtZero: true # Remove invalid points in 'organized' LiDAR scans

# --------------------------------------------------------
# 2) Per local frame filtering
# --------------------------------------------------------
filters:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: "raw"
      silently_ignore_no_timestamps: true
      method: "TimestampAdjustMethod::MiddleIsZero"

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: "raw"
      output_pointcloud_layer: "deskewed"
      method: ${SM2MM_DESKEW_METHOD|MotionCompensationMethod::IMU}
      ignore_accelerometer: true # Set to false if using high-quality IMU and in Drones, etc.
      #silently_ignore_no_timestamps: true # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # Keep all channels: intensity, ring, ...

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx, vy, vz, wx, wy, wz]

      # IMPORTANT: In the context of sm2mm, de-skew happens with points already in global coordinates
      # so we need the robot pose to correct points:
      points_already_global: true
      robot_pose:
        [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "deskewed"
      output_pointcloud_layer: "decimated"
      voxel_filter_resolution: ${SM2MM_VOXEL_SIZE|0.02} # [m]
      decimate_method: DecimateMethod::FirstPoint

  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "decimated"
      output_layer_outside: "filtered"
      range_min: 0.0
      range_max: ${SM2MM_MIN_RANGE|5.0}
      # Measure distances from the moving robot pose:
      center: [robot_x, robot_y, robot_z]

  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      # one or more layers to remove
      pointcloud_layer_to_remove: ["raw", "deskewed", "decimated"]
# -------------------------------------------------------------------
# 3) Final, overall filter pipeline to apply to the whole metric map
# -------------------------------------------------------------------
#final_filters:
#  - (none)


Pipeline: sm2mm_voxels_static_dynamic_points.yaml

Purpose: Separate point clouds into static and dynamic layers using voxel-based analysis, enabling differentiation between stationary map features and moving objects for robust localization and mapping.

sm2mm static/dynamic separation diagram

Example result screenshots

Overview of map with static/dynamic layers:

sm2mm dynamic layer overview

Detailed view of static layer:

sm2mm static layer detail

Voxel representation with dynamic filtering:

sm2mm dynamic voxel detail

Dynamic layer visualization:

sm2mm dynamic layer
Pipeline YAML code
# -----------------------------------------------------------------------------
# Pipeline definition file for sm2mm (simplemap-to-metricmap)
#
# See: https://github.com/MOLAorg/mp2p_icp/tree/develop/mp2p_icp_core/apps/sm2mm
#
# Explanation of this particular pipeline:
#  Creates a 3D voxel map and a point cloud (without downsampling! it may become quite large),
#  then in a final step, uses voxel occupancy to tell "dynamic" from "static" points.
#
# Variables (define them as environment variables to override these defaults):
#  SM2MM_VOXELMAP_RESOLUTION=0.25
#      voxel map resolution [m]
#  SM2MM_DESKEW_METHOD=MotionCompensationMethod::Linear
#      scan de-skew: MotionCompensationMethod::{None,Linear,IMU}
#  SM2MM_MIN_RANGE=5.0
#      drop points closer than this to the robot (e.g. its body) [m]
#  SM2MM_MAX_RANGE=100.0
#      drop points farther than this from the robot [m]
#  SM2MM_VOXEL_SIZE=0.20
#      voxel size of the per-keyframe downsampling [m]
#  SM2MM_OCCUPANCY_THRESHOLD=0.7
#      min voxel occupancy to keep its points as static [0,1]
# -----------------------------------------------------------------------------

# --------------------------------------------------------
# 1) Generator (observation -> local frame metric maps)
# --------------------------------------------------------
generators:
  # This first generator is used to just create the metric map "gridmap" once:
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "map"
      # The default '.*' is replaced by '' (none): do not insert directly any observation,
      # since we want to insert them after decimation
      process_class_names_regex: ""
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/stable/group_mrpt_maps_grp.html
        class: mrpt::maps::CGenericPointsMap
        #plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        #creationOpts:
        # none required for this class
        #insertionOpts:
        # none required for this class
        #likelihoodOpts:
        #renderOpts:
        # ...

  # This first generator is used to just create the metric map "gridmap" once:
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "voxelmap"
      # The default '.*' is replaced by '' (none): do not insert directly any observation,
      # since we want to insert them after decimation
      process_class_names_regex: ""
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/stable/group_mrpt_maps_grp.html
        class: mrpt::maps::CVoxelMap
        #plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          resolution: ${SM2MM_VOXELMAP_RESOLUTION|0.25} # [m]
          #resolution: $f{0.05*MAX_SENSOR_RANGE} # [m]  # You can also use formulas in any numeric field
        insertOpts:
          #max_range: -1
          prob_miss: 0.49 # P(occupied | miss): weakest miss, as this pipeline was tuned with
          prob_hit: 0.65
          clamp_min: 0.05
          clamp_max: 0.95
          ray_trace_free_space: true
          decimation: 1
        likelihoodOpts:
          decimation: 1
          occupiedThreshold: 0.51
        renderOpts:
          occupiedThreshold: 0.51
          freeThreshold: 0.40
          generateFreeVoxels: false

  # Default generator: convert all observations into a point cloud layer "raw":
  # If "raw" does not exist, it will be created
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "raw"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: "(mrpt::obs::CObservationPointCloud|mrpt::obs::CObservation3DRangeScan|mrpt::obs::CObservation2DRangeScan)"
      process_sensor_labels_regex: ".*"
      filterOutPointsAtZero: true # Remove invalid points in 'organized' LiDAR scans
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/stable/group_mrpt_maps_grp.html
        class: mrpt::maps::CGenericPointsMap

# --------------------------------------------------------
# 2) Per local frame filtering
# --------------------------------------------------------
filters:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: "raw"
      silently_ignore_no_timestamps: true
      method: "TimestampAdjustMethod::MiddleIsZero"

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: "raw"
      output_pointcloud_layer: "deskewed"
      method: ${SM2MM_DESKEW_METHOD|MotionCompensationMethod::Linear}
      ignore_accelerometer: true # Set to false if using high-quality IMU and in Drones, etc.
      silently_ignore_no_timestamps: true # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # Keep all channels: intensity, ring, ...

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx, vy, vz, wx, wy, wz]

      # IMPORTANT: In the context of sm2mm, de-skew happens with points already in global coordinates
      # so we need the robot pose to correct points:
      points_already_global: true
      robot_pose:
        [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "deskewed"
      output_layer_between: "filtered"
      range_min: ${SM2MM_MIN_RANGE|5.0}
      range_max: ${SM2MM_MAX_RANGE|100.0}
      center: [robot_x, robot_y, robot_z]

  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: "filtered"
      target_layer: "map"
      robot_pose:
        [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "filtered"
      output_pointcloud_layer: "pre_map"
      voxel_filter_resolution: ${SM2MM_VOXEL_SIZE|0.20} # [m]
      decimate_method: DecimateMethod::FirstPoint

  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: "pre_map"
      target_layer: "voxelmap"
      robot_pose:
        [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  # Remove layers not intended for map insertion:
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["deskewed", "raw", "filtered", "pre_map"]

# -------------------------------------------------------------------
# 3) Final, overall filter pipeline to apply to the whole metric map
# -------------------------------------------------------------------
final_filters:
  - class_name: FilterRemoveByVoxelOccupancy
    params:
      input_pointcloud_layer: "map"
      input_voxel_layer: "voxelmap"
      output_layer_static_objects: "static_map"
      output_layer_dynamic_objects: "dynamic_map"
      occupancy_threshold: ${SM2MM_OCCUPANCY_THRESHOLD|0.7}


Pipeline: sm2mm_traversability_obstacles2d.yaml

Purpose: Build the static obstacles for 2D path planning of a ground vehicle, as a sparse 2D point cloud (layer obstacles2d) with one point per non-traversable cell of a traversability analysis of the whole 3D map (FilterTraversability).

Stages:

  • Per keyframe: de-skew, crop a square window around the robot, remove the vehicle body, accumulate the points, and ray trace the whole scan into a voxel occupancy map.

  • The mapping trajectory is stored as the seeds of traversable space.

  • Final: points of dynamic objects (low voxel occupancy) are removed, then the traversability analysis builds a 2.5D ground model and marks as obstacles: ground tilted more than max_slope_deg, steps or drop-offs higher than max_step, anything within the vehicle body height band above the local ground, and the unobserved border of the region reachable from the trajectory. The obstacle cells are finally flattened to z=0.

sm2mm traversability obstacles diagram

Example result screenshots
sm2mm traversability obstacles example result

Main variables (see the file header for all of them):

Variable

Default

Description

SM2MM_MAX_SLOPE_DEG

15.0

Max traversable ground slope (tilt) [deg]

SM2MM_MAX_STEP

0.15

Max traversable step [m]

SM2MM_BODY_MIN_HEIGHT, SM2MM_BODY_MAX_HEIGHT

0.25, 1.20

Vehicle body band above the local ground [m]

SM2MM_GRID_RESOLUTION

0.20

Grid cell size, also the output point spacing [m]

SM2MM_OCCUPANCY_THRESHOLD

0.7

Min voxel occupancy to keep points as static [0,1]

SM2MM_DESKEW_METHOD

MotionCompensationMethod::IMU

Scan de-skew method (None for keyframes without timestamps or IMU data)
Pipeline YAML code
# -----------------------------------------------------------------------------
# Pipeline definition file for sm2mm (simplemap-to-metricmap)
#
# See: https://github.com/MOLAorg/mp2p_icp/tree/develop/mp2p_icp_core/apps/sm2mm
#
# Explanation of this particular pipeline:
#  Builds the static obstacles for 2D path planning of a ground vehicle, from
#  a traversability analysis of the whole 3D map (FilterTraversability).
#
#  Output: one layer "obstacles2d" (mrpt::maps::CSimplePointsMap), a sparse 2D
#  cloud (z=0) with one point per obstacle cell:
#   - a 2.5D ground model of the map (lowest points, unseen ground filled in),
#   - obstacles: ground tilted more than SM2MM_MAX_SLOPE_DEG, steps (or
#     drop-offs) higher than SM2MM_MAX_STEP, and anything at the height of the
#     vehicle body above the local ground (so it follows any slope),
#   - only around the region reachable from the mapping trajectory, which is
#     traversable by definition: never observed cells bordering it (e.g. the
#     hidden side of a drop-off) are obstacles too.
#
#  Dynamic objects (e.g. people walking by while mapping) are removed first
#  with a ray-traced voxel occupancy map: a voxel seen occupied in some
#  keyframes but crossed by laser rays in others ends up with a low occupancy,
#  and its points are dropped.
#
#  Keyframes must have point clouds with timestamps and an IMU velocity buffer
#  for the IMU de-skew (as stored by MOLA-LIO), or set
#  SM2MM_DESKEW_METHOD=MotionCompensationMethod::None.
#
# Variables (define them as environment variables to override these defaults):
#  SM2MM_DESKEW_METHOD=MotionCompensationMethod::IMU
#      scan de-skew: MotionCompensationMethod::{None,Linear,IMU}
#  SM2MM_WINDOW_RANGE=12.0
#      half side of the square window around each keyframe [m]
#  SM2MM_MIN_RANGE=1.0
#      drop points closer than this to the robot (e.g. its body) [m]
#  SM2MM_LIDAR_HEIGHT=0.55
#      height of the LiDAR above the robot frame origin, for ray tracing [m]
#  SM2MM_OCCUPANCY_THRESHOLD=0.7
#      min voxel occupancy to keep its points as static [0,1]
#  SM2MM_GRID_RESOLUTION=0.20
#      traversability grid cell size, also the output point spacing [m]
#  SM2MM_MAX_SLOPE_DEG=15.0
#      max traversable ground slope (tilt) [deg]
#  SM2MM_MAX_STEP=0.15
#      max traversable step [m]
#  SM2MM_BODY_MIN_HEIGHT=0.25
#      vehicle body band above the local ground: bottom [m]
#  SM2MM_BODY_MAX_HEIGHT=1.20
#      vehicle body band above the local ground: top [m] (higher overhangs are ignored)
#  SM2MM_SEEDS_RADIUS=0.50
#      traversable radius around each trajectory pose [m] (about half the vehicle length)
# -----------------------------------------------------------------------------

# --------------------------------------------------------
# 1) Generator (observation -> local frame metric maps)
# --------------------------------------------------------
generators:
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "raw"
      filterOutPointsAtZero: true # Remove invalid points in 'organized' LiDAR scans
      # Only the LiDAR: keyframes may also hold IMU or GNSS readings.
      process_class_names_regex: "mrpt::obs::CObservationPointCloud"

  # Just create the voxel map once; it is filled by the filters below:
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "voxelmap"
      process_class_names_regex: ""
      metric_map_definition:
        class: mrpt::maps::CVoxelMap
        creationOpts:
          resolution: 0.20 # [m]
        insertOpts:
          # P(occupied | miss): the weakest miss, since stronger misses erase
          # most of the ground through rays at grazing angles.
          prob_miss: 0.49
          prob_hit: 0.65
          clamp_min: 0.05
          clamp_max: 0.95
          ray_trace_free_space: true
          decimation: 1
        likelihoodOpts:
          decimation: 1
          occupiedThreshold: 0.51
        renderOpts:
          occupiedThreshold: 0.51
          freeThreshold: 0.40
          generateFreeVoxels: false

# --------------------------------------------------------
# 2) Per local frame filtering
# --------------------------------------------------------
filters:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: "raw"
      silently_ignore_no_timestamps: true
      method: "TimestampAdjustMethod::MiddleIsZero"

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: "raw"
      output_pointcloud_layer: "deskewed"
      method: ${SM2MM_DESKEW_METHOD|MotionCompensationMethod::IMU}
      ignore_accelerometer: true
      silently_ignore_no_timestamps: true
      output_layer_class: "mrpt::maps::CSimplePointsMap" # Only XYZ is needed
      twist: [vx, vy, vz, wx, wy, wz]
      # In sm2mm, points are already in global coordinates:
      points_already_global: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

  # Square window around the robot, from below the ground up to above the
  # vehicle body (treetops and ceilings are not needed):
  - class_name: mp2p_icp_filters::FilterBoundingBox
    params:
      input_pointcloud_layer: "deskewed"
      inside_pointcloud_layer: "window"
      bounding_box_min:
        - "robot_x - ${SM2MM_WINDOW_RANGE|12.0}"
        - "robot_y - ${SM2MM_WINDOW_RANGE|12.0}"
        - "robot_z - 10.0"
      bounding_box_max:
        - "robot_x + ${SM2MM_WINDOW_RANGE|12.0}"
        - "robot_y + ${SM2MM_WINDOW_RANGE|12.0}"
        - "robot_z + 3.0"

  # Drop the vehicle body:
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "window"
      output_layer_between: "near"
      range_min: ${SM2MM_MIN_RANGE|1.0} # [m]
      range_max: 1000.0 # [m]
      center: [robot_x, robot_y, robot_z]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "near"
      output_pointcloud_layer: "accum"
      voxel_filter_resolution: 0.05 # [m]
      decimate_method: DecimateMethod::FirstPoint

  # Rays are traced from the LiDAR to the whole scan (not only the window), so
  # people walking near the robot are crossed by many rays to the walls and
  # ceiling behind them. Decimated to the voxel size:
  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "deskewed"
      output_pointcloud_layer: "rays"
      voxel_filter_resolution: 0.20 # [m]
      decimate_method: DecimateMethod::FirstPoint

  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: "rays"
      target_layer: "voxelmap"
      robot_pose:
        - robot_x
        - robot_y
        - "robot_z + ${SM2MM_LIDAR_HEIGHT|0.55}"
        - robot_yaw
        - robot_pitch
        - robot_roll

  # The mapping trajectory: where the robot has been is traversable.
  - class_name: mp2p_icp_filters::FilterInsertPoint
    params:
      target_layer: "trajectory"
      point: [robot_x, robot_y, robot_z]

  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw", "deskewed", "window", "near", "rays"]

# -------------------------------------------------------------------
# 3) Final, overall filter pipeline to apply to the whole metric map
# -------------------------------------------------------------------
final_filters:
  # Keep only static points:
  - class_name: mp2p_icp_filters::FilterRemoveByVoxelOccupancy
    params:
      input_pointcloud_layer: "accum"
      input_voxel_layer: "voxelmap"
      output_layer_static_objects: "static"
      occupancy_threshold: ${SM2MM_OCCUPANCY_THRESHOLD|0.7}

  - class_name: mp2p_icp_filters::FilterTraversability
    params:
      input_pointcloud_layer: "static"
      seeds_pointcloud_layer: "trajectory"
      output_layer_obstacles: "obstacle_cells"
      resolution: ${SM2MM_GRID_RESOLUTION|0.20} # [m]
      max_slope_deg: ${SM2MM_MAX_SLOPE_DEG|15.0} # [deg]
      max_step: ${SM2MM_MAX_STEP|0.15} # [m]
      body_min_height: ${SM2MM_BODY_MIN_HEIGHT|0.25} # [m]
      body_max_height: ${SM2MM_BODY_MAX_HEIGHT|1.20} # [m]
      seeds_radius: ${SM2MM_SEEDS_RADIUS|0.50} # [m]

  # 2D: all points at z=0, one per cell:
  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "obstacle_cells"
      output_pointcloud_layer: "obstacles2d"
      voxel_filter_resolution: ${SM2MM_GRID_RESOLUTION|0.20} # [m]
      decimate_method: DecimateMethod::ClosestToAverage
      flatten_to: 0.0 # [m]

  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["accum", "voxelmap", "static", "trajectory", "obstacle_cells"]