# Quaternion of a 3D vector

My drone is to reach position x,y, z and orient itself along vector3D (ax, by, cz) wrt origin. In order for me to visualize this on rviz I must represent them in quaternion form. I understand that quaternions and vectors don't represent the same thing, but if I take my arbitrary axis as x-axis (vector [1,0,0]) and calculate quaternions between the two vectors, I get this result:

As you can see here, the poses aren't all normal to the faces of the cube (my vectors are!) on rviz. What must I do to simply get the vector representation in quaternion form to visualize them?

Originally posted by swethmandava on ROS Answers with karma: 102 on 2016-03-13

Post score: 0

Technically, no unique quaternion can represent an arbitrary 3D vector. Assuming the vector represents the x axis of the transformed frame, you're essentially solving the following equation for the quaternion parameters given your vector [a_x, a_y, a_z]

From this, it is clear that there is no unique solution, simply because there are four unknowns (the quaternion parameters), and only three independent equations. This can be verified intuitively as well. Imagine the x axis of a frame aligned along a particular orientation. The roll angle around that x axis is arbitrary, as it doesn't affect the orientation of the x axis relative to the base frame.

Therefore, the solution needs to be constrained in some way. One way is to constrain the y axis to be in a level plane w.r.t. the base frame (z = 0). Here is some code that I wrote using the Eigen library to construct a rotation matrix out of basis vectors that define the orientation of a vector. It assumes you have a using namespace Eigen; in your source file.

Vector3d u1, u2, u3;
Vector3d a;
a << 1, 2, 3; // Some arbitrary vector

u1 = a.normalized();

if ((fabs((double)u1(0)) > 0.001) || (fabs((double)u1(1)) > 0.001)) {
u2 << -u1(1), u1(0), 0;
} else {
u2 << 0, u1(2), -u1(1);
}
u2.normalize();

u3 = u1.cross(u2);

Matrix3d R;  // Rotation matrix defining orientation
R.col(0) = u1;
R.col(1) = u2;
R.col(2) = u3;


There are algorithms to convert a rotation matrix to a quaternion. Probably the simplest way to do this is to use a tf::Transform. First, convert the Eigen::Matrix3d to a tf::Matrix3x3 and call the setBasis() method of the tf::Transform. Then call the getRotation() method to get a tf::Quaternion with the parameters.

I hope this helps!

Originally posted by robustify with karma: 956 on 2016-03-13

This answer was NOT ACCEPTED on the original site

Post score: 3

Comment by swethmandava on 2016-03-16:
That makes sense! But since I only want the visual representation of the vector on rviz, I don't think I need a unique quaternion. As long as my x axis of the frame always points towards the vector, y and z shouldn't matter right. But again, let me know if I'm way off here @robustify

Comment by robustify on 2016-03-19:
If you only want a visual arrow in Rviz, I would just make an Arrow marker. If you put the starting and ending points of the arrow you want to display in index 0 and 1 of the points field, it will just draw an arrow between those points instead of using the pose field of the marker message.

Comment by robustify on 2016-03-19:
Also, even though you don't care about the alignment of the y and z axes, you still need to select a valid solution that yields the correct x axis alignment. While there are infinite solutions that provide the correct x axis alignment, there are also infinite incorrect solutions!

def  pose_from_vector3D(waypoint):
#http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
pose= Pose()
pose.position.x = waypoint[0]
pose.position.y = waypoint[1]
pose.position.z = waypoint[2]
#calculating the half-way vector.
u = [1,0,0]
norm = linalg.norm(waypoint[3:])
v = asarray(waypoint[3:])/norm
if (array_equal(u, v)):
pose.orientation.w = 1
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
elif (array_equal(u, negative(v))):
pose.orientation.w = 0
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 1
else:
half = [u[0]+v[0], u[1]+v[1], u[2]+v[2]]
pose.orientation.w = dot(u, half)
temp = cross(u, half)
pose.orientation.x = temp[0]
pose.orientation.y = temp[1]
pose.orientation.z = temp[2]
norm = math.sqrt(pose.orientation.x*pose.orientation.x + pose.orientation.y*pose.orientation.y +
pose.orientation.z*pose.orientation.z + pose.orientation.w*pose.orientation.w)
if norm == 0:
norm = 1
pose.orientation.x /= norm
pose.orientation.y /= norm
pose.orientation.z /= norm
pose.orientation.w /= norm
return pose


I realized my mistake, I didn't normalize the quaternion. This code works!

Originally posted by swethmandava with karma: 102 on 2016-04-13

This answer was ACCEPTED on the original site

Post score: 0