new quatToAffine() with any direction

src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java

@@ -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 [] quat2an =  QuatUtils.normalize(quat2a); 
 		        double [] quat2bn =  QuatUtils.normalize(quat2b); 
 		        double [] quat2apn = QuatUtils.normalize(quat2ap); 
 		        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 [] quat1aprn = QuatUtils.normalize(quat1apr); 
 		        double [] quat1bprn = QuatUtils.normalize(quat1bpr); 
 		        double [] quat1cprn = QuatUtils.normalize(quat1cpr); 
@@ -273,6 +283,7 @@ public class OrthoAltitudeMatch {
 		        double [] quat1brn = QuatUtils.normalize(quat1br); 
 		        double [] quat1crn = QuatUtils.normalize(quat1cr); 
 		        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]+"]");
@@ -307,7 +318,18 @@ public class OrthoAltitudeMatch {
 		        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()};
+		        
+		        // 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 [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch);
+/*
+		        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);
+		        
 		        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("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("svd_aff_combo=   " + svd_aff_combo.toString(use_degrees));
+		        System.out.println();
+		        
+		        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

@@ -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

@@ -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}};