drl_configs.yaml

#####################################################################################################################################################
#####################################################  Dynamic Robot Localization configuration #####################################################
#####################################################################################################################################################

# >>>>>>>>>>>>>>>>>>>>>> General information
#   This file provides the configuration layout for the dynamic_robot_localization ROS node.
#   The parser was designed to detect the presence of the supported processing blocks and load the appropriate configurations.
#   The parameters aren't loaded directly from the yaml file. Instead they are loaded from the ROS parameter server,
# in order to allow other nodes to query the configuration of the localization system.
#   Another advantage of using the parameter server is that the parameters can be inserted directly in the launch file,
# or loaded from one or even several yaml files. This allows to reuse and override parameters across similar configurations.
#   All parameters have default values, which means it is only necessary to specify the ones that are different from the defaults.
#   Nevertheless, the localization pipeline is empty, and the parameters are only queried if the processing block is specified.
#   Some of the configuration blocks are provided by default to allow some consistency across configurations.
#   The following blocks don't need to be specified: [ subscribe_topic_names | publish_topic_names | frame_ids | message_management ]
#   Parameters are by default expressed using ROS units conventions (REP 103 -> http://www.ros.org/reps/rep-0103.html) (unless stated otherwise).
#       - Lengths   -> meters
#       - Angles    -> radians
#       - Time      -> seconds
#       - Frequency -> hz
#       - Axis      -> x - forward | y - left | z - up
#   The ROS parameter server can store strings, doubles, ints and booleans. As such, the convention used is <parameter_name: value|list> and value is:
#       - string    -> '.*'             | ex: 'value'
#       - double    -> [0-9].[0-9]+     | ex: 0.0
#       - int       -> [0-9]+           | ex: 0
#       - boolean   -> [true|false]     | ex: true


# >>>>>>>>>>>>>>>>>>>>>> Reference cloud sources
#   The localization system can load the reference cloud from different sources.
#   The first is from a 2D ROS nav_msgs::OccupancyGrid and will limit the localization system to 3 DoF
#   The other sources allow 6 DoF localization by either loading point clouds from a file (.pcd|.ply|.obj|.stl|.vtk) or
# from a ROS topic with sensor_msgs::PointCloud2 messages (useful when using dynamic map update).
#   To allow fast switching between each source, a overriding methodology is employed.
#   As such, the highest priority is given to reference clouds coming from a file.
#   If a file isn't provided, the ROS topic with sensor_msgs::PointCloud2 is used.
#   If the ROS topic isn't specified, the 2D map is loaded from nav_msgs::OccupancyGrid.


# >>>>>>>>>>>>>>>>>>>>>> Saving reference cloud data
#   To allow fast startup of the localization system, the reference point cloud with / without normals and
# its associated keypoints / descriptors can be loaded / saved from / to files in either binary or text format.


# >>>>>>>>>>>>>>>>>>>>>> Configuration
#   Given that the parameter server stores values sorted by their name in each namespace,
# some configuration blocks allow the addition of pre and postfixes in their name, in order to ensure the desired processing order.
#   It should be noted that some sections only support the specification of one processing block,
# so the prefix can be used to switch between blocks in the yaml configuration file.
#   Also, if a publish topic name is empty, messages will not be created (avoids overhead of message creation and dispatch -> useful for final deployment of the localization system).
#   Bellow is the configuration and default values currently supported by the dynamic_robot_localization system.


# ===================================================================================================================================================
#   PCL point types supported by the localization system pipeline
localization_point_type: 'PointXYZRGBNormal'                                    # PointNormal | PointXYZINormal | PointXYZRGBNormal


# ===================================================================================================================================================
#   These are the topics from which the localization system will receive the reference and ambient point clouds
subscribe_topic_names:
    pose_topic: 'initial_pose'                                                  # geometry_msgs::Pose | Topic to reset localization pose
    pose_stamped_topic: 'initial_pose_stamped'                                  # geometry_msgs::PoseStamped | Topic to reset localization pose
    pose_with_covariance_stamped_topic: '/initialpose'                          # geometry_msgs::PoseWithCovarianceStamped | Topic to reset localization pose (rviz topic name)
    ambient_pointcloud_topic: 'ambient_pointcloud'                              # sensor_msgs::PointCloud2 | Cloud with points coming from sensing devices that will be used to update the localization pose (multiple topics are supported, by merging their names with + -> ex: cloud1+cloud2+cloud3)
    reference_costmap_topic: '/map'                                             # nav_msgs::OccupancyGrid  | Topic providing a 2D reference map
    reference_pointcloud_topic: ''                                              # sensor_msgs::PointCloud2 | Topic providing a 3D reference map -> OctoMap is configured to use topic 'reference_pointcloud_update'


