Press n or j to go to the next uncovered block, b, p or k for the previous block.
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 | 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 2x 2x 2x 2x 2x 2x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x 2x | /**
* @fileOverView
* project entry point
* defines rigid 3D transformation between two sets of points
*/
import { getCentroid, checkOrientation } from './geometry_helper';
import { checkValidPointSets } from './input_helper';
import { matrixMultiply } from './linear_algebra_gl';
import { SVD } from 'svd-js';
import {
add as matrixAdd,
subtract as matrixSubtract,
transpose,
dotMultiply
} from 'mathjs';
/**
* entry function for the module.
* @param setA
* @param setB
* @returns 4x3 rigid transformation matrix
*/
export function getRigidTransformation(setA: number[][], setB: number[][]) {
const validSize = checkValidPointSets(setA, setB);
if (!validSize) {
throw 'given parameters for getRigidTransformation were wrong!';
}
return getTransformation(setA, setB);
}
/**
* Given two point sets having one to one correspondence for each point pair,
* returns a rigid transformation matrix.
* @param setA
* @param setB
* @returns 4x3 matrix
*/
function getTransformation(setA: number[][], setB: number[][]) {
const rotationalMatrix = kabsch(setA, setB);
const unRotatedTranslation = dotMultiply(getCentroid(setA), -1.0).valueOf() as number[];
const unRotatedTranslationTransposed = unRotatedTranslation.map(el=>[el]);
const rotatedTranslation = matrixMultiply(
rotationalMatrix,
unRotatedTranslationTransposed
);
const rotatedTranslationPoint = rotatedTranslation.flat();
const pointSetBCentroid = getCentroid(setB);
const translationVector = matrixAdd(
rotatedTranslationPoint,
pointSetBCentroid
) as number[];
return [rotationalMatrix, translationVector];
}
/**
* implements kabsch algorithm to find rotation matrix between two point sets
* https://en.wikipedia.org/wiki/Kabsch_algorithm
* @param setA
* @param setB
* @returns 3x3 rotation matrix
*/
function kabsch(setA: number[][], setB: number[][]) {
const centroidA = getCentroid(setA);
const centroidB = getCentroid(setB);
const centroidAToOrigin = setA.map((row) => matrixSubtract(row, centroidA));
const centroidBToOrigin = setB.map((row) => matrixSubtract(row, centroidB));
const centroidAToOriginTransposed = transpose(centroidAToOrigin);
const covariance = matrixMultiply(
centroidAToOriginTransposed,
centroidBToOrigin
);
const { u, v } = SVD(covariance, true, true, Number.MIN_VALUE);
let rotationalMatrix = matrixMultiply(v, transpose(u));
rotationalMatrix = checkOrientation(rotationalMatrix);
return rotationalMatrix;
}
|