Invalid quaternion in rotation_valid function in ndt_localizer_nodes.hpp
Description
Invalid assignment to quaternion in function rotation_valid in ndt_localizer_nodes.hpp causing to reject all results from NDT as invalid angle
Current implementation is
virtual bool rotation_valid(const PoseWithCovarianceStamped & pose, const Transform guess)
{
EigRotation pose_rotation{pose.pose.pose.orientation.w,
pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z
};
EigRotation guess_rotation{guess.transform.rotation.x, // <---- here should be w component
guess.transform.rotation.x,
guess.transform.rotation.y,
guess.transform.rotation.z
};
return std::fabs(pose_rotation.angularDistance(guess_rotation)) <=
(m_predict_rotation_threshold + EPS);
}
The expected implementation
virtual bool rotation_valid(const PoseWithCovarianceStamped & pose, const Transform guess)
{
EigRotation pose_rotation{pose.pose.pose.orientation.w,
pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z
};
EigRotation guess_rotation{guess.transform.rotation.w,
guess.transform.rotation.x,
guess.transform.rotation.y,
guess.transform.rotation.z
};
return std::fabs(pose_rotation.angularDistance(guess_rotation)) <=
(m_predict_rotation_threshold + EPS);
}
How to Reproduce
Run P2DNDTLocalizerNode
Current Behavior
All results are rejected failing to satisfy the rotation threshold
Expected behavior
Only NDT results that truly exceed the rotation threshold should be rejected