# ===================================================================================================================================================
#   To avoid remaps in the launch file and collision with other nodes topics, the topic names in which the localization system publishes messages can be specified.
#   Also, topics are published latched, which means the last message is buffered and sent to any new subscribers (allows to switch on / off the pointclouds in rviz)
publish_topic_names:
    reference_pointcloud_publish_topic: 'reference_pointcloud'      # sensor_msgs::PointCloud2 | Reference cloud currently being used by the localization system
    publish_aligned_pointcloud_only_if_there_is_subscribers: true
    aligned_pointcloud_publish_topic: 'aligned_pointcloud'          # sensor_msgs::PointCloud2 | Point cloud coming from the ambient_pointcloud_topic after applying the registration correction
    pose_stamped_publish_topic: 'localization_pose'                 # geometry_msgs::PoseStamped | The localization system can publish poses (besides the tf between map and odom) -> (useful to interact with other packages or to visualize in rviz)
    pose_with_covariance_stamped_publish_topic: 'localization_pose_with_covariance' # geometry_msgs::PoseWithCovarianceStamped | The localization system can publish poses (besides the tf between map and odom) -> (useful to interact with other packages, such as amcl)
    pose_with_covariance_stamped_tracking_reset_publish_topic: 'initial_pose_with_covariance'       # geometry_msgs::PoseWithCovarianceStamped | Only published when tracking state is reset (initial pose estimation was performed)
    pose_array_publish_topic: 'localization_initial_pose_estimations' # geometry_msgs::PoseArray | Array with the initial pose estimations. When performing tracking, it will have 0 poses. When tracking is lost, and the initial pose estimation using features succeeds, it will have the accepted poses (of the last initial pose estimation). The next successful traking registrations will not publish a empty message. For that there is the LocalizationDetailed msg
    localization_detailed_publish_topic: 'localization_detailed'    # dynamic_robot_localization::LocalizationDetailed | Provides detailed information of the current pose computed by the localization system (pose + pose_corrections + outlier_percentage + aligmenet_fitness)
    localization_diagnostics_publish_topic: 'diagnostics'           # dynamic_robot_localization::LocalizationDiagnostics | Provides information about the number of points / keypoints in the reference / ambient cloud (before and after filtering) 
    localization_times_publish_topic: 'localization_times'          # dynamic_robot_localization::LocalizationTimes | Provides information about the wall clock times (in milliseconds) of the main localization steps (as well as the global time)


# ===================================================================================================================================================
#   Frame ids required to compute the appropriate world transformations.
#   The localization system publishes a tf correction between map_frame_id and base_link_frame_id
#   The sensor_frame_id is used in the surface normal estimation.
frame_ids:
    map_frame_id: 'map'
    odom_frame_id: 'odom'
    base_link_frame_id: 'base_footprint'
    sensor_frame_id: 'hokuyo_front_laser_link'


# ===================================================================================================================================================
#   Initial pose of the robot [ base_link_frame_id -> map_frame_id ]
#   The orientation can be specified using RPY angles or a quaternion (if both are specified, the quaternion will be used)
initial_pose:
    robot_initial_pose_in_base_to_map: false
    robot_initial_pose_available: true
    position: # Robot position in meters
        x: 0
        y: 0
        z: 0
    orientation_quaternion:
        x: 0
        y: 0
        z: 0
        w: 1
    orientation_rpy: # RPY in radians
        roll: 0
        pitch: 0
        yaw: 0


# ===================================================================================================================================================
#   Parameters to control the incoming message flow
message_management:
    tf_buffer_duration: 600                                            # Duration of TF buffer in seconds
    max_seconds_ambient_pointcloud_age: 3.0                             # Ambient point clouds with age larger than this value will be discarded
    max_seconds_ambient_pointcloud_offset_to_last_estimated_pose: 0.0   # Point clouds that older than this offset in relation to the last [estimated pose / sensor data received] are discarded (useful when there are several sources of sensor data and one has higher update rate -> ex: kinect+lasers)
    min_seconds_between_scan_registration: 0.0                          # Ambient point clouds received before this duration is reached (after a successful pose estimation) will be discarded
    min_seconds_between_reference_pointcloud_update: 5.0                # Clouds coming from topics reference_costmap_topic | reference_pointcloud_topic will be discarded if the last reference cloud was updated less than [this value] seconds ago
    minimum_number_of_points_in_ambient_pointcloud: 10
    maximum_number_points_ambient_pointcloud_circular_buffer: 0         # If != 0, the ambient pointcloud uses a circular buffer with the specified size of points
    localization_detailed_use_millimeters_in_root_mean_square_error_inliers: true
    localization_detailed_use_millimeters_in_translation_corrections: true
    localization_detailed_use_degrees_in_rotation_corrections: true
    localization_detailed_compute_pose_corrections_from_initial_and_final_pose_tfs: true   # If false it will use pointcloud correction matrixes


# ===================================================================================================================================================
#   Full path to the reference clouds
#       -> It is recommend to set these values in the launch file using the ROS $(find package_name)/file_name
#   The reference pointcloud can be updated after each successful scan registration using the full cloud or only the inliers / outliers
#       Integration modes:
#           NoIntegration           -> performs localization only (map can still be updated externally by OctoMap using the map update topic)
#           FullIntegration         -> the full registered cloud is integrated in the reference map
#           InliersIntegration      -> the inliers of the registered cloud are integrated in the reference map
#           OutliersIntegration     -> the outliers of the registered cloud are integrated in the reference map
#   If there are dynamic objects in the scene, OctoMap can be used to performed cloud integration (much more computation intensive)
#       -> The rate of map update from the point cloud topic can be configured with the parameter [message_management/min_seconds_between_reference_pointcloud_update]
#       -> The dynamic objects won't stay in the map due to the raytracing operations marking the cells between the sensor pose and cloud points as free
reference_pointclouds:
    reference_pointcloud_filename: ''
    reference_pointcloud_preprocessed_save_filename: ''
    reference_pointcloud_type: '3D'                                 # Supported modes: [ 2D | 3D ]
    reference_pointcloud_available: true                            # Informs if a reference point cloud (map) will be provided to the self-localization system
    reference_pointcloud_update_mode: 'NoIntegration'               # Supported modes: [ NoIntegration | FullIntegration | InliersIntegration | OutliersIntegration ]
    minimum_number_of_points_in_reference_pointcloud: 10
    use_incremental_map_update: false                               # Incremental SLAM mode will add new registered clouds without preprocessing (if false, it will preprocess the reference cloud after adding the new registered points)
    save_reference_pointclouds_in_binary_format: true


