fixing odom drift
Hey, I want to write a python node that publishes tf map->odom, and by that fixes the odometry drift, as the amcl does. I have the base_link desired position in the map, and from that I need to calculate the corract map->odom. I have the code part in amcl that does that. can someone explain what is happening in the following code section?
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
Asked by amit_z on 2020-12-02 03:46:48 UTC
Comments
Iam looking for more or less the same thing. However the amcl code Iam looking at seems to be slightly different, did you adapt it, or where did you get this code? What they do here:
inverse!? Also note that this
variable is destroyed when the try
block ends, they use the same name in the next code block, creating a new
variable there.)
-they interpret/transformpose to odom frame, fill in = odom_to_map (pose
message)
-then fill the odom_to_pose into a tf = latest_tf_
-create a StampedTransform = tmp_tf_stamped (fill it with
latest_tf position/orientation data - inverse again - fill in timestamp and frame information)
-broadcast it
I wonder why the inverse's and why the tf<->pose back and forth. But some functions only can interpret pose to tf, not pose to pose or inverse, Maybe legacy.
Asked by Dragonslayer on 2020-12-02 08:44:52 UTC
By the way, depending on what you want to do, you might also look into robot_localization, you can fuse odometry with other sources imu, pose. Maybe this way you dont even have to correct odometry drift via map->odom transform.
Asked by Dragonslayer on 2020-12-02 09:31:34 UTC