2D localization mode have roll pitch and Zaxis drit

[ZOUJIASHUAI]

[ZOUJIASHUAI]

@carlosmccosta the localization topic also seems have Zaxis drift.

[carlosmccosta]

Hello,

For 3 DoF localization (x, y, yaw), you must use the dynamic_robot_localization_system.launch, which is configured for using 2D lidars and 2D occupancy grids.

drl assumes that your lidar is mounted parallel to the floor and after transforming the lidar data to the odom TF, it will set the points Z coordinate to zero.

If your lidar is not parallel to the floor, you will not be able to use 3 DoF localization algorithms and maps, because they all assume sensors parallel to the floor to ensure that when the sensor changes place within the map, the measurements remain consistent.

Have a nice day,

[ZOUJIASHUAI]

@carlosmccosta
hello,
I do use the [dynamic_robot_localization_system.launch],and below is the launch parameter.

<!-- published pose -->
<arg name="pose_stamped_publish_topic" default="localization_pose" />

<!-- frame ids -->
<arg name="map_frame_id" default="map" />
<arg name="odom_frame_id" default="odom" />
<arg name="base_link_frame_id" default="laser" />
<arg name="sensor_frame_id" default="laser" />

<!-- main configurations -->
<arg name="use_ukf_filtering" default="false" />
<arg name="nodes_namespace" default="drl_slam" />
<arg name="nodes_respawn" default="true" />
<arg name="localization" default="localization" />
<arg name="pcl_verbosity_level" default="ALWAYS" /> <!-- VERBOSE | DEBUG | INFO | WARN | ERROR | ALWAYS || ALWAYS -> no console output -->
<arg name="ros_verbosity_level" default="FATAL" /> <!-- DEBUG | INFO | WARN | ERROR | FATAL -->
<arg name="yaml_configuration_filters_filename" default="$(find super_slam)/yaml/configs/filters/filters_large_map_2d.yaml" />
<arg name="yaml_configuration_pose_estimation_filename" default="$(find super_slam)/yaml/configs/empty.yaml" />
<arg name="yaml_configuration_tracking_filename" default="$(find super_slam)/yaml/configs/pose_tracking/cluttered_environments_dynamic_fast_2d.yaml" />
<arg name="yaml_configuration_recovery_filename" default="$(find super_slam)/yaml/configs/pose_recovery/recovery_slam_2d.yaml" />
<arg name="yaml_configuration_covariance_filename" default="$(find super_slam)/yaml/configs/covariance_estimation/covariance_estimation.yaml" if="$(arg use_ukf_filtering)" />
<arg name="yaml_configuration_covariance_filename" default="$(find super_slam)/yaml/configs/empty.yaml" unless="$(arg use_ukf_filtering)" />

<!-- reference map data -->
<arg name="reference_pointcloud_filename" default="" />
<arg name="reference_pointcloud_preprocessed_save_filename" default="" />
<arg name="reference_pointcloud_available" default="true" />
<arg name="reference_pointcloud_type_3d" default="false" />
<arg name="reference_pointcloud_type" default="2D" unless="$(arg reference_pointcloud_type_3d)"/>
<arg name="reference_pointcloud_type" default="3D" if="$(arg reference_pointcloud_type_3d)" />
<arg name="reference_pointcloud_update_mode" default="NoIntegration" if="$(arg reference_pointcloud_available)"/> <!-- NoIntegration | FullIntegration | InliersIntegration | OutliersIntegration -->
<arg name="reference_pointcloud_update_mode" default="FullIntegration" unless="$(arg reference_pointcloud_available)"/>
<arg name="reference_pointcloud_keypoints_filename" default="" />
<arg name="reference_pointcloud_keypoints_save_filename" default="" />
<arg name="reference_pointcloud_descriptors_filename" default="" />
<arg name="reference_pointcloud_descriptors_save_filename" default="" />
<arg name="reference_costmap_topic" default="" if="$(arg reference_pointcloud_type_3d)" />
<arg name="reference_costmap_topic" default="/map" unless="$(arg reference_pointcloud_type_3d)" />
<arg name="reference_costmap_topic_updated" default="/map_updated" />
<arg name="reference_pointcloud_topic" default="reference_pointcloud_updated" if="$(arg reference_pointcloud_type_3d)" /> <!-- OctoMap is configured to use topic 'reference_pointcloud_update' -->
<arg name="reference_pointcloud_topic" default="" unless="$(arg reference_pointcloud_type_3d)" />