# ===================================================================================================================================================
#   Several filters can be specified for the reference_pointcloud and ambient_pointcloud.
#   Filters are not shared between reference_pointcloud and ambient_pointcloud.
#   Bellow they are specified only in the ambient_pointcloud namespace to avoid repetition.
#   The ambient cloud can be filtered in the original TF frame or after being transformed into the map_frame_id (useful for pass through / crop box filters)
filters:
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    reference_pointcloud:                                           # Filters that will be applied to the reference point cloud
    ambient_pointcloud_integration_filters:                         # Filters that will be applied to the original point cloud (with the registration corrections) when performing pointcloud integration (SLAM)
    ambient_pointcloud_integration_filters_map_frame:               # Filters that will be applied to the original point cloud in the map frame (with the registration corrections) when performing pointcloud integration (SLAM) (outlier detection will be performed in this filtered cloud)
    ambient_pointcloud:                                             # Filters that will be applied to the ambient point cloud in the original TF frame
    ambient_pointcloud_map_frame:                                   # Filters that will be applied after the ambient point cloud is transformed into the map frame (useful to restrict points to a given world region -> ex. map boundaries)
    ambient_pointcloud_feature_registration:                        # Filters that will be applied to the ambient point cloud in the original TF frame when applying feature registration (overrides the ambient_pointcloud filters)
    ambient_pointcloud_map_frame_feature_registration:              # Filters that will be applied after the ambient point cloud is transformed into the map frame (overrides the ambient_pointcloud_map_frame filters)
        pass_through:                                               # Allows prefix and postfix of letters to ensure parsing order
            field_name: 'z'                                         # Field name -> [ x | y | z ]
            min_value: -5.0
            max_value: 5.0
            filtered_cloud_publish_topic: ''
        voxel_grid:                                                 # Allows prefix and postfix of letters to ensure parsing order
            leaf_size_x: 0.01
            leaf_size_y: 0.01
            leaf_size_z: 0.01
            filter_limit_field_name: 'z'                            # Field name -> [ x | y | z ]
            filter_limit_min: -5.0
            filter_limit_max: 5.0
            downsample_all_data: false                              # Set to true if all fields need to be downsampled, or false if just XYZ
            save_leaf_layout: false                                 # Set to true if leaf layout information needs to be saved for later access
            filtered_cloud_publish_topic: ''
        approximate_voxel_grid:                                     # Allows prefix and postfix of letters to ensure parsing order
            leaf_size_x: 0.01
            leaf_size_y: 0.01
            leaf_size_z: 0.01
            filtered_cloud_publish_topic: ''
            downsample_all_data: false                              # Set to true if all fields need to be downsampled, or false if just XYZ
        radius_outlier_removal:                                     # Allows prefix and postfix of letters to ensure parsing order
            radius_search: 0.5
            min_neighbors_in_radius: 1
            invert_removal: false
            filtered_cloud_publish_topic: ''
        crop_box:                                                   # Allows prefix and postfix of letters to ensure parsing order
            box_min_x: -10.0
            box_min_y: -10.0
            box_min_z: -10.0
            box_max_x: 10.0
            box_max_y: 10.0
            box_max_z: 10.0
            box_translation_x: 0.0
            box_translation_y: 0.0
            box_translation_z: 0.0
            box_rotation_roll: 0.0
            box_rotation_pitch: 0.0
            box_rotation_yaw: 0.0
            invert_selection: false                                 # If false -> selects points outside the box
            filtered_cloud_publish_topic: ''
        random_sample:                                              # Allows prefix and postfix of letters to ensure parsing order
            number_of_random_samples: 250
            invert_sampling: false                                  # If false = cloud_size - number_of_random_samples
            filtered_cloud_publish_topic: ''
        statistical_outlier_removal:
            number_of_neighbors_for_mean_distance_estimation: 4     # The number of points to use for mean distance estimation
            standard_deviation_multiplier: 1.0                      # The distance threshold will be equal to: mean + stddev_mult * stddev. Points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively.
            invert_selection: false                                 # Selects outliers instead
            filtered_cloud_publish_topic: ''
        covariance_sampling:
            number_of_samples: 250
            filtered_cloud_publish_topic: ''


