tf::Matrix3x3 GetTfRotationMatrix(tf::Vector3 xaxis, tf::Vector3 zaxis) {
tf::Matrix3x3 m;
tf::Vector3 yaxis = zaxis.cross(xaxis);
m.setValue(xaxis.getX(), yaxis.getX(), zaxis.getX(),
xaxis.getY(), yaxis.getY(), zaxis.getY(),
xaxis.getZ(), yaxis.getZ(), zaxis.getZ());
return m;
}
//Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up)
//p0-->p1 should point in Alvar's pos X direction
//p1-->p2 should point in Alvar's pos Y direction
int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
const CvPoint3D64f& p2, const CvPoint3D64f& p3,
tf::Transform &retT)
{
const tf::Vector3 q0(p0.x, p0.y, p0.z);
const tf::Vector3 q1(p1.x, p1.y, p1.z);
const tf::Vector3 q2(p2.x, p2.y, p2.z);
const tf::Vector3 q3(p3.x, p3.y, p3.z);
// (inverse) matrix with the given properties
const tf::Vector3 v = (q1-q0).normalized();
const tf::Vector3 w = (q2-q1).normalized();
const tf::Vector3 n = v.cross(w);
tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
m = m.inverse();
//Translate to quaternion
if(m.determinant() <= 0)
return -1;
//Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
Eigen::Matrix3f eig_m;
for(int i=0; i<3; i++){
for(int j=0; j<3; j++){
eig_m(i,j) = m[i][j];
}
}
Eigen::Quaternion<float> eig_quat(eig_m);
// Translate back to bullet
tfScalar ex = eig_quat.x();
tfScalar ey = eig_quat.y();
tfScalar ez = eig_quat.z();
tfScalar ew = eig_quat.w();
tf::Quaternion quat(ex,ey,ez,ew);
quat = quat.normalized();
double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
tf::Vector3 origin (qx,qy,qz);
tf::Transform tform (quat, origin); //transform from master to marker
retT = tform;
return 0;
}
int extractFrame (const pcl::ModelCoefficients& coeffs,
const ARPoint& p1, const ARPoint& p2,
const ARPoint& p3, const ARPoint& p4,
tf::Matrix3x3 &retmat)
{
// Get plane coeffs and project points onto the plane
double a=0, b=0, c=0, d=0;
if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
return -1;
const tf::Vector3 q1 = project(p1, a, b, c, d);
const tf::Vector3 q2 = project(p2, a, b, c, d);
const tf::Vector3 q3 = project(p3, a, b, c, d);
const tf::Vector3 q4 = project(p4, a, b, c, d);
// Make sure points aren't the same so things are well-defined
if((q2-q1).length() < 1e-3)
return -1;
// (inverse) matrix with the given properties
const tf::Vector3 v = (q2-q1).normalized();
const tf::Vector3 n(a, b, c);
const tf::Vector3 w = -v.cross(n);
tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
// Possibly flip things based on third point
const tf::Vector3 diff = (q4-q3).normalized();
//ROS_INFO_STREAM("w = " << w << " and d = " << diff);
if (w.dot(diff)<0)
{
//ROS_INFO_STREAM("Flipping normal based on p3. Current value: " << m);
m[1] = -m[1];
m[2] = -m[2];
//ROS_INFO_STREAM("New value: " << m);
}
// Invert and return
retmat = m.inverse();
//cerr << "Frame is " << retmat << endl;
return 0;
}