2
\$\begingroup\$

OSVR has a right-handed system: x is right, y is up, z is near.

I need to convert orientation data from a sensor in OSVR to a different right-handed system in which x is forward, y is left, and z is up.

I need to calculate the transformation that will convert a quaternion from one system to the other.

I naively tried:

void osvrPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg) { // osvr to ros
    geometry_msgs::PoseStamped outputMsg;

    outputMsg.header = msg->header;
    outputMsg.pose.orientation.x = msg->pose.orientation.y;
    outputMsg.pose.orientation.y = msg->pose.orientation.z;
    outputMsg.pose.orientation.z = msg->pose.orientation.x;
    outputMsg.pose.orientation.w = msg->pose.orientation.w;

    osvrPosePub.publish(outputMsg);
    ros::spinOnce();
}

But that made things really weird. Facing north pitch is pitch, facing west, pitch is yaw, facing south, pitch is negative pitch...

How can I convert my OSVR quaternion to the a corresponding quaternion in the new coordinate system?

\$\endgroup\$
1
  • \$\begingroup\$ Shoot, you're right! I got thrown off. Yes, both systems are right handed. \$\endgroup\$ Commented Apr 28, 2017 at 21:18

1 Answer 1

5
\$\begingroup\$

Summarizing your two coordinate spaces:

direction   OSVR    Output
------------------------------    
right         x      -y
up            y       z
backward      z      -x    

And since they're both right-handed, the sign of the angles does not change.

So to map from OSVR to the output coordinate system, we just map the letters as above:

outputMsg.pose.orientation.x = -1 * msg->pose.orientation.z;
outputMsg.pose.orientation.y = -1 * msg->pose.orientation.x;
outputMsg.pose.orientation.z = msg->pose.orientation.y;
outputMsg.pose.orientation.w = msg->pose.orientation.w;

Or, to go the other way and convert into OSVR's system, we flip it:

outputMsg.pose.orientation.x = -1 * msg->pose.orientation.y;
outputMsg.pose.orientation.y = msg->pose.orientation.z;
outputMsg.pose.orientation.z = -1 * msg->pose.orientation.x;
outputMsg.pose.orientation.w = msg->pose.orientation.w;
\$\endgroup\$
1
  • \$\begingroup\$ I did the math too and this answer seems correct to me. \$\endgroup\$ Commented Sep 1, 2017 at 23:01

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .