fixing quaternions-affines

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

@@ -56,6 +56,13 @@ public class OrthoAltitudeMatch {
 			String              orthoMapsCollection_path,
 			int                 debugLevel) {
 		boolean y_down_ccw = true;
+		boolean invert_q2a = true;
+		boolean test_quat = true;
+		if (test_quat) {
+			QuatUtils.testQuatAff();
+		}
+		
+		
 		int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs
 				available_pairs,  // pairs_defined_abs,// int [][] pairs,
 				null);            // int [] indices_in)  // preselected indices or null
@@ -197,7 +204,7 @@ public class OrthoAltitudeMatch {
     		}
 	        double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
     		
-    		boolean test_quat = true;
+
 	        if (test_quat) {
 	        	System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
 		        boolean use_degrees = true;
@@ -331,6 +338,7 @@ public class OrthoAltitudeMatch {
 					{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,
 		    			true,             //   boolean   stretch,
@@ -351,6 +359,25 @@ public class OrthoAltitudeMatch {
 		    			true,             //   boolean   stretch,
 		    			make__pure_tilt, // boolean   make__pure_tilt)
 		    			y_down_ccw); // boolean y_down_ccw);
+		        */
+		        double [][] aff1_stretch = QuatUtils.quatToAffine( // use old for stretch
+		        		quats01[0],       // double [] quat,
+		    			true,             //   boolean   stretch,
+		    			make__pure_tilt, // boolean   make__pure_tilt)
+		    			y_down_ccw); // boolean y_down_ccw);
+		        double [][] aff1_shrink = QuatUtils.quatToAffine(
+		        		quats01[0],       // double [] quat,
+		        		invert_q2a,            //   boolean   stretch,
+		    			y_down_ccw); // boolean y_down_ccw);
+		        double [][] aff2_shrink = QuatUtils.quatToAffine(
+		        		quats01[2],       // double [] quat,
+		        		invert_q2a,             //   boolean   stretch,
+		    			y_down_ccw); // boolean y_down_ccw);
+		        double [][] aff2_stretch = QuatUtils.quatToAffine( // use old for stretch
+		        		quats01[2],       // double [] quat,
+		    			true,             //   boolean   stretch,
+		    			make__pure_tilt, // boolean   make__pure_tilt)
+		    			y_down_ccw); // boolean y_down_ccw);
 		        
 		        double [][] qaffd_pm =    QuatUtils.affineToQuatScaled(affine_pair,true,  y_down_ccw);
 		        System.out.println("qaffd_pm[0]=    "+QuatUtils.toString(qaffd_pm[0],use_degrees));
@@ -390,8 +417,10 @@ public class OrthoAltitudeMatch {
 		        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);
+//		        	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);
+		        	raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],invert_q2a,true);
+		        	raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],invert_q2a,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]+"],");
@@ -444,17 +473,18 @@ public class OrthoAltitudeMatch {
 		        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);
 		        	taffines[i] = QuatUtils.quatToAffine(
 		        			qvariants[i],       // double [] quat,
-			    			false,             //   boolean   stretch,
-			    			false, // make__pure_tilt, // boolean   make__pure_tilt)
+		        			invert_q2a,         //   boolean   invert,
 			    			y_down_ccw); // boolean y_down_ccw);
 		        	svd_avars[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(taffines[i], y_down_ccw);
 		        }
@@ -473,8 +503,8 @@ public class OrthoAltitudeMatch {
 		        }
 		        
 		        
-		        double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invdert order?
-		        double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invdert order?
+		        double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invert order?
+		        double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invert 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),

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

