lidar2d.yaml

# =====================================================================================
# Pipeline: point-to-gridmap 2D ICP
#
# For paper references, see https://github.com/MOLAorg/mola_lidar_odometry/
#
# This file holds parameters for mola::LidarOdometry,
# for use either programmatically calling initialize(), or from a MOLA system
# launch file. See "mola-cli-launchs/*" examples or the main project docs.
# =====================================================================================

params:
  pipeline_name: "point-to-gridmap 2D ICP" # For display/debug only

  # These sensor labels will be handled as LIDAR observations:
  # Can be overridden with cli flag --lidar-sensor-label

  lidar_sensor_labels: ["${MOLA_LIDAR_NAME|lidar}"]
  multiple_lidars:
    lidar_count: ${MOLA_LIDAR_COUNT|1} # useful only if using several lidar_sensor_labels or regex's.
    max_time_offset: ${MOLA_LIDAR_MAX_TIME_OFFSET|25e-3} # [s]

  # These sensor labels will be handled as IMU observations:
  imu_sensor_label: "imu"

  # These sensor labels will be handled as GNSS (GPS) (For storage in simplemap only)
  gnss_sensor_label: "gps"

  # Optionally, drop lidar data too close in time:
  min_time_between_scans: 1e-3 # [seconds]

  # Parameters for max sensor range automatic estimation:
  observation_radius_filter_coefficient: 0.999
  absolute_minimum_observation_radius: 20.0

  # When publishing pose updates, the reference frame for both, estimated robot poses, and the local map.
  publish_reference_frame: "${MOLA_LO_PUBLISH_REF_FRAME|odom}"

  # When publishing pose updates, the vehicle frame name.
  publish_vehicle_frame: "${MOLA_LO_PUBLISH_VEHICLE_FRAME|base_link}"

  # How often to update the local map model:
  local_map_updates:
    enabled: "${MOLA_MAPPING_ENABLED|true}"
    load_existing_local_map: ${MOLA_LOAD_MM|""}
    load_map_after_gui_init: ${MOLA_LO_LOAD_MAP_AFTER_GUI|false}
    min_translation_between_keyframes: "${MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES|0.02*ESTIMATED_OBSERVATION_RADIUS}" # [m]
    min_rotation_between_keyframes: 15.0 # [deg]
    # Should match the "remove farther than" option of the local metric map. "0" means deletion of distant key-frames is disabled
    max_distance_to_keep_keyframes: "max(100.0, 1.50*ESTIMATED_OBSERVATION_RADIUS)" # [m]
    check_for_removal_every_n: 100
    min_nearby_poses_occupied: ${MOLA_MIN_NEARBY_POSES_OCCUPIED|1}

  # Minimum ICP quality to insert it into the map:
  min_icp_goodness: 0.25

  # Adaptive threshold:
  adaptive_threshold:
    enabled: true
    initial_sigma: ${MOLA_SIGMA_INITIAL|0.50} # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.04} # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|2.00} # [m]
    max_sigma_step: ${MOLA_SIGMA_MAX_STEP|0.05} # [m]
    icp_quality_controller_setpoint: ${MOLA_SIGMA_CONTROLLER_QUALITY_SETPOINT|0.85}
    kp: ${MOLA_SIGMA_CONTROLLER_GAIN|2.0}
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}
    # Sustained-failure recovery: if enabled, sigma is grown multiplicatively
    # after a streak of bad ICPs, capped at maximum_sigma, so the matcher
    # window can re-open and ICP can recover. Enabled by default: without it,
    # once sigma is driven down near min_motion by a run of easy/near-static
    # scans (e.g. goodness consistently above icp_quality_controller_setpoint),
    # a single larger inter-scan motion (a turn, a bump, or just ordinary
    # scan-to-scan variability once sigma is already pinned at its floor) can
    # push ICP into failure, and with sigma frozen the correspondence search
    # window never reopens, so the pipeline stalls indefinitely
    # (estimated_trajectory never grows again). Set to false to restore the
    # old behavior. `maximum_sigma` must be set strictly above
    # `initial_sigma`, or this mechanism is a no-op: sigma always starts
    # each run AT initial_sigma, so a bad ICP on frame 1 (e.g. a slightly
    # imprecise localization-only seed pose against a prebuilt map) has no
    # room to grow into and can never recover (observed in testing as ICP
    # goodness stuck just under min_icp_goodness for an entire run).
    #
    # recover_after_n_bad/recover_growth_factor default to a fast reaction
    # (2 bad frames, x2.0 growth) rather than a slow one (5 bad frames,
    # x1.5): every frame spent stuck is a frame of real, untracked vehicle
    # motion accumulating; the slower defaults let that gap grow to the
    # point where, once the search window finally reopens, ICP can lock onto
    # a self-consistent but WRONG registration (observed in testing as a
    # sudden ~30-40 deg yaw error that then persisted for the rest of a run)
    # instead of recovering the true pose.
    recover_on_sustained_failure: ${MOLA_ADAPT_THRESHOLD_RECOVER|true}
    recover_after_n_bad: ${MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD|2}
    recover_growth_factor: ${MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR|2.0}

  # If enabled, a map will be stored in RAM and (if using the CLI) stored
  # to a ".simplemap" file for later use for localization, etc.
  simplemap:
    generate: ${MOLA_GENERATE_SIMPLEMAP|false} # Can be overridden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}
    min_translation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_XYZ|0.05} # m
    min_rotation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_ROT|3.0} # deg

    # Revisiting an already-mapped area creates NO keyframes with the purely
    # spatial criterion above, which starves loop closure of the second endpoint
    # of the loop. Set to a positive value [s] so only keyframes newer than that
    # take part in the "is there one here already?" test.
    nearby_keyframe_time_window: ${MOLA_SIMPLEMAP_KF_TIME_WINDOW|0} # [s], 0=disabled
    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}
    save_gnss_max_age: 1.0 # [s] max age of GNSS observations to keep in the keyframe

  # Save the final trajectory in TUM format. Disabled by default.
  estimated_trajectory:
    save_to_file: ${MOLA_SAVE_TRAJECTORY|false}
    output_file: ${MOLA_TUM_TRAJECTORY_OUTPUT|'estimated_trajectory.txt'}

  # If run within a mola-cli container, and mola_viz is present, use these options
  # to show live progress:
  visualization:
    map_update_decimation: ${MOLA_GUI_MAP_UPDATE_DECIMATION|40}
    show_trajectory: true
    show_current_observation: true # shows "live raw" LiDAR points
    #current_pose_corner_size: 1.5
    #sensor_poses_corner_size: 0.5  # XYZ corner for each LiDAR sensor pose; 0 to disable
    show_current_pose_corner: ${MOLA_LO_SHOW_CURRENT_POSE_CORNER|true} # Set to false to hide the current-pose XYZ corner (e.g. for a first-person camera)
    local_map_point_size: 3
    local_map_render_voxelmap_free_space: ${MOLA_RENDER_VOXELMAP_FREESPACE|true} # for 2D-LIDAR SLAM this may be affordable; for 3D it slows down too much
    model:
      - file: ${MOLA_VEHICLE_MODEL_FILE|""} # Default: none
        tf.roll: 90.0 # deg

  # Profile the main steps of the odometry pipeline:
  pipeline_profiler_enabled: ${MOLA_PROFILER|true}
  # Profile the internal steps of the ICP implementation:
  icp_profiler_enabled: ${MOLA_PROFILER|true}

  # If set to false, the odometry pipeline will ignore incoming observations
  # until active is set to true (e.g. via the GUI).
  start_active: "${MOLA_START_ACTIVE|true}"

  # Optional initial guess for the twist (vx vy vz: m/s, wx wy wz: rad/s):
  initial_twist: ["${MOLA_INITIAL_VX|0.0}", 0.0, 0.0, 0.0, 0.0, 0.0]

# If "icp_settings_without_vel" is not defined here, defaults to be the same than 'icp_settings_with_vel'
# ICP settings can be included from an external YAML file if desired, or defined
# in this same YAML for self-completeness:
# Include example:
#icp_settings_with_vel: $include{./icp-pipeline-default.yaml}

# ICP parameters for a regular time step:
icp_settings_with_vel:
  # mp2p_icp ICP pipeline configuration file, for use in ICP
  # odometry and SLAM packages.
  #
  # YAML configuration file for use with the CLI tool mp2p-icp-run or
  # programmatically from function mp2p_icp::icp_pipeline_from_yaml()
  #
  class_name: mp2p_icp::ICP

  # See: mp2p_icp::Parameter
  params:
    maxIterations: 300
    minAbsStep_trans: 1e-4
    minAbsStep_rot: 5e-5

    #debugPrintIterationProgress: true  # Print iteration progress
    #generateDebugFiles: true  # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1"
    saveIterationDetails: false # Store partial solutions and pairings for each ICP iteration
    decimationIterationDetails: 3
    debugFileNameFormat: "icp-logs/icp-run-${SEQ|NO_SEQ}-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog"
    decimationDebugFiles: ${MP2P_ICP_LOG_FILES_DECIMATION|10}

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        maxIterations: 2
        robustKernel: "RobustKernel::GemanMcClure"
        robustKernelParam: "0.50*ADAPTIVE_THRESHOLD_SIGMA" # [m]  # (adaptive)
        # Blend [0,1] for the robust kernel residual reference toward the prior
        # mean pose (0=current iterate only, 1=prior mean only). See mp2p_icp.
        robustKernelPriorRefBlend: "${MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND|0.0}"
        #innerLoopVerbose: true

  # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
  # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Points_DistanceThreshold
      params:
        threshold: "2.0*ADAPTIVE_THRESHOLD_SIGMA" # [m]
        thresholdAngularDeg: 0 # deg
        pairingsPerPoint: 2
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        pointLayerMatches:
          - { global: "localmap", local: "decimated", weight: 1.0 }

  quality:
    - class: mp2p_icp::QualityEvaluator_PairedRatio
      params: ~ # none required

