Localization Failed after Several Frames

[zxl19]

Hi, thank you for your work! The demo bag worked well on my computer. So I decided to test the algorithm on my own data. I built a map of a parking lot(150m*100m) and tested hdl_localization on it(the start is at the bottom right corner).

I changed the launch file to accommodate my Robosense-32 LiDAR:

<?xml version="1.0"?>
<launch>
  <!-- arguments -->
  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
  <arg name="points_topic" default="/middle/rslidar_points" />
  <arg name="imu_topic" default="/ox_imu/data" />
  <arg name="odom_child_frame_id" default="middle_lidar" />

  <!-- in case you use velodyne_driver, comment out the following line -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>

    <!-- globalmap_server_nodelet -->
    <node pkg="nodelet" type="nodelet" name="globalmap_server_nodelet" args="load hdl_localization/GlobalmapServerNodelet $(arg nodelet_manager)">
      <param name="globalmap_pcd" value="$(find hdl_localization)/data/map.pcd" />
      <param name="downsample_resolution" value="0.05" />
    </node>

    <!-- hdl_localization_nodelet -->
    <node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
        <remap from="/velodyne_points" to="$(arg points_topic)" />
        <remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
        <!-- odometry frame_id -->
        <param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" />
        <!-- imu settings -->
        <!-- during "cool_time", imu inputs are ignored -->
        <param name="use_imu" value="true" />
        <param name="invert_imu" value="true" />
        <param name="cool_time_duration" value="2.0" />
        <!-- ndt settings -->
        <!-- if NDT is slow for your PC, try DIRECT1 serach method, which is a bit unstable but extremely fast -->
        <param name="ndt_neighbor_search_method" value="DIRECT7" />
        <param name="ndt_resolution" value="1.0" />
        <param name="downsample_resolution" value="0.1" />
        <!-- if "specify_init_pose" is true, pose estimator will be initialized with the following params -->
        <!-- otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" -->
        <param name="specify_init_pose" value="true" />
        <param name="init_pos_x" value="0.0" />
        <param name="init_pos_y" value="0.0" />
        <param name="init_pos_z" value="0.0" />
        <param name="init_ori_w" value="1.0" />
        <param name="init_ori_x" value="0.0" />
        <param name="init_ori_y" value="0.0" />
        <param name="init_ori_z" value="0.0" />
    </node>
</launch>

I used the same bag to build the map and to test the localization algorithm. For the first few frames the localization went on pretty well:

However, the localization fails after several steps, remaining at the same spot:

I noticed similar issues like #25 #30 and #31 and tried the solutions you gave. I tried different downsample rates of both the map and the pointcloud, different neighbor search methods, with and without IMU, but the problem still exists. Could you please give me some advice on this problem? Is it a problem of parameter tuning(too sparse map vs. too dense pointcloud) or is it specific to the scenario I'm using(lack of features)? Also, when changing the downsample rates, I received warnings like:

[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

I'm planning to use the algorithm on a larger map. Does this constrain the map size I'm using?

[koide3]

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? https://github.com/koide3/hdl_localization/blob/a76067cfeb89d11188e4bd01495af57880574e9a/launch/hdl_localization.launch#L26

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.

[zxl19]

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?

[lixiang103]

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?

[zxl19]

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.

[zxl19]

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.

[yzhou377]

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

[Xiangzhaohong]

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 .

[Babaevzhu]

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

[WilliamWoo45]

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.

[WilliamWoo45]

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!!

[koide3]

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.

[WilliamWoo45]

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

[koide3]

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

[WilliamWoo45]

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

[koide3]

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.

[koide3]

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

[WilliamWoo45]

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