<!-- sensor data from point cloud topic -->
<arg name="ambient_pointcloud_topic" default="ambient_pointcloud" />
<arg name="ambient_pointcloud_topic_disabled_on_startup" default="false" />
<arg name="imu_topic" default="/imu/data" />

<!-- initial pose setup -->
<arg name="robot_initial_pose_in_base_to_map" default="false" /> <!-- recommendation: use false to allow direct publish of tf between odom -> map -->
<arg name="robot_initial_pose_available" default="false" />
<arg name="robot_initial_x" default="0.0" />
<arg name="robot_initial_y" default="0.0" />
<arg name="robot_initial_z" default="0.0" />
<arg name="robot_initial_roll" default="0.0" />
<arg name="robot_initial_pitch" default="0.0" />
<arg name="robot_initial_yaw" default="0.0" />

<!-- laser scan assembler -->
<arg name="use_laser_assembler" default="true" />
<arg name="assemble_lasers_on_map_frame" default="false" /> <!-- if false -> assembles lasers on odom frame -->
<arg name="laser_assembly_frame" default="$(arg map_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
<arg name="laser_assembly_frame" default="$(arg odom_frame_id)" unless="$(arg assemble_lasers_on_map_frame)" />
<arg name="laser_assembly_motion_estimation_source_frame_id" default="" />
<arg name="laser_assembly_motion_estimation_target_frame_id" default="" />
<arg name="laser_scan_topics" default="/scan" />
<arg name="min_range_cutoff_percentage_offset" default="2.00" />
<arg name="max_range_cutoff_percentage_offset" default="0.95" />
<arg name="laser_assembler_remove_invalid_measurements" default="true" />
<arg name="number_of_tf_queries_for_spherical_interpolation" default="4" />
<arg name="number_of_scans_to_assemble_per_cloud" default="1" />
<arg name="timeout_for_cloud_assembly" default="0.5" />
<arg name="dynamic_update_of_assembly_configuration_from_twist_topic" default="" />
<arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="/odom" />
<arg name="dynamic_update_of_assembly_configuration_from_imu_topic" default="/imu/data" />
<arg name="min_number_of_scans_to_assemble_per_cloud" default="1" />
<arg name="max_number_of_scans_to_assemble_per_cloud" default="1" />
<arg name="min_timeout_seconds_for_cloud_assembly" default="0.15" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="0.15" />
<arg name="max_linear_velocity" default="0.0001" /> <!-- when the linear velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
<arg name="max_angular_velocity" default="0.0001" /> <!-- when the angular velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->

<!-- tf configurations -->
<arg name="publish_tf_map_odom" default="true" />
<arg name="publish_initial_pose" default="true" />
<arg name="publish_pose_tf_rate" default="10.0" />
<arg name="tf_lookup_timeout" default="0.1" />
<arg name="publish_last_pose_tf_timeout_seconds" default="-330.0" /> <!-- set to negative value for publishing a tf only when drl estimates a new pose -->
<arg name="invert_tf_transform" default="false" />
<arg name="invert_tf_hierarchy" default="false" />
<arg name="transform_pose_to_map_frame_id" default="true" />

<!-- map update -->
<arg name="use_octomap_for_dynamic_map_update" default="false" />
<arg name="use_octomap_only_to_build_occupancy_grid" default="false" />
<arg name="octomap_pointcloud_topic" default="aligned_pointcloud" />  <!-- ambient_pointcloud | aligned_pointcloud | aligned_pointcloud_outliers | aligned_pointcloud_inliers -->
<arg name="octomap_file" default="" />
<arg name="octomap_sensor_frame_id" default="$(arg sensor_frame_id)" />
<arg name="octomap_resolution" default="0.02" />
<arg name="octomap_minimum_amount_of_time_between_ros_msg_publishing" default="5.0" />
<arg name="octomap_minimum_number_of_integrations_before_ros_msg_publishing" default="10" />
<arg name="octomap_override_sensor_z" default="false" if="$(arg reference_pointcloud_type_3d)" />
<arg name="octomap_override_sensor_z" default="true" unless="$(arg reference_pointcloud_type_3d)" />
<arg name="octomap_override_sensor_z_value" default="0.0" />
<arg name="octomap_cloud_out_topic" default="reference_pointcloud_updated" />
<arg name="initial_2d_map_file" default="" />


