int getQuaternion (const tf::Matrix3x3& m, tf::Quaternion &retQ)
{
if(m.determinant() <= 0)
return -1;
//tfScalar y=0, p=0, r=0;
//m.getEulerZYX(y, p, r);
//retQ.setEulerZYX(y, p, r);
//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);
retQ = quat.normalized();
return 0;
}