# ===================================================================================================================================================
#   One normal estimator can be specified for the reference_pointcloud and another for the ambient_pointcloud.
#   The reference_pointcloud and ambient_pointcloud have the same parameters (they were omitted in the reference_pointcloud to avoid repetition).
#   The normal estimator is not shared because the reference and ambient pointcloud can have different point density, and as such, may require the normal estimator to have different parameters values.
#   Moreover, it may be useful to use more robust normal estimators in the ambient point cloud (such as moving_least_squares), and faster estimators in the reference cloud.
#   Bellow is an example of parameter override, in which the display_normals parameter in the normal_estimators/ambient_pointcloud/ namespace is overridden
# by the same parameter in the normal_estimators/ambient_pointcloud/normal_estimation_omp child namespace.
normal_estimators:
    reference_pointcloud:
        flip_normals_using_occupancy_grid_analysis: true            # Only used in the reference pointcloud
        use_filtered_cloud_as_normal_estimation_surface: true       # Normals can be estimated with the filtered cloud or with the original reference cloud as search surface
    ambient_pointcloud:
        compute_normals_when_tracking_pose: false                   # Some registration algorithms don't require normals. Only activate its calculation when required
        compute_normals_when_recovering_pose_tracking: false        # Some registration algorithms don't require normals. Only activate its calculation when required
        compute_normals_when_estimating_initial_pose: true          # Some registration algorithms don't require normals. Only activate its calculation when required
        use_filtered_cloud_as_normal_estimation_surface: true       # Normals can be estimated with the filtered cloud or with the original ambient cloud as search surface
        display_normals: false                                      # Can be overridden in child namespaces | Displays a blocking window with the estimated normals
        # If the normals are being computed from a nav_msgs::OccupancyGrid, their orientation can be corrected by flipping them to the side that has more empty space (instead of flipping to the sensor view) (to disable, set k to 0 and both radius to < 0)
        display_occupancy_grid_pointcloud: false                    # Can be overridden in child namespaces | Displays a blocking window with the pointcloud created from the nav_msgs::OccupancyGrid
        occupancy_grid_analysis_k: 0                                # Can be overridden in child namespaces | The number of neighbors to use when fliping normals (<= 0 -> ignore k, > 0 -> will be used instead of the radius specified in the parameters below)
        occupancy_grid_analysis_radius: -1.0                        # Can be overridden in child namespaces | The distance radius to use when flipping normals (< 0 -> ignore radius, will not be used if occupancy_grid_analysis_radius_resolution_percentage > 0)
        occupancy_grid_analysis_radius_resolution_percentage: 4.0   # Can be overridden in child namespaces | The distance radius to use when flipping normals in relation to the nav_msgs::OccupancyGrid resolution (radius = map_resolution * occupancy_grid_analysis_radius_resolution_percentage) (< 0 -> ignore this radius) 
        normal_estimator_sac:                                       # Allows prefix and postfix of letters to ensure parsing order | Estimates the normal of points by fitting either a plane or a line in the neighborhood of each point using Sample Consensus methods
            model_type: 'SACMODEL_LINE'                             # The type of geometry model to use. Supported geometry types: SACMODEL_LINE -> for planar pointclouds | SACMODEL_PLANE -> for 3D pointclouds
            method_type: 'SAC_RANSAC'                               # The type of sample consensus method to use. Supported methods: SAC_RANSAC | SAC_LMEDS | SAC_MSAC | SAC_RRANSAC | SAC_RMSAC | SAC_MLESAC | SAC_PROSAC
            inlier_distance_threshold: 0.025                        # Points with a distance to the estimated model less than this threshold will be considered inliers
            max_iterations: 50                                      # The maximum number of iterations that the sample consensus method will run
            probability_of_sample_without_outliers: 0.99            # Probability of choosing at least one sample free from outliers
            optimize_coefficients: true                             # true for enabling model coefficient refinement
            min_model_radius: -1.0                                  # Minimum expected model radius (< 0 will ignore model radius constraints)
            max_model_radius: -1.0                                  # Maximum expected model radius (< 0 will ignore model radius constraints)
            random_samples_max_k: 5                                 # The number of k nearest neighbors to use for the normal estimation. If search_k != 0 search_radius is ignored
            random_samples_max_radius: 0.05                         # The sphere radius that will be used to find the nearest neighbors used for the normal estimation
            minimum_inliers_percentage: 0.5                         # Minimum inliers percentage [0-1] to accept a model given by the SAC estimation
        normal_estimation_omp:                                      # Allows prefix and postfix of letters to ensure parsing order | Estimates the normal and curvature of points by performing Principal Component Analysis
            display_normals: true                                   # Overrides parameter in parent namespace
            search_k: 0                                             # The number of k nearest neighbors to use for the normal estimation. If search_k != 0 search_radius is ignored
            search_radius: 0.12                                     # The sphere radius that will be used to find the nearest neighbors used for the normal estimation
        moving_least_squares:                                       # Allows prefix and postfix of letters to ensure parsing order | Estimates the normal and curvature of points by performing surface fitting and resampling
            compute_normals: true                                   # Set whether the algorithm should also store the normals computed
            polynomial_order: 2                                     # The order of the polynomial to be fit
            polynomial_fit: true                                    # Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation
            search_radius: 0.12                                     # The sphere radius that is to be used for determining the k-nearest neighbors used for surface fitting
            sqr_gauss_param: 0.0144                                 # Set the parameter used for distance based weighting of neighbors (the square of the search radius works best in general)
            upsample_method: 'NONE'                                 # The upsampling method to be used ( NONE | DISTINCT_CLOUD | SAMPLE_LOCAL_PLANE | RANDOM_UNIFORM_DENSITY | VOXEL_GRID_DILATION )
            upsampling_radius: 0.05                                 # The radius of the circle in the local point plane that will be sampled (used only in the case of SAMPLE_LOCAL_PLANE upsampling)
            upsampling_step: 0.01                                   # The step size for the local plane sampling (used only in the case of SAMPLE_LOCAL_PLANE upsampling)
            desired_num_points_in_radius: 5                         # The parameter that specifies the desired number of points within the search radius (used only in the case of RANDOM_UNIFORM_DENSITY upsampling)
            dilation_voxel_size: 0.01                               # The voxel size for the voxel grid (used only in the VOXEL_GRID_DILATION upsampling method)
            dilation_iterations: 1                                  # The number of dilation steps of the voxel grid (used only in the VOXEL_GRID_DILATION upsampling method)


curvature_estimators:
    reference_pointcloud:
    ambient_pointcloud:
        principal_curvatures_estimation:
            search_k: 0                                                 # The number of k nearest neighbors to use for the curvature estimation. If search_k != 0 search_radius is ignored
            search_radius: 0.12                                         # The sphere radius that will be used to find the nearest neighbors used for the curvature estimation
            curvature_type: 'CURVATURE_TYPE_MEAN'                       # Type of curvature to compute from the Principal Curvature estimation k1 and k2 eigen values | Supported types: CURVATURE_TYPE_MEAN |CURVATURE_TYPE_GAUSSIAN
            update_normals_with_principal_component_directions: false   # If true, it will update the normals with the computed principal directions


