#####################################
#  Real-time LiDAR SLAM parameters  #
#####################################

# SLAM will compute the pose of tracking_frame in odometry_frame coordinates system, using measurements in input pointcloud frames.
odometry_frame: "odom"       # Frame in which SLAM odometry and maps are expressed (default: odom)
tracking_frame: "base_link"  # Frame to track (ensure a valid TF tree is published to link input pointcloud frame_id) (default: base_link)

# Input LiDAR frames topics, expected as sensor_msgs/PointCloud2 messages with the following fields:
# - x, y, z (float): point coordinates
# - time (double): time offset to add to the pointcloud header timestamp to get approximate point-wise acquisition timestamp
# - intensity (float): intensity/reflectivity of the point
# - laser_id (uint16): numeric identifier of the laser ring that shot this point. The lowest/bottom laser ring should be 0, and it should increase upward.
# - device_id (uint8): numeric identifier of the LiDAR device/sensor. This id should be the same for all points of the cloud acquired by the same sensor.
# - label (uint8): optional input, not yet used.
input: "lidar_points"  # single LiDAR input (default: lidar_points)
# input:  # multi LiDAR inputs
#   - "lidar_points_1"  # Main topic: SLAM will be run each time a non-empty frame is received on this topic
#   - "lidar_points_2"  # Secondary topics: non-empty frames will be buffered and added to main frame for multi-LiDAR SLAM.
#   - "lidar_points_3"

# SLAM node outputs
# If set to true, LidarSlamNode will publish the given output to a topic or to the TF server (default to true if not specified).
output:
  registered_points: true   # Publish registered (and undistorted) SLAM pointcloud as LidarPoint PointCloud2 msg to topic 'slam_registered_points'.
  pose:
    odom: true             # Publish SLAM pose as an Odometry msg on 'slam_odom' topic.
    tf: true               # Publish SLAM pose as a TF from 'odometry_frame' to 'tracking_frame'.
    predicted_odom: false  # Publish latency-corrected SLAM pose as an Odometry msg on 'slam_predicted_odom' topic (default: false).
    predicted_tf: false    # Publish latency-corrected SLAM pose as a TF from 'odometry_frame' to '<tracking_frame>_prediction' (default: false).
  maps:
    edges: true            # Publish edges keypoints map as a LidarPoint PointCloud2 msg to topic 'maps/edges'.
    planes: true           # Publish planes keypoints map as a LidarPoint PointCloud2 msg to topic 'maps/planes'.
    blobs: true            # Publish blobs keypoints map as a LidarPoint PointCloud2 msg to topic 'maps/blobs'.
  keypoints:
    edges: true            # Publish extracted edges keypoints from current frame as a PointCloud2 msg to topic 'keypoints/edges'.
    planes: true           # Publish extracted planes keypoints from current frame as a PointCloud2 msg to topic 'keypoints/planes'.
    blobs: true            # Publish extracted blobs keypoints from current frame as a PointCloud2 msg to topic 'keypoints/blobs'.

# Pointclouds saving to PCD files.
# To save keypoints maps, send command SlamCommand::SAVE_KEYPOINTS_MAPS to 'slam_command' topic.
maps_saving:
  pcd_format: 2  # Save with given PCD file format : 0) ascii,  1) binary,  2) binary_compressed.

# Optional GPS position use (if 'gps/use_gps'=true), subscribing to Odometry msg on 'gps_odom' topic.
# It allows to estimate calibration between GPS and SLAM trajectories,
# or post-optimize SLAM trajectory with pose graph optimization (PGO).
gps:

  # Optional GPS positions use to calibrate output SLAM pose to world coordinates and/or optimize SLAM trajectory.
  # If true, subscribes to topic 'gps_odom' (nav_msgs/Odometry), and logs GPS positions during 'slam/logging_timeout' seconds.
  # (WARNING Can be overridden in slam.launch with 'gps' arg).
  use_gps: false

  # Approximate GPS/SLAM calibration (available if use_gps=true)
  # Fit GPS trajectory to SLAM trajectory with ICP to estimate the rigid transform that links 'gps_odom/header/frame_id'
  # to 'odometry_frame', and publish it to static TF server, linking SLAM pose to world coordinates.
  # GPS/SLAM calibration can be triggered by sending SlamCommand::GPS_SLAM_CALIBRATION to 'slam_command' topic.
  calibration:
    no_roll: false            # If true, impose calibration to have no roll rotation. DEBUG
    publish_icp_paths: false  # If true, publish ICP-aligned GPS and SLAM trajectories to 'icp_gps_path' and 'icp_slam_path' latched topics (default: false).

  # Pose Graph Optimization (PGO) (available if use_gps=true)
  # Use GPS positions and covariances to optimize SLAM trajectory and maps by correcting drift,
  # and more precisely link SLAM pose to world coordinates.
  # PGO can be triggered by sending SlamCommand::GPS_SLAM_POSE_GRAPH_OPTIMIZATION to 'slam_command' topic.
  # WARNING : this process is not real-time.
  pgo:
    g2o_file: "/home/user/pgo.g2o"  # Save pose graph to file (or do not save if null string or unset).
    publish_path: false             # If true, publish optimized SLAM trajectory to 'pgo_slam_path' latched topic (default: false).

