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

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 close merge delete

Sort by » oldest newest most voted

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

more

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;

more

( 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!

( 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.

( 2012-04-02 06:09:56 -0600 )edit

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


0.995022   0.061675 -0.0353901
0.061675   0.235831   0.438492
0.0353901  -0.438492   0.230853

more

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.

more