# ===================================================================================================================================================
#   Several keypoint detectors can be specified for the reference_pointcloud and for the ambient_pointcloud.
#   They were split in reference / ambient to allow parameter tuning to specific point cloud density and point distribution.
#   However, they should be the same algorithms in both clouds. ===> This is critical, if the same keypoints are not found in both clouds
# the descriptor matching will fail.
keypoint_detectors:
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    reference_pointcloud:
        reference_pointcloud_keypoints_filename: ''
        reference_pointcloud_keypoints_save_filename: ''
    ambient_pointcloud:
        compute_keypoints_when_tracking_pose: false                 # Keypoints can take a long time to compute and usually require the computation of normals. Only activate its calculation if required
        compute_keypoints_when_recovering_pose_tracking: false      # Keypoints can take a long time to compute and usually require the computation of normals. Only activate its calculation if required
        compute_keypoints_when_estimating_initial_pose: true        # Keypoints can take a long time to compute and usually require the computation of normals. Only activate its calculation if required
        intrinsic_shape_signature_3d:                               # Allows prefix and postfix of letters to ensure parsing order
            salient_radius: 0.06                                    # The radius of the spherical neighborhood used to compute the scatter matrix (usually 6 * model_resolution)
            non_max_radius: 0.04                                    # The radius for the application of the non maxima supression algorithm (usually 4 * model_resolution)
            normal_radius: 0.04                                     # The radius used for the estimation of the surface normals of the input cloud. If the radius is too large, the temporal performances of the detector may degrade significantly (usually 4 * model_resolution)
            border_radius: 0.0                                      # The radius used for the estimation of the boundary points. If the radius is too large, the temporal performances of the detector may degrade significantly (0 disables the border estimation) (usually 1 * model_resolution)
            threshold21: 0.975                                      # The upper bound on the ratio between the second and the first eigenvalue
            threshold32: 0.975                                      # The upper bound on the ratio between the third and the second eigenvalue
            min_neighbors: 5                                        # The minimum number of neighbors that has to be found while applying the non maxima suppression algorithm
            angle_threshold: 1.57                                   # The decision boundary (angle threshold) that marks points as boundary or regular. (default pi / 2.0)
            iss3d_keypoints_cloud_publish_topic: ''                 # Topic where the detected keypoints will be published (sugestion ambient_keypoints)
        sift_3d:                                                    # Allows prefix and postfix of letters to ensure parsing order
            min_scale: 0.01                                         # The standard deviation of the smallest scale in the scale space
            number_octaves: 3                                       # The number of octaves (i.e. doublings of scale) to compute
            number_scales_per_octave: 4                             # The number of scales to compute within each octave
            min_contrast: 0.001                                     # The minimum contrast required for detection
            sift3d_keypoints_cloud_publish_topic: ''                # Topic where the detected keypoints will be published (sugestion ambient_keypoints)