@@ -75,26 +75,6 @@ public final class QuatUtils { // static class
 		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,
@@ -114,6 +94,164 @@ public final class QuatUtils { // static class
 		return multiply(r, invertScaled(s));
 	}	
 	
+	public static void testQuatAff() {
+		double rot1_deg = 5.0;
+		double dir1_deg = 80.0;
+		double tilt1_deg = 10.0;
+		double scale1 = 1.0;
+		double rot2_deg = 15.0;
+		double dir2_deg = 20.0;
+		double tilt2_deg = 12.0;
+		double scale2 = 1.1;
+		boolean run = true;
+		boolean invert_q2a= true;
+		boolean y_down_ccw = true;
+		System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
+		System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
+		System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
+		
+		while (run) {
+			double rot1 =  rot1_deg*Math.PI/180;
+			double dir1 =  dir1_deg*Math.PI/180;
+			double tilt1 = tilt1_deg*Math.PI/180;
+			double rot2 =  rot2_deg*Math.PI/180;
+			double dir2 =  dir2_deg*Math.PI/180;
+			double tilt2 = tilt2_deg*Math.PI/180;
+			System.out.println("-----------------------------------");
+			System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
+			System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
+			System.out.println("rot1="+rot1+", dir1="+dir1+", tilt1="+tilt1+", scale1="+scale1);
+			System.out.println("rot2="+rot2+", dir2="+dir2+", tilt2="+tilt2+", scale2="+scale2);
+			System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
+			double [] quat1 = quatRotDirTiltScale(rot1, dir1,tilt1,scale1);
+			double [] quat2 = quatRotDirTiltScale(rot2, dir2,tilt2,scale2);
+//	        System.out.println("quats01[2]=  "+QuatUtils.toString(quats01[2],use_degrees));
+			testQuatAff(quat1, quat2, invert_q2a, y_down_ccw);
+			System.out.println("run="+run);
+		}
+	}
+	public static void testQuatAff(
+			double [] quat1,
+			double [] quat2,
+			boolean invert_q2a,
+			boolean y_down_ccw) {
+		boolean use_degrees= true;
+		double [] quat_diff = divideScaled(quat2, quat1);
+        System.out.println("quat1=         "+QuatUtils.toString(quat1,use_degrees));
+        System.out.println("quat2=         "+QuatUtils.toString(quat2,use_degrees));
+        System.out.println("quat_diff=     "+QuatUtils.toString(quat_diff,use_degrees));
+        double [][] quats= {quat1,quat2,quat_diff};
+        double [][][] affines = new double [quats.length][][];
+        SingularValueDecomposition [] svds= new SingularValueDecomposition [quats.length];
+        /*
+        for (int i = 0; i < quats.length; i++){
+        	affines[i] = quatToAffine(quats[i],	    invert_q2a,y_down_ccw);
+        	String name = (i==2)? "_diff" : "["+i+"]  ";
+        	System.out.println(affinesToString(affines[i], "affines"+name));
+        	svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
+	        System.out.println("svd_affines"+name+" = "+svds[i].toString(use_degrees));
+        }
+        */
+        for (int i = 0; i < 2; i++){
+        	affines[i] = quatToAffine(quats[i],	    invert_q2a,y_down_ccw);
+        	System.out.println(affinesToString(affines[i], "affines["+i+"]  "));
+        	svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
+	        System.out.println("svd_affines["+i+"] =   "+svds[i].toString(use_degrees));
+        }
+        // get differential affines
+        affines[2] = matMult2x2 (affines[1], matInverse2x2(affines[0]));
+    	System.out.println(affinesToString(affines[2], "affines_diff"));
+    	svds[2] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw);
+        System.out.println("svd_affines_diff = "+svds[2].toString(use_degrees));
+        
+        double [][][] quats_pm = new double [2][][];
+        double [][][] quats_diff_pm = new double [2][2][];
+        for (int i = 0; i <2; i++) {
+        	quats_pm[i] = affineToQuatScaled(affines[i], false, y_down_ccw);
+        	quats_diff_pm[i][0]= divideScaled(quats_pm[i][0], quats[i]);
+        	quats_diff_pm[i][1]= divideScaled(quats_pm[i][1], quats[i]);
+        }
+        
+        System.out.println("quat1=         "+QuatUtils.toString(quat1,use_degrees));
+        System.out.println("quats_pm[0][0]="+QuatUtils.toString(quats_pm[0][0],use_degrees));
+        System.out.println("quats_pm[0][1]="+QuatUtils.toString(quats_pm[0][1],use_degrees));
+        System.out.println("qdiff_pm[0][0]="+QuatUtils.toString(quats_diff_pm[0][0],use_degrees));
+        System.out.println("qdiff_pm[0][1]="+QuatUtils.toString(quats_diff_pm[0][1],use_degrees));
+        System.out.println();
+        System.out.println("quat2=         "+QuatUtils.toString(quat2,use_degrees));
+        System.out.println("quats_pm[1][0]="+QuatUtils.toString(quats_pm[1][0],use_degrees));
+        System.out.println("quats_pm[1][1]="+QuatUtils.toString(quats_pm[1][1],use_degrees));
+        System.out.println("qdiff_pm[1][0]="+QuatUtils.toString(quats_diff_pm[1][0],use_degrees));
+        System.out.println("qdiff_pm[1][1]="+QuatUtils.toString(quats_diff_pm[1][1],use_degrees));
+        System.out.println();
+        double [][] qvariants = {
+        		QuatUtils.divideScaled(quats_pm[1][0], quats_pm[0][0]),
+        		QuatUtils.divideScaled(quats_pm[1][1], quats_pm[0][0]),
+        		QuatUtils.divideScaled(quats_pm[1][0], quats_pm[0][1]),
+        		QuatUtils.divideScaled(quats_pm[1][1], quats_pm[0][1])};
+        System.out.println("quat_diff=     "+QuatUtils.toString(quat_diff,use_degrees));
+        double [][] qvariants_diff = new double [qvariants.length][2];
+        for (int i = 0; i < qvariants_diff.length; i++)	{
+        	qvariants_diff[i] = QuatUtils.divideScaled(qvariants[i], quat_diff);
+        }
+        
+        for (int i = 0; i < qvariants.length; i++)	{
+            System.out.println("quat_var["+i+"]=   "+QuatUtils.toString(qvariants[i],use_degrees));
+        }
+        System.out.println();
+        for (int i = 0; i < qvariants.length; i++)	{
+            System.out.println("qvar_diffs["+i+"]= "+QuatUtils.toString(qvariants_diff[i],use_degrees));
+        }
+        return;
+	}
+	
+	public static String affinesToString(
+			double [][] a,
+			String      name) {
+		int flen = name.length();
+		String fmt1 = String.format("%%%ds = [[",flen);
+		String fmt2 = String.format("%%%ds    [",flen);
+		String s = "";
+		for (int i = 0; i < a.length; i++) {
+			if (i==0) s+= String.format(fmt1, name);
+			else      s+= String.format(fmt2, "");
+			for (int j=0; j < a[i].length; j++) {
+				s += String.format("%12.9f", a[i][j]);
+				if (j < (a[i].length-1)) {
+					s+= ", ";
+				} else {
+					if (i < (a.length-1)) {
+						s += "],\n";
+					} else {
+						s += "]]";
+					}
+				}
+			}
+		}
+		return s;
+	}
+	
+	public static double [] quatRotDirTiltScale(
+			double rot,
+			double dir,
+			double tilt,
+			double scale) {
+		double chalfrot = Math.cos(rot/2); 
+		double shalfrot = Math.sin(rot/2); 
+		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
+		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
+		double beta = dir;;  
+		double cmbeta = Math.cos(-beta);
+		double smbeta = Math.sin(-beta);
+		double tiltAngle = tilt; // >0
+		double ctilt = Math.cos(tiltAngle/2);
+		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 [] qs = scale(multiply(qrot,q_plus), scale);
+		return qs;
+
+	}
 	
 	
 	
