Check the (Eigen based) fix operations

[HannesSommer]

As suggested in https://github.com/ethz-asl/minkindr/pull/17/files#r22783093.

[kruesip]

Here's my Eigen implementation of a function finding the closest proper rotation matrix for a given input matrix:

Matrix3d rotationHealing(const Matrix3d& rotMat)  
{
   JacobiSVD<Matrix3d> svd(rotMat, ComputeFullV);

   Matrix3d correction = svd.matrixV().col(0) * svd.matrixV().col(0).transpose() / svd.singularValues()(0) +
                         svd.matrixV().col(1) * svd.matrixV().col(1).transpose() / svd.singularValues()(1) +
                         svd.matrixV().col(2) * svd.matrixV().col(2).transpose() / svd.singularValues()(2);

   return rotMat * correction;
}

The theory behind it is here: http://people.csail.mit.edu/bkph/articles/Nearest_Orthonormal_Matrix.pdf

Plus you need the following (copied from Wikipedia: http://en.wikipedia.org/wiki/Singular_value_decomposition):

• The right-singular vectors of M are eigenvectors of M*M.
• The non-zero singular values of M (found on the diagonal entries of Σ) are the square roots of the non-zero eigenvalues of both MM and MM.

[HannesSommer]

Ah, perfect. Thanks for providing that already :).