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 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 10: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 11: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 11:09:56 -0600 )edit

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