<!-- ==================================================== localization system ============================================= -->
<!-- <node pkg="super_slam" type="localization" name="$(arg localization)" respawn="$(arg nodes_respawn)" ns="$(arg nodes_namespace)" clear_params="true" output="screen" launch-prefix="gdbserver localhost:1337" > -->
<node pkg="super_slam" type="localization" name="$(arg localization)" respawn="$(arg nodes_respawn)" ns="$(arg nodes_namespace)" clear_params="true" output="screen" >
	<param name="pcl_verbosity_level" type="str" value="$(arg pcl_verbosity_level)" />
	<param name="ros_verbosity_level" type="str" value="$(arg ros_verbosity_level)" />
	<param name="message_management/tf_timeout" type="double" value="$(arg tf_lookup_timeout)" />
	<param name="frame_ids/map_frame_id" type="str" value="$(arg map_frame_id)" />
	<param name="frame_ids/odom_frame_id" type="str" value="$(arg odom_frame_id)" />
	<param name="frame_ids/base_link_frame_id" type="str" value="$(arg base_link_frame_id)" />
	<param name="frame_ids/sensor_frame_id" type="str" value="$(arg sensor_frame_id)" />
	<param name="initial_pose/robot_initial_pose_in_base_to_map" type="bool" value="$(arg robot_initial_pose_in_base_to_map)" />
	<param name="initial_pose/robot_initial_pose_available" type="bool" value="$(arg robot_initial_pose_available)" />
	<param name="initial_pose/position/x" type="double" value="$(arg robot_initial_x)" />
	<param name="initial_pose/position/y" type="double" value="$(arg robot_initial_y)" />
	<param name="initial_pose/position/z" type="double" value="$(arg robot_initial_z)" />
	<param name="initial_pose/orientation_rpy/roll" type="double" value="$(arg robot_initial_roll)" />
	<param name="initial_pose/orientation_rpy/pitch" type="double" value="$(arg robot_initial_pitch)" />
	<param name="initial_pose/orientation_rpy/yaw" type="double" value="$(arg robot_initial_yaw)" />
	<param name="subscribe_topic_names/pose_topic" type="str" value="$(arg pose_topic)" />
	<param name="subscribe_topic_names/pose_stamped_topic" type="str" value="$(arg pose_stamped_topic)" />
	<param name="subscribe_topic_names/pose_with_covariance_stamped_topic" type="str" value="$(arg pose_with_covariance_stamped_topic)" />
	<param name="subscribe_topic_names/ambient_pointcloud_topic" type="str" value="$(arg ambient_pointcloud_topic)" />
	<param name="subscribe_topic_names/ambient_pointcloud_topic_disabled_on_startup" type="bool" value="$(arg ambient_pointcloud_topic_disabled_on_startup)" />
	<param name="subscribe_topic_names/reference_costmap_topic" type="str" value="$(arg reference_costmap_topic)" />
	<param name="subscribe_topic_names/reference_pointcloud_topic" type="str" value="$(arg reference_pointcloud_topic)" />
	<param name="publish_topic_names/pose_with_covariance_stamped_publish_topic" type="str" value="$(arg pose_with_covariance_stamped_publish_topic)" />
	<param name="publish_topic_names/pose_stamped_publish_topic" type="str" value="$(arg pose_stamped_publish_topic)" />
	<param name="reference_pointclouds/reference_pointcloud_filename" type="str" value="$(arg reference_pointcloud_filename)" />
	<param name="reference_pointclouds/reference_pointcloud_preprocessed_save_filename" type="str" value="$(arg reference_pointcloud_preprocessed_save_filename)" />
	<param name="reference_pointclouds/reference_pointcloud_type" type="str" value="$(arg reference_pointcloud_type)" />
	<param name="reference_pointclouds/reference_pointcloud_available" type="bool" value="$(arg reference_pointcloud_available)" />
	<param name="reference_pointclouds/reference_pointcloud_update_mode" type="str" value="$(arg reference_pointcloud_update_mode)" />
	<param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_filename" type="str" value="$(arg reference_pointcloud_keypoints_filename)" />
	<param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_save_filename" type="str" value="$(arg reference_pointcloud_keypoints_save_filename)" />
	<param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_filename" type="str" value="$(arg reference_pointcloud_descriptors_filename)" />
	<param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_save_filename" type="str" value="$(arg reference_pointcloud_descriptors_save_filename)" />
	<rosparam command="load" file="$(arg yaml_configuration_filters_filename)" subst_value="true" />
	<rosparam command="load" file="$(arg yaml_configuration_pose_estimation_filename)" subst_value="false" />
	<rosparam command="load" file="$(arg yaml_configuration_recovery_filename)" subst_value="true" />
	<rosparam command="load" file="$(arg yaml_configuration_tracking_filename)" subst_value="true" />
	<rosparam command="load" file="$(arg yaml_configuration_covariance_filename)" subst_value="true" />
</node>


<include file="$(find super_slam)/launch/ukf.launch" ns="$(arg nodes_namespace)" if="$(arg use_ukf_filtering)" >
	<arg name="planar_pose_estimation" default="false" if="$(arg reference_pointcloud_type_3d)" />
	<arg name="planar_pose_estimation" default="true" unless="$(arg reference_pointcloud_type_3d)" />
	<arg name="tracking_state_reset_topic" default="initial_pose_with_covariance" />
	<arg name="sensor_timeout" default="$(arg publish_last_pose_tf_timeout_seconds)" />
	<arg name="map_frame_id" default="$(arg map_frame_id)" />
	<arg name="odom_frame_id" default="$(arg odom_frame_id)" />
	<arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
	<arg name="odometry_topic" default="$(arg dynamic_update_of_assembly_configuration_from_odometry_topic)" />
	<arg name="imu_topic" default="$(arg imu_topic)" />
	<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
</include>


<!-- map to odom publisher -->
<include file="$(find pose_to_tf_publisher)/launch/pose_to_tf_publisher.launch" if="$(arg publish_tf_map_odom)" >
	<arg name="publish_rate" default="$(arg publish_pose_tf_rate)" />
	<arg name="tf_lookup_timeout" default="1.5" />
	<arg name="publish_last_pose_tf_timeout_seconds" default="$(arg publish_last_pose_tf_timeout_seconds)" />
	<arg name="pose_with_covariance_stamped_topic" default="$(arg pose_with_covariance_stamped_topic)" /> <!-- rviz topic -->
	<arg name="pose_stamped_topic" default="$(arg pose_stamped_publish_topic)" />
	<arg name="odometry_topic" default="" />
	<arg name="map_frame_id" default="$(arg map_frame_id)_drl" if="$(arg use_ukf_filtering)" />
	<arg name="map_frame_id" default="$(arg map_frame_id)" unless="$(arg use_ukf_filtering)" />
	<arg name="odom_frame_id" default="$(arg odom_frame_id)" />
	<arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
	<arg name="publish_initial_pose" default="$(arg publish_initial_pose)" />
	<arg name="initial_pose_in_base_to_map" default="$(arg robot_initial_pose_in_base_to_map)" />
	<arg name="initial_x" default="$(arg robot_initial_x)" />
	<arg name="initial_y" default="$(arg robot_initial_y)" />
	<arg name="initial_z" default="$(arg robot_initial_z)" />
	<arg name="initial_roll" default="$(arg robot_initial_roll)" />
	<arg name="initial_pitch" default="$(arg robot_initial_pitch)" />
	<arg name="initial_yaw" default="$(arg robot_initial_yaw)" />
	<arg name="invert_tf_transform" default="true" if="$(arg use_ukf_filtering)" />
	<arg name="invert_tf_transform" default="$(arg invert_tf_transform)" unless="$(arg use_ukf_filtering)" />
	<arg name="invert_tf_hierarchy" default="true" if="$(arg use_ukf_filtering)" />
	<arg name="invert_tf_hierarchy" default="$(arg invert_tf_hierarchy)" unless="$(arg use_ukf_filtering)" />
	<arg name="transform_pose_to_map_frame_id" default="false" if="$(arg use_ukf_filtering)" />
	<arg name="transform_pose_to_map_frame_id" default="$(arg transform_pose_to_map_frame_id)" unless="$(arg use_ukf_filtering)" />
	<arg name="parameters_namespace" default="super_slam" />
	<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
