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


# ===================================================================================================================================================
#   PCL and ROS verbosity levels for console loggers
pcl_verbosity_level: 'ERROR'                                    # VERBOSE | DEBUG | INFO | WARN | ERROR | ALWAYS || ALWAYS -> no console output
ros_verbosity_level: 'INFO'                                     #           DEBUG | INFO | WARN | ERROR | FATAL


# ===================================================================================================================================================
#   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)
    ambient_pointcloud_topic_disabled_on_startup: false                         # if true, drl will not start the sensor data subscribers on startup
    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'

service_servers_names:
    reload_localization_configuration_service_server_name: "reload_localization_configuration"
    start_processing_sensor_data_service_server_name: "start_processing_sensor_data"
    stop_processing_sensor_data_service_server_name: "stop_processing_sensor_data"


# ===================================================================================================================================================
#   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
    reference_pointcloud_keypoints_publish_topic: 'reference_pointcloud_keypoints'
    publish_filtered_pointcloud_only_if_there_is_subscribers: true
    publish_aligned_pointcloud_only_if_there_is_subscribers: true
    filtered_pointcloud_publish_topic: 'filtered_pointcloud'         # sensor_msgs::PointCloud2 | Ambient_pointcloud_topic after applying all the filters and circular buffer
    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'
    map_frame_id_for_transforming_pointclouds: 'map' # override for the frame_id that will be used to transform the point clouds to the map coordinate system
    map_frame_id_for_publishing_pointclouds: 'map'   # override for the frame_id of published point clouds in the map frame
    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:
    reset_initial_pose_when_tracking_is_lost: false
    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
    tf_timeout: 0.5                                                     # Timeout when looking for TFs
    override_pointcloud_timestamp_to_current_time: false                # If true, the timestamp of incoming pointclouds will be set to ros::Time::now()
    max_seconds_ambient_pointcloud_age: 3.0                             # Ambient point clouds with age larger than this value will be discarded -> for disabling this check, set to <= 0
    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) -> for disabling this check, set to <= 0
    min_seconds_between_scan_registration: 0.0                          # Ambient point clouds received before this duration is reached (after a successful pose estimation) will be discarded -> for disabling this check, set to <= 0
    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
    remove_points_in_sensor_origin: false
    minimum_number_of_points_in_ambient_pointcloud: 10
    circular_buffer_require_reception_of_pointcloud_msgs_from_all_topics_before_doing_registration: false
    circular_buffer_clear_inserted_points_if_registration_fails: false
    minimum_number_points_ambient_pointcloud_circular_buffer: 5000
    maximum_number_points_ambient_pointcloud_circular_buffer: 0         # If != 0, the ambient pointcloud uses a circular buffer with the specified size of points
    limit_of_pointclouds_to_process: -1                                # If > 0, only k point clouds will be processed
    use_odom_when_transforming_cloud_to_map_frame: true
    use_base_link_frame_when_publishing_registration_pose: false
    use_base_link_frame_when_publishing_initial_poses_array: false
    apply_cloud_registration_inverse_to_initial_poses_array: false
    invert_cloud_to_map_transform: false
    invert_registration_transformation: false
    invert_initial_poses_from_msgs: false
    initial_pose_msg_needs_to_be_in_map_frame: true
    localization_detailed_use_millimeters_in_root_mean_square_error_inliers: false
    localization_detailed_use_millimeters_in_root_mean_square_error_of_last_registration_correspondences: false
    localization_detailed_use_millimeters_in_translation_corrections: false
    localization_detailed_use_degrees_in_rotation_corrections: false
    localization_detailed_compute_pose_corrections_from_initial_and_final_pose_tfs: true   # If false it will use pointcloud correction matrices
    publish_global_inliers_and_outliers_pointclouds_only_if_there_is_subscribers: true
    normalize_ambient_pointcloud_normals: false


# ===================================================================================================================================================
#   General configurations
general_configurations:
    publish_tf_map_odom: false
    publish_tf_when_resetting_initial_pose: false


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

# For avoiding specifying the full path in each reference point cloud to load / save, a folder database path can be specified
reference_pointclouds_database_folder_path: ''

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
    republish_reference_pointcloud_after_successful_registration: false
    normalize_normals: true


# ===================================================================================================================================================
#   Some algorithms support the selection of a given cluster from the point cloud (such as [ euclidean_clustering | region_growing ])
cluster_selector:
    load_clusters_indices_from_parameter_server_before_filtering: true
    load_tf_name_for_sorting_clusters_before_processing: true
    sorting_algorithm: "MaxClusterSizeSorter"                           # Available sorters: [ MinClusterSizeSorter | MaxClusterSizeSorter | MinDistanceToOriginSorter | MaxDistanceToOriginSorter | MinAxisValueSorter | MaxAxisValueSorter ]
    sorting_axis: "Z"                                                   # Available axis: [ X | Y | Z ] -> only used with [ MinAxisValueSorter | MaxAxisValueSorter ]
    # Indices for picking the sorted clusters -> [min_cluster_index, max_cluster_index[ (they start at 0 and do not include the last)
    # Both indices can be set here or in any parent namespace since they are searched from this namespace up to the root using ros::param::search
    min_cluster_index: 0                                                # First cluster index to pick after sorting (it will be overridden as 0 if [min_cluster_index < 0])
    max_cluster_index: 1                                                # Last cluster index to pick after sorting (it will be overridden as [number_of_clusters] if [max_cluster_index < 0] or [max_cluster_index > number_of_clusters])
    tf_name_for_sorting_clusters: ''                                    # TF name for sorting clusters. If empty, the point cloud origin will be used
    use_cloud_time_for_retrieving_tf_transform: true                    # If false, ros::Time(0) will be used
    clusters_colored_cloud_publish_topic: ''
    clusters_colored_cloud_publish_topic_frame_id: ''


