# Avoid SVD to compute optimal rotation between point sets

Draft note

Original article:

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

And here the javascript code for this article that finds the best transformation (rotation and translation) to match two sets of point clouds.

```/*
Original author:
https://zalo.github.io/blog/kabsch/

*/

import * as THREE from "https://cdn.jsdelivr.net/npm/three@0.114/build/three.module.js";
import {
OrbitControls
}
from "https://cdn.jsdelivr.net/npm/three@0.114/examples/jsm/controls/OrbitControls.js";
import {
DragControls
}
from "https://cdn.jsdelivr.net/npm/three@0.114/examples/jsm/controls/DragControls.js";
("use strict");

var g_camera, g_scene, g_renderer;
var g_controls;

var g_pointsRef = []; // blue boxes (The aim)
var g_pointsToMatch = []; // white boxes (to be transformed to match the aim)

var g_avg;
var g_curQuaternion = new THREE.Quaternion(0, 0, 0, 1);
var g_numPoints = 6;
var g_nb_iterations = 1;

// set to "true" to make the blue cubes rotates
// and observe we are able to find back the rotation
var g_animate = false;
var g_do_matching = true;

// ===============================================================

init();
animate();

// ===============================================================

function pseudo_rand_1(i){ return ((i * 212435 + 2312  ) % 255) / 255.0; }
function pseudo_rand_2(i){ return ((i * 878923 + 123342  ) % 255) / 255.0; }
function pseudo_rand_3(i){ return ((i * 18464 + 79874  ) % 255) / 255.0; }

function pseudo_rand_4(i){ return ((i * 896559 + 626  ) % 255) / 255.0; }
function pseudo_rand_5(i){ return ((i * 3645 + 1666) % 255) / 255.0; }
function pseudo_rand_6(i){ return ((i * 85236 + 375961  ) % 255) / 255.0; }

function initPoints(draggableObjects) {
let boxGeometry = new THREE.BoxBufferGeometry(100, 100, 100);
let blue = new THREE.MeshPhongMaterial({
color: 0x3399dd
});
let white = new THREE.MeshLambertMaterial({
color: 0x888888
});

let scl = new THREE.Vector3(100, 100, 100);
for (let i = 0; i < g_numPoints; i++) {
let box = new THREE.Mesh(boxGeometry, blue);
box.scale.set(0.075, 0.075, 0.075);
if(true){
box.position.set(
pseudo_rand_1(i+628) * scl.x - scl.x / 2.0,
pseudo_rand_2(i+923) * scl.y - scl.y / 2.0 + 100.0,
pseudo_rand_3(i+123) * scl.z - scl.z / 2.0
);
}else{
box.position.set(
Math.random() * scl.x - scl.x / 2,
Math.random() * scl.y - scl.y / 2 + 100,
Math.random() * scl.z - scl.z / 2
);
}
draggableObjects.push(box);
g_pointsRef.push(box);
}

let rcl = new THREE.Vector3(20, 20, 20);
for (let i = 0; i < g_numPoints; i++) {
let box = new THREE.Mesh(boxGeometry, white);
box.scale.set(0.05, 0.05, 0.05);
let randomOffset;
if(true) {
randomOffset = new THREE.Vector3(
pseudo_rand_4(i+1535) * rcl.x - rcl.x / 2 + pseudo_rand_1(i+628) * rcl.x * 3,
pseudo_rand_5(i+6389) * rcl.y - rcl.y / 2 + 93,
pseudo_rand_6(i+1453) * rcl.z - rcl.z / 2 + 3);
}else{
randomOffset = new THREE.Vector3(
Math.random() * rcl.x - rcl.x / 2,
Math.random() * rcl.y - rcl.y / 2 + 100,
Math.random() * rcl.z - rcl.z / 2);
}

g_pointsToMatch.push(box);
}

let average = getAverage(g_pointsRef);

g_camera.position.y = 150;
g_camera.lookAt(average);
if (g_controls) {
g_controls.target.set(average.x, average.y, average.z);
g_controls.update();
}

g_avg = new THREE.Mesh(
boxGeometry,
new THREE.MeshPhongMaterial({
color: 0xdd3333
}));
g_avg.scale.set(0.075, 0.075, 0.075);
g_avg.position.set(average.x, average.y, average.z);
}

function getAverage(points) {
let average = new THREE.Vector3(0, 0, 0);
for (let i = 0; i < points.length; i++) {
}
average.divideScalar(points.length);
return average;
}

//https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf
//Iteratively apply torque to the basis using Cross products (in place of SVD)
function quaternionTorqueDecomposition(A, curQuaternion, iterations = 9) {
// Cancels out the momentum from the prior frame
let resQuaternion = new THREE.Quaternion(0, 0, 0, 1);
// or take int account previous frame for faster convergence:
//resQuaternion.copy(curQuaternion);

let QuatBasis = [
new THREE.Vector3(1, 0, 0),
new THREE.Vector3(0, 1, 0),
new THREE.Vector3(0, 0, 1)
];

let quatMatrix = new THREE.Matrix4().makeRotationFromQuaternion(resQuaternion);

for (let iter = 0; iter < iterations; iter++) {
quatMatrix.makeRotationFromQuaternion(resQuaternion);
quatMatrix.extractBasis(QuatBasis[0], QuatBasis[1], QuatBasis[2]);

QuatBasis[0].dot(A[0]) +
QuatBasis[1].dot(A[1]) +
QuatBasis[2].dot(A[2]) +
0.00000001);

let omega = QuatBasis[0]
.clone()
.cross(A[0])

let w = omega.length();
if (w < 0.00000001) {
break;
}
resQuaternion.premultiply(
new THREE.Quaternion().setFromAxisAngle(omega.normalize(), w));
resQuaternion.normalize(); //Normalizes the Quaternion; critical for error suppression
}

return resQuaternion;
}

function transposeMult(vec1, vec2) {
let covariance = [
// Initialize Cross Covariance Matrix
new THREE.Vector3(0, 0, 0),
new THREE.Vector3(0, 0, 0),
new THREE.Vector3(0, 0, 0)
];

for (let i = 0; i < 3; i++) {
//i is the row in this matrix
for (let j = 0; j < 3; j++) {
//j is the column in the other matrix
for (let k = 0; k < vec1.length; k++) {
//k is the column in this matrix
covariance[i].setComponent(
j,
covariance[i].getComponent(j) +
vec1[k].getComponent(i) * vec2[k].getComponent(j));
}
}
}
return covariance;
}

function kabschPoints(
pointsIn /*To be transformed to match the aim (white*/,
pointsRef /*Base position / aim (blue)*/)
{
let workingRef = [];
let workingIn = [];
let refAverage = getAverage(pointsRef);
let inAverage = getAverage(pointsIn);

// Mean-center the points for the optimal translation
for (let i = 0; i < pointsRef.length; i++) {
// p_i - avg
workingRef.push(pointsRef[i].position.clone().sub(refAverage));
workingIn .push(pointsIn[i] .position.clone().sub(inAverage ));
}

// Calculate the optimal rotation
let crossCovarianceMatrix = transposeMult(/*const*/workingIn, /*const*/workingRef);
g_curQuaternion = quaternionTorqueDecomposition(crossCovarianceMatrix, g_curQuaternion, g_nb_iterations);

// Apply the optimal translation and rotation
for (let i = 0; i < workingIn.length; i++) {
workingIn[i].applyQuaternion(g_curQuaternion);
}
}

function init() {
let container = document.createElement("div");
document.body.appendChild(container);

g_camera = new THREE.PerspectiveCamera(
60,
window.innerWidth / window.innerHeight,
1,
2000);
g_camera.position.set(50, 100, 150);

g_scene = new THREE.Scene();
g_scene.background = new THREE.Color(0xa0a0a0); //0xffffff
g_scene.fog = new THREE.Fog(0xa0a0a0, 200, 800); //0xffffff

let light1 = new THREE.HemisphereLight(0xffffff, 0x444444);
light1.position.set(0, 200, 0);

let light2 = new THREE.DirectionalLight(0xbbbbbb);
light2.position.set(0, 200, 100);

// ground
let mesh = new THREE.Mesh(
new THREE.PlaneBufferGeometry(2000, 2000),
new THREE.MeshPhongMaterial({
color: 0x999999,
depthWrite: false
}));
mesh.rotation.x = -Math.PI / 2;

let grid = new THREE.GridHelper(2000, 20, 0x000000, 0x000000);
grid.material.opacity = 0.2;
grid.material.transparent = true;

g_renderer = new THREE.WebGLRenderer({
antialias: true
});
g_renderer.setPixelRatio(window.devicePixelRatio);
g_renderer.setSize(window.innerWidth, window.innerHeight);
document.body.style.margin = 0;
document.body.style.overflow = "hidden";
document.body.style.position = "fixed";
container.appendChild(g_renderer.domElement);

g_controls = new /*THREE.*/ OrbitControls(g_camera, g_renderer.domElement);
g_controls.target.set(0, 45, 0);
g_controls.update();

var draggableObjects = [];

//draggableObjects.push(mesh);

initPoints(draggableObjects);

var dragControls = new /*THREE.*/ DragControls(
draggableObjects,
g_camera,
g_renderer.domElement);

g_controls.enabled = false;
});
g_controls.enabled = true;
});
}

function onWindowResize() {
g_camera.aspect = window.innerWidth / window.innerHeight;
g_camera.updateProjectionMatrix();
g_renderer.setSize(window.innerWidth, window.innerHeight);
}

function animate() {

if(g_animate){
for (let i = 0; i < g_pointsRef.length; i++) {
var axis = new THREE.Vector3(0, 1, 0 );
var angle = Math.PI / 100;
g_pointsRef[i].position.applyAxisAngle( axis, angle );
}
}

if(g_do_matching)
{
kabschPoints(g_pointsToMatch, g_pointsRef);

let averageRef = getAverage(g_pointsRef);
g_avg.position.set(averageRef.x, averageRef.y, averageRef.z);
}

requestAnimationFrame(animate);
g_renderer.render(g_scene, g_camera);
}

```