</include>


<!-- laser assembler -->
<include file="$(find laserscan_to_pointcloud)/launch/laserscan_to_pointcloud_assembler.launch" ns="$(arg nodes_namespace)" if="$(arg use_laser_assembler)" >
	<arg name="laser_scan_topics" default="$(arg laser_scan_topics)" />
	<arg name="number_of_tf_queries_for_spherical_interpolation" default="$(arg number_of_tf_queries_for_spherical_interpolation)" />
	<arg name="number_of_scans_to_assemble_per_cloud" default="$(arg number_of_scans_to_assemble_per_cloud)" />
	<arg name="timeout_for_cloud_assembly" default="$(arg timeout_for_cloud_assembly)" />
	<arg name="remove_invalid_measurements" default="$(arg laser_assembler_remove_invalid_measurements)" />
	<arg name="dynamic_update_of_assembly_configuration_from_twist_topic" default="$(arg dynamic_update_of_assembly_configuration_from_twist_topic)" />
	<arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="$(arg dynamic_update_of_assembly_configuration_from_odometry_topic)" />
	<arg name="dynamic_update_of_assembly_configuration_from_imu_topic" default="$(arg dynamic_update_of_assembly_configuration_from_imu_topic)" />
	<arg name="min_range_cutoff_percentage_offset" default="$(arg min_range_cutoff_percentage_offset)" />
	<arg name="max_range_cutoff_percentage_offset" default="$(arg max_range_cutoff_percentage_offset)" />
	<arg name="min_number_of_scans_to_assemble_per_cloud" default="$(arg min_number_of_scans_to_assemble_per_cloud)" />
	<arg name="max_number_of_scans_to_assemble_per_cloud" default="$(arg max_number_of_scans_to_assemble_per_cloud)" />
	<arg name="min_timeout_seconds_for_cloud_assembly" default="$(arg min_timeout_seconds_for_cloud_assembly)" />
	<arg name="max_timeout_seconds_for_cloud_assembly" default="$(arg max_timeout_seconds_for_cloud_assembly)" />
	<arg name="max_linear_velocity" default="$(arg max_linear_velocity)" />
	<arg name="max_angular_velocity" default="$(arg max_angular_velocity)" />
	<arg name="target_frame" default="$(arg laser_assembly_frame)" />
	<arg name="recovery_frame" default="$(arg odom_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
	<arg name="recovery_frame" default="" unless="$(arg assemble_lasers_on_map_frame)" />
	<arg name="initial_recovery_transform_in_base_link_to_target" default="$(arg robot_initial_pose_in_base_to_map)" />
	<arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
	<arg name="motion_estimation_source_frame_id" default="$(arg laser_assembly_motion_estimation_source_frame_id)" />
	<arg name="motion_estimation_target_frame_id" default="$(arg laser_assembly_motion_estimation_target_frame_id)" />
	<arg name="recovery_to_target_frame_transform_initial_x" default="$(arg robot_initial_x)" />
	<arg name="recovery_to_target_frame_transform_initial_y" default="$(arg robot_initial_y)" />
	<arg name="recovery_to_target_frame_transform_initial_z" default="$(arg robot_initial_z)" />
	<arg name="recovery_to_target_frame_transform_initial_roll" default="$(arg robot_initial_roll)" />
	<arg name="recovery_to_target_frame_transform_initial_pitch" default="$(arg robot_initial_pitch)" />
	<arg name="recovery_to_target_frame_transform_initial_yaw" default="$(arg robot_initial_yaw)" />
	<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
</include>


