How do I remove rotation from a pose? [closed]

asked 2020-03-03 12:44:41 -0500

uz gravatar image

Hi all,

I am using ROS Melodic in Gazebo 9 with Ardupilot to simulate the standard Iris quadocopter. The drone carries a camera on a gimbal which is used to detect a WhyCon fiducial marker. I am carrying out coordinate frame transforms using the tf2 library and I believe that my operations are correct so far, except that the final determined pose of the marker relative to the drone has non-zero rotation (as in w,x,y,z != 1,0,0,0). The values for the pose seem correct generally, but in order to use them I need to have "unrotated" values, as I am using these values to direct the drone toward the marker. I am attempting to create a transform in order to zero-out the rotation and give me the corresponding point in space:

geometry_msgs::PoseStamped straighten_pose( const geometry_msgs::PoseStamped & _pose_in )
    // declare pose variables
    geometry_msgs::PoseStamped pose_out;
    geometry_msgs::PoseStamped pose_in = _pose_in;

    // make the child_frame_id equal to the parent's frame_id with '_straightened' appended
    std::string child_frame_id = pose_in.header.frame_id + "_straightened";

    // access the transform broadcaster
    static tf2_ros::TransformBroadcaster transform_broadcaster;

    // declare a message in which to store the new transform
    geometry_msgs::TransformStamped transform_stamped_message;

    // set the transform's header
    transform_stamped_message.header.stamp = ros::Time::now();
    transform_stamped_message.header.frame_id = pose_in.header.frame_id;
    transform_stamped_message.child_frame_id = child_frame_id;

    // set the rotation from the parent to the child
    transform_stamped_message.transform.rotation = pose_in.pose.orientation;

    // send the transform

    // transform the pose and return it
    return transform_buffer.transform(pose_in, child_frame_id, ros::Duration(0.05));

I believe this works in theory, but I could be mistaken. In practice it does reduce the rotation to approximately (w,x,y,z)=(1,0,0,0). I say "approximately" because the 0s are more like some tiny number ~ 1e-50, and the w is similarly close to 1. Most of the time, this gives me the values that I am looking for, but sometimes there are non-negligible jumps in the pose_out.position which makes the code practically unusable. These jumps are associated with similarly unpredictable changes in the signs of the "zero" components in the pose's orientation. Can anyone suggest a better way of doing this, which does not incur the (probable) floating point errors that I am experiencing now? Or even better, could someone suggest a way that has already been implemented and is reliable?

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by uz
close date 2020-03-04 10:37:04.073672



If you're not interested in the orientation of a pose, why not overwrite it with Quaternion(0, 0, 0, 1)?

gvdhoorn gravatar image gvdhoorn  ( 2020-03-04 02:35:55 -0500 )edit

This would not give a solution, as the xyz coordinates of the pose are influenced by the orientation itself. It is not so much that I am not interested in the orientation, but rather that I am interested in an orientation of Quaternion(0, 0, 0, 1). However, setting the orientation to this artificially does not solve the issue.

Anyway, I solved the issue and it was from dirty data at the beginning of the pipeline, not from the above code. The above code appears to work.

uz gravatar image uz  ( 2020-03-04 10:36:19 -0500 )edit