You can play with it using this code pen: https://codepen.io/brainexcerpts/pen/QWzvxPm

More notes:

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();
}
}
```

```#include <Eigen/Dense>
#include <Eigen/SVD>
#include <toolbox_maths/eigen_lib_interop.hpp>
// From Mathias Muller optimized svd:
// https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf
void extract_rotation(const Mat3& A, Quat& q,const unsigned int max_iter = 30)
{
for (unsigned int iter = 0; iter < max_iter; iter++){
Mat3 R = q.to_matrix3();

Vec3 omega = (
(R.x().cross( (A.x()) ) +
R.y().cross( (A.y()) ) +
R.z().cross( (A.z()) ) )
* (1.0 / fabs(R.x().dot( (A.x()) ) +
R.y().dot( (A.y()) ) +
R.z().dot( (A.z()) )) + 1.0e-9)
);

float w = omega.norm();
if (w < 1.0e-9)
break;

q = Quat::from_axis_angle((1.0f / w)*omega, w ) * q;
q.normalize();
}
}

// -----------------------------------------------------------------------------

void compute_SVD_rotations_with_quats(
std::vector<tbx::Mat3>& svd_rotations,
std::vector<tbx::Quat>& quat_rotations,
const std::vector<std::map<int, float> >& weights,
const First_ring_it& topo,
const std::vector<Vec3>& vertices, // rest pose / start vertices
const Sub_mesh& sub_mesh,
const std::vector< std::vector<float> >& per_edge_weights, // I think this is optional and could be set to 1.0f

const Skin_deformer& dfm)
{
Eigen::Matrix3f eye = Eigen::Matrix3f::Identity();

for(unsigned i : sub_mesh)// Loop over mesh vertices.
{
unsigned valence = topo.nb_neighbors(i);

Eigen::MatrixXf P(3, valence);
Eigen::MatrixXf Q(3, valence);
for(unsigned n = 0; n < valence; n++)
{
int vert_j = topo.neighbor_to(i, n);

// eij = pi - pj
// compute: P_i * D_i in the paper
float w_cotan = per_edge_weights[i][n];
Vec3 v = (vertices[i] - vertices[vert_j]) * w_cotan;
//P.col(degree) = Eigen::Vector3f(v.x, v.y, v.z);
P(0, n) = v.x;
P(1, n) = v.y;
P(2, n) = v.z;

// eij' = pi' - pj'
//compute: P_i'
v = dfm.vert_anim_pos(weights, i) - dfm.vert_anim_pos(weights, vert_j);
//Q.col() = Eigen::Vector3f(v.x, v.y, v.z);
Q(0, n) = v.x;
Q(1, n) = v.y;
Q(2, n) = v.z;
}
// Compute the 3 by 3 covariance matrix:
// actually S = (P * W * Q.t()); W is already considerred in the previous step (P=P*W)
Eigen::Matrix3f S = (P * Q.transpose());

#if 0
// Compute the singular value decomposition S = UDV.t
Eigen::JacobiSVD<Eigen::MatrixXf> svd(S, Eigen::ComputeThinU | Eigen::ComputeThinV); // X = U * D * V.t()

Eigen::MatrixXf V = svd.matrixV();
Eigen::MatrixXf Ut = svd.matrixU().transpose();

eye(2,2) = (V * Ut).determinant();	// remember: Eigen starts from zero index

// V*U.t may be reflection (determinant = -1). in this case, we need to change the sign of
// column of U corresponding to the smallest singular value (3rd column)
Eigen::Matrix3f rot = (V * eye * Ut);

svd_rotations[i] = Mat3((float)rot(0,0), (float)rot(0,1), (float)rot(0,2),
(float)rot(1,0), (float)rot(1,1), (float)rot(1,2),
(float)rot(2,0), (float)rot(2,1), (float)rot(2,2) ); //Ri = (V * eye * U.t());
#else
extract_rotation( tbx::to_mat3(S), quat_rotations[i]);
svd_rotations[i] = quat_rotations[i].to_matrix3();
#endif

}
}

