Fix bug in math::geometry::getClosestPoses
#1475
Merged
math::geometry::getClosestPoses
#1475