# Local map updates:
# Very first observation: Use the mp2p_icp pipeline generator to create the local map:
localmap_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "localmap"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: "" # NONE: don't process observations in the generator.
      #process_sensor_labels_regex: '.*'
      # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/localmap_definition_voxelmap.ini'

      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: 0.05 # '$f{max(0.05, min(1.0, 0.005*ESTIMATED_OBSERVATION_RADIUS))}' # [m]
        insertOpts:
          prob_miss: 0.30
          prob_hit: 0.70
          clamp_min: 0.05
          clamp_max: 0.95
          ray_trace_free_space: true
          decimation: 1
          remove_voxels_farther_than: 60 # 0: disabled, otherwise, voxels farther away than this distance in meters will be erased.
        likelihoodOpts:
          occupiedThreshold: 0.60

# ---------------------------------------------------------------------------------
# LIDAR observations are, first, loaded using a generator
# from "observations_generator".
# then, optionally, filtered before being registered with ICP
# against the local map with filter "observations_filter_1st_pass".
# ---------------------------------------------------------------------------------
observations_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: "raw"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ".*"
      process_sensor_labels_regex: ".*"

# Path to an (optional) user-customizable pipeline definition file. Default: empty = none.
observations_prefilter_file: ${MOLA_LO_OBS_PREFILTER_PIPELINE_FILE|""}

observations_filter_1st_pass:
  # Filters:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # input metric_map_t object.
  #
  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: "raw"
      output_pointcloud_layer: "deskewed"
      silently_ignore_no_timestamps: true # To handle more dataset types
      output_layer_class: "mrpt::maps::CPointsMapXYZIRT" # Keep intensity & ring channels

      # 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]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: "deskewed"
      output_pointcloud_layer: "decimated_pre"
      voxel_filter_resolution: 0.05 # [m]
      minimum_input_points_to_filter: 2000 # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

  # Remove points too close, to prevent "noise" from the vehicle,
  # the person next to the robot, etc.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "decimated_pre"
      output_layer_between: "decimated"
      range_min: max(0.10, 0.03*ESTIMATED_OBSERVATION_RADIUS)
      range_max: 1.25*ESTIMATED_OBSERVATION_RADIUS

  # Remove layers to save memory and log file storage
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw", "deskewed", "decimated_pre"]

# To populate the local map, one or more observation layers are merged
# into the local map via this pipeline:
insert_observation_into_local_map:
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: "decimated"
      target_layer: "localmap"
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]