hdl_localization: Localization Failed after Several Frames

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?