<!-- OctoMap for dynamic map update -->
<group if="$(arg use_octomap_for_dynamic_map_update)" ns="$(arg nodes_namespace)" >
	<include file="$(find super_slam)/launch/octo_map.launch" >
		<arg name="octomap_file" default="$(arg octomap_file)" />
		<arg name="cloud_in" default="$(arg octomap_pointcloud_topic)" />
		<arg name="map_frame_id" default="$(arg map_frame_id)" />
		<arg name="base_frame_id" default="$(arg base_link_frame_id)" />
		<arg name="sensor_frame_id" default="$(arg octomap_sensor_frame_id)" />
		<arg name="override_sensor_z" default="$(arg octomap_override_sensor_z)" />
		<arg name="override_sensor_z_value" default="$(arg octomap_override_sensor_z_value)" />
		<arg name="resolution" default="$(arg octomap_resolution)" />
		<arg name="minimum_amount_of_time_between_ros_msg_publishing" default="$(arg octomap_minimum_amount_of_time_between_ros_msg_publishing)" />
		<arg name="minimum_number_of_integrations_before_ros_msg_publishing" default="$(arg octomap_minimum_number_of_integrations_before_ros_msg_publishing)" />
		<arg name="cloud_out_topic" default="$(arg octomap_cloud_out_topic)" />
		<arg name="map_out_topic" default="$(arg reference_costmap_topic_updated)" if="$(arg use_octomap_only_to_build_occupancy_grid)" />
		<arg name="map_out_topic" default="$(arg reference_costmap_topic)" unless="$(arg use_octomap_only_to_build_occupancy_grid)" />
		<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
	</include>

	<group if="$(arg reference_pointcloud_available)" >
		<node name="map_server" pkg="map_server" type="map_server" args="$(arg initial_2d_map_file)" respawn="$(arg nodes_respawn)" clear_params="true" output="screen" unless="$(arg reference_pointcloud_type_3d)" >
			<param name="frame_id" type="str" value="$(arg map_frame_id)" />
			<remap from="map" to="initial_2d_map" />
		</node>
	</group>
</group>

[carlosmccosta]

Hello,

Check if the roll and pitch angles between the TFs odom and laser are 0 degrees.
The 2d icp configured in drl assumes that the laser is parallel to the ground and uses the pcl transformation_estimation_2D

Check your ROS parameter server (rosparam get / -p) to confirm that the 2d icp was correctly loaded.

Looking at your rviz screenshot it seems that the laser is tilted and not parallel to the ground.

Have a nice day :)

[carlosmccosta]

Hello,

What is your TF chain configuration from map_frame_id to base_link_frame_id ?

From your previous launch files, I assumed that it was map -> odom -> laser

If your odom is not accurate, you can disable its usage within drl by:

• Setting the odom_frame_id to a value equal to the base_link_frame_id
• Since a TF frame can only have one parent in the TF tree, for bypassing the odom_frame_id, it is necessary to:
  • Set to true the flag invert_tf_transform
  • Set to true the flag invert_tf_hierarchy
  • This way, the TF and pose_stamped_publish_topic will be published directly from base_link_frame_id to map_frame_id (the pose is specified in the map_frame_id coordinate system)
  • You can check the drl pose and TF in rviz with the fixed frame configured to map and can also check in the console with:
    • rostopic echo /dynamic_robot_localization/localization_pose
    • rosrun tf tf_echo map laser _rate:=30 _precision:=4

I rechecked one of my validation testes from a rosbag (freiburg2_pioneer_slam1.launch) and the latest commit of drl in the noetic-devel branch is performing as expected and without drift, as shown in this video:
https://www.youtube.com/watch?v=jAJ5wiN-mJ8

Have a nice day,

[ZOUJIASHUAI]

hello @carlosmccosta
I found another problem from drl,my odom is accurate so I read the map->laser tf to navigation,and the pose will jump to far a way if the map changed alot and the initial_pose_estimation is set,so I disabled the initial_pose_estimation.but some time it wouldn‘t be able to find it self in the map even the environment is not so bad,I also tune some param from pose_recovery file,so does the initial_pose_estimation will help this situation？or how to tune the initial_pose_estimation so the pose will not jump to far?

[carlosmccosta]

Hello,

If drl is rejecting its pose estimation in your typical use case / environment, then you may need to increase the tolerance to outliers, by specifying to drl that the map data might be X% different in relation to the sensor data.

This is controlled with max_inliers_distance (in meters) and max_outliers_percentage (range 0 to 1 | note that currently this is an outlier_ratio -> 1 equals to 100%)

It is possible to control how far the tracking and recovery algorithms corrections translate and rotate the robot pose with max_transformation_distance | max_transformation_angle and for recovery recovery_2d.yaml#L27 | recovery_2d.yaml#L26

