search

carlosmccosta / dynamic_robot_localization

Point cloud registration pipeline for robot localization and 3D perception
BSD 3-Clause "New" or "Revised" License
797 stars 195 forks source link

2D localization mode have roll pitch and Zaxis drit #33

Open ZOUJIASHUAI opened 2 years ago

ZOUJIASHUAI commented 2 years ago

1

ZOUJIASHUAI commented 2 years ago

@carlosmccosta the localization topic also seems have Zaxis drift.

carlosmccosta commented 2 years ago

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 commented 2 years ago

@carlosmccosta hello, I do use the [dynamic_robot_localization_system.launch],and below is the launch parameter. <?xml version="1.0" encoding="UTF-8"?>

carlosmccosta commented 2 years ago

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 commented 2 years ago

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:

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 commented 2 years ago

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 commented 2 years ago

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 commented 1 year ago

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 commented 1 year ago

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:

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:

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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