How do I effectively go from a Matrix3f to a Quaternionf, given that my 3x3 is a guaranteed to be a rotation matrix, since it orthonormal?

Code: Select all

inline void Mat3fToQuat(Eigen::Quaternionf& quat, const Eigen::Matrix3f& mat)
  {
    Eigen::AngleAxisf aa;
    aa = mat;
    quat = mat;
  }


Is not working for me, since i am getting a roll in that quat where there was none (since i constructed it).

This is the correct way to convert a 3x3 rotation matrix to a quaternion. Can you be more specific about the input matrix, the output you get and what you don't like in the result?

Here is what I am trying to do, Essentially building a lookat system for my graphics code. For one particular area i need the lookat parameters in euler angles. A lookat with an up vector of [0 1 0], should produce an euler angle with yaw and pitch but no roll. I am getting roll in there for some reason, so the results are off (not point at the right location).

I should also mention that I am doing this for my camera which point down neg z at identity, that is why i have that negZ bool that is set to true. I even tried this with just using the LookAtQuatf, and applying the resulting quaternion to my camera is still wrong (just off, sometimes by alot).

The euler angle order i use is Yaw then Pitch then Roll, applied in that order right side multiplication applying the next transform locally. So Model is Yawed (rot about y) first then pitched (rot about x) from that yawed orientation, then rolled (rot along z) from that yawed and pitched orientation. My coord system is right handed BTW, _Z forward, X right, Y up.

My code so far looks like so (not sure what I am doing wrong):

Code: Select all

void MathUtil::LookAtMat3f(Eigen::Matrix3f& mat,
                           const Eigen::Vector3f& loc,
                           const Eigen::Vector3f& target,
                           const Eigen::Vector3f& up,
                           bool negZ)
{
  Eigen::Vector3f dir = target - loc;
  dir.normalize();
  if(negZ)
  { dir *= -1.0f; }

  Eigen::Vector3f nup = up.normalized();
  Eigen::Vector3f xaxis = nup.cross(dir);
  Eigen::Vector3f yaxis = dir.cross(xaxis);

  mat(0,0) = xaxis[0]; mat(0,1) = xaxis[1]; mat(0,2) = xaxis[2];
  mat(1,0) = yaxis[0]; mat(1,1) = yaxis[1]; mat(1,2) = yaxis[2];
  mat(2,0) = dir[0];   mat(2,1) = dir[1];   mat(2,2) = dir[2];
}

void MathUtil::LookAtQuatf(Eigen::Quaternionf& quat,
                           const Eigen::Vector3f& loc,
                           const Eigen::Vector3f& target,
                           const Eigen::Vector3f& up,
                           bool negZ)
{
  Eigen::Matrix3f mat;
  LookAtMat3f(mat,loc,target,up,negZ);
  Mat3fToQuat(quat,mat);
}

void MathUtil::LookAtEuler(Eigen::Vector3f& euler,
                           const Eigen::Vector3f& loc,
                           const Eigen::Vector3f& target,
                           const Eigen::Vector3f& up,
                           bool negZ)
{
  Eigen::Quaternionf quat;
  LookAtQuatf(quat,loc,target,up,negZ);

  QuatfToEuler(euler,quat);
}

void MathUtil::QuatfToEuler(Eigen::Vector3f& euler,
                            const Eigen::Quaternionf& quat)
{
  float sqw = quat.w()*quat.w();
  float sqx = quat.x()*quat.x();
  float sqy = quat.y()*quat.y();
  float sqz = quat.z()*quat.z();
  float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
  float test = quat.x()*quat.y() + quat.z()*quat.w();
  if (test > 0.499*unit) { // singularity at north pole
    euler[1] = 2 * atan2(quat.x(),quat.w());
    euler[2] = F_PI/2;
    euler[0] = 0;
    return;
  }
  if (test < -0.499*unit) { // singularity at south pole
    euler[1] = -2 * atan2(quat.x(),quat.w());
    euler[2] = -F_PI/2;
    euler[0] = 0;
    return;
  }
  euler[1] = atan2(2*quat.y()*quat.w()-2*quat.x()*quat.z() , sqx - sqy - sqz + sqw);
  euler[2] = asin(2*test/unit);
  euler[0] = atan2(2*quat.x()*quat.w()-2*quat.y()*quat.z() , -sqx + sqy - sqz + sqw);
}

