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
Bad:
0.995022 0.061675 -0.0353901
0.061675 0.235831 0.438492
0.0353901 -0.438492 0.230853