I have been trying to look into hector mapping code specifically how the pose covariance is estimated (geometry_msgs/PoseWithCovariance). In Scanmatcher.h (matchData function), the matrix (H) calculated during the Gauss Newton optimization is published directly in the update function (in HectorMappingRos.cpp) as the pose covariance.
In contrast, in the hector mapping paper (A Flexible and Scalable SLAM System with Full 3D Motion Estimation) the authors mention that the covariance matrix can be calculated by using the unscented kalman filter or by the approximation of the Hessian as
R = $\sigma^2 . (H)^{-1}$ (Eq 15 in paper) -- apologies for the latex code,
however this equation does not appear in the code (neither is the unscented transform used for estimating the covariance).
I would like to inquire if anyone has used the pose covariance from hector mapping? If the matrix H is directly used as the covariance, will the results be different from those by calculating the pose covariance using the above mentioned methods (Unscented transform and Eq 15). If anyone has experimented with this and has some opinion on it, it will be really helpful for me.
↧