# ===================================================================================================================================================
#   Several tracking matchers can be used to perform high accuracy pose estimation.
#   There are two main categories of cloud matchers. One that registers clouds of points and another that matches point descriptors.
#   They share some parameters that are present in the tracking_matchers/ namespace, and that can be overridden in the child namespaces.
#   An example of parameter override (max_number_of_registration_iterations) is present in namespace tracking_matchers/point_matchers/iterative_closest_point
#   Point matchers are very fast algorithms that can be used for pose tracking. They can work with points only or also with surface normals. However, they require
# an initial pose close to the real world pose, otherwise they wont be able to perform the cloud registration.
#   Feature matchers are much slower, because they require feature detection, description and descriptor matching, and should only be used to estimate the initial pose (or when the odometry is very unreliable)
# in order to allow the point matchers to perform tracking.
tracking_matchers:
    ignore_height_corrections: false                                # When activated, the pose corrections in the z axis will be ignored
    use_internal_tracking: true                                     # If true it will store the transform [map_frame_id -> odom_frame_id] internally, otherwise it will query TF
    pose_tracking_timeout: 30.0                                     # When point cloud registration has failed during this amount of time (seconds), the initial pose recovery algorithms will be activated
    pose_tracking_minimum_number_of_failed_registrations_since_last_valid_pose: 25  # Initial pose estimation will be activated if the registration has failed at least [this number] and the pose_tracking_timeout has been reached
    pose_tracking_maximum_number_of_failed_registrations_since_last_valid_pose: 50 # When cloud registration fails for more than [this number], the initial pose recovery algorithms will be activated
    pose_tracking_recovery_timeout: 0.5                             # When point cloud registration has failed during this amount of time (seconds), the pose tracking recovery algorithms will be activated
    pose_tracking_recovery_minimum_number_of_failed_registrations_since_last_valid_pose: 3  # Pose tracking recovery will be activated if the registration has failed at least [this number] and the pose_tracking_recovery_timeout has been reached
    pose_tracking_recovery_maximum_number_of_failed_registrations_since_last_valid_pose: 5 # When cloud registration fails for more than [this number], the pose tracking recovery algorithms will be activated
    max_correspondence_distance: 0.1                                # Can be overridden in child namespaces | The maximum distance threshold between two correspondent points in source <-> target. If the distance is larger than this threshold, the points will be ignored in the alignment process
    correspondence_estimation_approach: ''                          # Can be overridden in child namespaces | If not specified it will not change the correspondence estimator | [ CorrespondenceEstimation | CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting | CorrespondenceEstimationOrganizedProjection ]
    correspondence_estimation_k: 10                                 # Can be overridden in child namespaces | Only used if -> correspondence_estimation_approach: [ CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting ]
    correspondence_estimation_organized_projection:                 # Can be overridden in child namespaces | Only used if -> correspondence_estimation_approach: CorrespondenceEstimationOrganizedProjection
      fx: 525.0
      fy: 525.0
      cx: 319.5
      cy: 239.5
    transformation_estimation_approach: ''                          # Can be overridden in child namespaces | If not specified it will not change the transformation estimator | [ TransformationEstimation2D | TransformationEstimationDualQuaternion | TransformationEstimationLM | TransformationEstimationPointToPlane | TransformationEstimationPointToPlaneLLS | TransformationEstimationPointToPlaneLLSWeighted | TransformationEstimationPointToPlaneWeighted | TransformationEstimationSVD | TransformationEstimationSVDScale ]
    last_pose_weighted_mean_filter: -1.0                            # Valid values are in range ]0, 1[. The filtered pose is computed using linear interpolation (using the last and current estimated pose as the two interpolating extremes). Values close to 0 result in a final pose closer to the last pose. Values close to 1 result in a final pose close to the current estimated pose (based on the sensor data).
    transformation_epsilon: 1e-8                                    # Can be overridden in child namespaces | The transformation epsilon (maximum allowable difference between two consecutive transformations -> TranslationThreshold) in order for an optimization to be considered as having converged to the final solution (translation threshold squared)
    euclidean_fitness_epsilon: 1e-6                                 # Can be overridden in child namespaces | The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged. The error is estimated as the sum of the differences between correspondences in an Euclidean sense, divided by the number of correspondences (Relative Mean Square Error) -> (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_
    max_number_of_registration_iterations: 250                      # Can be overridden in child namespaces | The maximum number of iterations the internal optimization should run for
    max_number_of_ransac_iterations: 50                             # Can be overridden in child namespaces | The number of iterations RANSAC should run for
    ransac_outlier_rejection_threshold: 0.05                        # Can be overridden in child namespaces | The matcher considers a point to be an inlier, if the distance between the target data index and the transformed source index is smaller than the given inlier distance threshold
    match_only_keypoints: false                                     # Can be overridden in child namespaces | Uses only keypoints in the coud registration (if available)
    display_cloud_aligment: false                                   # Can be overridden in child namespaces | Display cloud aligment in a separate window
    maximum_number_of_displayed_correspondences: 0                  # Can be overridden in child namespaces | How many correspondances of the registration will be displayed
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    feature_matchers:                                               # Feature matchers are applied before point matchers.
        display_feature_matching: false                             # Can be overridden in child namespaces | Display feature matching registration
        #   One keypoint descriptor can be specified for both the reference and ambient point clouds.
        #   It must be the same descriptor algorithm in order to allow the matching of the generated descriptors.
        reference_pointcloud_descriptors_filename: ''               # Can be overridden in child namespaces of matchers/
        reference_pointcloud_descriptors_save_filename: ''          # Can be overridden in child namespaces of matchers/
        save_descriptors_in_binary_format: true                     # Can be overridden in child namespaces of matchers/
        keypoint_descriptors:
            #   feature_descriptor_k_search has higher priority than feature_descriptor_radius_search
            #   As such, if feature_descriptor_k_search > 0 then feature_descriptor_radius_search = 0.0 (will be ignored)
            feature_descriptor_k_search: 0                          # Can be overridden in child namespaces | The number of k nearest neighbors to use for the descriptor estimation
            feature_descriptor_radius_search: 0.2                   # Can be overridden in child namespaces | The sphere radius that is to be used for determining the nearest neighbors used for the descriptor estimation
            fpfh:                                                   # Allows prefix and postfix of letters to ensure parsing order
                number_subdivisions_f1: 11                          # The number of subdivisions for each angular feature interval
                number_subdivisions_f2: 11                          # The number of subdivisions for each angular feature interval
                number_subdivisions_f3: 11                          # The number of subdivisions for each angular feature interval
            pfh:                                                    # Allows prefix and postfix of letters to ensure parsing order
                use_internal_cache: true                            # Whether to use an internal cache mechanism for removing redundant calculations or not
                maximum_cache_size: 33554432                        # The maximum internal cache size. Defaults to 2GB worth of entries
            shot:                                                   # Allows prefix and postfix of letters to ensure parsing order
                lrf_radius: 0.05                                    # The radius used for local reference frame estimation
            shape_context_3d:                                       # Allows prefix and postfix of letters to ensure parsing order
                minimal_radius: 0.2                                 # Minimal radius value for the search sphere
                point_density_radius: 0.4                           # This radius is used to compute local point density density = number of points within this radius
            unique_shape_context:                                   # Allows prefix and postfix of letters to ensure parsing order
                minimal_radius: 0.1                                 # Minimal radius value for the search sphere
                point_density_radius: 0.2                           # This radius is used to compute local point density density = number of points within this radius
                local_radius: 2.5                                   # The radius to compute the Local Reference Frame
            spin_image:                                             # Allows prefix and postfix of letters to ensure parsing order
                image_width: 8                                      # The resolution of the spin image (the number of bins along one dimension)
                support_angle_cos: 0.0                              # The maximum angle for the point normal to get to support region
                min_point_count_in_neighbourhood: 0                 # Minimal points count for spin image computation
                use_angular_domain: false                           # Angular spin-image differs from the vanilla one in the way that not the points are collected in the bins but the angles between their normals and the normal to the reference point
                use_radial_structure: false                         # Instead of rectangular coordinate system for reference frame polar coordinates are used. Binning is done depending on the distance and inclination angle from the reference point
            esf:                                                    # Allows prefix and postfix of letters to ensure parsing order
                feature_descriptor_radius_search: 0.25              # Overrides parameter in parent namespace
        #   Several feature matchers can be specified. This allows the use of preprocessing matchers for filtering the point descriptors that will be used on matchers later on.
        matchers:
            registered_cloud_publish_topic: ''                              # Can be overridden in child namespaces
            sample_consensus_initial_alignment_prerejective:        # Allows prefix and postfix of letters to ensure parsing order
                convergence_time_limit_seconds: -1.0                # Allows to define a time limit for the point clod registration (if < 0.0 no time limit is applied)
                similarity_threshold: 0.8                           # The similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object, where 1 is a perfect match
                inlier_fraction: 0.25                               # Required inlier fraction, must be in [0,1]
                inlier_rmse: 0.2                                    # Maximum inlier root mean square error
                number_of_samples: 3                                # Set the number of samples to use during each iteration
                correspondence_randomness: 10                       # The number of neighbors to use when selecting a random feature correspondence. A higher value will add more randomness to the feature matching
            sample_consensus_initial_alignment:                     # Allows prefix and postfix of letters to ensure parsing order
                convergence_time_limit_seconds: -1.0                # Allows to define a time limit for the point clod registration (if < 0.0 no time limit is applied)
                min_sample_distance: 1.0                            # The minimum distances between samples
                number_of_samples: 3                                # The number of samples to use during each iteration
                correspondence_randomness: 10                       # The number of neighbors to use when selecting a random feature correspondence
    #   Several point matchers can be specified. This is useful to have a fast matcher that can achieve a rough registration and other matchers to refine it.
    point_matchers:
        registered_cloud_publish_topic: ''                          # Can be overridden in child namespaces
        iterative_closest_point:                                    # Allows prefix and postfix of letters to ensure parsing order
        iterative_closest_point_2d:                                 # Allows prefix and postfix of letters to ensure parsing order
        iterative_closest_point_non_linear:                         # Allows prefix and postfix of letters to ensure parsing order
        iterative_closest_point_with_normals:                       # Allows prefix and postfix of letters to ensure parsing order | Cannot be used for 3 DoF because the PCL implementation of pcl::registration::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation will produce a 6x6 matrix that cannot be inverted (singular), and will result in a transformation estimation with NaNs
            # the next set of parameters applies to the 4 icp algorithms mentioned above (iterative_closest_point | iterative_closest_point_2d | iterative_closest_point_non_linear | iterative_closest_point_with_normals)
            convergence_absolute_mse_threshold: 1e-12               # (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)
            convergence_rotation_threshold: -1337.0                 # Only applied if higher than -1337.0 | cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1) || cos_angle >= rotation_threshold
            convergence_max_iterations_similar_transforms: 0        # The maximum number of iterations that the internal rotation, translation, and MSE differences are allowed to be similar
            convergence_time_limit_seconds: -1.0                    # Allows to define a time limit for the point clod registration (if < 0.0 no time limit is applied, if > 0 this value will be the maximum time limit, even when using the percentage of the mean convergence time)
            convergence_time_limit_seconds_as_mean_convergence_time_percentage: 3.0                 # Allows to update the convergence time limit value based on the percentage of the mean convergence time [1 -> 100%] (if < 0, the time limit isn't updated)
            minimum_number_of_convergence_time_measurements_to_adjust_convergence_time_limit: 25    # Minimum number of convergence time measurements required to update the convergence time limit value
            use_reciprocal_correspondences: false
            max_number_of_registration_iterations: 100              # Overrides parameter in parent namespace
        iterative_closest_point_generalized:                        # Allows prefix and postfix of letters to ensure parsing order
            use_reciprocal_correspondences: false
            rotation_epsilon: 0.002                                 # The rotation epsilon (maximum allowable difference between two consecutive rotations) in order for an optimization to be considered as having converged to the final solution
            correspondence_randomness: 20                           # The number of neighbors used when selecting a point neighborhood to compute covariances
            maximum_optimizer_iterations: 20                        # Number of iterations at the optimization step
        normal_distributions_transform_2d:                          # Allows prefix and postfix of letters to ensure parsing order
            grid_center_x: 0.0                                      # X center of the ndt grid (target coordinate system)
            grid_center_y: 0.0                                      # Y center of the ndt grid (target coordinate system)
            grid_step_x: 1.0                                        # Grid spacing (x step) of the NDT grid
            grid_step_y: 1.0                                        # Grid spacing (y step) of the NDT grid
            grid_extent_x: 20.0                                     # NDT Grid extent (in x direction from the grid center)
            grid_extent_y: 20.0                                     # NDT Grid extent (in y direction from the grid center)
            grid_optimization_step_size_x: 1.0                      # Lambda x step size: 1 is simple newton optimization, smaller values may improve convergence
            grid_optimization_step_size_y: 1.0                      # Lambda y step size: 1 is simple newton optimization, smaller values may improve convergence
            grid_optimization_step_size_theta: 1.0                  # Lambda theta  size: 1 is simple newton optimization, smaller values may improve convergence
        normal_distributions_transform_3d:                          # Allows prefix and postfix of letters to ensure parsing order
            voxel_grid_resolution: 1.0                              # Resolution side length of voxels
            line_search_step_size: 0.1                              # The newton line search maximum step length
            outlier_ratio: 0.55                                     # Point cloud outlier ratio


