Revision history [back]

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 most 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

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 most 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