Avoid SVD when looking for optimal rotation between point sets.

Draft note

 https://zalo.github.io/blog/kabsch/

 A Robust Method to Extract the Rotational Part of Deformations

Best used in an incremental system where you can re-use the rotation q from the previous frame (faster convergence) max iter should probably be set around 10-30

Eigen code:

// A: covariance matrix, q: shape matching rotation
void extractRotation(const Matrix3d& A, Quaterniond& q,const unsigned int maxIter)
{
    for (unsigned int iter = 0; iter < maxIter; iter++){
        Matrix3d R = q.matrix();
        Vector3d omega = (R.col(0).cross(A.col(0)) +
                          R.col(1).cross(A.col(1)) +
                          R.col(2).cross(A.col(2)))
                * (1.0 / fabs(R.col(0).dot(A.col(0)) +
                              R.col(1).dot(A.col(1)) +
                              R.col(2).dot(A.col(2))) +1.0e-9);
        double w = omega.norm();
        if (w < 1.0e-9)
            break;

        q = Quaterniond(AngleAxisd(w, (1.0 / w)*omega))*q;
        q.normalize();
    }
}

Original algorithm (but slow):

https://en.wikipedia.org/wiki/Kabsch_algorithm

https://igl.ethz.ch/projects/ARAP/svd_rot.pdf

No comments

All html tags except <b> and <i> will be removed from your comment. You can make links by just typing the url or mail-address.
Spam bot question: