LOAM Based Localization Task 4 - Implement feature matching

[ataparlar]

Checklist

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

Description

In order to find the pose of the ego vehicle in the environment, the transform between each point cloud frame and the edge feature map must be known.

Purpose

Completing the feature matching between point cloud frame and previously generated edge point cloud.

Possible approaches

• This repository can be used as a reference: https://github.com/tier4/lidar_feature_extraction/tree/develop

Definition of done

• [x] Match the point cloud features
• [ ] Find the transform between them

[ataparlar]

Hi everyone,

I am working with LIO-SAM directly because it is our implementation source.

When I change the feature matching point cloud source with our ground truth feature maps instead of the map generated by itself and configure the program the following results occur.

The turquoise points are the corner feature cloud that we generated before. In the first two photos, we observed that it matches the features well and puts the lidar_linkinto the correct position. In the 3rd and 4th images, while turning in the tunnel, the map corrupts. The position comes from IMU processing is got too far from the LiDAR feature matched position. This could be related to IMU data usage in the LIO-SAM.

The below images are taken in a different test with a feature point cloud map that relies on over 20 kilometers. LIO-SAM could not process any correct lidar_link position in this test and didn't move in to correct direction.

Depending on the situation, I can try it with dynamic point cloud loading or directly try with LOAM instead of LIO-SAM. More updates will follow.

[ataparlar]

I tried to downsample the point clouds. It really improved the performance. It works well with tunnel and bridge datasets. Rosbags were running at low frequency. The only way to observe it was to slow down the rosbags because of the computational cost of larger point cloud maps. Here are some images:

We may use this with dynamic point cloud loading for Autoware and then it will work like a charm in my opinion.

[ataparlar]

I have closed the GPS integration and loop closure detection. So, the code works faster with denser point clouds.

I noticed an error in the pipeline. It can be related to point cloud undistortion or calibration angles. I changed the calibration angles and I observed no difference.

Here are the images and videos of the error.

https://youtu.be/8XVvodyqYUI

White points show the ground truth point cloud to match the features. Colored points are the points overlayed on the exported TF.

[ataparlar]

That is caused by an error in point cloud undistortion. The correction with the linear acceleration was disabled. When it is enabled, the problem eliminated.

[ataparlar]

We have the TF published from the package. It works as we want but there is a time-consuming error available.

The optimization part lasts long right now. The total time of scan to map optimization for each frame is more than 650 milliseconds. The part that lasts very long is here. This is a kdsearch process and I am investigating why this happens. Here is the debug output of loam_feature_localization. Please note that the time numbers are in milliseconds:

[component_container_mt-1] end_update_initial_guess time: 0
[component_container_mt-1] extract_surrounding_key_frames time: 0
[component_container_mt-1] downsample_current_scan time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 14
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 14
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 14
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1]  corner_optimization time: 2
[component_container_mt-1]  surface_optimization time: 15
[component_container_mt-1]  combine_optimization_coeffs time: 0
[component_container_mt-1] scan_to_map_optimization time: 664