# ===================================================================================================================================================
#   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
    ambient_pointcloud_integration_filters_preprocessed_pointcloud_save_filename: ''
    ambient_pointcloud_integration_filters_preprocessed_pointcloud_save_original_pointcloud: true
    filtered_pointcloud_save_filename: ''
    filtered_pointcloud_save_frame_id: ''
    filtered_pointcloud_save_frame_id_with_cloud_time: false        # If false, ros::Time(0) will be used instead of the cloud time
    stop_processing_after_saving_filtered_pointcloud: true
    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_custom_frame:
        custom_frame_id: ''
    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: ''
        hsv_segmentation:                                           # Allows prefix and postfix of letters to ensure parsing order
            minimum_hue: 0.0                                        # If the minimum_hue > maximum_hue, then the color segmentation will use the hue from [minimum_hue, 360] and [0, maximum_hue]
            maximum_hue: 360.0
            minimum_saturation: 0.0
            maximum_saturation: 1.0
            minimum_value: 0.0
            maximum_value: 1.0
            invert_segmentation: false
        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
            reinitialize_seed_before_filtering: true
            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: ''
        scale:
            scale_factor: 0.001
            filtered_cloud_publish_topic: ''
        euclidean_clustering:
            cluster_tolerance: 0.02
            min_cluster_size: 25
            max_cluster_size: INT_MAX                               # Default does not limit the clusters maximum size
            load_clusters_indices_from_parameter_server_before_filtering: true
            filtered_cloud_publish_topic: ''
            filtered_cloud_publish_topic_frame_id: ''
            cluster_selector:
                sorting_algorithm: "MaxClusterSizeSorter"           # Available sorters: [ MinClusterSizeSorter | MaxClusterSizeSorter | MinDistanceToOriginSorter | MaxDistanceToOriginSorter | MinAxisValueSorter | MaxAxisValueSorter ]
                sorting_axis: "Z"                                                   # Available axis: [ X | Y | Z ] -> only used with [ MinAxisValueSorter | MaxAxisValueSorter ]
                clusters_colored_cloud_publish_topic: ''
                clusters_colored_cloud_publish_topic_frame_id: ''
                min_cluster_index: 0                                # [min,max[ cluster size allows to selected which clusters are selected (the clusters are sorted by the number of points they have and the largest is the first in the vector, at index 0)
                max_cluster_index: 1                                # maximum index for the cluster selection (indices start at 0 and do not include the max value -> [min,max[) (if both indexes are negative, all clusters are selected)
    ambient_pointcloud_filters_after_normal_estimation:             # Other filters from above can be used here
        plane_segmentation:                                         # Segments points above and at a specified range from the largest plane in the point cloud and also within its boundary. It allows the usage of surface normals for improved plane segmentation (if the usage of normals is disabled it can be included in the previous filters categories)
            sample_consensus_method: "SAC_RANSAC"                   # SAC_RANSAC | SAC_LMEDS | SAC_MSAC | SAC_RRANSAC | SAC_RMSAC | SAC_MLESAC | SAC_PROSAC
            use_surface_normals: true
            sample_consensus_maximum_distance_of_sample_to_plane: 0.01
            sample_consensus_normals_difference_weight: 0.1
            sample_consensus_number_of_iterations: 1000
            sample_consensus_probability_of_sample_not_be_an_outlier: 0.5
            plane_convex_hull_scaling_factor: 1.0
            segmentation_minimum_distance_to_plane: 0.01
            segmentation_maximum_distance_to_plane: 0.42
            plane_inliers_cloud_publish_topic: ''
            plane_inliers_cloud_publish_topic_frame_id: ''
            plane_inliers_convex_hull_cloud_publish_topic: ''
            plane_inliers_convex_hull_cloud_publish_topic_frame_id: ''
            filtered_cloud_publish_topic: ''
            filtered_cloud_publish_topic_frame_id: ''
        region_growing:
            use_pointcloud_rgb_information: false                   # If true, [pcl::RegionGrowingRGB] will be used. Otherwise [pcl::RegionGrowing]
            min_cluster_size: 100                                   # Regions with less than [min_cluster_size] will be discarded
            max_cluster_size: 2147483647                            # std::numeric_limits<int>::max()
            use_smoothness_constraint: true                         # If true, the comparison of normals will use the current seed normal instead of the initial seed normal that started the current region. This allows the growing of the current region as long as it is locally smooth.
                                                                    # If true, a point will belong to a growing region if [abs(current_seed_neighbor_normal.dot(current_seed_normal)) > cos_deg(smoothness_threshold_in_degrees)].
                                                                    # Otherwise, [abs(current_seed_neighbor_normal.dot(initial_seed_normal)) > cos_deg(smoothness_threshold_in_degrees)]
            use_curvature_test: true                                # Curvature test allows to avoid marking regions with high curvature / normal variability as seeds (usually associated with surface borders or when there is high sensor noise)
                                                                    # If true, a given seed neighboring point will only be considered as a possible seed if [current_seed_neighbor.curvature < curvature_threshold]
            use_residual_test: true                                 # Residual test serves to avoid marking border points or parallel surfaces as seeds
                                                                    # If true, a given seed neighboring point will only be considered as a possible seed if [current_seed_normal.dot(current_seed_point - current_seed_neighbor_point) < cos_deg(residual_threshold_in_degrees)]
            smoothness_threshold_in_degrees: 70.0
            residual_threshold_in_degrees: 10.0
            curvature_threshold: 0.2
            number_of_neighbors: 50                                 # Number of neighbors when looking for new seeds around a current seed
            filtered_cloud_publish_topic: ''
            filtered_cloud_publish_topic_frame_id: ''
            cluster_selector:
                sorting_algorithm: "MinDistanceToOriginSorter"      # Available sorters: [ MinClusterSizeSorter | MaxClusterSizeSorter | MinDistanceToOriginSorter | MaxDistanceToOriginSorter | MinAxisValueSorter | MaxAxisValueSorter ]
                sorting_axis: "Z"                                   # Available axis: [ X | Y | Z ] -> only used with [ MinAxisValueSorter | MaxAxisValueSorter ]
                clusters_colored_cloud_publish_topic: ''
                clusters_colored_cloud_publish_topic_frame_id: ''
            # Extra parameters when using pcl::RegionGrowingRGB
            point_color_threshold: 1200.0                           # A seed neighbor is considered as belonging to the current growing region if [color_difference_squared(current_seed, current_seed_neighbor) < (point_color_threshold * point_color_threshold)]
                                                                    # Color difference is computed as the squared distance of the rgb fields of the 2 points
            region_color_threshold: 1200.0                          # Two regions will be merged together if a region has a point that has in its neighborhood (computed using [number_of_region_neighbors]) a point of the other region that is at a distance lower than [distance_threshold] while also having a similar mean cluster color [color_difference_squared(region1_mean_color, region2_mean_color) < (region_color_threshold * region_color_threshold)]
            distance_threshold: 0.01
            number_of_region_neighbors: 50                          # Overrides [number_of_neighbors] of class pcl::RegionGrowing
            use_normal_test: false                                  # If true, the angle between the normals will be used for testing if a point belongs to a growing region (using the [smoothness_threshold_in_degrees])




