Using a de-grounded point cloud for NDT matching gives a more reasonable match score

cyn-liu commented 2 years ago

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

Autoware uses NDT matching for localization. The NDT input point cloud contains ground, which is less helpful for localization, and the ground is very similar in many locations, so the ground point cloud contributes a non-negligible score in NDT matching, resulting in a still high NDT score when localization is lost, so using a de-grounded point cloud in NDT matching for localization may achieve better results.

Using bag1 for point cloud matching, both the point cloud with ground and the point cloud without ground have high matching scores when the localization is successful. The following are the test videos: bag1: Positioning success - NDT input point cloud with ground bag1: Positioning success - NDT input without ground point cloud The following is a comparison of the NDT matching scores with and without ground point clouds:

Using bag2 for point cloud matching, both the point cloud with ground and the point cloud without ground have high matching scores when the localization is successful. The following are the test videos: bag2: Positioning success - NDT input point cloud with ground bag2: Positioning success - NDT input without ground point cloud The following is a comparison of the NDT matching scores with and without ground point clouds: This proves that de-grounded point clouds can achieve good localization results.

Using bag2 for point cloud matching, the point cloud with ground scored high when the localization failed and the localization failure was not detected in time. The following is the test video: bag2: Positioning failure - NDT input point cloud with ground Using bag2 for point cloud matching, when the localization fails, the point cloud without ground has a very low score and cannot be matched successfully. The following is the test video: bag2: Positioning failure - NDT input without ground point cloud The following is a comparison of the NDT matching scores with and without ground point clouds:

This demonstrates that using a de-grounded point cloud for NDT matching yields a more reasonable score, as well as a faster assessment of whether localization is lost.

Purpose

NDT matching using de-grounded point clouds.

Possible approaches

NDT matching using de-grounded point clouds.

Definition of done

The ground is removed from the original point cloud, and then pre-processing operations such as downsampling are performed on the ground-removed point cloud.

YamatoAndo commented 2 years ago

@cyn-liu Thanks for the suggestion. I thought it was problematic that the score is still high when the localization is lost, because it could be matched with the ground point clouds.

However, if the ground point clouds is removed, the height constraint may be lost. So, how about dividing the score into the following two categories? ・The score with de-grounded point clouds ・The score with only ground point clouds

wjxjmj commented 2 years ago

@cyn-liu Thanks for the suggestion. I thought it was problematic that the score is still high when the localization is lost, because it could be matched with the ground point clouds.

However, if the ground point clouds is removed, the height constraint may be lost. So, how about dividing the score into the following two categories? ・The score with de-grounded point clouds ・The score with only ground point clouds

By height constraint do you mean the altitude of the vehicle (z information in pos)?

In the simulation, although NDT provides z information and EKF filters it using method updateSimple1DFilters (separately from x and y information), z information seems to be of any practical use. Beside, as shown in map_height_fitter_core.cpp, z information can be evaluated only using the map's information. Moreover, the potential loss of Z information does not appear to be a problem at this time.

Can you please explain futher about the height constraint?

YamatoAndo commented 2 years ago

By height constraint do you mean the altitude of the vehicle (z information in pos)?

Yes, correct.

In the simulation, although NDT provides z information and EKF filters it using method updateSimple1DFilters (separately from x and y information), z information seems to be of any practical use. Beside, as shown in map_height_fitter_core.cpp, z information can be evaluated only using the map's information. Moreover, the potential loss of Z information does not appear to be a problem at this time.

Regardless of whether the z information is used or not for other nodes, the z information must also be properly matched since the NDT scan matching is performed in 3D coordinate.

wjxjmj commented 2 years ago

By height constraint do you mean the altitude of the vehicle (z information in pos)?

Yes, correct.

In the simulation, although NDT provides z information and EKF filters it using method updateSimple1DFilters (separately from x and y information), z information seems to be of any practical use. Beside, as shown in map_height_fitter_core.cpp, z information can be evaluated only using the map's information. Moreover, the potential loss of Z information does not appear to be a problem at this time.

Regardless of whether the z information is used or not for other nodes, the z information must also be properly matched since the NDT scan matching is performed in 3D coordinate.

