search

rsasaki0109 / lidarslam_ros2

ROS 2 package of 3D lidar slam using ndt/gicp registration and pose-optimization
BSD 2-Clause "Simplified" License
449 stars 111 forks source link

Initial Pose set true, initial position get shifted issue #70

Closed kimjeff-korea closed 11 months ago

kimjeff-korea commented 11 months ago

Does anyone have the same problem when initial pose is set on the right position but the current pose start at the other place.

rsasaki0109 commented 11 months ago

Do you have anything you can offer me? Parameters, terminal output, video, rosbag, etc.

kimjeff-korea commented 11 months ago

I am testing the algorithm with original parameters except the set_initial_pose parameter.

When the initial_pose is set, the current_pose get transformed as expected but 2 meters shifted is the problem.

j@j:~$ ros2 run lidarslam lidarslam registration_method:NDT ndt_resolution[m]:5 ndt_num_threads:0 gicp_corr_dist_threshold[m]:5 trans_for_mapupdate[m]:1.5 vg_size_for_input[m]:0.2 vg_size_for_map[m]:0.1 use_min_max_filter:false scan_min_range[m]:0.1 scan_max_range[m]:100 set_initial_pose:true use_odom:false use_imu:false scan_period[sec]:0.1 debug_flag:false map_publish_period[sec]:15 num_targeted_cloud:10

[INFO] [1686612220.178196029] [scan_matcher]: initialize Publishers and Subscribers [INFO] [1686612220.179587287] [scan_matcher]: initialization end [INFO] [1686612220.181837643] [graph_based_slam]: initialization start registration_method:NDT voxel_leaf_size[m]:0.2 ndt_resolution[m]:5 ndt_num_threads:0 loop_detection_period[Hz]:1000 threshold_loop_closure_score:1 distance_loop_closure[m]:20 range_of_searching_loop_closure[m]:20 search_submap_num:3 num_adjacent_pose_cnstraints:5 use_save_map_in_loop:true debug_flag:false

[INFO] [1686612220.182158226] [graph_based_slam]: initialize Publishers and Subscribers [INFO] [1686612220.182683501] [graph_based_slam]: initialization end [INFO] [1686612222.132553256] [scan_matcher]: create a first map [INFO] [1686612223.248649765] [scan_matcher]: initial_pose is received [INFO] [1686612237.260371774] [scan_matcher]: publish a map number of map　points: 109938 [INFO] [1686612257.606718771] [scan_matcher]: publish a map number of map　points: 226481

Here are a few images on Rviz.

left upper side axis is the published initial pose, left bottom axis is the reference odom axis, and the right axis is the current pose from lidar_slam.

I was wondering if someone had same issue. I might have missed something.

Screenshot from 2023-06-13 08-17-25

kimjeff-korea commented 11 months ago

Found the solution for the issue. Creating a first map before the initial_pose received occurs the shift error.

Making sure let the slam node gets initial_pose before creating first map.

j@j:~$ ros2 run lidarslam lidarslam registration_method:NDT ndt_resolution[m]:5 ndt_num_threads:0 gicp_corr_dist_threshold[m]:5 trans_for_mapupdate[m]:1.5 vg_size_for_input[m]:0.2 vg_size_for_map[m]:0.1 use_min_max_filter:false scan_min_range[m]:0.1 scan_max_range[m]:100 set_initial_pose:true use_odom:false use_imu:false scan_period[sec]:0.1 debug_flag:false map_publish_period[sec]:15 num_targeted_cloud:10

[INFO] [1686619327.045654622] [scan_matcher]: initialize Publishers and Subscribers [INFO] [1686619327.046985028] [scan_matcher]: initialization end [INFO] [1686619327.049016834] [graph_based_slam]: initialization start registration_method:NDT voxel_leaf_size[m]:0.2 ndt_resolution[m]:5 ndt_num_threads:0 loop_detection_period[Hz]:1000 threshold_loop_closure_score:1 distance_loop_closure[m]:20 range_of_searching_loop_closure[m]:20 search_submap_num:3 num_adjacent_pose_cnstraints:5 use_save_map_in_loop:true debug_flag:false

[INFO] [1686619327.049276943] [graph_based_slam]: initialize Publishers and Subscribers [INFO] [1686619327.049737715] [graph_based_slam]: initialization end [INFO] [1686619334.122100180] [scan_matcher]: initial_pose is received [INFO] [1686619335.526126605] [scan_matcher]: create a first map [INFO] [1686619353.498211912] [scan_matcher]: publish a map