# SLAM parameters (see Slam.h for description). Comment parameter to get default value.
slam:

  n_threads: 8      # Max number of threads to use for parallel processing (default: 1)
  use_blobs: false  # Use only edge and planar keypoints (do not use blob keypoints)

  # How to estimate Ego-Motion (approximate relative motion since last frame).
  # The ego-motion step aims to give a fast and approximate initialization of new
  # frame world pose to ensure faster and more precise convergence in Localization step.
  #  0) No ego-motion step is performed : relative motion is Identity, new estimated
  #     Tworld is equal to previous Tworld. Fast, but may lead to unstable and imprecise
  #     Localization step if motion is important.
  #  1) Previous motion is linearly extrapolated to estimate new Tworld pose from the
  #     2 previous poses. Fast and precise if motion is roughly constant and continuous
  #  2) Estimate Trelative (and therefore Tworld) by globally registering new frame
  #     on previous frame. Slower and need textured enough environment, but do not
  #     rely on constant motion hypothesis.
  #  3) Previous motion is linearly extrapolated to estimate new Tworld pose from
  #     the 2 previous poses. Then this estimation is refined by globally registering
  #     new frame on previous frame. Slower and need textured enough environment,
  #     but should be more precise and rely less on constant motion hypothesis.
  ego_motion: 1


  # Undistortion mode, to correct rolling shutter distortion during frame acquisition.
  # The undistortion should greatly improve the accuracy for smooth motions,
  # but might be unstable for high-frequency motions.
  #  0) NONE: no undistortion is performed :
  #     - End scan pose is optimized using rigid registration of raw scan and map.
  #     - Raw input scan is added to map.
  #  1) ONCE: undistortion is performed only one using estimated ego-motion:
  #     - Begin and end scan poses are linearly interpolated using estimated ego-motion.
  #     - Scan is linearly undistorted between begin and end scan poses.
  #     - Scan pose is iteratively optimized using rigid registration of undistorted scan and map.
  #     - Undistorted scan is added to map.
  #  2) REFINED: undistortion is iteratively refined using optimized ego-motion:
  #     - Begin and end scan poses are linearly interpolated using ego-motion.
  #     - Scan is linearly undistorted between begin and end scan poses.
  #     - Scan pose is optimized using rigid registration of undistorted scan and map.
  #     - Iterate the three previous steps with updated ego-motion and poses.
  #     - Undistorted scan is added to map.
  undistortion: 2

  # Verbosity level :
  #  0) print errors, warnings or one time info
  #  1) 0 + frame number, total frame processing time
  #  2) 1 + extracted features, used keypoints, localization variance, ego-motion and localization summary
  #  3) 2 + sub-problems processing duration
  #  4) 3 + ceres optimization summary
  #  5) 4 + logging/maps memory usage
  verbosity: 3

  # Optional logging of computed pose, localization covariance and keypoints of each processed frame.
  #  - A value of 0. will disable logging.
  #  - A negative value will log all incoming data, without any timeout.
  #  - A positive value will keep only most recent data, forgetting all previous data older than LoggingTimeout seconds.
  # Logged data will be used for pose graph optimization or GPS antenna/LiDAR sensor calibration using GPS data.
  logging_timeout: 0.  # [s]

  # How to store pointclouds data during keypoints logging (if logging_timeout != 0):
  #  0) PCL pointcloud                    (in RAM,     no compression,      no overhead)
  #  1) Octree compressed binary data     (in RAM,    ~5x compression,   ~3 ms overhead)
  #  2) Ascii format PCD file             (on disk, ~0.6x compression,   ~5 ms overhead)
  #  3) Binary format PCD file            (on disk, ~1.3x compression, ~0.3 ms overhead)
  #  4) Binary compressed format PCD file (on disk, ~1.5x compression, ~0.8 ms overhead)
  logging_storage: 0

  max_distance_for_ICP_matching: 5.     # [m] Max distance between a keypoint from the current frame and its neighborhood from the map to build an ICP match.

  # ICP and LM parameters for Ego-Motion registrayion step (used only if ego_motion is 2 or 3)
  ego_motion_registration:
    ICP_max_iter: 4                     # Max number of iterations of ICP matching.
    LM_max_iter: 15                     # Max number of iteration in the ego motion LM optimization step.
    line_distance_nbr_neighbors: 8      # Number of nearest neighbors to look for in previous frame (all scan lines considered) to build line model.
    minimum_line_neighbor_rejection: 3  # Min number of valid nearest neighbors (max 1 per scan line) to keep neighborhood to build line model.
    line_distance_factor: 5.            # PCA eigenvalues ratio to consider a neighborhood fits a line model (V2 > factor * V1).
    plane_distance_nbr_neighbors: 5     # Number of nearest neighbors to look for in previous frame to build plane model.
    plane_distance_factor1: 35.         # PCA eigenvalues ratio to consider a neighborhood fits a plane model :
    plane_distance_factor2: 8.          #   V2 < factor2 * V1  and  V1 > factor1 * V0
    max_line_distance: 0.2              # [m] Max distance to line model allowed to keep neighborhood.
    max_plane_distance: 0.2             # [m] Max distance to plane model allowed to keep neighborhood.
    init_saturation_distance: 5.        # Initial distance beyon which residuals are saturated using Tukey loss to limit outlier contribution.
    final_saturation_distance: 1.       # Final distance beyon which residuals are saturated using Tukey loss to limit outlier contribution.
  # ICP and LM parameters for Localization step
  localization:
    ICP_max_iter: 3                     # Max number of iterations of ICP matching.
    LM_max_iter: 15                     # Max number of iteration in the ego motion LM optimization step.
    line_distance_nbr_neighbors: 10     # Number of nearest neighbors to look for in map to build line model.
    minimum_line_neighbor_rejection: 4  # Min number of valid nearest neighbors (close to line model) to keep neighborhood to conisder line model.
    line_distance_factor: 5.            # PCA eigenvalues ratio to consider a neighborhood fits a line model (V2 > factor * V1).
    plane_distance_nbr_neighbors: 5     # Number of nearest neighbors to look for in map to build plane model.
    plane_distance_factor1: 35.         # PCA eigenvalues ratio to consider a neighborhood fits a plane model :
    plane_distance_factor2: 8.          #   V2 < factor2 * V1  and  V1 > factor1 * V0
    max_line_distance: 0.2              # [m] Max distance to line model allowed to keep neighborhood.
    max_plane_distance: 0.2             # [m] Max distance to plane model allowed to keep neighborhood.
    init_saturation_distance: 2.        # Initial distance beyon which residuals are saturated using Tukey loss to limit outlier contribution.
    final_saturation_distance: 0.5      # Final distance beyon which residuals are saturated using Tukey loss to limit outlier contribution.

  # Keypoints maps, saved as downsampled voxel grids
  voxel_grid:
    leaf_size_edges: 0.30   # [m] Resolution used to downsample the edges map with a VoxelGrid filter.
    leaf_size_planes: 0.60  # [m] Resolution used to downsample the planes map with a VoxelGrid filter.
    leaf_size_blobs: 0.30   # [m] Resolution used to downsample the blobs map with a VoxelGrid filter.
    size: 50                # [voxels] Size of the voxel grid used to store keypoints maps : n*n*n voxels.
    resolution: 10.         # [m/voxel] Resolution of a voxel.

  # Keypoint extractor for each LiDAR sensor
  ke:
    # Single LiDAR input
    min_distance_to_sensor: 3.         # [m] Minimal point to sensor distance to consider a point as valid.
    neighbor_width: 4                  # [>1] Width of the neighborhood used to compute discrete differential operators.
    plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
    edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
    edge_depth_gap_threshold: 0.15     # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
    edge_saliency_threshold: 1.5       # [m] Threshold upon saliency of a neighborhood to select an edge keypoint.
    edge_intensity_gap_threshold: 50.  # [0-255] Threshold upon intensity gap to select an edge keypoint.

    # # Multi LiDAR inputs
    # # The multiple devices to use for SLAM.
    # # A keypoint extractor will be initialized for each device.
    # device_ids: [0, 10]
    # # Keypoint extractors parameters for each LiDAR sensor "device_<device_id>"
    # device_0:
    #   min_distance_to_sensor: 3.         # [m] Minimal point to sensor distance to consider a point as valid.
    #   neighbor_width: 4                  # [>1] Width of the neighborhood used to compute discrete differential operators.
    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
    #   edge_depth_gap_threshold: 0.15     # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
    #   edge_saliency_threshold: 1.5       # [m] Threshold upon saliency of a neighborhood to select an edge keypoint.
    #   edge_intensity_gap_threshold: 50.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
    # device_10:
    #   min_distance_to_sensor: 1.         # [m] Minimal point to sensor distance to consider a point as valid.
    #   neighbor_width: 4                  # [>1] Width of the neighborhood used to compute discrete differential operators.
    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
    #   edge_depth_gap_threshold: 0.30     # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
    #   edge_saliency_threshold: 1.5       # [m] Threshold upon saliency of a neighborhood to select an edge keypoint.
    #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