For the initial pose algorithms, currently it not applied this jump limit to allow the robot to start lost and then find itself in the map.

To ensure that ICP runs fast, confirm that you have the look up table enabled with the correspondence_estimation_approach set to CorrespondenceEstimationLookupTable and with its map_cell_resolution equal to your map, otherwise the icp might stop its convergence after reaching its time limit defined in convergence_time_limit_seconds

If your robot is moving very fast, you may need to increase max_correspondence_distance

Have a nice day :)

[ZOUJIASHUAI]

hello @carlosmccosta
my odom is accurate，in odom topic I set 0 to angular_velocity and oritation in X Y axis,and set ignore_height_corrections: true, and it doesnt have roll pitch and Zaxis drit,but it still have x y pose drift and yaw drift,you can see the video below,with the increasement of running time ,the drift also increase,and the drift can be clear by send a initialpose topic ,seem the initialpose will triggle another program that recalculate the pose in the map and clear some drift,what I did is send initialpose every 60s ,but I think it can be fix in code which I can not found it,by the way I disabled the pose_estimation module,so maybe the initpose triggled the pose recovery module and cleared the drift?
the tf tree is map->odom->base_link->laser
https://drive.google.com/file/d/18HqVuU20vCfN-7rdiI9YQJneGD024vOo/view?usp=share_link
this video been record after the robot running 1 hours
the laser scan which is base on tf or pose topic should match the point clould ,but it didnt,seems the point clould topic have the correct pose with no drift, but the pose from tf or /localization_detail topic have drift,and this drift can be clear by initalpose

[carlosmccosta]

Hello,

When you send an initial pose, the matching algorithm changes from yaml_configuration_tracking_filename to yaml_configuration_recovery_filename

Assuming you have not changed the yamls loaded, you can compare the configurations here:

• cluttered_environments_dynamic_large_map_2d.yaml
• recovery_2d.yaml

You should not resend initial poses to fix this issue. Instead, you should adjust the configuration of drl.

Namely, try to increase these 3 parameters:

• max_correspondence_distance
• convergence_time_limit_seconds
• max_number_of_registration_iterations

You can also try and change the tracking algorithm from icp 2d to the standard icp, by commenting this line and uncommenting this line and then changing transformation_estimation_approach to TransformationEstimationSVD

Lastly, if it still has drift, you can disable the look up tables and use the standard k-d trees for correspondence estimation (they are slower, but you can try then and see if the problem persists). For that, change the correspondence_estimation_approach to CorrespondenceEstimation

Have a nice day,

[ZOUJIASHUAI]

CorrespondenceEstimation

hello @carlosmccosta
I have copy [cluttered_environments_dynamic_large_map_2d.yaml]
and [recovery_2d.yaml] to the drl and change the [correspondence_estimation_approach] in cluttered_environments_dynamic_large_map_2d.yaml to CorrespondenceEstimation,as you can see the link below,the problem still exist,I think this problem is not about the parameter,the point clould topic /dynamic_robot_localization/ambient_pointcloud and laserscan topic still drift slowly to map seems the drift are connect with the running distance,but /dynamic_robot_localization/aligned_pointcloud always aligned with map.maybe some TF code didn`t wrote properly?

https://drive.google.com/file/d/1AA0dVNP_Xgvrz4QMpX9hlSJ_GixUyJhv/view?usp=share_link

[ZOUJIASHUAI]

CorrespondenceEstimation

@carlosmccosta
I found out that the /dynamic_robot_localization/aligned_pointcloud frame id is map,however the /dynamic_robot_localization/ambient_pointcloud frame id is odom,but the /dynamic_robot_localization/localization_detailed frame is map,

[ZOUJIASHUAI]

CorrespondenceEstimation

@carlosmccosta I found out that the /dynamic_robot_localization/aligned_pointcloud frame id is map,however the /dynamic_robot_localization/ambient_pointcloud frame id is odom,but the /dynamic_robot_localization/localization_detailed frame is map,

@carlosmccosta
looks like the accumulation error of odom has been add to loc pose,I set "assemble_lasers_on_map_frame" default="true" ，the drift problem seems to be fixed, but it came out a new problem,when I send initpose topic,the pose seems very unstable,and is very hard to let laser aligned with map at startup. and the pose is not stable as before when the robot is static.
https://drive.google.com/file/d/1Bf7CYHju0hmNpTgOpUr5S3t4NAqcuFPK/view?usp=share_link

[carlosmccosta]

Hello,

The ambient_pointcloud topic should be assembled in the odom frame and not in the map frame, because laser -> odom provides a smooth tf chain, while laser -> map has discrete small jumps that are necessary and introduced to correct the odom drift over time. These jumps distort the point cloud assebly when the spherical interpolation is enabled. As such, the point cloud assemby should not be done on the map frame, otherwise, these distortions will cause major oscilations in the alignment performed by drl.

The aligned_pointcloud is the result of the alignment performed by drl and is on map frame, and as such, is the one you should be analyzing in rviz. You should not evaluate the ambient_pointcloud because that is the input for drl and not its result. And of course, since the ambient_pointcloud is on odom frame, it will show on rviz with the odometry drift. That is working as is supposed to.

These frames names are this way on purpose and follow the standard ros tf for mobile robotics:
https://www.ros.org/reps/rep-0105.html
https://navigation.ros.org/setup_guides/transformation/setup_transforms.html

Looking at your videos, the aligned_pointcloud is correctly overlapping the map. So drl was working correctly all along !
Keep in mind that every localization system has oscillations (which is different than drift) in its pose estimation due to lidar noise and also due to sliding of the alignment within the walls. Namely, if your map has walls with more than one pixel thickness in the occupancy grid, the alignment algorithms may oscillate. This is normal and if you want to minimize this oscillation, you need to have maps with 1 pixel thickness walls.

For inspecting the results of drl, set your rviz world frame to map and inspect the aligned_pointcloud overlap with the occupancy grid / map.
For checking the drl pose, you have the topics localization_pose and localization_detailed
Alternatively, you can check the pose between map and base_link or base_footprint

In drl_configs.yaml you can check the subscribe / input topics and the publish / output topics of drl

Have a nice day,

[ZOUJIASHUAI]

Hello,

The ambient_pointcloud topic should be assembled in the odom frame and not in the map frame, because laser -> odom provides a smooth tf chain, while laser -> map has discrete small jumps that are necessary and introduced to correct the odom drift over time. These jumps distort the point cloud assebly when the spherical interpolation is enabled. As such, the point cloud assemby should not be done on the map frame, otherwise, these distortions will cause major oscilations in the alignment performed by drl.

The aligned_pointcloud is the result of the alignment performed by drl and is on map frame, and as such, is the one you should be analyzing in rviz. You should not evaluate the ambient_pointcloud because that is the input for drl and not its result. And of course, since the ambient_pointcloud is on odom frame, it will show on rviz with the odometry drift. That is working as is supposed to.

These frames names are this way on purpose and follow the standard ros tf for mobile robotics: https://www.ros.org/reps/rep-0105.html https://navigation.ros.org/setup_guides/transformation/setup_transforms.html

Looking at your videos, the aligned_pointcloud is correctly overlapping the map. So drl was working correctly all along ! Keep in mind that every localization system has oscillations (which is different than drift) in its pose estimation due to lidar noise and also due to sliding of the alignment within the walls. Namely, if your map has walls with more than one pixel thickness in the occupancy grid, the alignment algorithms may oscillate. This is normal and if you want to minimize this oscillation, you need to have maps with 1 pixel thickness walls.

For inspecting the results of drl, set your rviz world frame to map and inspect the aligned_pointcloud overlap with the occupancy grid / map. For checking the drl pose, you have the topics localization_pose and localization_detailed Alternatively, you can check the pose between map and base_link or base_footprint

In drl_configs.yaml you can check the subscribe / input topics and the publish / output topics of drl

Have a nice day,

hello @carlosmccosta
I tried all the param you suggest,please watch this video,this is the robot ran 1 hours and stoped,the pose of localization_pose localization_detailed and the map->base_link is x=10.29 y=-15.39,then I send a initalpose at x=10.29 y=-15.39,and the pose of X Y axis has changed over 13cm after initpose to x = 10.42 y=-15.25 which is the correct pose to the real world,the map is 0.02 res,and I sure this is not because of oscillations .
https://drive.google.com/file/d/18HqVuU20vCfN-7rdiI9YQJneGD024vOo/view