Localization Failed after Several Frames about hdl_localization HOT 17 CLOSED

@WilliamWoo45
I just pushed a fix to the fix_stamp branch. Could you check if it resolves the issue?

from hdl_localization.

Hi @koide3,

Thanks for your prompt actions.

I confirm now it works pretty good on all my testing datasets.
There would be no drifts whether by giving the roughly correct initial pose by 2D Pose Estimate in Rviz, or by calling the /relocalize service.

Cheers!

Best Regards,
William

from hdl_localization.

Hi @koide3,

Thanks for your prompt responses.

Yes, it is quite strange.....The mobile robot was driven manually with a almost constant velocity (around 0.4m/s) when collecting the localization data, and the use_imu was set to false.

To reproduce the problem, I've uploaded the following files in the OneDrive link:

1. Carpark localization rosbag,
2. The corresponding .pcd maps (built by Fast-LIO2 and built by hdl_graph_SLAM, you can try either one or both)
3. The information for the global localization waypoints. 10 waypoints were randomly selected and you can play with any one of them.
4. My hdl_localization.launch file

Actually I found that hdl_localization can track the robot pose quite well when the localization starts at some places near (0m, 0m) and yaw angle close to 0 degrees, and there would be no drift at all during this tracking process. Thus, the position and orientation information of each global localization waypoint was collected by running the hdl_localization with the initial pose (0m, 0m, 0 degrees) and echo the rostopic /status.

Looking forward to your comments and have a nice day!

Best Regards,
William

from hdl_localization.

I saw a similar issue when IMU is not properly calibrated w.r.t. LIDAR. Can you re-check if you see this issue when use_imu is set to false?

Regarding downsampling, I think resolution = 0.05 is too small for the large environment. Try to change it to 0.2 ~ 0.5. If the warning Leaf size is too small... persists, try to change pcl::VoxelGrid in hdl_localization_nodelet.cpp to pcl::ApproxVoxelGrid that can properly handle small resolutions and large point clouds.

from hdl_localization.

Hi @koide3

try to change pcl::VoxelGrid in hdl_localization_nodelet.cpp to pcl::ApproxVoxelGrid that can properly handle small resolutions and large point clouds.

I changed the voxelgrid filtering method and the warning no longer appears, thanks!

Can you re-check if you see this issue when use_imu is set to false?

I switched this parameter to see the difference, but the problem persists. In fact, I tried different parameter combinations, namely different downsample rates of both the map and the pointcloud, different neighbor search methods, with and without IMU, but localization still fails after several steps.

Also I'm wondering what is the effect of ndt_resolution, can I tune this parameter for a better performance?

from hdl_localization.

Hello, I'm having the same problem with you, but changing the resolution and downsampling methods doesn't work. My car is still stuck in the initial position, has your problem been solved?

from hdl_localization.

Hi @lixiang103

I haven't solved this problem yet and I'm still waiting for the author's reply. I ran the demo bag at first and it worked on well. When I ran my own bag, the localization failed after the initial few frames. Here are several assumptions that I haven't tested:

1. Hdl_localization uses UKF, I think we should check the filter status and parameter configurations.
2. The ground plane may influence the performance of registration algorithms. I think we should filter the ground in LiDAR scans to see if it gets any better.
3. Or it could be specific to the data we use, the lack of features, etc.

I'm still trying those methods and do please let me know your results. It would be wonderful if @koide3 could help us with the questions above.

Sincerely.

from hdl_localization.

Hi @lixiang103 !

I'm going to check the results of the demo bag. I'm a beginner in SLAM as well, but I'll see what I can do. I will contact you afterward. Also, you might need to edit your reply above to delete your Wechat ID to protect your privacy.

Best regards.

from hdl_localization.

I am using the same kind of LIDAR and facing the same problem... Any findings so far?

from hdl_localization.

I find that when I play a bag , localization would have this problem
but I use it realtime in my robot, it works well !
I use jetson xavier and rslidar32 .

from hdl_localization.

hi @zxl19，you can add into the launch file, and it may work

from hdl_localization.

Hi @lixiang103

I haven't solved this problem yet and I'm still waiting for the author's reply. I ran the demo bag at first and it worked on well. When I ran my own bag, the localization failed after the initial few frames. Here are several assumptions that I haven't tested:

1. Hdl_localization uses UKF, I think we should check the filter status and parameter configurations.
2. The ground plane may influence the performance of registration algorithms. I think we should filter the ground in LiDAR scans to see if it gets any better.
3. Or it could be specific to the data we use, the lack of features, etc.

I'm still trying those methods and do please let me know your results. It would be wonderful if @koide3 could help us with the questions above.

Sincerely.

Hi @zxl19, I'm having a similar issue with you. I use Fast-LIO2 to build the point cloud map and I'm using Ouster-32 LiDAR.
When I was trying to do /relocalize in a carpark environment, the robot was successfully localized in the first few frames, as can be seen in here:

Then it drifts away and obviously the localization fails:

The use_imu was set to false. I changed the voxelgrid filtering method to ApproximateVoxelGrid, and I also tweaked the downsample & ndt resolutions. But the localization still failed.
Similar results can be seen from the experiments in an industrial factory environment as below, where initially it succeed:

Then few seconds later, it failed:

May I know how did you manage to solve it? Thanks!
Also appreciate @koide3, if you could give some suggestions and comments. I discussed with my colleagues and found that some of them have similar issues when runing the hdl_localization whether using Velodyne or Ouster LiDAR.

from hdl_localization.

To addon:

I recently found that for the aforementioned issues (i.e., /relocalize), there would be no drift as long as specify_init_pose is set to true and a roughly accurate initial pose is given in the hdl_localization.launch file.

However, if the initial pose information is obtained by manually specifying 2D pose estimate in the Rviz, or by rosservice call /relocalize as mentioned in issue#68 (even if I manually checked through the point cloud alignment and make sure the obtained initial poses were quite accurate), the robot will still drift after a few seconds. This is really weired.

So I'm wondering does this hdl_localization algorithm inherently set the initial pose as (0 m, 0 m, 0 degrees), thus the UKF was influenced by this "bad" initial pose information and give wrong predictions which cause the drifts later on?
If so, does the specify_init_pose command has a higher execution priority than the defaultly set initial pose (0 m, 0 m, 0 degrees), so the UKF was not influenced by the "bad" initial pose information?

Much appreciated if @koide3 @narutojxl could help to clarify this issue. Thanks a lot!!

from hdl_localization.

Hi @WilliamWoo45 ,
It's very strange behavior, and I suspect there are some issues in velocity or IMU bias estimation. Is it possible to provide some small test data to reproduce the problem? I would like to try to resolve it in the next weekend.

from hdl_localization.

Hi @WilliamWoo45 ,
Thanks for sharing the data. I tested the localizer on the given dataset and launch file, but I couldn't reproduce the issue. Could you a bit elaborate the reproducing procedure? I tried:

1. Launch the given hdl_localization.launch with the initial pose of a random waypoint (e.g., waypoint#6)
2. Play carpark*.bag with the given starting time (e.g., -s 145)
3. Call /relocalize and/or use 2D Pose Estimate

from hdl_localization.

Hi @koide3,

Thanks for your comments.

The reproducing procedure 1 -> 2 mentioned by you is correct. If you set specify_init_pose to true and input the roughly correct initial pose information in the given file hdl_localization.launch, hdl_localization is able to correctly track the robot all the time without drift.

However, you will observe that the robot may be localized correctly at the beginning for a few seconds, then the robot would be drifting away later on, for any one of the following reproducing procedures:

Procedure #1 that would cause failure: launch the given hdl_localization.launch with specify_init_pose set to false, then give the roughly correct initial pose by 2D Pose Estimate in Rviz, or by calling the /relocalize service to obtain the initial pose.

Procedure #2 that would cause failure: launch the given hdl_localization.launch with specify_init_pose set to true, and set the initial pose by default as (pos_x=0, pos_y=0, pos_z=0, ori_x=0, ori_y=0, ori_z=0, ori_w=1), then give the roughly correct initial pose by 2D Pose Estimate in Rviz, or by calling the /relocalize service to obtain the initial pose.

Hope this clarifies.

Best Regards,
William

from hdl_localization.

Hi @WilliamWoo45 ,
Thanks for your clarification. I could reproduce the issue.

It seems the cause is that while the pose estimator is initialized with the system time (ros::Time::now()), the data timestamp (msg->header.stamp) is used for updating the estimator. In case the data timestamp is not synchronized with the system clock (e.g., in the given rosbag, data timestamps were around 850 s while the system clock was around 1662470423 s), a time condition check in the state prediction always fails and prevents the prediction pose from being updated.

I'll push a workaround to fix the issue soon.

from hdl_localization.