pose estimation is not converging

[ahsan155]

I launched autoware with command ros2 launch autoware_launch autoware.launch.xml map_path:=MAP_PATH vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit launch_perception:=false launch_planning:=false launch_control:=false
I only have one lidar attached to vehicle and its publishing data at /localization/util/downsample/pointcloud as expected by ndt. The vehicle is stationary as it records data. As ndt is doing localization, vehicle position keeps moving back and fourth.

Screencast.from.06-09-2024.02.49.06.PM.mp4

baselink and ndt_base_link estimated by ndt are very close.

I am also getting log that '[localization.pose_estimator.ndt_scan_matcher]: pose_buffer.size() < 2'. shouldn't ekf_localizer and ndt_scan_matcher start exchanging information when lidar data start publishing, so there will be more pose in buffer?

what could be the reason that pose is not converging?

[SakodaShintaro]

First of all, LiDAR data contains fine noise, so if you use only LiDAR for self-pose estimation, it is possible that movements of this magnitude will occur.

However, I am concerned about the large number of pose_buffer_.size() < 2 displayed.

In the basic settings, the EKF runs at 50Hz and the NDT runs at 10Hz (this depends on the LiDAR frequency), so it is expected that there will be about five pieces of data when the NDT runs.

What will the following commands do?

ros2 topic hz /localization/pose_estimator/pose_with_covariance
ros2 topic hz /localization/kinematic_state

[ahsan155]

the above two command returns

I am getting the data from carla with lidar config:

self.lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('range', '20')
self.lidar_bp.set_attribute('rotation_frequency', '80')      
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('points_per_second', '320000')

[SakodaShintaro]

It seems that LiDAR data is coming in quite frequently.

If there are not two or more EKF data when NDT runs, the data will be skipped

• nullopt is returned from pose_buffer_
  • https://github.com/autowarefoundation/autoware.universe/blob/251dfb55aae5de7da1dd15c8eb3d5483aa2cc432/localization/localization_util/src/smart_pose_buffer.cpp#L34-L37
• If nullopt is returned, NDT will not run
  • https://github.com/autowarefoundation/autoware.universe/blob/251dfb55aae5de7da1dd15c8eb3d5483aa2cc432/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L401-L410

There is a slight possibility that the instability of the pose is due to the NDT frequency not being stable.

Since there is no odometry, it is inevitable that it will be unstable, but it may be possible to stabilize it by tweaking the covariance value.

• https://github.com/autowarefoundation/autoware_launch/blob/7f8a7af3b54f73a403ded4cd96147b5a58fd69ad/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml#L92-L100
• https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/localization/ekf_localizer.param.yaml#L24-L28

[thiagodeborba]

@ahsan155 could you solve this problem?