Fix bug in math::geometry::getClosestPoses

[f0reachARR]

Description

Abstract

Fix bug in math::geometry::getClosestPoses. The bug is explained in #1332

Details

This PR also added some test cases for math::geometry::getClosestPoses

References

#1332

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[HansRobo]

LGTM with no regression.

But, waiting for internal team now.

[HansRobo]

The internal team confirmation has finished.
I'll merge this after all CIs are finished.

[sonarqubecloud]

Quality Gate passed

Issues
0 New issues
2 Accepted issues

Measures
0 Security Hotspots
78.6% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud