ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Thu, 08 Oct 2020 02:43:43 -0500How to update map to odom with corrections from SLAM ?https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/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.Mon, 19 Jan 2015 18:51:14 -0600https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/Answer by Stefan Kohlbrecher for <p>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.</p>
<p>The strategy we are trying right now looks like:</p>
<pre><code>//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
</code></pre>
<p>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.</p>
<p>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.</p>
<pre><code>//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())));
</code></pre>
<p>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?</p>
<p>Any help would be appreciated, thanks in advance.</p>
<p><strong>Edit:</strong>
The second approach is correct way to solve the problem.</p>
https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/?answer=201338#post-id-201338You can have a look at existing implementations of this:
- [slam_gmapping L514](https://github.com/ros-perception/slam_gmapping/blob/hydro-devel/gmapping/src/slam_gmapping.cpp#L514)
- [AMCL L1167](https://github.com/ros-planning/navigation/blob/hydro-devel/amcl/src/amcl_node.cpp#L1167)
These examples should both do what you want (and given the ubiquitous use of both approaches they should work :) ).Tue, 20 Jan 2015 01:30:58 -0600https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/?answer=201338#post-id-201338Comment by indraneel for <p>You can have a look at existing implementations of this:</p>
<ul>
<li><a href="https://github.com/ros-perception/slam_gmapping/blob/hydro-devel/gmapping/src/slam_gmapping.cpp#L514">slam_gmapping L514</a></li>
<li><a href="https://github.com/ros-planning/navigation/blob/hydro-devel/amcl/src/amcl_node.cpp#L1167">AMCL L1167</a></li>
</ul>
<p>These examples should both do what you want (and given the ubiquitous use of both approaches they should work :) ).</p>
https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/?comment=363041#post-id-363041Thanks brilliant suggestion!Thu, 08 Oct 2020 02:43:43 -0500https://answers.ros.org/question/201314/how-to-update-map-to-odom-with-corrections-from-slam/?comment=363041#post-id-363041