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

edit

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.