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?

4 answers

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.

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.

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.

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: