private static double[] solution(DoubleMatrix2D X, DoubleMatrix2D Y, int k) {
// Solve X * Beta = Y for Beta
// Only the first column of Y is used
// k is number of beta coefficients
QRDecomposition qr = new QRDecomposition(X);
if (qr.hasFullRank()) {
DoubleMatrix2D B = qr.solve(Y);
return B.viewColumn(0).toArray();
} else {
DoubleMatrix1D Y0 = Y.viewColumn(0); // first column of Y
SingularValueDecomposition svd = new SingularValueDecomposition(X);
DoubleMatrix2D S = svd.getS();
DoubleMatrix2D V = svd.getV();
DoubleMatrix2D U = svd.getU();
Algebra alg = new Algebra();
DoubleMatrix2D Ut = alg.transpose(U);
DoubleMatrix1D g = alg.mult(Ut, Y0); // Ut*Y0
for (int j = 0; j < k; j++) {
// solve S*p = g for p; S is a diagonal matrix
double x = S.getQuick(j, j);
if (x > 0.) {
x = g.getQuick(j) / x; // p[j] = g[j]/S[j]
g.setQuick(j, x); // overwrite g by p
} else g.setQuick(j, 0.);
}
DoubleMatrix1D beta = alg.mult(V, g); // V*p
return beta.toArray();
}
}
public static DenseDoubleMatrix2D computeTransitiveTransformation(
DenseDoubleMatrix2D priorTransformationOfMover,
DenseDoubleMatrix2D actualMovementKabsch,
DenseDoubleMatrix2D priorMovementOfBlueprint) {
Algebra alg = new Algebra();
priorTransformationOfMover = (DenseDoubleMatrix2D) alg.inverse(priorTransformationOfMover);
return (DenseDoubleMatrix2D)
alg.mult(
priorMovementOfBlueprint, alg.mult(priorTransformationOfMover, actualMovementKabsch));
// return
// (DenseDoubleMatrix2D)alg.mult(alg.mult(actualMovementKabsch,priorTransformationOfMover),priorMovementOfBlueprint);
}
public static void main(String[] args) {
double[][] arg1 = {{1, 1, 1}, {2, 3, 2}, {4, 7, 3}};
DenseDoubleMatrix2D obj1 = new DenseDoubleMatrix2D(arg1); // rot
double[][] arg2 = {{5, 5, 5}, {8, 8, 8}, {2, 2, 2}};
DenseDoubleMatrix2D obj2 = new DenseDoubleMatrix2D(arg2); // grŸn
double[][] arg3 = {{4, 5, 6}, {3, 2, 1}, {1, 2, 1}};
DenseDoubleMatrix2D obj3 = new DenseDoubleMatrix2D(arg3); // schwarz
System.out.println("obj1\n" + obj1);
System.out.println("obj2\n" + obj2);
System.out.println("obj3\n" + obj3);
double[][][] pair21 = {arg1, arg2};
Transformation trans21 = Kabsch.calculateTransformation(pair21); // grŸn -> rot
DenseDoubleMatrix2D rot21 = trans21.peekRotation();
DenseDoubleMatrix1D trl21 = trans21.peekTranslation();
System.out.println("rot21\n" + rot21);
System.out.println("trl21\n" + trl21);
double[][][] pair12 = {arg2, arg1};
Transformation trans12 = Kabsch.calculateTransformation(pair12); // grŸn -> rot
DenseDoubleMatrix2D rot12 = trans12.peekRotation();
DenseDoubleMatrix1D trl12 = trans12.peekTranslation();
System.out.println("rot12\n" + rot12);
System.out.println("trl12\n" + trl12);
double[][][] pair32 = {arg2, arg3};
Transformation trans32 = Kabsch.calculateTransformation(pair32); // schwarz -> grŸn
DenseDoubleMatrix2D rot32 = trans32.peekRotation();
DenseDoubleMatrix1D trl32 = trans32.peekTranslation();
System.out.println("rot32\n" + rot32);
System.out.println("trl32\n" + trl32.toString());
DenseDoubleMatrix2D arg2onarg1 =
new DenseDoubleMatrix2D(trans21.transform(arg2)); // grŸn -> rot
System.out.println(arg2onarg1.toString());
DenseDoubleMatrix2D uni21 = createUniMatrix(rot21, trl21);
DenseDoubleMatrix2D uni32 = createUniMatrix(rot32, trl32);
System.out.println("uni21\n" + uni21.toString());
System.out.println("uni32\n" + uni32.toString());
Algebra alg = new Algebra();
DenseDoubleMatrix2D result =
(DenseDoubleMatrix2D)
alg.mult(
DoubleFactory2D.dense.identity(4),
computeTransitiveTransformation(
(DenseDoubleMatrix2D) DoubleFactory2D.dense.identity(4), uni32, uni21));
System.out.println("action results\n" + result);
System.out.println("moved points\n" + movePoints(result, obj3).toString());
}
