The way the ICP covariance calculations are being applied to the output ROS message is incorrect in several ways.
First, the wrong values are being used and, second, the 2x2 xy covariance matrix needs to be rotated into the odom frame from the laser frame of the reference keyframe.
The way the ICP covariance calculations are being applied to the output ROS message is incorrect in several ways.
First, the wrong values are being used and, second, the 2x2 xy covariance matrix needs to be rotated into the odom frame from the laser frame of the reference keyframe.
This pull request addresses those issuss.
current incorrect covariances (video)
fixed covariances (video)