LOAM Based Localization Task 4 - Implement feature matching

[ataparlar]

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

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

• 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

[ataparlar]

We noticed that the optimization process makes 30 iterations every time. This was related to the map size and we tried to use the map dynamically with octrees. Red points show the Surface Feature Map and turquoise points show the Corner Feature Map:

We observed that in that way, our optimization time is reduced from an average of 1 second to 500 milliseconds. Please note that this is for 30 iteration counts. Normally, the optimization is complete in 10 iterations. So, the runtime will be normal. Still, we are testing.

However, in some areas, the optimization fails and position cannot be found. I am investigating that right now. The image of the positions is below:

[ataparlar]

We noticed some problems related to reading the points. Normally, each point in the dataset has a 16-bit unsigned integer time field inside it. The program tries to read it as float. So, the time difference between the points cannot be detected and the point cloud undistortion never worked. We have fixed that.
Additionally, we noticed something with acceleration directions in the dataset and fixed that in the code. Now, the program works fluently without breaking. Here is the situation:

• Red points: Surface Map Points
• Turquoise points: Corner Map Points
• Yellow Points: Surface Cloud Points
• Blue Points: Corner Cloud Points
  

In the video below, you can see the performance in the Eurasia Tunnel (the long tunnel). In the end, LOAM stuck but we solved that with IMU bias parameters. So, we can get in and get out of the Eurasia Tunnel.
https://www.youtube.com/watch?v=lo1u--8MLwQ

We recorded a different video after the tunnel. We travel here on a highway and travel the Bosphorus Bridge on this part. After the city environment, we get into the Bomonti Tunnel (small tunnel) and the feature matching fails here:
https://www.youtube.com/watch?v=JO3q8bNIxqs This video only contains the area after the Eurasia Tunnel. As you can see, the rosbag leaps frames. This is related to my ethernet settings at that moment. So, the error in the Bomonti Tunnel might be related to that because we have successfully traversed this part in a different dataset.

It doesn't break in the long tunnel, because there are very many features to match. In the small tunnel, there are fewer and sometimes it struggles to find the perfect position. I am working on it and I will keep here posted. It is nearly done.

[ataparlar]

Hi,

I have worked on a modified LIO-SAM repo until now. Here is the link. This branch works with MGRS point cloud map and the XYZ coordinates of all of the points were very large. So, we are translating the point cloud segments to local and calculating the position here. After that, we translate back the calculated pose to the global. In that scenario, the pose is calculated fine but due to the large coordinate numbers, imuPreintegration can corrupt or precision reduces for position calculation. In the end, the position inside of a tunnel can't be calculated. It is stuck.
You can find the MGRS point clouds here.

• You need to change line 11 and 12 in config/params.yaml to work with the point clouds.
• We changed the allocateMemory() function in that file to initialize ground truth point clouds and octree objects.
• We are loading the initialized point cloud in the same function.
• extractCloud() function is changed for loading the new map part.
• The point P(66459, 43620, 42.75) is the starting point in MGRS point cloud. So, the first point cloud will be loaded around that area. After the lidar position comes, lidar_link is translated to that area.
• For translating the first position, we have changed

In this branch, which works with a local point cloud map, we do not have that many large coordinates. So, we don't translate anything to local and calculate everything as where it is. In that scenario, we are able to travel the long tunnel (this one has more features) and we are able to travel every urban or highway area. However, it is stuck in the second tunnel. It is 3-4 kilometers far from the origin and it has much fewer features than the long tunnel. Still, we were able to travel that tunnel with a map starting from the start of the tunnel. We have tested that with previously collected data before.
You can find the local point clouds here.
You need to change line 11 and 12 in config/params.yaml to work with the point clouds.

• We changed the allocateMemory() function in that file to initialize ground truth point clouds and octree objects.
• We are loading the initialized point cloud in the same function.
• extractCloud() function is changed for loading the new map part.

So, as far as I observed, the distance from the origin is the issue right now. Used libraries don't accept double type so I couldn't change it.

[xmfcx]

• See LOAM based localization #7454 (comment) for the final status.

Closing this since there are no resources to complete this task for now.

If anyone is interested, feel free to reopen.