In most senario, point clouds in the environment are sufficient to provide z information. Fig.1 The difference of z information with and without ground points, as shown in the above figure, is not significant. In this figure, z information are collected from these two simulation: bag1: Positioning success - NDT input point cloud with ground bag1: Positioning success - NDT input without ground point cloud. It is shown that de-grounded points can also provides acceptable z information.

kminoda commented 2 years ago

Thanks @wjxjmj, the result you've shared is interesting. Seems the difference is around 10-20 cm?

Since the current Autoware design is dependent on accurate 6-DoF pose estimation, we should discuss from the perspective of perception & planning modules in deciding whether this relatively-inaccurate z estimation is acceptable or not. Just a quick thought, but it would affect the followings:

map-based detection for traffic light recognition (perception)
compare map filter for obstacle segmentation (perception)

IMHO we can consider removing ground information from LiDAR scan if we can guarantee that the estimation difference would not affect the other modules. But unless so, I think it is better to adopt an alternative (more conservative) idea for now, e.g. the idea from @.YamatoAndo above.

cyn-liu commented 2 years ago

Thanks @wjxjmj, the result you've shared is interesting. Seems the difference is around 10-20 cm?

YES.

I think it is better to adopt an alternative (more conservative) idea for now, e.g. the idea from @.YamatoAndo above.

We have discussed the approach proposed by @.YamatoAndo above, but since the NDT matching operation of the localization module is the most time consuming in general, it is likely to be computationally costly to use two types of point clouds for NDT matching.

Is NDT matching using ground point clouds just to get z? But de-ground point clouds matching can also get a similar z. Generally the z obtained by NDT matching is the ground height of the point cloud map at current location, and we find that the accuracy of the height information provided by the point cloud map is poor.

In addition, experiments must be done to prove that using both de-grounded and ground point clouds for NDT matching, to ensure that the localization module works well.

kminoda commented 2 years ago

@cyn-liu His proposal does NOT mean to perform NDT scan matching twice, but to perform NDT scan matching as it is right now and instead add two new metrics (alternative to transform_probability) based on de-grounding idea you've proposed.

So to sum up, there are two ideas came up so far.

Idea 1. de-grounding LiDAR scan (from @.wjxjmj)
Idea 2. adding new de-grounded metric (from @.cyn-liu)

For the idea 1, I guess the point is that removing the ground pointclouds from LiDAR scan may work on some environment, but not on the other environment. To further discuss this idea, I would propose

to share data (like @wjxjmj posted here) in various environments, including feature-rich urban area or feature-less rural area (we would also like to work on this, but not sure when we will be available for this)
to discuss whether the impact on the localization accuracy is acceptable for perception and planning (especially traffic light recognition and compare map filter, as I posted before)

cyn-liu commented 2 years ago

What can be determined so far is that the planning module does not use height information and z does not affect the operation of the planning module. The perception module is still open to debate.

cyn-liu commented 2 years ago

@kminoda Thanks for your reply！

I think you have some misunderstanding about our issue, our point is that using the de-grounded point cloud for NDT matching gives a more accurate score and enables faster and more accurate detection of localization failure.

After your proposal, we also found that whether the accuracy of the localization height has an acceptable impact on the perception module is the next issue to be considered.

After your explanation, we are more confused about YamatoAndo's IDEA, what does dividing the score into the following two categories mean??? How to get the matching score for the de-grounded point cloud and the matching score for the ground-only point cloud if it does not match twice???

kminoda commented 2 years ago

@cyn-liu Thank you for the response! We agree that we should at least discuss the impact on perception module.

How to get the matching score for the de-grounded point cloud and the matching score for the ground-only point cloud if it does not match twice???

I think this is also open question, but I would say that one possible (and also the most simple) way would be to

separate LiDAR scan into ground and non-ground
NDT performs scan matching with concatenated scan (= ground + non-ground = raw LiDAR scan)
after this, we calculate
- 1. transform_probability for ground LiDAR scan and pointcloud map
- 1. transform_probability for non-ground LiDAR scan and pointcloud map

xmfcx commented 1 year ago

@cyn-liu will create a PR soon.

autowarefoundation / autoware.universe