How to transform a pose
I have a pose of a frame that is not correctly positioned (think about it like the /odom frame when doing AMCL navigation, it gets continuously corrected). I calculate this amount of necessary correction and save it as a Pose variable (from geometry_msgs). Now I want to apply this correction to the pose of my frame. How can I do that with tf in Python?
What I tried until now is:
# Get pose of camera in world frame (true pose from marker observation=
pose_from_marker = self.tf_listener.transformPose("/map", pose_camera_marker_frame)
# Now get transform between odom_marker and camera_frame_marker (copy of normal odometry)
(trans, rot) = self.tf_listener.lookupTransform('camera_frame_marker', 'odom_marker', rospy.Time(0))
print rot
R_cam_odom = quaternion_matrix(rot)
t_cam_odom = np.zeros((3, 1))
t_cam_odom[0] = trans[0]
t_cam_odom[1] = trans[1]
t_cam_odom[2] = trans[2]
print trans
t_true_pose = np.zeros((3, 1))
t_true_pose[0, 0] = pose_from_marker.pose.position.x
t_true_pose[1, 0] = pose_from_marker.pose.position.y
t_true_pose[2, 0] = pose_from_marker.pose.position.z
R_true_pose = quaternion_matrix((pose_from_marker.pose.orientation.x, pose_from_marker.pose.orientation.y, pose_from_marker.pose.orientation.z, pose_from_marker.pose.orientation.w))
R_final = np.dot(R_cam_odom, R_true_pose)
t_final = np.dot(R_cam_odom[:-1, :-1], t_true_pose) + t_cam_odom
q_final = quaternion_from_matrix(R_final)
# update the new odom with correct values
self.odom_marker_pose.position.x = t_final[0]
self.odom_marker_pose.position.y = t_final[1]
self.odom_marker_pose.position.z = t_final[2]
self.odom_marker_pose.orientation.x = q_final[0]
self.odom_marker_pose.orientation.y = q_final[1]
self.odom_marker_pose.orientation.z = q_final[2]
self.odom_marker_pose.orientation.w = q_final[3]