# Avoid SVD to compute 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

Related:

http://nghiaho.com/?page_id=671 