# ===================================================================================================================================================
#   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:
    display_normals: false                                          # Can be overridden in child namespaces | Displays a blocking window with the estimated normals
    normalize_normals: true                                         # Can be overridden in child namespaces
    colorize_pointcloud_with_curvatures: false
    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
        # 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
            flip_normals_towards_custom_viewpoint: false            # Normal flipping can be used in the other normal estimators
            normals_viewpoint:
                px: 0.0
                py: 0.0
                pz: 0.0
            normals_viewer:                                         # Viewer can be used in the other normal estimators
                camera_px: 0.0
                camera_py: 0.0
                camera_pz: 2.0
                camera_up_x: 0.0
                camera_up_y: -1.0
                camera_up_z: 0.0
                normals_size: 0.01
                axis_size: 0.2
                background_r: 0.0
                background_g: 0.0
                background_b: 0.0
        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 | All the parameters of [normal_estimation_omp] above can be used with the exception of [ search_k | search_radius ]
            compute_normals: true                                   # Set whether the algorithm should also store the normals computed
            polynomial_order: 2                                     # The order of the polynomial to be fit (order > 1 indicates using a polynomial fit)
            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:                                               # If normals are loaded from CAD files, then they must be normalized (using MeshLab for example) or a normal estimator presented above must be used
    reference_pointcloud:
    ambient_pointcloud:
        principal_curvatures_estimation:
            colorize_pointcloud_with_curvatures: false
            display_curvatures: false
            curvatures_viewer:
                camera_px: 0.0
                camera_py: 0.0
                camera_pz: 2.0
                camera_up_x: 0.0
                camera_up_y: -1.0
                camera_up_z: 0.0
                normals_size: 0.01
                axis_size: 0.2
                background_r: 0.0
                background_g: 0.0
                background_b: 0.0
            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 | CorrespondenceEstimationLookupTable | CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting | CorrespondenceEstimationOrganizedProjection ]
    correspondence_estimation_k: 0                                  # Can be overridden in child namespaces | Only used if -> correspondence_estimation_approach: [ CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting ]
    correspondence_estimation_normals_angle_filtering_threshold: 80.0 # Can be overridden in child namespaces | Only used if -> correspondence_estimation_approach: [ CorrespondenceEstimationBackProjection | CorrespondenceEstimationNormalShooting ]
    correspondence_estimation_normals_angle_penalty_factor: 4.0     # 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
    correspondence_estimation_lookup_table:                         # Can be overridden in child namespaces | Only used if -> correspondence_estimation_approach: CorrespondenceEstimationLookupTable
      map_cell_resolution: 0.01                                     # Cell size in meters
      map_margin_x: 1.0                                             # Margin around map data with extra lookup cells
      map_margin_y: 1.0                                             # Margin around map data with extra lookup cells
      map_margin_z: 1.0                                             # Margin around map data with extra lookup cells
      map_use_search_tree_when_query_point_is_outside_lookup_table: true   # True for using the search tree as a fall back strategy when the query points are outside the lookup table bounds.
      map_compute_distance_from_query_point_to_closest_point: false        # True for computing the distance between query point and the closest point. False for using the distance between the centroids of the cells associated with the query and closest point
      map_initialize_lookup_table_using_euclidean_distance_transform: true # True for using the Euclidean Distance Transform (much faster). False for using a k-d tree (more accurate).
      sensor_cell_resolution: 0.01                                  # Cell size in meters
      sensor_margin_x: 1.0                                          # Margin around map data with extra lookup cells
      sensor_margin_y: 1.0                                          # Margin around map data with extra lookup cells
      sensor_margin_z: 1.0                                          # Margin around map data with extra lookup cells
      sensor_use_search_tree_when_query_point_is_outside_lookup_table: true   # True for using the search tree as a fall back strategy when the query points are outside the lookup table bounds.
      sensor_compute_distance_from_query_point_to_closest_point: false        # True for computing the distance between query point and the closest point. False for using the distance between the centroids of the cells associated with the query and closest point
      sensor_initialize_lookup_table_using_euclidean_distance_transform: true # True for using the Euclidean Distance Transform (much faster). False for using a k-d tree (more accurate).
    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: 0.0000001                                    # Can be overridden in child namespaces | Ignored if lower than 0 | The transformation epsilon (maximum allowable translation squared 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: 0.000001                                 # 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
            reference_cloud_publish_topic: ''                       # Can be overridden in child namespaces
            reference_cloud_publish_frame: ''                       # 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: 30                               # Set the number of samples to use during each iteration
                correspondence_randomness: 3                       # The number of neighbors to use when selecting a random feature correspondence. A higher value will add more randomness to the feature matching
                tf_publisher:                                       # The TF publisher can be attached to a feature_matcher or point_matcher for showing the transformation that it computed (using either normal or static TF broadcaster)
                    publish_tf: false                               # For activating the publishing of TF
                    publish_static_tf: false                        # For activating the publishing of static TF
                    tf_broadcaster_frame_id: ''                     # Frame id that will be placed in the geometry_msgs::TransformStamped header
                    tf_broadcaster_child_frame_id: ''               # Child frame iD that will be placed in the geometry_msgs::TransformStamped
                    update_registered_pointcloud_with_tf_broadcaster_child_frame_id: false  # Flag for indicating that the registered point cloud frame id should use the tf_broadcaster_child_frame_id specified above
            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
        reference_cloud_publish_topic: ''                           # Can be overridden in child namespaces
        reference_cloud_publish_frame: ''                           # 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
            # 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: 0.0000001               # (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)
            convergence_rotation_threshold: 0.99999                     # Only applied if > 0 | cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1) || cos_angle >= rotation_threshold || (epsilon is the cos(angle) in a axis-angle representation) -> cos_angle = 0.99999 -> 0.256 degrees threshold
            convergence_max_iterations_similar_transforms: 5        # 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_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
            use_symmetric_objective_cost_function: false
            ensure_normals_with_same_direction_when_using_symmetric_objective_cost_function: false
        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
            transformation_rotation_epsilon: 0.0                    # Only used if > 0 | Maximum allowable rotation difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution (epsilon is the cos(angle) in a axis-angle representation) -> cos_angle = 0.99999 -> 0.256 degrees threshold
            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
            transformation_rotation_epsilon: 0.0                    # Only used if > 0 | Maximum allowable rotation difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution (epsilon is the cos(angle) in a axis-angle representation) -> cos_angle = 0.99999 -> 0.256 degrees threshold
            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
        principal_component_analysis:                               # Allows prefix and postfix of letters to ensure parsing order (PCA has 3 axis of symmetry, and as such, two postprocessing stages are supported for ensuring consistency of the PCA axis)
            reload_configurations_from_parameter_server_before_alignment: true
            compute_offset_to_reference_pointcloud_pca: false                               # If true, the algorithm will return the matrix transformation that aligns the sensor point cloud PCA to the reference point cloud PCA. If false, the algorithms returns the sensor point cloud PCA
            flip_pca_z_axis_for_aligning_it_to_the_cluster_centroid_z_normal: true          # Flipping of PCA Z axis for ensuring that the Z+ always points to the surfaces outside region (if the angle between the PCA Z axis and the centroid normal is higher than 180º, then a rotation of 180º will be performed along the PCA X axis)
            flip_pca_z_axis_for_aligning_it_to_the_cluster_centroid_z_normal_reference_pointcloud: true
            flip_pca_z_axis_for_aligning_it_to_the_pointcloud_custom_z_flip_axis: false     # Flipping of PCA Z axis for ensuring that it consistently points along a given direction (if the angle between the PCA Z axis and the custom_z_flip_axis is higher than 180º, then a rotation of 180º will be performed along the PCA X axis)
            flip_pca_z_axis_for_aligning_it_to_the_pointcloud_custom_z_flip_axis_reference_pointcloud: false
            flip_pca_x_axis_for_aligning_it_to_the_pointcloud_custom_x_flip_axis: true      # Flipping of PCA X axis for ensuring that it consistently points along a given direction (if the angle between the PCA X axis and the custom_x_flip_axis is higher than 180º, then a rotation of 180º will be performed along the PCA Z axis)
            flip_pca_x_axis_for_aligning_it_to_the_pointcloud_custom_x_flip_axis_reference_pointcloud: true
            custom_z_flip_axis:
                x: 0.0
                y: 0.0
                z: 1.0
            custom_z_flip_axis_reference_pointcloud:
                x: 0.0
                y: 0.0
                z: 1.0
            custom_x_flip_axis:
                x: 0.0
                y: 0.0
                z: 1.0
            custom_x_flip_axis_reference_pointcloud:
                x: 0.0
                y: 0.0
                z: 1.0
            registered_cloud_publish_topic: ''


#   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


# ===================================================================================================================================================
# When the reference point cloud has symmetry axis, a transformation aligner can be used to rotate the object in its origin in [ roll | pitch | yaw ] for aligning its axis with an external reference frame (the origin of the reference point cloud should be placed in the origin of the symmetry rotation axis)
# For example, if the reference point cloud is a cone with Z+ going through its base and head, the registration algorithms may oscillate in yaw because there is rotational symmetry around Z
# For correcting this, an external TF frame can be used to constrain the yaw rotation
# The external frame does not need to be perfectly aligned with the object frame, since line to plane intersection is used on the rotation planes
# However, for proper operation, the rotation_alignment_frame_id should be +- aligned with the expected frame of the object and the appropriate [roll|pitch|yaw] correction should be chosen (the correction order is roll -> pitch -> yaw, which allows to align the 3 DoF orientation of the object with the rotation_alignment_frame if the 3 corrections are enabled)
# The transformation aligner is run after the point cloud registration algorithms and it will affect the outlier detectors and transformation validators
transformation_aligner:
    anchor_tf_frame_id: "camera_link"                                   # Frame id that can be either the [ map_frame_id ] or [ base_link_frame_id ] above, and that specifies in which side of the TF tree the [ rotation_alignment_frame_id ] is closer (for example, if you have a table tf attached to camera_link and drl is estimating the pose of the object in relation to camera_link, then the anchor_tf_frame_id should be camera_link -> this is required for not including in the tf lookupTransform the chain that is being estimated by drl)
    rotation_alignment_frame_id: "reference_for_rotation_alignment"     # External TF reference frame for correcting the alignment around the symmetry axis
    correct_roll: false                                                 # Corrects the object roll so that the object Z axis aligns as much as possible with the rotation_alignment_frame Z axis
    correct_pitch: false                                                # Corrects the object pitch so that the object Z axis aligns as much as possible with the rotation_alignment_frame Z axis
    correct_yaw: true                                                   # Corrects the object yaw so that the object X axis aligns as much as possible with the rotation_alignment_frame X axis


