Evaluate performance of localization pipeline

[mitsudome-r]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

Evaluate performance of Localization pipeline.

Purpose

Evaluate performance of perception pipeline to confirm if current perception has enough capability for Localization

Possible approaches

Find available open datasets for evaluation and do benchmark testing.
Possible datasets:

• KITTI dataset

Definition of done

• plan data conversion for open datasets to feed into Autoware localization pipeline
• Develop benchmark tools to test localization pipeline
• Run tests on datasets and make results available in public. (e.g. in documentation)

[angry-crab]

KITTI might not be a good choice since no ground truth PCD map is provided, which is required by the current localization method. An alternative is Newer College Dataset, which provides ground truth PCD map created by Leica.
To use KITTI, another slam algorithm is required.

[angry-crab]

• Build a ground truth map (Compile and run)
• Create script convert raw data to ros2 bag ( python script )
• Implement evaluator (postponed)

[angry-crab]

Current issues:

• The ground truth poses of KITTI is provided by post-processed GPS-RTK. The original IMU data is not released.
• New College Dataset is collected by a hand holding device with rotates a lot.

[angry-crab]

Able to build a pcd map with ground truth poses and point cloud frames, but slow.
Map suffers from distortion. Maybe point cloud has to be compensated due to motion.

[plane-li]

Able to build a pcd map with ground truth poses and point cloud frames, but slow. Map suffers from distortion. Maybe point cloud has to be compensated due to motion.

Hi, which opensource lidar-slam did you use to create the pcd?

[plane-li]

Purpose

Evaluate performance of perception pipeline to confirm if current perception has enough capability for Localization

Sorry, to be honest! I really do not know the meaning of "if current perception has enough capability for Localization"?
Cloud you explain the reason?
Thank you!

[angry-crab]

Able to build a pcd map with ground truth poses and point cloud frames, but slow. Map suffers from distortion. Maybe point cloud has to be compensated due to motion.

Hi, which opensource lidar-slam did you use to create the pcd?

Hi, this map is generated by simply concatenating pcd frames because the dataset provides post-processed ground truth poses. The implementation can be referenced here. pcd_mapping

Purpose

Evaluate performance of perception pipeline to confirm if current perception has enough capability for Localization

Sorry, to be honest! I really do not know the meaning of "if current perception has enough capability for Localization"? Cloud you explain the reason? Thank you!

We are trying to evaluate the accuracy of the localization module of universe to see if it is enough to localize in different settings.

[angry-crab]

Clear map could be generated.

[angry-crab]

• - Sensing modules takes only raw velydone message not pointcloud2 as input. Have to turn off lidar sensing but lidar preprocessing is disabled.
• - twist_estimator requires both imu and vehicle status messages. Have to turn off vehicle_velocity_converter and feed gps velocity into gyro_odometer.
• - use_sim_time requires a simulated clock topic. Have to create a clock topic using kitti.

[mitsudome-r]

Related Discussion: autowarefoundation/autoware#2618

[angry-crab]

Converted sample map and ros2 bag
kitti_sample_data

[angry-crab]

KITTI Localization
Localization is successful.

[angry-crab]

Instructions on running localization with KITTI

Tools could be found in the following repo:
kitti_to_ros2bag

1. Create a pcd map

• clone the repo above
• in create_map.cc, change pose_paths and pcd_paths to where your data path, and path of saving output map.
• mkdir build && cd build, cmake .., make
• run pcd_mapping and wait for the map to be generated.

2. Create a ros2 bag

• in create_bag.py, change base_dir, date, and drive to the data you want to use.
• run create_bag.py
• wait for the bag to be generated.

(A ready-to-go dataset could be found here sample map and bag, I'm not sure if it is correctly shared, please let me know if it could not be accessed. )
3. Bring up Universe

• Follow the instructions below, up to vcs import src < autoware.repos (DO NOT BUILD AT THIS TIME) Autoware Doc
• Terminal 1
  cd src/launcher/autoware_launch
git checkout kitti_test
cd ../../..
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
ros2 launch autoware_launch logging_simulator.launch.xml map_path:=PATH_OF_YOUR_MAP vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit vehicle:=false system:=false map:=true sensing:=true localization:=true perception:=false planning:=false control:=false
• New Terminal 2
  source ~/autoware/install/setup.bash
ros2 launch autoware_launch kitti_static.launch.py
• New Terminal 3
  source ~/autoware/install/setup.bash
ros2 bag play PATH_TO_ROS2BAG/ros2_bag.db3 --qos-profile-overrides-path qos.yaml -d 3
• New Terminal 4
  source ~/autoware/install/setup.bash
ros2 service call /rosbag2_player/toggle_paused rosbag2_interfaces/srv/TogglePaused
• Go to Rviz of Terminal 1, set an intial pose, when Terminal 1 output Alignment called, go to Terminal 4 and rerun the last command. Then you are good to go.
  Check the link below for a video demonstration. Sample Video

[angry-crab]

Duplicates of the related RP is found. Will try to merge this feature into the existing localization_evaluator.

[angry-crab]

Since #928 has already included most features of #1066 , this issue can be closed.
However, #1066 might be able to give a lower error theoretically, since it takes sub routes into consideration for calculation. We can get back to #1066 if needed.