search

rsasaki0109 / lidar_localization_ros2

3D LIDAR Localization using NDT/GICP and pointcloud map in ROS 2 (Not SLAM)
BSD 2-Clause "Simplified" License
287 stars 56 forks source link

How to reduce drift? #41

Open Hailemichael12m opened 4 months ago

Hailemichael12m commented 4 months ago

Hi rsasaki, thanks for this project.

I have been using this localization with nav2 stack (slicing the 3rd dimension to 0) and I have faced some issues.

  1. When using NDT or GICP, system suffers from loss of localization.
  2. While using NDT_OMP, mostly good but drifts in translation after running for a while (I found a drift of 1 m in about 30 mins)
  3. I could not use GICP_OMP because of the following error: ./kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:237: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, unsigned int, pcl::Indices&, std::vector<float>&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple<float>; pcl::Indices = std::vector<int>]: Assertionpointrepresentation->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed. `

I have checked that both the initial map and received clouds are being filtered before registration but I don't know why it still throws this error.

Here is my setup and config:

/**:
  ros__parameters:
        registration_method: "NDT_OMP"
        score_threshold: 2.0
        ndt_resolution: 0.01
        ndt_step_size: 0.01
        ndt_num_threads: 4
        transform_epsilon: 0.001
        voxel_leaf_size: 0.6
        scan_max_range: 50.0
        scan_min_range: 1.0
        scan_period: 0.1
        use_pcd_map: true
        map_path: "/home/map.pcd"
        set_initial_pose: true
        initial_pose_x: 0.0
        initial_pose_y: 0.0
        initial_pose_z: 0.0
        initial_pose_qx: 0.0
        initial_pose_qy: 0.0
        initial_pose_qz: 0.0
        initial_pose_qw: 0.0
        use_odom: true
        use_imu: true
        enable_debug: true
        global_frame_id: map
        odom_frame_id: odom
        base_frame_id: base_link

I have seen from #20 that ndt_resolution should be kept with in 0.5 ~ 1.0 but for me that just makes localization to be lost, plus it also has high cpu usage.

I would appreciate any hint you might give to solve this problem. Thanks!

rsasaki0109 commented 4 months ago

Sorry. GICP_OMP is under-verified. It's possible that NDT and GICP are not processing fast enough. I will provide advice regarding NDT_OMP. The recommended parameters are as follows, and if the CPU usage is high with these settings, I feel this package might not run well on that PC.

/**:
  ros__parameters:
        registration_method: "NDT_OMP"
        score_threshold: 2.0
        ndt_resolution: 1.0 # 0.5 ~ 1.0
        ndt_step_size: 0.1
        ndt_num_threads: 4
        transform_epsilon: 0.1
        voxel_leaf_size: 0.5 # 0.1 ~ 0.5
        scan_max_range: 50.0
        scan_min_range: 1.0
        scan_period: 0.1
        use_pcd_map: true
        map_path: "/home/map.pcd"
        set_initial_pose: true
        initial_pose_x: 0.0
        initial_pose_y: 0.0
        initial_pose_z: 0.0
        initial_pose_qx: 0.0
        initial_pose_qy: 0.0
        initial_pose_qz: 0.0
        initial_pose_qw: 0.0
        use_odom: true
        use_imu: true
        enable_debug: true
        global_frame_id: map
        odom_frame_id: odom
        base_frame_id: base_link
Hailemichael12m commented 3 months ago

Thanks for your response. Can we use GPU accelerators with this package to reduce CPU load?

rsasaki0109 commented 3 months ago

It has not been implemented yet. You might want to consider adding the following: https://github.com/koide3/fast_gicp