The Kabsch Algorithm finds the optimal translation and rotation that minimizes the distance between two sets of matched points.

Optimal Translation

Finding the optimal rigid transformation involves finding the optimal translations and rotations.

The optimal translation is simply the offset between the averages of the two sets of points.

Vector3 getAverage(points){
  centroid = Vector3.zero;
  foreach point in points {
    centroid += point;
  }
  centroid /= points.count;
}
optimalTranslation = 
  getAverage(toPoints) - getAverage(fromPoints);

Optimal Rotation

The optimal rotation requires a few more steps.

First, you must mean-center the points (that is, subtract their means from them)

fromPoints = fromPoints - getAverage(fromPoints);
toPoints = toPoints - getAverage(toPoints);

Second, you must calculate the 3x3 Cross-Covariance Matrix between them

covariance = [
  new Vector3(0, 0, 0), 
  new Vector3(0, 0, 0), 
  new Vector3(0, 0, 0)]
for(i = 0; i < 3; i++) {
  for(j = 0; j < 3; j++) {
    for(k = 0; k < fromPoints.length; k++) {
      covariance[i][j] += 
        fromPoints[k][i] * toPoints[k][j];
    }
  }
}

And Third, you must find the polar decomposition of that matrix

for (iter = 0; iter < iterations; iter++) {
  setBasesFromQuaternion(curQuaternion, 
                         curXBasis, curYBasis, curZBasis);
  omega = (cross(curXBasis, covariance[0]) +
           cross(curYBasis, covariance[1]) +
           cross(curZBasis, covariance[2])) / 
       abs(dot(curXBasis, covariance[0]) +
           dot(curYBasis, covariance[1]) +
           dot(curZBasis, covariance[2]) + 0.000000001);
  w = omega.magnitude;
  if (w < 0.000000001) break;
  curQuaternion = angleAxis(w, omega / w) * curQuaternion;
}
optimalRotation = curQuaternion;

Shape Matching

One application of Kabsch is in Particle-based Physics Simulations. It allows you to tie multiple particles together into a single rigid-body

Stiffness:
Number of Particles:
Collision Iterations: 
// Apply Particle Inertia and Gravity
rigidPoints *= kabsch(rigidPoints, physicsPoints);
for(i = 0; i < physicsPoints.length; i++) {
  lerp(physicsPoints[i], rigidPoints[i], stiffnessAlpha);
}
// Collide particles with the Ground

Shape Matching has a number of unique advantages over distance constraints:

  • It’s cheap for large numbers (O(n) rather than O(n^2))
  • It is unconditionally stable; the configuration cannot be inverted
  • It’s branchless and requires relatively few square roots
  • Computation is iterative, allowing for dynamically scaling compute
  • Particles otherwise simulate totally in parallel, allowing for GPU Compute
  • Shockwaves travel instantaneously across the entire body of particles
  • Does not require a connectivity graph; topology can change dynamically

Comments