filtering out accurate pose
How can the map position calculated from the slam/localization framework can be filtered out in each iteration inside the code? Currently i'm doing it by if new pose has more less covariance than this new pose is considered as more accurate pose.
double covariance_radi = robotPose.pose.covariance[0] > robotPose.pose.covariance[7] ? robotPose.pose.covariance[0] : robotPose.pose.covariance[7];
double covariance_yaw = robotPose.pose.covariance[35];
bool more_accurate = true;
if(readOnly) {
more_accurate = covariance_radi < prev_covariance_radi ? true : false;
}
ROS_INFO("covariance_radi: %f, covariance_yaw: %f, more_accurate: %d", covariance_radi, covariance_yaw, more_accurate);
if(covariance_radi < max_covariance_radi && covariance_yaw < max_covariance_yaw && more_accurate) {
prev_covariance_radi = covariance_radi;
Is their any better method than this also to filter out the accurate map pose in each iteration?
I'm using ros noetic right now. And for map localization i'm using fiducial_slam framework. I'm testing all in ubuntu 20.04.
Asked by dinesh on 2022-02-22 03:53:35 UTC
Comments