// -----------------------------------------------------------------------------

void compute_SVD_rotations(std::vector<tbx::Mat3>& svd_rotations,
const std::vector<std::map<int, float> >& weights,
const First_ring_it& topo,
const std::vector<Vec3>& vertices,
const Sub_mesh& sub_mesh,
const std::vector< std::vector<float> >& per_edge_weights,
const Skin_deformer& dfm)
{
Eigen::Matrix3f eye = Eigen::Matrix3f::Identity();

for(unsigned i : sub_mesh)
{
unsigned valence = topo.nb_neighbors(i);

Eigen::MatrixXf P(3, valence);
Eigen::MatrixXf Q(3, valence);
for(unsigned n = 0; n < valence; n++)
{
int vert_j = topo.neighbor_to(i, n);

// eij = pi - pj
// compute: P_i * D_i in the paper
float w_cotan = per_edge_weights[i][n];
Vec3 v = (vertices[i] - vertices[vert_j]) * w_cotan;
//P.col(degree) = Eigen::Vector3f(v.x, v.y, v.z);
P(0, n) = v.x;
P(1, n) = v.y;
P(2, n) = v.z;

// eij' = pi' - pj'
//compute: P_i'
v = dfm.vert_anim_pos(weights, i) - dfm.vert_anim_pos(weights, vert_j);
//Q.col() = Eigen::Vector3f(v.x, v.y, v.z);
Q(0, n) = v.x;
Q(1, n) = v.y;
Q(2, n) = v.z;
}
// Compute the 3 by 3 covariance matrix:
// actually S = (P * W * Q.t()); W is already considerred in the previous step (P=P*W)
Eigen::MatrixXf S = (P * Q.transpose());

// Compute the singular value decomposition S = UDV.t
Eigen::JacobiSVD<Eigen::MatrixXf> svd(S, Eigen::ComputeThinU | Eigen::ComputeThinV); // X = U * D * V.t()

Eigen::MatrixXf V = svd.matrixV();
Eigen::MatrixXf Ut = svd.matrixU().transpose();

eye(2,2) = (V * Ut).determinant();	// remember: Eigen starts from zero index

// V*U.t may be reflection (determinant = -1). in this case, we need to change the sign of
// column of U corresponding to the smallest singular value (3rd column)
Eigen::Matrix3f rot = (V * eye * Ut);

svd_rotations[i] = Mat3((float)rot(0,0), (float)rot(0,1), (float)rot(0,2),
(float)rot(1,0), (float)rot(1,1), (float)rot(1,2),
(float)rot(2,0), (float)rot(2,1), (float)rot(2,2) ); //Ri = (V * eye * U.t());
}
}
```

```void PointCloud::computeCovarianceMatrix() {
covarianceMatrix[3][3];

double means[3] = {0, 0, 0};
for (int i = 0; i < points.size(); i++)
means[0] += points[i].x,
means[1] += points[i].y,
means[2] += points[i].z;
means[0] /= points.size(), means[1] /= points.size(), means[2] /= points.size();

for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++) {
covarianceMatrix[i][j] = 0.0;
for (int k = 0; k < points.size(); k++)
covarianceMatrix[i][j] += (means[i] - points[k].coord(i)) *
(means[j] - points[k].coord(j));
covarianceMatrix[i][j] /= points.size() - 1;
}
}
```

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