Skip to content

I
imagej-elphel
Commit d9f79276 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

new quatToAffine() with any direction

parent ace24186
Hide whitespace changes
Inline Side-by-side
Showing with 359 additions and 68 deletions
src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java
View file @ d9f79276
......@@ -33,6 +33,7 @@ import java.util.Calendar;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import Jama.Matrix;
import ij.IJ;
public class OrthoAltitudeMatch {
......@@ -199,6 +200,8 @@ public class OrthoAltitudeMatch {
boolean test_quat = true;
if (test_quat) {
System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
boolean use_degrees = true;
boolean [][] masks = new boolean[2][];
double [][] alt_data5s = new double[2][];
double [][] data_overlap = new double[2][];
......@@ -246,25 +249,32 @@ public class OrthoAltitudeMatch {
}
double [] quat_rdiff = QuatUtils.invert(quat_diff);
/// double [] quat2a = QuatUtils.multiply(quats01[0], quat_diff);
/// double [] quat2b = QuatUtils.multiply(quat_diff, quats01[0]);
double [] quat2a = QuatUtils.multiply(quats01[0], quat_diff);
double [] quat2b = QuatUtils.multiply(quat_diff, quats01[0]);
double [] quat2ap = QuatUtils.pureTilt(quat2a);
double [] quat2bp = QuatUtils.pureTilt(quat2b);
double [] quat2bp = QuatUtils.pureTilt(quat2b);
/*
double [] quat2an = QuatUtils.normalize(quat2a);
double [] quat2bn = QuatUtils.normalize(quat2b);
double [] quat2apn = QuatUtils.normalize(quat2ap);
double [] quat2bpn = QuatUtils.normalize(quat2bp);
double [] quat2bpn = QuatUtils.normalize(quat2bp);
*/
/*
double [] quat1ar = QuatUtils.multiply(quats01[1], quat_rdiff);
double [] quat1br = QuatUtils.multiply(quat_rdiff, quats01[1]);
double [] quat1cr = QuatUtils.multiply(quats01[2], quat_rdiff);
double [] quat1dr = QuatUtils.multiply(quat_rdiff, quats01[2]);
*/
/*
double [] quat1apr = QuatUtils.pureTilt(quat1ar);
double [] quat1bpr = QuatUtils.pureTilt(quat1br);
double [] quat1cpr = QuatUtils.pureTilt(quat1cr);
double [] quat1dpr = QuatUtils.pureTilt(quat1dr);
double [] quat1dpr = QuatUtils.pureTilt(quat1dr);
*/
/*
double [] quat1aprn = QuatUtils.normalize(quat1apr);
double [] quat1bprn = QuatUtils.normalize(quat1bpr);
double [] quat1cprn = QuatUtils.normalize(quat1cpr);
......@@ -272,7 +282,8 @@ public class OrthoAltitudeMatch {
double [] quat1arn = QuatUtils.normalize(quat1ar);
double [] quat1brn = QuatUtils.normalize(quat1br);
double [] quat1crn = QuatUtils.normalize(quat1cr);
double [] quat1drn = QuatUtils.normalize(quat1dr);
double [] quat1drn = QuatUtils.normalize(quat1dr);
*/
//pureTilt
System.out.println("alt_data= ["+alt_data[0]+","+alt_data[1]+","+alt_data[2]+"]");
System.out.println("alt_datas[0]=["+alt_datas[0][0]+","+alt_datas[0][1]+","+alt_datas[0][2]+"]");
......@@ -306,8 +317,19 @@ public class OrthoAltitudeMatch {
System.out.println("quat1crn= ["+quat1crn[0]+","+quat1crn[1]+","+quat1crn[2]+","+quat1crn[3]+"] "+QuatUtils.norm(quat1crn)); // +++
System.out.println("quat1drn= ["+quat1drn[0]+","+quat1drn[1]+","+quat1drn[2]+","+quat1drn[3]+"] "+QuatUtils.norm(quat1drn)); // +++
*/
double [][] affine_pair = pairwiseOrthoMatch.getAffine();
double [][][] affines = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine()};
double [][] affine_pair = pairwiseOrthoMatch.getAffine();
// calculate second from first and pair
double [][] affine0 = ortho_maps[ipair[0]].getAffine();
Matrix A0 = new Matrix (
new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
Matrix A10 = new Matrix (
new double [][] {{affine_pair[0][0],affine_pair[0][1]},{affine_pair[1][0],affine_pair[1][1]}});
Matrix A1a = A10.times(A0);
double [][] affine1a = new double[][] {
{A1a.get(0,0),A1a.get(0,1)},
{A1a.get(1,0),A1a.get(1,1)}};
double [][][] affines = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine(), affine1a};
boolean make__pure_tilt = false;
double [][] aff1_stretch = QuatUtils.quatToAffine(
quats01[0], // double [] quat,
......@@ -331,90 +353,198 @@ public class OrthoAltitudeMatch {
y_down_ccw); // boolean y_down_ccw);
double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair,true, y_down_ccw);
double [] sqaffd_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaffd_pm[1])}; // normalizes
System.out.println("qaffd_pm[0]= ["+qaffd_pm[0][0]+ ","+qaffd_pm[0][1]+ ","+qaffd_pm[0][2]+ ","+qaffd_pm[0][3]+ "] scale="+sqaffd_pm[0]);
System.out.println("qaffd_pm[1]= ["+qaffd_pm[1][0]+ ","+qaffd_pm[1][1]+ ","+qaffd_pm[1][2]+ ","+qaffd_pm[1][3]+ "] scale="+sqaffd_pm[1]);
System.out.println("qaffd_pm[0]= "+QuatUtils.toString(qaffd_pm[0],use_degrees));
System.out.println("qaffd_pm[1]= "+QuatUtils.toString(qaffd_pm[1],use_degrees));
double [][] qaff1a_pm = QuatUtils.affineToQuatScaled(aff1_shrink, false,y_down_ccw);
double [] sqaff1a_pm = {QuatUtils.normalizeInPlace(qaff1a_pm[0]),QuatUtils.normalizeInPlace(qaff1a_pm[1])}; // normalizes
System.out.println("qaff1a_pm[0]= ["+qaff1a_pm[0][0]+ ","+qaff1a_pm[0][1]+ ","+qaff1a_pm[0][2]+ ","+qaff1a_pm[0][3]+ "] scale="+sqaff1a_pm[0]);
System.out.println("qaff1a_pm[1]= ["+qaff1a_pm[1][0]+ ","+qaff1a_pm[1][1]+ ","+qaff1a_pm[1][2]+ ","+qaff1a_pm[1][3]+ "] scale="+sqaff1a_pm[1]);
System.out.println("qaff1a_pm[0]= "+QuatUtils.toString(qaff1a_pm[0],use_degrees));
System.out.println("qaff1a_pm[1]= "+QuatUtils.toString(qaff1a_pm[1],use_degrees));
double [][] qaff1_pm = QuatUtils.affineToQuatScaled(aff1_stretch, true, y_down_ccw);
double [] sqaff1_pm = {QuatUtils.normalizeInPlace(qaff1_pm[0]),QuatUtils.normalizeInPlace(qaff1_pm[1])}; // normalizes
System.out.println("qaff1_pm[0]= ["+qaff1_pm[0][0]+ ","+qaff1_pm[0][1]+ ","+qaff1_pm[0][2]+ ","+qaff1_pm[0][3]+ "] scale="+sqaff1_pm[0]);
System.out.println("qaff1_pm[1]= ["+qaff1_pm[1][0]+ ","+qaff1_pm[1][1]+ ","+qaff1_pm[1][2]+ ","+qaff1_pm[1][3]+ "] scale="+sqaff1_pm[1]);
System.out.println("qaff1_pm[0]= "+QuatUtils.toString(qaff1_pm[0],use_degrees));
System.out.println("qaff1_pm[1]= "+QuatUtils.toString(qaff1_pm[1],use_degrees));
double [][] qaff2a_pm = QuatUtils.affineToQuatScaled(aff2_shrink, false, y_down_ccw);
double [] sqaff2a_pm = {QuatUtils.normalizeInPlace(qaff2a_pm[0]),QuatUtils.normalizeInPlace(qaff2a_pm[1])}; // normalizes
System.out.println("qaff2a_pm[0]= ["+qaff2a_pm[0][0]+","+qaff2a_pm[0][1]+","+qaff2a_pm[0][2]+","+qaff2a_pm[0][3]+"] scale="+sqaff2a_pm[0]);
System.out.println("qaff2a_pm[1]= ["+qaff2a_pm[1][0]+","+qaff2a_pm[1][1]+","+qaff2a_pm[1][2]+","+qaff2a_pm[1][3]+"] scale="+sqaff2a_pm[1]);
System.out.println("qaff2a_pm[0]= "+QuatUtils.toString(qaff2a_pm[0],use_degrees));
System.out.println("qaff2a_pm[1]= "+QuatUtils.toString(qaff2a_pm[1],use_degrees));
double [][] qaff2_pm = QuatUtils.affineToQuatScaled(aff2_stretch, true, y_down_ccw);
double [] sqaff2_pm = {QuatUtils.normalizeInPlace(qaff2_pm[0]),QuatUtils.normalizeInPlace(qaff2_pm[1])}; // normalizes
System.out.println("qaff2_pm[0]= ["+qaff2_pm[0][0]+ ","+qaff2_pm[0][1]+ ","+qaff2_pm[0][2]+ ","+qaff2_pm[0][3]+ "] scale="+sqaff2_pm[0]);
System.out.println("qaff2_pm[1]= ["+qaff2_pm[1][0]+ ","+qaff2_pm[1][1]+ ","+qaff2_pm[1][2]+ ","+qaff2_pm[1][3]+ "] scale="+sqaff2_pm[1]);
System.out.println("qaff2_pm[0]= "+QuatUtils.toString(qaff2_pm[0],use_degrees));
System.out.println("qaff2_pm[1]= "+QuatUtils.toString(qaff2_pm[1],use_degrees));
double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], false, y_down_ccw);
double [] sqaffine0_pm ={QuatUtils.normalizeInPlace(qaffine0_pm[0]),QuatUtils.normalizeInPlace(qaffine0_pm[1])}; // normalizes
System.out.println("qaffine0_pm[0]=["+qaffine0_pm[0][0]+ ","+qaffine0_pm[0][1]+ ","+qaffine0_pm[0][2]+ ","+qaffine0_pm[0][3]+ "] scale="+sqaffine0_pm[0]);
System.out.println("qaffine0_pm[1]=["+qaffine0_pm[1][0]+ ","+qaffine0_pm[1][1]+ ","+qaffine0_pm[1][2]+ ","+qaffine0_pm[1][3]+ "] scale="+sqaffine0_pm[1]);
double [][] qa0_pm = {qaffine0_pm[0].clone(),qaffine0_pm[1].clone()};
System.out.println("qaffine0_pm[0]= "+QuatUtils.toString(qaffine0_pm[0],use_degrees));
System.out.println("qaffine0_pm[1]= "+QuatUtils.toString(qaffine0_pm[1],use_degrees));
double [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1], false, y_down_ccw);
double [] sqaffine1_pm ={QuatUtils.normalizeInPlace(qaffine1_pm[0]),QuatUtils.normalizeInPlace(qaffine1_pm[1])}; // normalizes
System.out.println("qaffine1_pm[0]=["+qaffine1_pm[0][0]+ ","+qaffine1_pm[0][1]+ ","+qaffine1_pm[0][2]+ ","+qaffine1_pm[0][3]+ "] scale="+sqaffine1_pm[0]);
System.out.println("qaffine1_pm[1]=["+qaffine1_pm[1][0]+ ","+qaffine1_pm[1][1]+ ","+qaffine1_pm[1][2]+ ","+qaffine1_pm[1][3]+ "] scale="+sqaffine1_pm[1]);
double [][] qa1_pm = {qaffine1_pm[0].clone(),qaffine1_pm[1].clone()};
System.out.println("qaffine1_pm[0]= "+QuatUtils.toString(qaffine1_pm[0],use_degrees));
System.out.println("qaffine1_pm[1]= "+QuatUtils.toString(qaffine1_pm[1],use_degrees));
double [][] qaffine1a_pm = QuatUtils.affineToQuatScaled(affines[2], false, y_down_ccw);
double [][] qa1a_pm = {qaffine1a_pm[0].clone(),qaffine1a_pm[1].clone()};
System.out.println("qaffine1a_pm[0]="+QuatUtils.toString(qaffine1a_pm[0],use_degrees));
System.out.println("qaffine1a_pm[1]="+QuatUtils.toString(qaffine1a_pm[1],use_degrees));
double [][][][] raffines_pm = new double [affines.length][2][][];
SingularValueDecomposition[][] rsvd_pm = new SingularValueDecomposition [affines.length][2];
for (int i = 0; i < raffines_pm.length;i++) {
raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],false,true,true);
raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],false,false,true);
rsvd_pm[i][0] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][0], y_down_ccw);
rsvd_pm[i][1] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][1], y_down_ccw);
System.out.println("raffines["+i+"][0]= [["+raffines_pm[i][0][0][0]+ ","+raffines_pm[i][0][0][1]+"],");
System.out.println( " ["+raffines_pm[i][0][1][0]+ ","+raffines_pm[i][0][1][1]+"]]");
System.out.println("raffines["+i+"][1]= [["+raffines_pm[i][1][0][0]+ ","+raffines_pm[i][1][0][1]+"],");
System.out.println( " ["+raffines_pm[i][1][1][0]+ ","+raffines_pm[i][1][1][1]+"]]");
System.out.println("rsvd_pm["+i+"][0]= "+ rsvd_pm[i][0].toString(use_degrees));
System.out.println("rsvd_pm["+i+"][1]= "+ rsvd_pm[i][1].toString(use_degrees));
System.out.println();
}
/*
double [][] qvariants = {
QuatUtils.divideScaled(qa0_pm[0], qa1_pm[0]),
QuatUtils.divideScaled(qa0_pm[1], qa1_pm[0]),
QuatUtils.divideScaled(qa0_pm[0], qa1_pm[1]),
QuatUtils.divideScaled(qa0_pm[1], qa1_pm[1])};
double [][] qvariants1 = {
QuatUtils.divideScaled1(qa0_pm[0], qa1_pm[0]),
QuatUtils.divideScaled1(qa0_pm[1], qa1_pm[0]),
QuatUtils.divideScaled1(qa0_pm[0], qa1_pm[1]),
QuatUtils.divideScaled1(qa0_pm[1], qa1_pm[1])};
double [] svariants= new double [qvariants.length];
double [] svariants1= new double [qvariants1.length];
for (int i = 0; i < svariants.length; i++) svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]);
for (int i = 0; i < svariants1.length; i++) svariants1[i] = QuatUtils.normalizeInPlace(qvariants1[i]);
for (int i = 0; i < svariants.length; i++) {
System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]);
System.out.println("qvariant1["+i+"] = ["+qvariants1[i][0]+ ","+qvariants1[i][1]+ ","+qvariants1[i][2]+ ","+qvariants1[i][3]+ "] scale="+svariants1[i]);
}
*/
double [][] qvariants = {
QuatUtils.divideScaled(qa1_pm[0], qa0_pm[0]),
QuatUtils.divideScaled(qa1_pm[1], qa0_pm[0]),
QuatUtils.divideScaled(qa1_pm[0], qa0_pm[1]),
QuatUtils.divideScaled(qa1_pm[1], qa0_pm[1])};
/*
double [][] qvariants1 = {
QuatUtils.divideScaled1(qa1_pm[0], qa0_pm[0]),
QuatUtils.divideScaled1(qa1_pm[1], qa0_pm[0]),
QuatUtils.divideScaled1(qa1_pm[0], qa0_pm[1]),
QuatUtils.divideScaled1(qa1_pm[1], qa0_pm[1])};
*/
double [] svariants= new double [qvariants.length];
double [][][] taffines = new double [qvariants.length][][];
SingularValueDecomposition[] svd_avars = new SingularValueDecomposition[qvariants.length];
/// double [] svariants1= new double [qvariants1.length];
for (int i = 0; i < svariants.length; i++) svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]);
/// for (int i = 0; i < svariants1.length; i++) svariants1[i] = QuatUtils.normalizeInPlace(qvariants1[i]);
for (int i = 0; i < svariants.length; i++) {
System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]);
/// System.out.println("qvariant1["+i+"] = ["+qvariants1[i][0]+ ","+qvariants1[i][1]+ ","+qvariants1[i][2]+ ","+qvariants1[i][3]+ "] scale="+svariants1[i]);
taffines[i] = QuatUtils.quatToAffine(
qvariants[i], // double [] quat,
false, // boolean stretch,
false, // make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
svd_avars[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(taffines[i], y_down_ccw);
}
double [][] qvariantsa = {
QuatUtils.divideScaled(qa1a_pm[0], qa0_pm[0]),
QuatUtils.divideScaled(qa1a_pm[1], qa0_pm[0]),
QuatUtils.divideScaled(qa1a_pm[0], qa0_pm[1]),
QuatUtils.divideScaled(qa1a_pm[1], qa0_pm[1])};
for (int i = 0; i < qvariantsa.length; i++) {
System.out.println("qvarianta["+i+"] = "+QuatUtils.toString(qvariantsa[i],use_degrees));
System.out.println("taffines["+i+"]= [["+taffines[i][0][0]+ ","+taffines[i][0][1]+"]");
System.out.println( " ["+taffines[i][1][0]+ ","+taffines[i][1][1]+"]]");
System.out.println("svd_avars["+i+"]= "+svd_avars[i].toString(use_degrees));
System.out.println();
}
double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invdert order?
double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invdert order?
SingularValueDecomposition svd_affine_pair = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
SingularValueDecomposition[] svd_affines = {
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw),
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};
SingularValueDecomposition svd_aff1_shrink = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_shrink, y_down_ccw);
SingularValueDecomposition svd_aff1_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_stretch, y_down_ccw);
SingularValueDecomposition svd_aff2_shrink = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_shrink, y_down_ccw);
SingularValueDecomposition svd_aff2_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_stretch, y_down_ccw);
SingularValueDecomposition svd_aff_combo = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo, y_down_ccw);
SingularValueDecomposition svd_aff_combo1 = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo1, y_down_ccw);
double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch);
System.out.println("affine_pair= [["+affine_pair[0][0]+","+affine_pair[0][1]+","+affine_pair[0][2]+"]");
System.out.println(" ["+affine_pair[1][0]+","+affine_pair[1][1]+","+affine_pair[1][2]+"]]");
System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
System.out.println();
System.out.println("affines[0]= [["+affines[0][0][0]+ ","+affines[0][0][1]+ ","+affines[0][0][2]+"]");
System.out.println(" ["+affines[0][1][0]+ ","+affines[0][1][1]+ ","+affines[0][1][2]+"]]");
System.out.println("svd_affines[0]= " + svd_affines[0].toString(use_degrees));
System.out.println();
System.out.println("affines[1]= [["+affines[1][0][0]+ ","+affines[1][0][1]+ ","+affines[1][0][2]+"]");
System.out.println(" ["+affines[1][1][0]+ ","+affines[1][1][1]+ ","+affines[1][1][2]+"]]");
System.out.println("svd_affines[1]= " + svd_affines[1].toString(use_degrees));
System.out.println();
System.out.println("affines[2]= [["+affines[2][0][0]+ ","+affines[2][0][1]+"]");
System.out.println(" ["+affines[2][1][0]+ ","+affines[2][1][1]+"]]");
System.out.println("svd_affines[2]= " + svd_affines[2].toString(use_degrees));
System.out.println();
System.out.println("aff1_shrink= [["+aff1_shrink[0][0]+ ","+aff1_shrink[0][1]+"]");
System.out.println(" ["+aff1_shrink[1][0]+ ","+aff1_shrink[1][1]+"]]");
System.out.println("svd_aff1_shrink=" + svd_aff1_shrink.toString(use_degrees));
System.out.println();
System.out.println("aff1_stretch= [["+aff1_stretch[0][0]+ ","+aff1_stretch[0][1]+"]");
System.out.println(" ["+aff1_stretch[1][0]+ ","+aff1_stretch[1][1]+"]]");
System.out.println("svd_aff1_stretch=" + svd_aff1_stretch.toString(use_degrees));
System.out.println();
System.out.println("aff2_shrink= [["+aff2_shrink[0][0]+ ","+aff2_shrink[0][1]+ "]");
System.out.println(" ["+aff2_shrink[1][0]+ ","+aff2_shrink[1][1]+ "]]");
System.out.println("svd_aff2_shrink= " + svd_aff2_shrink.toString(use_degrees));
System.out.println();
System.out.println("aff2_stretch= [["+aff2_stretch[0][0]+ ","+aff2_stretch[0][1]+"]");
System.out.println(" ["+aff2_stretch[1][0]+ ","+aff2_stretch[1][1]+"]]");
System.out.println("svd_aff2_stretch=" + svd_aff2_stretch.toString(use_degrees));
System.out.println();
System.out.println("aff_combo= [["+aff_combo[0][0]+ ","+aff_combo[0][1]+ "]");
System.out.println(" ["+aff_combo[1][0]+ ","+aff_combo[1][1]+ "]]");
/*
double [] svd_affine_pair = OrthoMap.singularValueDecomposeScaleTilt(affine_pair, y_down_ccw); // boolean y_down_ccw)
double [][] svd_affines = {OrthoMap.singularValueDecomposeScaleTilt(affines[0], y_down_ccw), OrthoMap.singularValueDecomposeScaleTilt(affines[1], y_down_ccw)};
double [] svd_aff1_stretch = OrthoMap.singularValueDecomposeScaleTilt(aff1_stretch, y_down_ccw);
double [] svd_aff2_shrink = OrthoMap.singularValueDecomposeScaleTilt(aff2_shrink, y_down_ccw);
double [] svd_aff2_stretch = OrthoMap.singularValueDecomposeScaleTilt(aff2_stretch, y_down_ccw);
double [] svd_aff_combo = OrthoMap.singularValueDecomposeScaleTilt(aff_combo, y_down_ccw);
*/
SingularValueDecomposition svd_affine_pair = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
SingularValueDecomposition[] svd_affines = {SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw), SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw)};
SingularValueDecomposition svd_aff1_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_stretch, y_down_ccw);
SingularValueDecomposition svd_aff2_shrink = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_shrink, y_down_ccw);
SingularValueDecomposition svd_aff2_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_stretch, y_down_ccw);
SingularValueDecomposition svd_aff_combo = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo, y_down_ccw);
System.out.println("scale,tilt_rad,gamma,rot");
System.out.println("svd_affine_pair= ["+svd_affine_pair.scale+ ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+ "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_affines[0]= ["+svd_affines[0].scale+ ","+ svd_affines[0].getTiltAngle()+ ","+svd_affines[0].gamma+ ","+svd_affines[0].rot+ "] tilt="+(svd_affines[0].getTiltAngle()* 180/Math.PI)+ "\u00B0, dir="+(svd_affines[0].gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_affines[1]= ["+svd_affines[1].scale+ ","+ svd_affines[1].getTiltAngle()+ ","+svd_affines[1].gamma+ ","+svd_affines[1].rot+ "] tilt="+(svd_affines[1].getTiltAngle()* 180/Math.PI)+ "\u00B0, dir="+(svd_affines[1].gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff1_stretch= ["+svd_aff1_stretch.scale+","+ svd_aff1_stretch.getTiltAngle()+","+svd_aff1_stretch.gamma+","+svd_aff1_stretch.rot+"] tilt="+(svd_aff1_stretch.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff1_stretch.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff2_shrink= ["+svd_aff2_shrink.scale+ ","+ svd_aff2_shrink.getTiltAngle()+ ","+svd_aff2_shrink.gamma+ ","+svd_aff2_shrink.rot+ "] tilt="+(svd_aff2_shrink.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff2_shrink.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff2_stretch= ["+svd_aff2_stretch.scale+","+ svd_aff2_stretch.getTiltAngle()+","+svd_aff2_stretch.gamma+","+svd_aff2_stretch.rot+"] tilt="+(svd_aff2_stretch.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff2_stretch.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff_combo= ["+svd_aff_combo.scale+ ","+ svd_aff_combo.getTiltAngle()+ ","+svd_aff_combo.gamma+ ","+svd_aff_combo.rot+ "] tilt="+(svd_aff_combo.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff_combo.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff_combo= " + svd_aff_combo.toString(use_degrees));
System.out.println();
System.out.println("quat_diff= ["+quat_diff[0]+ ","+quat_diff[1]+ ","+quat_diff[2]+ ","+quat_diff[3]+ "] tilt="+2*Math.acos(quat_diff[0])+ " dir="+Math.atan2(quat_diff[2],quat_diff[1])+", tilt="+(2*Math.acos(quat_diff[0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quat_diff[2],quat_diff[1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[0]= ["+quats01[0][0]+","+quats01[0][1]+","+quats01[0][2]+","+quats01[0][3]+"] tilt="+2*Math.acos(quats01[0][0])+" dir="+Math.atan2(quats01[0][2],quats01[0][1])+", tilt="+(2*Math.acos(quats01[0][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[0][2],quats01[0][1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[1]= ["+quats01[1][0]+","+quats01[1][1]+","+quats01[1][2]+","+quats01[1][3]+"] tilt="+2*Math.acos(quats01[1][0])+" dir="+Math.atan2(quats01[1][2],quats01[1][1])+", tilt="+(2*Math.acos(quats01[1][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[1][2],quats01[1][1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[2]= ["+quats01[2][0]+","+quats01[2][1]+","+quats01[2][2]+","+quats01[2][3]+"] tilt="+2*Math.acos(quats01[2][0])+" dir="+Math.atan2(quats01[2][2],quats01[2][1])+", tilt="+(2*Math.acos(quats01[2][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[2][2],quats01[2][1])*180/Math.PI)+"\u00B0");
/*
System.out.println("quat_diff_tilt= "+2*Math.acos(quat_diff[0]));
System.out.println("quats01_0_tilt= "+2*Math.acos(quats01[0][0]));
System.out.println("quats01_1_tilt= "+2*Math.acos(quats01[1][0]));
System.out.println("quats01_2_tilt= "+2*Math.acos(quats01[2][0]));
*/
System.out.println("aff_combo1= [["+aff_combo1[0][0]+ ","+aff_combo1[0][1]+ "]");
System.out.println(" ["+aff_combo1[1][0]+ ","+aff_combo1[1][1]+ "]]");
System.out.println("svd_aff_combo1= " + svd_aff_combo1.toString(use_degrees));
System.out.println();
// System.out.println("scale,tilt_rad,gamma,rot");
System.out.println("quat_diff= "+QuatUtils.toString(quat_diff,use_degrees));
System.out.println("quats01[0]= "+QuatUtils.toString(quats01[0],use_degrees));
System.out.println("quats01[1]= "+QuatUtils.toString(quats01[1],use_degrees));
System.out.println("quats01[2]= "+QuatUtils.toString(quats01[2],use_degrees));
//QuatUtils.toString(qvariantsa[i],use_degrees));
System.out.println();
System.out.println();
System.out.println();
}
......
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java
View file @ d9f79276
......@@ -36,6 +36,18 @@ public final class QuatUtils { // static class
* t3 = (r0s3 - r1s2 + r2s1 + r3s0)
*/
public static String toString(
double [] quat_i,
boolean degrees) {
double [] quat=quat_i.clone();
double scale =normalizeInPlace(quat);
String fmt_rad = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.9f, dir=%12.9f, scale = %12.10f";
String fmt_deg = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.7f\u00B0, dir=%12.7f\u00B0, scale=%12.10f";
double s = degrees ? (180/Math.PI):1;
String fmt=degrees ? fmt_deg : fmt_rad;
return String.format(fmt, quat[0], quat[1],quat[2],quat[3], s*2*Math.acos(quat[0]), s*Math.atan2(quat[2],quat[1]), scale);
}
/**
* Multiply to quaternions
* @param r first quaternion
......@@ -44,8 +56,7 @@ public final class QuatUtils { // static class
*/
public static double [] multiply(
double [] r,
double [] s
) {
double [] s ) {
double [] t = {
r[0]*s[0] - r[1]*s[1] - r[2]*s[2] - r[3]*s[3],
r[0]*s[1] + r[1]*s[0] - r[2]*s[3] + r[3]*s[2],
......@@ -54,6 +65,59 @@ public final class QuatUtils { // static class
return t;
}
public static double [] multiplyScaled(
double [] r,
double [] s ) {
return scale(multiply(normalize(r),normalize(s)),norm(r)*norm(s));
}
public static double [] invertScaled(double [] quat) {
return scale(invert(normalize(quat)),1.0/norm(quat));
}
/*
public static double [] divide( // pure rotation
double [] r,
double [] s ) {
return multiply(invert(r), s);
}
public static double [] divideScaled(
double [] r,
double [] s ) {
return multiplyScaled(invertScaled(r), s);
}
public static double [] divideScaled1(
double [] r,
double [] s ) {
return multiply(invertScaled(r), s);
}
*/
public static double [] divide( // pure rotation
double [] r,
double [] s ) {
return multiply(r, invert(s));
}
public static double [] divideScaled(
double [] r,
double [] s ) {
return multiplyScaled(r, invertScaled(s));
}
public static double [] divideScaled1(
double [] r,
double [] s ) {
return multiply(r, invertScaled(s));
}
public static double norm(
double [] quat) {
return Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
......@@ -199,6 +263,86 @@ public final class QuatUtils { // static class
return affine;
}
public static double [][] quatToAffine(
double [] quat,
boolean invert,
boolean y_down_ccw){
/*
The product of two quaternions:
t = rs
(t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3)
t0 = (r0s0 - r1s1 - r2s2 - r3s3)
t1 = (r0s1 + r1s0 - r2s3 + r3s2)
t2 = (r0s2 + r1s3 + r2s0 - r3s1)
t3 = (r0s3 - r1s2 + r2s1 + r3s0)
p = (0, x, y, z)
p'= inv(q) * p *q for active rotation (we'll need an inverse to get source pixel coordinates x,y
point (z==0), starting with map coordinates
p = (0, x, y, 0)
pq0 = ( - r1s1 - r2s2 ) = (-x * q[1] - y * q[2])
pq1 = ( + r1s0 - r2s3 ) = ( x * q[0] - y * q[3])
pq2 = ( + r1s3 + r2s0 ) = ( x * q[3] + y * q[0])
pq3 = ( - r1s2 + r2s1 ) = (-x * q[2] + y * q[1])
~q=[q0,-q1,-q2,-q3]
~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + -q3pq3) = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 + -q1pq0 - -q2pq3 + -q3pq2) = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 + -q1pq3 + -q2pq0 - -q3pq1) = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 - -q1pq2 + -q2pq1 + -q3pq0) = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
~q*p*q0 = q[0]*(-x * q[1] - y * q[2]) + q[1]*( x * q[0] - y * q[3]) + q[2]*( x * q[3] + y * q[0]) + q[3]*(-x * q[2] + y * q[1])
~q*p*q1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
~q*p*q2 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
~q*p*q3 = q[0]*(-x * q[2] + y * q[1]) + q[1]*( x * q[3] + y * q[0]) - q[2]*( x * q[0] - y * q[3]) - q[3]*(-x * q[1] - y * q[2])
x1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
y1 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
x1 = x* (q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) + y* (-q[0]*q[3] + q[1]*q[2] + q[2]*q[1] - q[3]*q[0])
y1 = x* (q[0]*q[3] + q[1]*q[2] + q[2]*q[1] + q[3]*q[0]) + y* ( q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3])
x1 = x* (q00 + q11 - q22 - q33) + y* (-q03 + q12 + q12 - q03)
y1 = x* (q03 + q12 + q12 + q03) + y* ( q00 - q11 + q22 - q33)
x1 = x* (q00 + q11 - q22 - q33) + y*2*(q12 - q03)
y1 = x*2*(q03 + q12) + y* ( q00 - q11 + q22 - q33)
where:
q00 = q[0]*q[0]
q11 = q[1]*q[1]
q22 = q[1]*q[1]
q33 = q[1]*q[1]
q03 = q[0]*q[3]
q12 = q[1]*q[2]
*/
double scale = normQuat(quat);
double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
double q00 = quat[0]*quat[0];
double q11 = quat[1]*quat[1];
double q22 = quat[1]*quat[1];
double q33 = quat[1]*quat[1];
double q03 = quat[0]*quat[3];
double q12 = quat[1]*quat[2];
double [][] affine = {
{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
{scale_y*2*(q03 + q12), scale_y*(q00 - q11 + q22 - q33)}};
if (invert) {
affine = matInverse2x2(affine);
}
return affine;
}
/**
* Restore quaternion (rotation+scale) from affine traqnsform (only [2][2] is used, ok to have [2][3] input
* As there are 2 solutions, both are provided in the output. As the
......@@ -231,7 +375,8 @@ public final class QuatUtils { // static class
double stilt = Math.sin(tiltAngle/2);
double [] q_plus = {ctilt, stilt*cmbeta, stilt*smbeta, 0};
double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
/// double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
double [][] quats_pm = {scale(multiply(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
return quats_pm;
}
......@@ -253,7 +398,7 @@ public final class QuatUtils { // static class
return m;
}
public static double [] matMul (
public static double [] matMult (
double [][] m1,
double [] v){
if (m1[0].length != v.length) {
......@@ -268,7 +413,14 @@ public final class QuatUtils { // static class
}
return v2;
}
public static double [][] matInverse2x2(
double [][] a) {
// throw new IllegalArgumentException("Only [2][2] arrays");
double idet = 1.0/(a[0][0] * a[1][1] - a[0][1] * a[1][0]);
return new double [][] {
{ idet*a[1][1], -idet*a[0][1]},
{-idet*a[1][0], idet*a[0][0]}};
}
......
src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java
View file @ d9f79276
......@@ -43,6 +43,15 @@ public class SingularValueDecomposition {
public double getMinScale() {return Math.min(w1,w2);}
public double getMaxScale() {return Math.max(w1,w2);}
public String toString(boolean degrees) {
//System.out.println("svd_affine_pair= ["+svd_affine_pair.scale+ ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+
// "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
String fmt_rad = "scale=%10.8f,tilt= %10.7f, gamma=%10.7f, beta=%10.7f, rot=%10.7f, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
String fmt_deg = "scale=%10.8f,tilt= %10.5f\u00B0, gamma=%10.5f\u00B0, beta=%10.5f\u00B0, rot=%10.5f\u00B0, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
double s = degrees ? (180/Math.PI):1;
String fmt=degrees ? fmt_deg : fmt_rad;
return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, ratio);
}
public double [][] getW(){
return new double [][] {{w1,0},{0,w1}};
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment