Point Cloud Filters

Point cloud Filters in mp2p_icp are used to modify or extract part of the information from an input point cloud. This can involve tasks like removing noise, downsampling, segmenting points into different layers based on properties (like intensity or curvature), or adjusting per-point attributes (like timestamps or intensity). All filters inherit from mp2p_icp_filters::FilterBase and can be configured either programmatically or via a YAML file using the filter pipeline API, e.g. mp2p_icp_filters::filter_pipeline_from_yaml() or mp2p_icp_filters::filter_pipeline_from_yaml_file().




Filter: FilterAbsoluteTimestamp

Description: Creates a new per-point channel containing absolute UNIX timestamps. This filter reads the relative timestamp channel t (float) from each point and adds the reference “zero time” of the cloud (retrieved from the attached processing context’s velocity buffer) to compute an absolute UNIX timestamp for every point. The absolute timestamps are stored in a new double-precision field (default name: timestamp_abs).

Parameters:

  • pointcloud_layer (std::string, default: raw): The point cloud layer to process.

  • output_field_name (std::string, default: timestamp_abs): The name of the new output field where absolute timestamps will be stored.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterAbsoluteTimestamp
    params:
      pointcloud_layer: 'raw'
      output_field_name: 'timestamp_abs'

Filter: FilterAdjustTimestamps

Description: Modifies the per-point timestamps of a map layer according to a set of criteria.

Parameters:

  • pointcloud_layer (std::string): The name of the point cloud layer to process.

  • silently_ignore_no_timestamps (bool, default: false): If true, no exception is thrown if the input layer does not contain timestamps; an empty output is produced instead.

  • time_offset (double, default: 0.0): An optional time offset to be added on top of the adjustment. Useful for synchronizing multiple sensors.

  • method (TimestampAdjustMethod, default: MiddleIsZero): The criterion for adjusting timestamps:

    • EarliestIsZero: Adjusts such that the earliest timestamp is 0. Successive ones represent real elapsed seconds.

    • MiddleIsZero: Adjusts such that the middle timestamp is 0. The rest are positive/negative elapsed seconds.

    • Normalize: Normalizes all timestamps to the range \([0, 1]\).

filters:
  #...
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: 'raw'
      silently_ignore_no_timestamps: true
      method: 'MiddleIsZero'

Filter: FilterBoundingBox

Description: Splits a point cloud into points inside and outside a given 3D bounding box.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • inside_pointcloud_layer (std::string, optional): The output layer name for points INSIDE the bounding box. If empty, these points are discarded.

  • outside_pointcloud_layer (std::string, optional): The output layer name for points OUTSIDE the bounding box. If empty, these points are discarded.

  • bounding_box_min (float[3]): The \((x, y, z)\) coordinates of the minimum corner of the bounding box (e.g., [-10, -10, -5]). Can use robocentric variables (e.g., robot_x).

  • bounding_box_max (float[3]): The \((x, y, z)\) coordinates of the maximum corner of the bounding box (e.g., [10, 10, 5]). Can use robocentric variables.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterBoundingBox
    params:
      input_pointcloud_layer: 'raw'
      inside_pointcloud_layer: 'close_points'
      bounding_box_min: [ -10, -10, -1 ]
      bounding_box_max: [ 10, 10, 5 ]

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterBoundingBox

Filter: FilterByIntensity

Description: Thresholds an input cloud by its intensity values, segmenting points into low, mid, and high intensity layers.

Parameters:

  • input_pointcloud_layer (std::string): The input point cloud layer name.

  • output_layer_low_intensity (std::string, optional): Output layer for points with \(\text{intensity} < \text{low\_threshold}\).

  • output_layer_high_intensity (std::string, optional): Output layer for points with \(\text{intensity} > \text{high\_threshold}\).

  • output_layer_mid_intensity (std::string, optional): Output layer for points with \(\text{intensity} \in [\text{low\_threshold}, \text{high\_threshold}]\).

  • low_threshold (float, default: 0.10): The minimum intensity value for the ‘mid’ range.

  • high_threshold (float, default: 0.90): The maximum intensity value for the ‘mid’ range.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterByIntensity
    params:
      input_pointcloud_layer: 'raw'
      output_layer_high_intensity: 'high_i'
      low_threshold: 0.1
      high_threshold: 0.9

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterByIntensity

Filter: FilterByRange

Description: Filters points based on their range (distance) from a specified center point (default is the origin \((0, 0, 0)\)).

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • output_layer_between (std::string, optional): Output layer for points within the \([\text{range\_min}, \text{range\_max}]\) distance range.

  • output_layer_outside (std::string, optional): Output layer for points outside the \([\text{range\_min}, \text{range\_max}]\) distance range.

  • range_min (float, default: 3.0): The minimum distance threshold.

  • range_max (float, default: 90.0): The maximum distance threshold.

  • center (float[3], default: [0, 0, 0]): The center point from which ranges are measured. Can use robot pose variables (e.g., robot_x).

  • metric_l_infinity (bool, default: false): If true, the L-infinity norm (maximum component) is used for distance calculation, which is more efficient than the default L2 Euclidean norm.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: 'raw'
      output_layer_between: 'valid_range'
      range_min: 3.0
      range_max: 90.0

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterByRange

Filter: FilterByRing

Description: Keeps only a given subset of an input cloud based on the LiDAR “ring_id” (assuming the point cloud has ring data).

Parameters:

  • input_pointcloud_layer (std::string): The input point cloud layer name.

  • output_layer_selected (std::string, optional): Output layer for points whose ring ID is in selected_ring_ids.

  • output_layer_non_selected (std::string, optional): Output layer for points whose ring ID is NOT in selected_ring_ids.

  • selected_ring_ids (std::set): A list of ring IDs to keep/select (e.g., [0, 1, 5, 6]).

filters:
  #...
  - class_name: mp2p_icp_filters::FilterByRing
    params:
      input_pointcloud_layer: 'raw'
      output_layer_selected: 'ground_rings'
      selected_ring_ids: [ 0, 1, 15, 16 ]

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterByRing

Filter: FilterCurvature

Description: Classifies a sorted input point cloud (e.g., a single LiDAR scan line) by local curvature, estimated from the angle between a point and its immediate former and posterior neighbors. Useful for edge extraction (LOAM-style).

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name. Must be sorted for meaningful results.

  • output_layer_larger_curvature (std::string, optional): Output layer for points with larger curvature (often “edges”).

  • output_layer_smaller_curvature (std::string, optional): Output layer for points with smaller curvature (often “flatter” surfaces).

  • output_layer_other (std::string, optional): Output layer for points that do not fall into the above two categories.

  • max_cosine (float, default: 0.5f): A threshold related to the angle of the local neighborhood for classifying high curvature.

  • min_clearance (float, default: 0.02f): The minimum distance a neighbor must be to be considered in the curvature calculation (m).

  • max_gap (float, default: 1.00f): The maximum gap distance between a point and its neighbor (m).

filters:
  #...
  - class_name: mp2p_icp_filters::FilterCurvature
    params:
      input_pointcloud_layer: 'raw_sorted'
      output_layer_larger_curvature: 'edges'
      output_layer_smaller_curvature: 'planes'

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterCurvature

Filter: FilterDecimate

Description: Naive point cloud downsampling that reduces the number of points by keeping only one out of every N points. The filter can be configured either with a fixed decimation factor or by specifying a target maximum size (in which case the decimation factor is computed automatically).

If the output layer already exists, new points are accumulated on it; previous contents are not cleared.

Note

This filter is non-deterministic regarding the spatial distribution of points, as it relies purely on the internal storage order. For spatially-aware downsampling, consider using FilterDecimateVoxels or FilterDecimateAdaptive instead.

Parameters:

  • input_layer (std::string, default: raw): The input point cloud layer name.

  • output_layer (std::string): The output layer name for the decimated cloud.

  • decimation (uint32_t, default: 0): Keep one out of every N points. If greater than 0, this parameter takes precedence over target_max_size. For example, decimation=10 keeps every 10th point, resulting in approximately 10% of the original points.

  • target_max_size (uint64_t, default: 0): Target maximum number of points in the output cloud. Used only if decimation is 0. The decimation factor is computed automatically to achieve approximately this many output points.

filters:
  #...
  # Example 1: Fixed decimation factor
  - class_name: mp2p_icp_filters::FilterDecimate
    params:
      input_layer: 'raw'
      output_layer: 'decimated'
      decimation: 5  # Keep every 5th point

  # Example 2: Target maximum size
  - class_name: mp2p_icp_filters::FilterDecimate
    params:
      input_layer: 'raw'
      output_layer: 'decimated'
      target_max_size: 10000  # Aim for ~10k points

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterDecimate

Filter: FilterDecimateAdaptive

Description: Accepts an input point cloud, voxelizes it, and generates a new layer with an adaptive sampling to aim for a specific desired output point count.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • output_pointcloud_layer (std::string): The output layer name for the adaptively decimated cloud.

  • desired_output_point_count (unsigned int, default: 1000): The target number of points in the output cloud.

  • minimum_input_points_per_voxel (unsigned int, default: 1): Voxels with fewer points than this threshold will not generate any output point.

  • voxel_size (float, default: 0.10): The size of the voxel grid used for downsampling (m).

  • parallelization_grain_size (size_t, default: 16384): Grain size for parallel processing of input clouds (used when TBB is enabled).

filters:
  #...
  - class_name: mp2p_icp_filters::FilterDecimateAdaptive
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'adaptively_decimated'
      desired_output_point_count: 5000
      voxel_size: 0.2

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterDecimateAdaptive

Filter: FilterDecimateVoxels

Description: Builds a new layer with a decimated version of one or more input layers using a uniform voxel grid.

Parameters:

  • input_pointcloud_layer (std::vector, default: [raw]): One or more input layers to read and merge points from.

  • error_on_missing_input_layer (bool, default: true): If true, an exception is thrown if an input layer is missing. Otherwise, it’s silently ignored.

  • output_pointcloud_layer (std::string): The output point cloud layer name. New points are appended if the layer already exists.

  • voxel_filter_resolution (float, default: 1.0f): Size of each voxel edge (m).

  • use_tsl_robin_map (bool, default: true): Whether to use tsl::robin_map (faster for smaller clouds) or std::map (faster for large clouds) as the container implementation.

  • minimum_input_points_to_filter (uint32_t, default: 0): If the total number of input points is less than this, all points are passed through without decimation.

  • flatten_to (std::optional): If defined, the 3D points are “flattened” into a 2D planar cloud at a constant height \(z\). Additional point fields (ring, intensity, timestamp) are NOT copied in this mode.

  • decimate_method (DecimateMethod, default: FirstPoint): The method to pick the representative point for each voxel:

    • DecimateMethod::FirstPoint: Picks the first point inserted into the voxel (the fastest method).

    • DecimateMethod::ClosestToAverage: Picks the point closest to the average position of all voxel points.

    • DecimateMethod::VoxelAverage: Calculates and uses the average position of all voxel points (a new point).

    • DecimateMethod::RandomPoint: Picks one of the voxel points at random.

  • minimum_points_per_voxel (uint32_t, default: 0): Minimum number of points in each voxel to use that voxel output. It applies to all decimate_method options, except DecimateMethod::FirstPoint.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: [ 'raw', 'intensity_low' ]
      output_pointcloud_layer: 'decimated'
      voxel_filter_resolution: 0.1
      decimate_method: 'DecimateMethod::VoxelAverage'

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterDecimateVoxels

Filter: FilterDeleteLayer

Description: Completely removes one or more point layers from the metric map.

Parameters:

  • pointcloud_layer_to_remove (std::vector): One or more layers to remove.

  • error_on_missing_input_layer (bool, default: true): If true, an exception is thrown if a layer to be removed does not exist. Otherwise, it’s silently ignored.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: [ 'raw', 'temp_layer' ]
      error_on_missing_input_layer: false

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterDeleteLayer

Filter: FilterDeskew

Description: Deskew (motion compensate) a point cloud from a moving LiDAR.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • output_pointcloud_layer (std::string, optional): The output layer name. Required unless in_place is true.

  • in_place (bool, default: false): If true, the deskewed points replace the input layer (most efficient).

  • output_layer_class (std::string, default: mrpt::maps::CPointsMapXYZI): The class name for the output layer if it needs to be created.

  • silently_ignore_no_timestamps (bool, default: false): If true, no exception is thrown if the input layer lacks timestamps.

  • method (MotionCompensationMethod, default: Linear): The motion compensation method used to interpolate or integrate point positions:

    • MotionCompensationMethod::None — No compensation; all points are assumed to be acquired at the same vehicle pose.

    • MotionCompensationMethod::Linear — Constant velocity (linear and angular) model using the provided twist.

    • MotionCompensationMethod::IMU — Integration using IMU data with constant linear acceleration and angular velocity.

    • MotionCompensationMethod::IMUh — Higher-order IMU integration assuming constant jerk and angular acceleration.

    • MotionCompensationMethod::IMUt — Trapezoidal IMU integration using constant linear acceleration and angular velocity.

  • twist (std::optional): The velocity (linear and angular) of the vehicle in the local frame. Only used when method=Linear. May be dynamically bound via the mp2p_icp::Parameterizable API.

  • points_already_global (bool, default: false): If true, the input points are already expressed in global coordinates (e.g. in sm2mm pipelines). When enabled, robot_pose must be provided to reference the global transformation.

  • robot_pose (mrpt::math::TPose3D, optional): The robot’s pose in global coordinates. Only used when points_already_global=true.

  • bias_acc (mrpt::math::TVector3D, default: [0, 0, 0]): Accelerometer bias in sensor frame coordinates. Used for IMU-based motion compensation methods.

  • bias_gyro (mrpt::math::TVector3D, default: [0, 0, 0]): Gyroscope bias in sensor frame coordinates.

  • gravity_vector (mrpt::math::TVector3D, default: [0, 0, -9.81]): Gravity vector in global frame coordinates. Used when integrating IMU data to maintain alignment with world gravity.

YAML Example

filters:
  # ...
  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'deskewed'
      method: 'MotionCompensationMethod::Linear'   # Normally, use `IMU` if you have an IMU, or `Linear` otherwise
      twist: [vx, vy, vz, wx, wy, wz]
      #bias_acc: [0.00, 0.0, 0.0]
      #bias_gyro: [0.0, 0.0, 0.0]
      #gravity_vector: [0, 0, -9.81]
      # IMPORTANT: In the context of sm2mm, set this to 'true' since
      # 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]

Notes:

  • The input layer must contain timestamps, so it can be either of class mrpt::maps::CPointsMapXYZIRT, or mrpt::maps::CGenericPointsMap with a timestamp field named t.

  • Timestamps are assumed to be in seconds relative to a reference time (e.g., scan start). Use FilterAdjustTimestamps to adjust this if necessary.

  • If timestamps are missing and silently_ignore_no_timestamps=false, an exception is thrown.

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterDeskew

Filter: FilterFartherPointSampling

Description: Subsamples a cloud using the Farther Point Sampling (FPS) algorithm, aiming for a desired number of output points.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • output_pointcloud_layer (std::string): The output layer name for the sampled cloud.

  • desired_output_point_count (unsigned int, default: 1000): The target number of points in the output cloud.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterFartherPointSampling
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'sampled'
      desired_output_point_count: 2000

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterFartherPointSampling

Filter: FilterMerge

Description: Takes a point cloud layer (or a mrpt::maps::CVoxelMap layer) and inserts it into another existing layer of arbitrary metric map type. This uses the target layer’s standard insertObservation() method.

Parameters:

  • input_pointcloud_layer (std::string): The point cloud or map layer to be inserted.

  • target_layer (std::string): The destination layer into which the points will be merged.

  • input_layer_in_local_coordinates (bool, default: false): If true, the input_pointcloud_layer is assumed to be in the vehicle frame and is transformed by robot_pose before insertion.

  • robot_pose (mrpt::math::TPose3D): The pose of the robot/vehicle. Required if input_layer_in_local_coordinates is true.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: 'local_scan'
      target_layer: 'global_map'
      input_layer_in_local_coordinates: true

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterMerge

Filter: FilterMLS

Description: Applies a Moving Least Squares (MLS) surface reconstruction filter to a point cloud for smoothing, normal estimation, and optional upsampling, mimicking the functionality of PCL’s pcl::MovingLeastSquares. The filter computes a weighted polynomial surface in the local neighborhood of each point.

The computed normals are stored as new per-point fields in the output map: normal_x, normal_y, and normal_z.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The point cloud layer used to build the MLS surface (i.e., the source of the local neighborhoods).

  • output_pointcloud_layer (std::string, default: mls): The layer where the smoothed/projected points and their computed normals will be stored.

  • distinct_cloud_layer (std::string, optional): Required if upsampling_method is set to DISTINCT_CLOUD. Points from this layer are projected onto the surface computed from input_pointcloud_layer.

  • search_radius (double, default: 0.05): The search radius (in meters) for finding neighbors to fit the MLS surface.

  • polynomial_order (int, default: 2): The order of the polynomial to fit (e.g., 1 for planar, 2 for quadratic).

  • min_neighbors_for_fit (int, default: 3): The minimum number of neighbors required within the search radius to successfully compute the MLS fit.

  • projection_method (ProjectionMethod, default: SIMPLE): The method used for projecting points onto the fitted surface:

    • SIMPLE: Projects the point along the normal direction onto the polynomial surface.

  • upsampling_method (UpsamplingMethod, default: NONE): The method to determine which points are processed:

    • NONE: The points from input_pointcloud_layer are smoothed/projected.

    • DISTINCT_CLOUD: The points from distinct_cloud_layer are projected onto the surface built from input_pointcloud_layer.

  • parallelization_grain_size (size_t, default: 1024): Grain size for parallel processing of query points when TBB is enabled.

filters:
  #...
  - 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: "raw"

      # 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: 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).
      polynomial_order: 2

      # Minimum number of neighbors required to successfully compute a fit.
      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: 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: NONE

      # Required only if upsampling_method is DISTINCT_CLOUD.
      # distinct_cloud_layer: "sparse_scan"

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

      # If TBB is enabled, this is the grain size for parallel execution.
      parallelization_grain_size: 1024

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterMLS

Filter: FilterNormalizeIntensity

Description: Normalizes the intensity channel of a point cloud layer such that intensity values end up in the range \([0, 1]\). The data is updated in-place in the input/output layer.

Parameters:

  • pointcloud_layer (std::string): The point cloud layer to process.

  • remember_intensity_range (bool, default: false): If true, the filter keeps an internal record of the minimum and maximum intensities observed in past clouds to maintain a consistent normalization.

  • fixed_maximum_intensity (double, default: 0.0): If non-zero, this value is used as the fixed maximum intensity for normalization.

  • fixed_minimum_intensity (double, default: 0.0): If fixed_maximum_intensity is non-zero, this value is used as the fixed minimum intensity for normalization.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterNormalizeIntensity
    params:
      pointcloud_layer: 'raw'
      remember_intensity_range: true

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterNormalizeIntensity

Filter: FilterPoleDetector

Description: Identifies and separates points that appear to belong to a pole or vertical structure. This is done by analyzing the min/max Z-span in 2D grid cells.

Parameters:

  • input_pointcloud_layer (std::string, default: raw): The input point cloud layer name.

  • output_layer_poles (std::string, optional): Output layer name for points that are poles.

  • output_layer_no_poles (std::string, optional): Output layer name for points that are not poles.

  • grid_size (float, default: 2.0f): Size of the 2D grid cell used for analysis (m).

  • minimum_relative_height (float, default: 2.5f): Minimum required height span in a cell to be considered a pole candidate.

  • maximum_relative_height (float, default: 25.0f): Maximum allowed height span in a cell.

  • minimum_points_per_cell (uint32_t, default: 50): Minimum number of points required in a cell for analysis.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterPoleDetector
    params:
      input_pointcloud_layer: 'raw'
      output_layer_poles: 'poles'
      grid_size: 1.0
      minimum_relative_height: 3.0

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterPoleDetector

Filter: FilterRemoveByVoxelOccupancy

Description: Removes points from an input point cloud based on the occupancy status of the corresponding voxels in a separate mrpt::maps::CVoxelMap layer. This is typically used to separate static (high occupancy) and dynamic (low occupancy) objects.

Parameters:

  • input_pointcloud_layer (std::string): The input point cloud to be filtered.

  • input_voxel_layer (std::string): The layer containing the occupancy data (mrpt::maps::CVoxelMap).

  • output_layer_static_objects (std::string, optional): Output layer for points within high-occupancy voxels (“static objects”).

  • output_layer_dynamic_objects (std::string, optional): Output layer for points within low-occupancy voxels (“dynamic objects”).

  • occupancy_threshold (float, default: 0.6f): The occupancy probability threshold. Voxels above this are considered “static”.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterRemoveByVoxelOccupancy
    params:
      input_pointcloud_layer: 'raw'
      input_voxel_layer: 'voxel_map'
      output_layer_static_objects: 'static'
      occupancy_threshold: 0.7

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterRemoveByVoxelOccupancy

Filter: FilterSOR

Description: Statistical Outlier Removal (SOR) filter for point cloud denoising. For each point, it computes the average distance to its k-nearest neighbors. Points are considered outliers if their average distance is greater than (mean + std_dev_multiplier × std_dev) of the entire cloud’s average distances. This filter is effective at removing sparse noise and isolated points while preserving the overall structure of the point cloud.

Parameters:

  • input_pointcloud_layer (std::string, default: "raw"): The name of the point cloud layer to process.

  • output_layer_inliers (std::string, optional): The name of the output layer for points classified as inliers (non-outliers). If empty, inliers are not stored in a separate layer.

  • output_layer_outliers (std::string, optional): The name of the output layer for points classified as outliers. If empty, outliers are not stored in a separate layer.

  • mean_k (unsigned int, default: 20): Number of nearest neighbors to analyze for each point when computing the average distance. Higher values provide more robust statistics but increase computation time.

  • std_dev_mul (double, default: 1.0): Standard deviation multiplier threshold. Points with average distances greater than (mean + std_dev_mul × std_dev) are classified as outliers. Lower values make the filter more aggressive (removes more points), while higher values are more conservative.

  • parallelization_grain_size (size_t, default: 1024): When TBB is enabled, the minimum number of points processed per parallel task. Adjust this value to optimize performance for your hardware and point cloud size.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterSOR
    params:
      input_pointcloud_layer: 'raw'
      output_layer_inliers: 'filtered'
      #output_layer_outliers: 'noise'
      mean_k: 20
      std_dev_mul: 1.0
      parallelization_grain_size: 1024

Usage tips:

  • For dense point clouds with sparse noise (e.g., from LiDAR), start with mean_k=20 and std_dev_mul=1.0.

  • For point clouds with more uniform noise, try increasing std_dev_mul to 2.0 or 3.0 to be more conservative.

  • Adjust mean_k based on local point density: higher values for denser clouds, lower values for sparser ones.

  • If you only need the cleaned point cloud, specify only output_layer_inliers and leave output_layer_outliers empty.


Filter: FilterVoxelSlice

Description: Takes an input layer of type mrpt::maps::CVoxelMap (e.g., Bonxai) and extracts a single 2D slice at a specified Z-range, collapsing the voxel column into an occupancy gridmap.

Parameters:

  • input_voxel_layer (std::string): The input voxel map layer (mrpt::maps::CVoxelMap).

  • output_gridmap_layer (std::string): The output 2D occupancy grid map layer.

  • z_min (double): The minimum Z-coordinate for the slice.

  • z_max (double): The maximum Z-coordinate for the slice.

filters:
  #...
  - class_name: mp2p_icp_filters::FilterVoxelSlice
    params:
      input_voxel_layer: 'voxel_map'
      output_gridmap_layer: '2d_slice'
      z_min: -0.5
      z_max: 0.5

Before → After Screenshot

Screenshot showing point cloud before and after applying FilterVoxelSlice