#   Several recovery matchers can be specified, and will be applied if the cloud registration specified above fails or if the registration is rejected by the transformation validators specified below.
#   This is useful to tune the cloud registration to the environment and robot operation (above), and also have a recovery configuration with more robust / computation expensive setup for anomalous operation situations.
tracking_recovery_matchers:          # Any of the feature / point matchers shown above can be used (same configuration layout). Allows prefix and postfix of letters to ensure parsing order inside each type of matcher.
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    feature_matchers:       # Feature matchers are applied before point matchers.
        registered_cloud_publish_topic: ''                              # Can be overridden in child namespaces
    point_matchers:
        registered_cloud_publish_topic: ''                              # Can be overridden in child namespaces


#    Given that the initial pose estimation can have a very different set of algorithms and parameters (in relation to tracking), the localization system will rely on the configuration under initial_pose_estimators_matchers/ namespace
# for initial pose estiamtion when the tracking has failed to recover for over tracking_matchers/pose_tracking_timeout seconds.
initial_pose_estimators_matchers:   # Any of the feature / point matchers shown above can be used (same configuration layout). Allows prefix and postfix of letters to ensure parsing order inside each type of matcher.
    publish_pointclouds_only_if_there_is_subscribers: true              # Can be overridden in child namespaces
    initial_pose_estimation_timeout: 600.0                              # Maximum time (seconds) that the initial pose estimation systems are allowed to try to find the initial pose of the robot (useful when the robot starts in an unkown section of the map and it is undesirable to try to find the robot pose indefinitely)
    feature_matchers:               # Feature matchers are applied before point matchers.
        registered_cloud_publish_topic: ''                              # Can be overridden in child namespaces
    point_matchers:
        registered_cloud_publish_topic: ''                              # Can be overridden in child namespaces



# ===================================================================================================================================================
#   Several outlier detectors can be used. This processing step is applied after cloud registration, and will only be used to validate the registration in the transformation_validators stage.
#   It is also useful to perform dynamic map update, since the outliers pointcloud can be published and used in OctoMap.
outlier_detectors:
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    euclidean_outlier_detector:                                     # Allows prefix and postfix of letters to ensure parsing order
        max_inliers_distance: 0.01                                  # A point in the ambient cloud will be considered outlier if it does't have a point in the reference cloud within a sphere with this radius
        aligned_pointcloud_outliers_publish_topic: ''               # Pointcloud topic for the registered outliers. OctoMap is configured to use aligned_pointcloud_outliers topic. If empty, messages will not be dispatched
        aligned_pointcloud_inliers_publish_topic: ''                # Pointcloud topic for the registered inliers. If empty, messages will not be dispatched


# ===================================================================================================================================================
#   The inliers and outliers angular point distribution can be computed using one of the supported methods.
#   It is useful for supervisers to analyze the confidence of the localization estimate.
cloud_analyzers:
    compute_inliers_angular_distribution: true
    compute_outliers_angular_distribution: true
    angular_distribution_analyzer:
        number_of_angular_bins: 180                                 # Angular bins all around the robot (360º)


# ===================================================================================================================================================
#   The covariance matrix of the registration can be computed using a point-to-point or point-to-plane approach
registration_covariance_estimator:
    error_metric: 'PointToPoint3D'                                  # PointToPoint3D | PointToPlane3D | PointToPointPM3D | PointToPlanePM3D
    correspondance_estimator: 'CorrespondenceEstimation'            # CorrespondenceEstimation | CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting
    correspondence_distance_threshold: 0.05                         # Maximum distance between point correspondences
    sensor_std_dev_noise: 0.01                                      # The mean noise expected in the sensor readings
    use_reciprocal_correspondences: false
    filtered_reference_cloud_publish_topic: ''
    filtered_ambient_cloud_publish_topic: ''
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    random_sample:                                                  # Allows prefix and postfix of letters to ensure parsing order
        number_of_random_samples: 250
        invert_sampling: false                                      # If false = cloud_size - number_of_random_samples
        filtered_cloud_publish_topic: ''



# ===================================================================================================================================================
#   Several transformation validators can be employed. They can be used to refine or reject a new pose estimation.
#   A new pose estimation is rejected if one transformation validator doesn't accept the new pose. As such, the most restrictive validators should be applied first.
#   Two separate groups of validators can be specified:
#     - The <transformation_validators> are applied when the registration matchers perform as expected.
#     - The <transformation_validators_recovery> will override the first and will only be applied when the recovery matchers were successfully applied (if not specified, the <transformation_validators> will be used instead)
#   If the inliers RMSE is lower than [min_overriding_root_mean_square_error] and the outliers percentage is lower than [max_outliers_percentage], then the other validation parameters are ignored
transformation_validators:
transformation_validators_tracking_recovery:
    euclidean_transformation_validator:                             # Allows prefix and postfix of letters to ensure parsing order
        max_transformation_angle: 0.4                               # Pose corrections with a angle greater than this value will be rejected
        max_transformation_distance: 0.04                           # Pose corrections with a translation greater than this value will be rejected
        max_new_pose_diff_angle: 0.6                                # Pose recovery with a angle greater than this value will be rejected (will be used after cloud registration have failed for <pose_tracking_timeout> seconds)
        max_new_pose_diff_distance: 0.2                             # Pose recovery with a translation greater than this value will be rejected (will be used after cloud registration have failed for <pose_tracking_timeout> seconds)
        max_root_mean_square_error: 0.05                            # Pose corrections which have a cloud registration fitnesss greater than this value will be rejected
        min_overriding_root_mean_square_error: 0.01
        max_outliers_percentage: 0.7                                # Pose corrections which have a cloud registration outlier percentage [0-1] greater than this value will be rejected
        min_overriding_outliers_percentage: 0.1
        min_inliers_angular_distribution: 0.125                     # Minimum angular distribution [0-1] that the inliers must occupy around the robot (0 -> 0º | 360º  -> 1)
        max_outliers_angular_distribution: 0.875                    # Maximum angular distribution [0-1] that the outliers are allowed to occupy around the robot (0 -> 0º | 360º -> 1)