Avoid SVD to compute optimal rotation between point sets
Optimal rotation in shape matching. - 06/2019 - #Math
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);
g_scene.add(box);
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
);
}
box.castShadow = true;
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);
g_scene.add(box);
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);
}
box.position.copy(randomOffset.add(g_pointsRef[i].position));
box.castShadow = true;
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_scene.add(g_avg);
g_avg.scale.set(0.075, 0.075, 0.075);
g_avg.position.set(average.x, average.y, average.z);
g_avg.castShadow = true;
}
function getAverage(points) {
let average = new THREE.Vector3(0, 0, 0);
for (let i = 0; i < points.length; i++) {
average.add(points[i].position);
}
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]);
let omegaDenom = Math.abs(
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])
.add(QuatBasis[1].clone().cross(A[1]))
.add(QuatBasis[2].clone().cross(A[2]))
.divideScalar(omegaDenom);
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);
pointsIn[i].position.copy(workingIn[i].add(refAverage));
}
}
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);
g_scene.add(light1);
let light2 = new THREE.DirectionalLight(0xbbbbbb);
light2.position.set(0, 200, 100);
light2.castShadow = true;
light2.shadow.camera.top = 180;
light2.shadow.camera.bottom = -100;
light2.shadow.camera.left = -120;
light2.shadow.camera.right = 120;
g_scene.add(light2);
//scene.add(new THREE.CameraHelper(light.shadow.camera));
// ground
let mesh = new THREE.Mesh(
new THREE.PlaneBufferGeometry(2000, 2000),
new THREE.MeshPhongMaterial({
color: 0x999999,
depthWrite: false
}));
mesh.rotation.x = -Math.PI / 2;
mesh.receiveShadow = true;
g_scene.add(mesh);
let grid = new THREE.GridHelper(2000, 20, 0x000000, 0x000000);
grid.material.opacity = 0.2;
grid.material.transparent = true;
g_scene.add(grid);
g_renderer = new THREE.WebGLRenderer({
antialias: true
});
g_renderer.setPixelRatio(window.devicePixelRatio);
g_renderer.setSize(window.innerWidth, window.innerHeight);
g_renderer.shadowMap.enabled = true;
document.body.style.margin = 0;
document.body.style.padding = 0;
document.body.style.overflow = "hidden";
document.body.style.position = "fixed";
container.appendChild(g_renderer.domElement);
window.addEventListener("resize", onWindowResize, false);
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);
//g_scene.add(g_cogPointMesh);
initPoints(draggableObjects);
var dragControls = new /*THREE.*/ DragControls(
draggableObjects,
g_camera,
g_renderer.domElement);
dragControls.addEventListener("dragstart", function () {
g_controls.enabled = false;
});
dragControls.addEventListener("dragend", function () {
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
// 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
No comments