@@ -263,6 +401,52 @@ public final class QuatUtils { // static class
 		return affine;
 	}
 	
+// external invert
+	public static double [][] affineToQuatScaled (
+			double [][] affine,
+			boolean     y_down_ccw) {
+		boolean invert = true;
+		if (invert) {
+			affine = matInverse2x2(affine);
+		} else {
+			affine = affine.clone(); // isolate from input
+		}
+//		double [] eigen_vals= SingularValueDecomposition.getMinMaxEigenValues(affine);
+//		double scale =  eigen_vals[1];
+
+		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
+				affine,
+				y_down_ccw);
+		double y_sign = y_down_ccw? -1 : 1;
+		double scale = svd.getMaxScale();
+		matScale(affine, 1/scale);
+		double rot = svd.getRotAngle(); // rot sign should correspond Y-up (quaterions)
+		double chalfrot = Math.cos(rot/2); 
+		double shalfrot = Math.sin(rot/2); 
+		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
+		
+		double [][] invRotMatrix = SingularValueDecomposition.rotMatrix(-y_sign*rot); // undo rotation from affine
+		affine = matMult2x2(affine, invRotMatrix); // check symmetrical diagonal
+		
+		// undo rotation
+		
+		// Now beta,rot correspond to Y - up
+		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
+		boolean stretch = false;
+		double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
+		double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
+		double tiltAngle = svd.getTiltAngle(); // >0
+		double ctilt = Math.cos(tiltAngle/2);
+		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(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
+		return quats_pm;
+	}
+	
+	
+	
 	public static double [][] quatToAffine(
 			double [] quat,
 			boolean   invert,
@@ -325,26 +509,50 @@ q22 = q[1]*q[1]
 q33 = q[1]*q[1]
 q03 = q[0]*q[3]
 q12 = q[1]*q[2]
+
+
+q00 + q11 - q22 - q33 = a00
+q12 - q03 =             a01/2
+q03 + q12 =             a10/2
+q00 - q11 + q22 - q33 = a11
+
+q00 - q33 =            (a00+a11)/2
+q11 - q22 =            (a00-a11)/2
+q12 - q03 =             a01/2
+q03 + q12 =             a10/2
+
+q12 =       (a10+a01)/2
+q03 =       (a10-a01)/2
+q00 - q33 = (a00+a11)/2
+q11 - q22 = (a00-a11)/2
+
+q12 =       c0
+q03 =       c1
+q00 - q33 = c2
+q11 - q22 = c3
+
 		 */
+		quat = quat.clone(); // not to modify original
 		double scale = normQuat(quat);
-		double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
+		double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
+//		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 q22 = quat[2]*quat[2];
+		double q33 = quat[3]*quat[3];
 		double q03 = quat[0]*quat[3];
-		double q12 = quat[1]*quat[2];
+		double q12 = quat[1]*quat[2]*y_sign;
 		
 		double [][] affine = {
 				{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
-				{scale_y*2*(q03 + q12),         scale_y*(q00 - q11 + q22 - q33)}};
+				{scale*2*(q03 + q12),           scale*(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
+	 * Restore quaternion (rotation+scale) from affine transform (only [2][2] is used, ok to have [2][3] input
 	 * As there are 2 solutions, both are provided in the output. As the
 	 * input linear transformation matrix converts ground coordinates to source
 	 * image coordinates, the scale in the tilt direction is > than scale in the
@@ -357,9 +565,15 @@ q12 = q[1]*q[2]
 			double [][] affine,
 			boolean     stretch,
 			boolean     y_down_ccw) {
+		affine = matInverse2x2(affine);
+		
+		// TODO: why have to use gamma for direction?
 		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
 				affine,
 				y_down_ccw);
+//		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(
+//				affine,
+//				y_down_ccw);
 //		double scale = stretch? svd.getMinScale(): (1/svd.getMinScale());
 		double scale = stretch? svd.getMinScale(): svd.getMaxScale();
 		double rot = svd.getRotAngle(); // TODO: check sign !
@@ -368,8 +582,11 @@ q12 = q[1]*q[2]
 		double shalfrot = Math.sin(rot/2); 
 		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
 		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
-		double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
-		double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
+//		double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
+//		double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
+		double mbeta= svd.gamma; // -svd.beta
+		double cmbeta = stretch ? Math.cos(mbeta) : (Math.sin(mbeta));
+		double smbeta = stretch ? Math.sin(mbeta) : (-Math.cos(mbeta)); // TODO: check sign !
 		double tiltAngle = svd.getTiltAngle(); // >0
 		double ctilt = Math.cos(tiltAngle/2);
 		double stilt = Math.sin(tiltAngle/2);
@@ -397,6 +614,19 @@ q12 = q[1]*q[2]
 		}
 		return m;
 	}
+	public static double [][] matMult2x2 (
+			double [][] m1,
+			double [][] m2){
+		double [][] m = new double[m1.length][m2[0].length];
+		for (int i = 0; i < 2; i++) {
+			for (int j = 0; j < 2; j++) {
+				for (int k = 0; k < 2; k++) {
+					m[i][j] += m1[i][k]*m2[k][j];
+				}
+			}
+		}
+		return m;
+	}
 	
 	public static double [] matMult (
 			double [][] m1,
@@ -413,6 +643,17 @@ q12 = q[1]*q[2]
 		}
 		return v2;
 	}
+	
+	public static void matScale (
+			double [][] a,
+			double      scale){
+		for (int i = 0; i < 2; i++) {
+			for (int j=0; j < 2; j++) {
+				a[i][j] *= scale;
+			}
+		}
+	}
+	
 	public static double [][] matInverse2x2(
 			double [][] a) {
 //			throw new IllegalArgumentException("Only [2][2] arrays");

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

@@ -90,6 +90,21 @@ public class SingularValueDecomposition {
 		svd.ratio = svd.w2/svd.w1;
 		return svd;
 	}
+	
+	public static double [] getMinMaxEigenValues(double [][] A) {
+		double a00=A[0][0],a01=A[0][1],a10=A[1][0],a11=A[1][1];
+		double b00=(a00+a11)/2; // , b11 = b00;
+		double c00=(a00-a11)/2; //, c11 =-c00;
+		double b01=(a01-a10)/2; //, b10 =-b01;
+		double c01=(a01+a10)/2; //, c10 = c01;
+		double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
+		double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
+		double w1 = w1_p_w2_2 + w1_m_w2_2;
+		double w2 = w1_p_w2_2 - w1_m_w2_2;
+		return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});
+
+	}
+	
 	/**
 	 * Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
 	 * into overall scaling caused by zoom != 1.0 because of altitude error
@@ -153,7 +168,7 @@ public class SingularValueDecomposition {
 		while (beta >= Math.PI/2) {
 			beta -= Math.PI;
 		}
-		while (beta < Math.PI/2) {
+		while (beta < -Math.PI/2) {
 			beta += Math.PI;
 		}