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 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());
}
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);
}
/** Creates a new instance of testmatrix */
public GLSsolver(double[][] p_MatrixgleichNull) throws IllegalArgumentException {
// --------------------------------------
// Kontrolle, ob Eingabematrix rechteckig
// --------------------------------------
int nplus1 = p_MatrixgleichNull[0].length;
for (int i = 1; i < p_MatrixgleichNull.length; i++) { // Zeilen i
if (p_MatrixgleichNull[i].length != nplus1) {
System.err.println(
"Programmfehler: Matrix des GLS ist nicht rechteckig! (im solver entdeckt)");
throw new IllegalArgumentException();
}
}
if (nplus1 <= 1) throw new IllegalArgumentException("keine Unbekannte"); // keine Unbekannte!!!
// Umgeht einen Fehler in der colt-Bibliothek // TODO wenn behoben, Workaround entfernen
// ------
int anzGl = p_MatrixgleichNull.length;
if (anzGl < nplus1 - 1) { // anzGleichungen < anz Unbekannte
if (debug) System.out.println("WorkAround fuer Fehler in colt: 0 = 0 Gleichungen anhaengen");
anzGl = nplus1 - 1; // = Anzahl Unbek, 0 0 0 ... 0 = 0 Zeile angehängt
}
// -------------------------
// Daten in A und b einlesen
// -------------------------
// so dass A*x = b
A = new DenseDoubleMatrix2D(anzGl, (nplus1 - 1));
DenseDoubleMatrix2D b = new DenseDoubleMatrix2D(anzGl, 1);
for (int i = 0; i < p_MatrixgleichNull.length; i++) { // Zeilen i
for (int j = 0; j < nplus1 - 1; j++) { // Spalten
A.set(i, j, p_MatrixgleichNull[i][j]);
}
b.set(i, 0, -p_MatrixgleichNull[i][nplus1 - 1]);
}
if (debug) {
System.out.println(" A = " + A.toString());
System.out.println(" b = " + b.toString());
System.out.println("");
}
// --------------
// LR - Zerlegung
// --------------
LUDecomposition ALU = new LUDecomposition(A);
if (debug) System.out.println(ALU.toString());
DoubleMatrix2D L = ALU.getL();
R = ALU.getU();
int[] piv = ALU.getPivot();
Algebra alg = new Algebra();
// if (debug) System.out.println("L = " + L.toString());
// if (debug) System.out.println("Kontrolle L*R = " + alg.mult(L,R).toString());
// if (debug) System.out.println("Kontrolle P*b = " + alg.permute(b, piv, null) );
//
// if (debug) System.out.println("Rx = c: R = " + R.toString());
// if (debug) System.out.println("alg.permute(b, piv, null) = " + alg.permute(b, piv,
// null).toString());
c = alg.solve(L, alg.permute(b, piv, null)); // TODO: kann zu Problemen führen,
// wenn weniger Gleichungen als Unbek --> s.Workaround oben
if (debug) System.out.println("Lc = Pb: c = " + c.toString());
if (debug) {
System.out.println("Rang A: " + alg.rank(A));
System.out.println("Rang R: " + alg.rank(R));
}
assert (alg.rank(A) == alg.rank(R)) : "Rang von A ungleich Rang von R --> Programmfehler";
anzUnbestParam = A.columns() - alg.rank(A);
if (debug) System.out.println("Anz unbest Parameter: " + anzUnbestParam);
}
