Calculate the transformation from map to odom

edit

Hi all,

For the last a few days, I am struggling with the correct way to calculate the transformation from map to odom. I have a robot system, which publish odom message. Since odom drift, I wrote my own algorithm. My algorithm can compute the location and orientation of the robot in the world (map) system. And I would like to publish a "map" frame and the transformation from map to odom.

I am working with a 2D robot and suppose geometry_msgs::Point position1 is the robot location in odom, geometry_msgs/Quaternion orientation1 is the robot orientation in odom, x, y, theta is real location and orientation of the robot in the world system I calculated with my algorithm. theta uses the x axis as 0 degree. What is the correct way to calculate the transformation from map to odom and publish it in tf?

Below is the code I am using to publish tf:

tf::StampedTransform tf_map_to_odom;

// set up parent and child frames
tf_map_to_odom.frame_id_ = string("map");
tf_map_to_odom.child_frame_id_ = string("odom");

// set up publish rate
ros::Rate loop_rate(100);

// set the time
tf_map_to_odom.stamp_ = ros::Time::now();

//set the transformation
tf_map_to_odom.setOrigin(***How to calculate this?***));
tf_map_to_odom.setRotation(***How to calculate this?***);

// broadcast transform
_tf_br.sendTransform(tf_map_to_odom);

Thank you very much for your help.