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; }