[proposal] Localization Covariance Updater

[mertyavuz41]

Hi there,

I think that the lanelet map could be use in the localization arch. to update pose covariance or to detect the lose of the localization. For example, if the lidar pose is not inside the lane we can increase the covariance of the pose, thus we can increase reliability of the system.

|scan_matche_moduler| ->lidar_pose -> |covariance_updater_module| -> cov_updated_pose

Or we can use the center points to initialize the scan matcher. Actually, it could be any source(GNSS/INS, IMU) that we want to update covariance.

I try to say that lanelet map has a good limitation. It would be awesome the use it in the localization arch.

What you think?

[mitsudome-r]

related issue: autowarefoundation/autoware.universe#1375 (comment)

[meliketanrikulu]

Thanks for your suggestion @mertyavuz41 . Maybe the lanelet can be used for error detection. However, the localization error must be detected very quickly so that it does not cause any risk. When the localization is disturbed, it may take a long time for the lanelet to come out of it.

However, can we understand that ndt is broken by following the distance between gnss ins pose and over a certain value?
What do you think about this subject. thanks for sharing your ideas

[mertyavuz41]

However, can we understand that ndt is broken by following the distance between gnss ins pose and over a certain value?
What do you think about this subject. thanks for sharing your ideas

Yes, this is good idea also this approach already used in the first version of the ndt scan matcher implementation of the autoware.AI. (https://github.com/autowarefoundation/autoware_ai_perception/blob/411330c5bb0eb5a963393f869c19d7815bb2dafc/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp#L161)

Maybe the lanelet can be used for error detection. However, the localization error must be detected very quickly so that it does not cause any risk. When the localization is disturbed, it may take a long time for the lanelet to come out of it.

You are right, it may not solve all unexpected case of the localization, but I think that it is a good approach to observe the a few bad case. Also, the lanelet includes such a good information like position, heading, velocity... We can directy compare closest center point of the lanelet's heading value or velocity with ndt heading value to detect lost. This is just a example but I am respect to your experiance if you say that this usesless idea.

[kminoda]

Thanks for sharing your idea @mertyavuz41!

Would you share a sample data or scenario in which the current localization arch would fail but the proposed algorithm would work? I guess it would be much easier for us to discuss your proposal.

[mertyavuz41]

Actually I have not a sample data or scenario to demonstrate to the approach. But I can try to prepare a scenario that represent how I expect to the approach should work.

[mertyavuz41]

Hi,

I throught using the lanelet map information inside the localization architecture,

Update the scan matcher or any position/oriantation source covariance values:

• position update:

The green ones the positions which are the inside lanelet map and red ones are positions on the ouside of the lanelet. We should always expect that we have to be inside lanelet map. Hence, if the scan matcher position fall into outside of the lanelet map for some reason we can increase the covariance value. Same case valid to any position source.

• orientation update:

The red triangler represent a car and red arrow is yaw angle of the car. The green points are center points of the lanelet map and I assue that the each center point has yaw angle, if they have not yaw angle, this information easly computable using consecutive center points. Above approach valid for this case too, we have to expect that the vehicle yaw angle have to be almost same according to nearest center point's yaw angle. Let's say that we have a threshhold for alpha angle, if it pass 40 degree, we can update the yaw angle covariance value.

I have only mentioned the yaw angle here, but the same assumption is valid for pitch and roll angles.

Note: In this approach, it does not change the value of any source, but only offers to perform transactions related to its reliability according to our expectations.