More robust quaternion from matrix
Submitted by Michael Norel
Assigned to Nobody
Link to original bugzilla bug (#1318)
Version: 3.4 (development)
Description
Conversion code, assumes strong orthonormal 3x3 matrix as input. But in real data our matrix can have small errors. This errors produce nonunity of quaternion.
To demonstrate, how important to keep quaternion normalized, we can use this code:
Eigen::Quaterniond qc = Eigen::Quaterniond(1.1, 2.2, 3.3, 4.4).normalized();
for (int i = 0; i < 1000; i++)
{
qc *= qc;
qc = Eigen::Quaterniond(qc.toRotationMatrix()*qc.toRotationMatrix());
std::cout << "1.0 - qc.squaredNorm(): " << 1.0 - qc.squaredNorm() << std::endl;
} Using VC12, i have INF result with only 1000 iterations.
I propose use normalization instead of analytic scale of quaternion in this conversion. It can be slower, but produce unit quaternion. The performance depends on performance of "normalize" and its vectorization.
In Eigen it can look:
{
...
q.w() = t + Scalar(1.0);
q.x() = mat.coeff(2,1) - mat.coeff(1,2));
q.y() = mat.coeff(0,2) - mat.coeff(2,0));
q.z() = mat.coeff(1,0) - mat.coeff(0,1));
// same with other cases
...
normalize(); // normalize on exit }
I can provide full version of function if required.