Revision history [back]

I did something like this before to populate a geometry_msgs/PoseArray to visualize a cloud of vectors in Rviz. Is this what you're trying to do as well? On its own, a vector does not completely define an orientation, so you can't just directly calculate a quaternion from a vector.

However, you can make the vector the first basis vector (first column) in a rotation matrix and then arbitrarily construct the other two orthogonal basis vectors to make a complete orientation. This is physically meaningless, but it does result in a geometry_msgs/PoseArray that renders the vector properly in Rviz.

Here is how you can do this and get a quaternion:

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
...
tf2::Vector3 p1(x1, y1, z1); // Your start point
tf2::Vector3 p2(x2, y2, z2); // Your end point

tf2::Vector3 v = (p2 - p1).normalized();

tf2::Quaternion q;
tf2::Matrix3x3 rot_mat;
rot_mat[0] = v;
if (std::abs(v.z()) > 0.01) {
  rot_mat[1] = tf2::Vector3(0, -v.z(), v.y()).normalized();
} else {
  rot_mat[1] = tf2::Vector3(-v.y(), v.x(), 0).normalized();
}
rot_mat[2] = rot_mat[0].cross(rot_mat[1]);
rot_mat.getRotation(q);