# ===================================================================================================================================================
#   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
    aligned_pointcloud_global_outliers_publish_topic: ''            # Outliers from all detectors
    aligned_pointcloud_global_inliers_publish_topic: ''             # Inliers from all detectors
    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
        colorize_inliers_based_on_correspondence_distance: false    # If true, the inliers will be colorized using a green -> red gradient, in which a distance of 0 meters will have a hue of 120º (green) and a distance equal to max_inliers_distance will have a hue of 0º (red)
        colorize_outliers_with_red_color: false                     # If true, the outliers will be colorized with red color
        max_curvature_difference: 0.0                               # If 0.0, curvatures will not be compared
        max_normals_angular_difference_in_degrees: -30.0            # Range ]0.0, 180.0] || If outside range, the normals angular difference will not be computed and used to filter the inliers
        max_hsv_color_hue_difference_in_degrees: -30.0              # Range ]0.0, 360.0[ || If outside range, the color hsv difference will not be computed and used to filter the inliers
        max_hsv_color_saturation_difference: 0.3                    # Range ]0.0, 1.0]   || If outside range, the color hsv difference will not be computed and used to filter the inliers
        max_hsv_color_value_difference: 0.3                         # Range ]0.0, 1.0]   || If outside range, the color hsv difference will not be computed and used to filter the inliers
        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


# ===================================================================================================================================================
#   Several 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_reference_pointcloud:
    publish_pointclouds_only_if_there_is_subscribers: true          # Can be overridden in child namespaces
    reference_pointcloud_global_outliers_publish_topic: ''          # Outliers from all detectors
    reference_pointcloud_global_inliers_publish_topic: ''           # Inliers from all detectors
    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
        reference_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
        reference_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: false
    compute_outliers_angular_distribution: false
    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_initial_alignment:
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 (negative -> disabled)
        max_root_mean_square_error_reference_pointcloud: -1.0       # Pose corrections which have a reference point cloud overlap fitnesss greater than this value will be rejected (negative -> disabled)
        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
        max_outliers_percentage_reference_pointcloud: -1.0          # Pose corrections which have a cloud registration outlier percentage [0-1] greater than this value will be rejected (overlap between reference point cloud and ambient point cloud)
        min_overriding_outliers_percentage: 0.1
        min_overriding_outliers_percentage_reference_pointcloud: 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)