Ask Your Question

How to update map to odom with corrections from SLAM ? [closed]

asked 2015-01-19 18:51:14 -0500

ajain gravatar image

updated 2015-05-29 13:30:06 -0500

Our basic goal is to publish a map to odom transform that corrects for the drift of our odometry. We have a localization algorithm that uses landmarks to give an absolute position wrt the map frame. Our problem so far has been with how to best achieve this using the tf library.

The strategy we are trying right now looks like:

//Get newPose
tf::Pose newPose = getPoseFromLandmarks();
//Get difference between current pose (which may be different than previous getPoseFromLandmarks because of odom updates) and newPose
tf::Transform poseDifference = currentPose.inverse() * newPose;
//Apply that difference to our current map to odom transfrom
tf::Transform newMapToOdom = poseDifference * currentMapToOdom;
//Now broadcast the transform and we're done

This was working fine up until we started calculating rotation in getPoseFromLandmarks(). When we apply rotation to the newPose, the poseDifference as well as the newMapToOdom values are not as we expect. It seems that the rotation happens before the translation (e.g. {x,y,z of poseDifference} != {x,y,z currentPose} - {x,y,z newPose}). This usually causes the corrections to accumulate and our x,y,z quickly goes to infinity.

We have tried every combination we could think of to make this work; right vs left multiply, changing what we inverse and where, calculating translation difference separately from rotation difference. The only successful strategy we have had is to manually calculate the difference in the pose origins, manually calculate the difference in the yaw of both poses (which works since we are only updating the robots yaw for now), and then manually apply these results to the currentMapToOdom transform.

//Get the difference in translation and yaw 
poseDifference.setOrigin(newPose.getOrigin() - currentPose.getOrigin());
poseDifference.setRotation(tf::createQuaternionFromYaw(tf::getYaw(newPose.getRotation()) - tf::getYaw(currentPose.getRotation())));

//Apply the difference to correct mapToOdom
newMapToOdom.setOrigin(currentMapToOdom.getOrigin() + poseDifference.getOrigin());
newMapToOdom.setRotation(tf::createQuaternionFromYaw(tf::getYaw(currentMapToOdom.getRotation()) + tf::getYaw(poseDifference.getRotation())));

While this works, it feels really hacky and won't work roll/pitch corrections. It seems like there should be a more elegant way to achieve this with TF math, or maybe a better strategy for calculating the corrected mapToOdom transform?

Any help would be appreciated, thanks in advance.

Edit: The second approach is correct way to solve the problem.

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by ajain
close date 2015-05-29 13:30:28.545430

1 Answer

Sort by ยป oldest newest most voted

answered 2015-01-20 01:30:58 -0500

You can have a look at existing implementations of this:

These examples should both do what you want (and given the ubiquitous use of both approaches they should work :) ).

edit flag offensive delete link more


Thanks brilliant suggestion!

indraneel gravatar image indraneel  ( 2020-10-08 02:43:43 -0500 )edit

Question Tools

1 follower


Asked: 2015-01-19 18:51:14 -0500

Seen: 1,121 times

Last updated: May 29 '15