ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
3

How can a Vector3 axis be used to produce a Quaternion?

asked 2012-04-02 04:59:27 -0600

updated 2016-10-24 09:09:58 -0600

ngrennan gravatar image

Hi everyone!

I'm extracting a cylinder using PCL and the Kinect as shown in this tutorial:

http://pointclouds.org/documentation/tutorials/cylinder_segmentation.php#cylinder-segmentation

One of the characteristics of the cylinder that the algorithm provides me with is its axis. In order to show it in rviz as a Marker however I need to convert this Vector3<x, y, z> into a Quaternion<x, y, z, w>.

I am aware that both things cannot hold the same information, I am trying to calculate the look at quaternion given to me by the vector that represents the axis of the cylinder that provides me a direction and not a rotation. How do I do that?

Thanks!

edit retag flag offensive close merge delete

4 Answers

Sort by » oldest newest most voted
7

answered 2012-04-02 05:27:58 -0600

A quaternion represents a relative difference in orientation rather than an orientation itself. Rviz displays that difference by applying the quaternion to the canonical axes.

To "convert" an axis to a quaternion you need to create a convention for what the default axis should be and producing a quaternion that represents the rotation from one to the other. Eigen::Quaternion<d or f>::setFromTwoVectors will do this do this for you. This leaves out one piece of the orientation which is the rotation about the source/destination vector, but you don't care about this since a cylinder is axially symmetric.

TL;DR: use Eigen::Quaternion<d or f>::setFromTwoVectors

edit flag offensive delete link more
3

answered 2012-04-02 05:08:11 -0600

The cylinder's axis if unrotated is originally pointing along the z axis of the referential so that will be the up direction (0, 0 1).

The cross product of the desired cylinder axis (normalized) and the up vector will give us the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector.

The dot product of the cylinder axis and the up vector will provide the angle of rotation.

The following code snippet shows how to do this in ROS:

tf::Vector3 axis_vector(coef_cylinder_->values[3], coef_cylinder_->values[4], coef_cylinder_->values[5]);

tf::Vector3 up_vector(0.0, 0.0, 1.0);
tf::Vector3 right_vector = axis_vector.cross(up_vector);
right_vector.normalized();
tf::Quaternion q(right_vector, -1.0*acos(axis_vector.dot(up_vector)));
q.normalize();
geometry_msgs::Quaternion cylinder_orientation;
tf::quaternionTFToMsg(q, cylinder_orientation);

//(...)

marker.pose.orientation = cylinder_orientation;
edit flag offensive delete link more

Comments

Sorry, I started answering before I realized you had already posted an answer.

Asomerville gravatar image Asomerville  ( 2012-04-02 05:54:03 -0600 )edit
1

The more answers the better :) Plus I didn't know about the setFromTwoVectors method, even though I looked for something like that!

Gonçalo Cabrita gravatar image Gonçalo Cabrita  ( 2012-04-02 06:03:03 -0600 )edit

Unfortunately fromTwoVectors requires converting to Eigen first and then back to tf... it would be nice if tf used Eigen in the first place, but another underlying lib KDL would probably need to switch first.

Asomerville gravatar image Asomerville  ( 2012-04-02 06:09:56 -0600 )edit
1

answered 2014-04-04 13:14:12 -0600

Dave Coleman gravatar image

updated 2014-04-04 13:14:58 -0600

Thanks Goncalo Cabrita, your answer was really helpful. It took me a very long time to figure out how to get a vector from one point in space pointing to another. My one modification was that your code gives me a vector normal to my two points, so I rotate the vector by PI/2 on the Y axis.

I do have a problem though that is related enough to this thread that i'm just going to post it here because its also a solution to the original question. I'd like to get a pure Eigen implementation of your code, however I've run into a bug. Here is my code - the tf version works but the pure Eigen version does not. There must be some little implementation detail between TF and Eigen that I'm missing:

// Input - two points x,y,z with respect to the origin
Eigen::Vector3d a, b;

// Declare goal output pose
geometry_msgs::Pose output_vector;
Eigen::Quaterniond q;

Eigen::Vector3d axis_vector = b - a;
axis_vector.normalize();

Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
Eigen::Vector3d right_axis_vector = axis_vector.cross(up_vector);
right_axis_vector.normalized();
double theta = axis_vector.dot(up_vector);
double angle_rotation = -1.0*acos(theta);

//-------------------------------------------
// Method 1 - TF - works
//Convert to TF
tf::Vector3 tf_right_axis_vector;
tf::vectorEigenToTF(right_axis_vector, tf_right_axis_vector);

// Create quaternion
tf::Quaternion tf_q(tf_right_axis_vector, angle_rotation);

// Convert back to Eigen
tf::quaternionTFToEigen(tf_q, q);
//-------------------------------------------

//-------------------------------------------
// Method 2 - Eigen - broken
q = Eigen::AngleAxis<double>(angle_rotation, right_axis_vector);
//-------------------------------------------

// Rotate so that vector points along line    
Eigen::Affine3d pose;
q.normalize();
pose = q * Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY());
pose.translation() = a;
tf::poseEigenToMsg(pose, output_vector);

Using the two different methods I get different output rotation matrices (converted from their quaternion counterpart):

Good:

0.988685   0.140197 -0.0533575
0.140197  -0.737071   0.661113
0.0533575  -0.661113  -0.748386

Bad:

0.995022   0.061675 -0.0353901
0.061675   0.235831   0.438492
0.0353901  -0.438492   0.230853
edit flag offensive delete link more
0

answered 2012-04-02 22:05:37 -0600

Supposedly, rviz can do this calculation for you: This page claims that you can put the start and end point into the points list member. This also has the advantage that you can scale the arrow's shaft and head independently.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2012-04-02 04:59:27 -0600

Seen: 15,196 times

Last updated: Apr 04 '14