void MathUtil::EulerToQuatf(Eigen::Quaternionf& quat,
                        const Eigen::Vector3f& euler,
                        EulerOrder order)
{
  Eigen::Quaternionf xrot;
  Eigen::Quaternionf yrot;
  Eigen::Quaternionf zrot;

  xrot = Eigen::AngleAxisf(euler[0],Eigen::Vector3f::UnitX());
  yrot = Eigen::AngleAxisf(euler[1],Eigen::Vector3f::UnitY());
  zrot = Eigen::AngleAxisf(euler[2],Eigen::Vector3f::UnitZ());


  switch ( order )
  {
  case EULER_ZYX: quat = zrot * yrot * xrot; break;
  case EULER_YZX: quat = yrot * zrot * xrot; break;
  case EULER_ZXY: quat = zrot * xrot * yrot; break;
  case EULER_XZY: quat = xrot * zrot * yrot; break;
  case EULER_YXZ: quat = yrot * xrot * zrot; break;
  case EULER_XYZ: quat = yrot * yrot * zrot; break;
  }
}


1 - You have to normalize the xaxis vector
2 - If your system is right handed, then I guess Z is rather pointing to the backward direction, isn-it?
3 - Any reason for not using Eigen's Euler conversion routine? In your case:

Code: Select all

// mat to Y-P-R
Vector3f ea = mat33.eulerAngles(1,0,2);

// Y-P-R to mat
mat33 = AngleAxisf(ea[0], Vec::UnitY())
           *  AngleAxisf(ea[1], Vec::UnitX())
           *  AngleAxisf(ea[2], Vec::UnitZ());


4 - You can directly fill you look-at matrix with comma initializer:

Code: Select all

mat.transpose() << xAxis, yAxis, dir;


Hmm, so I changed my LookAts to this:

Code: Select all

void MathUtil::LookAtMat3f(Eigen::Matrix3f& mat,
                           const Eigen::Vector3f& loc,
                           const Eigen::Vector3f& target,
                           const Eigen::Vector3f& up,
                           bool negZ)
{
  Eigen::Vector3f dir = target - loc;
  dir.normalize();
  if(negZ)
  { dir *= -1.0f; }

  Eigen::Vector3f nup = up.normalized();
  Eigen::Vector3f xaxis = nup.cross(dir).normalized();
  Eigen::Vector3f yaxis = dir.cross(xaxis).normalized();

  mat.transpose() << xaxis, yaxis, dir;
}

void MathUtil::LookAtQuatf(Eigen::Quaternionf& quat,
                           const Eigen::Vector3f& loc,
                           const Eigen::Vector3f& target,
                           const Eigen::Vector3f& up,
                           bool negZ)
{
  Eigen::Matrix3f mat;
  LookAtMat3f(mat,loc,target,up,negZ);
  Mat3fToQuat(quat,mat);
}

inline void Mat3fToQuat(Eigen::Quaternionf& quat, const Eigen::Matrix3f& mat)
  {
    Eigen::AngleAxisf aa;
    aa = mat;
    quat = aa;
  }


And I am trying to use LookAtQuat, for testing before I use LookAtEuler. My camera class takes a quaternion as its native orientation specification. The reason why I am trying to have neg z point at the target, is that my camera at identity points down neg z. Its a wrapper on an OpenGL camera thus the neg z pointing into the screen.

So with this code, I am still getting a roll in my camera, and it is still not pointing at the target.

BTW, how do check whether you all roll or not? The matrix computed by LookAt is the matrix from global to camera space so you have to extract the Yaw-Pitch-Roll components from its inverse, or extract these quantities in the other order (Roll-Pitch-Yaw or 2,0,1 using .eulerAngles(...)).

Just as an experiment I let the negZ bool in those functions to be false, so z-axis should be pointing at the target. When calling LookAtEuler, i am still getting a roll in z, not sure why.

To extract the euler angles from the quaternion I use the QuatfToEuler function i first posted:

Code: Select all

void MathUtil::QuatfToEuler(Eigen::Vector3f& euler,
                            const Eigen::Quaternionf& quat)
{
  float sqw = quat.w()*quat.w();
  float sqx = quat.x()*quat.x();
  float sqy = quat.y()*quat.y();
  float sqz = quat.z()*quat.z();
  float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
  float test = quat.x()*quat.y() + quat.z()*quat.w();
  if (test > 0.499*unit) { // singularity at north pole
    euler[1] = 2 * atan2(quat.x(),quat.w());
    euler[2] = F_PI/2;
    euler[0] = 0;
    return;
  }
  if (test < -0.499*unit) { // singularity at south pole
    euler[1] = -2 * atan2(quat.x(),quat.w());
    euler[2] = -F_PI/2;
    euler[0] = 0;
    return;
  }
  euler[1] = atan2(2*quat.y()*quat.w()-2*quat.x()*quat.z() , sqx - sqy - sqz + sqw);
  euler[2] = asin(2*test/unit);
  euler[0] = atan2(2*quat.x()*quat.w()-2*quat.y()*quat.z() , -sqx + sqy - sqz + sqw);
}


I have used this before, and it seemed to work fine, is there a better way?

but you extract them on the global-to-camera transformation right? if that's right, then you either have to call QuatfToEuler on the inverse transformation or adapt your QuatfToEuler code to extract Roll * Pitch * Yaw instead of Yaw * Pitch * Roll.