quaternions-affines

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

@@ -57,11 +57,18 @@ public class OrthoAltitudeMatch {
 			int                 debugLevel) {
         boolean dbg_planes = false;
 		boolean y_down_ccw = true;
-		boolean invert_q2a = true;
+		boolean invert_q2a = false;
 		boolean test_quat =  false;
 		boolean test_quat0 = false;
 		boolean test_quat2=  true;
         boolean use_degrees = true;
+        boolean debug_tilts = true;
+        
+		boolean invert_order= false; // print them
+		boolean invert_y =    true; // only for tilts->affines
+        boolean show_details =false;
+        
+        boolean do_not_save = true;
 
 		if (test_quat0) {
 			QuatUtils.testQuatAff();
@@ -212,12 +219,33 @@ public class OrthoAltitudeMatch {
     		}
 	        double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
 			if (test_quat2) {
-	        	System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
+	        	System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]+
+	        			", invert_q2a="+invert_q2a+", invert_order="+invert_order+", invert_y="+invert_y);
 		        boolean [][] masks = new boolean[2][];
 		        double [][] alt_data5s = new double[2][];
 	        	double [][] data_overlap = new double[2][];
 	        	double [][] alt_datas = new double[3][];
 		        double [][] affine_pair = pairwiseOrthoMatch.getAffine();
+		        boolean remove_rs = false;
+		        if (remove_rs) {
+		        	affine_pair= SingularValueDecomposition.removeTiltRotScale(
+		        			affine_pair, // double [][] A,
+		        			false, // boolean removeTilt,
+		        			true,  // boolean removeRot,
+		        			true,  // boolean removeScale,
+		        			false, // boolean removeOffset,
+		        			false); // boolean max_is_scale);
+		        } else {
+		        	affine_pair= new double [][] {affine_pair[0].clone(),affine_pair[1].clone()};  
+		        }
+		        double [][] affine_pair_nors = SingularValueDecomposition.removeTiltRotScale(
+		        		affine_pair, // double [][] A,
+		    			false, // boolean removeTilt,
+		    			true,  // boolean removeRot,
+		    			true,  // boolean removeScale,
+		    			false, // boolean removeOffset,
+		    			false); // boolean max_is_scale);
+		        
 		        
 		        // calculate second from first and pair
 		        double [][] affine0 = ortho_maps[ipair[0]].getAffine();
@@ -268,10 +296,27 @@ public class OrthoAltitudeMatch {
 		    		}
 		    		alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
 		        }
+		        
+//		        alt_datas[0] = alt_datas[0].clone();
+		        double [] tilts0_mod =  QuatUtils.manualFitTilt(
+		        		alt_datas[0], // double [] tilts0,
+		        		alt_data,     // double [] tilts_diff,
+		        		affine_pair_nors, // double [][] affine_pair_nors,
+		        		invert_q2a, // boolean invert_q2a) // invert result affines (to match "usual")
+//	        			debug_tilts,
+	        			invert_order, // boolean invert_order,
+	        			invert_y); // boolean invert_y)
+		        if (!show_details) {
+		        	continue;
+		        }
+		        
+		        alt_datas[0]=tilts0_mod.clone();
+		        
 		        alt_datas[2] = new double [3];
 		        for (int i = 0; i < alt_datas[2].length; i++) {
 		        	alt_datas[2][i] = alt_datas[0][i]+alt_data[i]; // simulated alt_datas[1] 
 		        }
+		        
 	        	System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair  "));
 	        	System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
 		        System.out.println();
@@ -303,9 +348,9 @@ public class OrthoAltitudeMatch {
 		        for (int ns = 0; ns < quat_scenes.length; ns++) {
 		        	quat_scenes[ns] = QuatUtils.sceneRelLocalGround(
 		        			alt_datas[ns], // double []   txy,
-		        			affines[ns], // double [][] affine,
+		        			remove_rs? null:affines[ns], // double [][] affine,
 		        			y_down_ccw); // boolean y_down_ccw)
-			        System.out.println("quat_scenes["+ns+"]="+QuatUtils.toString(quat_scenes[ns],use_degrees));
+			        System.out.println("quat_scenes["+ns+"]=    "+QuatUtils.toString(quat_scenes[ns],use_degrees));
 			        // 0, 1 and simulated 2?  
 		        }
 		        double [] quat1a = QuatUtils.multiplyScaled(quat_scenes[0], quat_diff_scenes);
@@ -322,9 +367,35 @@ public class OrthoAltitudeMatch {
 			        System.out.println();
 		        	
 		        }
+		        System.out.println("================== compare diff affines w/o rot and scale");
+		        SingularValueDecomposition svd_affine_pair_nors =
+		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair_nors, y_down_ccw);
+	        	System.out.println(QuatUtils.affinesToString(affine_pair_nors, "affine_pair_nors"));
+	        	System.out.println("svd_affine_pair_nors= " + svd_affine_pair_nors.toString(use_degrees));
+		        System.out.println();
+	        	double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(
+	        			alt_datas[0], // double [] tilts0,
+	        			alt_data,     // double [] tilts_diff,
+	        			invert_q2a, // boolean invert_q2a) // false
+	        			debug_tilts,
+	        			invert_order, // boolean invert_order,
+	        			invert_y); // boolean invert_y)
+		        SingularValueDecomposition svd_affine_tilt_diff =
+		        		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_tilt_diff, y_down_ccw);
+	        	System.out.println(QuatUtils.affinesToString(affine_tilt_diff, "affine_tilt_diff"));
+	        	System.out.println("svd_affine_tilt_diff= " + svd_affine_tilt_diff.toString(use_degrees));
+		        System.out.println();
+
+		        double [][] adq_err =  QuatUtils.matMult2x2(affine_tilt_diff, QuatUtils.matInverse2x2(affine_pair_nors));
+		        SingularValueDecomposition svd_adq_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq_err, y_down_ccw);
+	        	System.out.println(QuatUtils.affinesToString(adq_err, "adq_err"));
+		        System.out.println("svd_adq_err=   " + svd_adq_err.toString(use_degrees));
+		        System.out.println();
+		        
+		        System.out.println("+++++++++++++ Relative affine from quat2 to quat0 (from tilts, should match affine_pair)");
+		        
 	        	System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair  "));
 	        	System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
-		        System.out.println("+++++++++++++ Relative affine from quat2 to quat0 (from tilts, should match affine_pair)");
 
 		        double [][] adq20 =  QuatUtils.matMult2x2(aff_qscenes[2], QuatUtils.matInverse2x2(aff_qscenes[0]));
 		        SingularValueDecomposition svd_adq20=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20, y_down_ccw);
@@ -332,6 +403,12 @@ public class OrthoAltitudeMatch {
 		        System.out.println("svd_adq20=   " + svd_adq20.toString(use_degrees));
 		        System.out.println();
 
+		        double [][] adq20_err =  QuatUtils.matMult2x2(adq20, QuatUtils.matInverse2x2(affine_pair));
+		        SingularValueDecomposition svd_adq20_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20_err, y_down_ccw);
+	        	System.out.println(QuatUtils.affinesToString(adq20_err, "adq20_err"));
+		        System.out.println("svd_adq20_err=    " + svd_adq20_err.toString(use_degrees));
+		        System.out.println();
+
 //		        double [][] adq20i =  QuatUtils.matMult2x2(QuatUtils.matInverse2x2(aff_qscenes[0]),aff_qscenes[2]);
 		        double [][] adq20i =  QuatUtils.matInverse2x2(adq20);
 		        SingularValueDecomposition svd_adq20i=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20i, y_down_ccw);
@@ -339,11 +416,6 @@ public class OrthoAltitudeMatch {
 		        System.out.println("svd_adq20i=  " + svd_adq20i.toString(use_degrees));
 		        System.out.println();
 		        
-		        double [][] adq20_diff =  QuatUtils.matMult2x2(adq20, QuatUtils.matInverse2x2(affine_pair));
-		        SingularValueDecomposition svd_adq20_diff=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20_diff, y_down_ccw);
-	        	System.out.println(QuatUtils.affinesToString(adq20_diff, "adq20_diff"));
-		        System.out.println("svd_adq20_diff=   " + svd_adq20_diff.toString(use_degrees));
-		        System.out.println();
 		        
 		        /*
 		        double [][] iadq20 = QuatUtils.matInverse2x2(adq20);
@@ -734,7 +806,9 @@ public class OrthoAltitudeMatch {
 	        
 //	        double [] alt_data = new double[3];
 //	        System.arraycopy(alt_data5, 0, alt_data, 0, alt_data.length);
+	        if (!do_not_save) {
 	        	pairwiseOrthoMatch.setAltData(alt_data);
+	        }
 	        
 			if (log_append && (log_path != null)) { // assuming directory exists
 				StringBuffer sb = new StringBuffer();

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

@@ -4,7 +4,7 @@ public class SingularValueDecomposition {
 	double beta;
 	double gamma;
 	double w1;
-	double w2;
+	double w2; // < w1 after singularValueDecompose
 	double rot;
 	double scale;  // min (w1,w2) or sqrt(w1*w2) for raw singularValueDecompose()
 	double ratio;  // <=1.0, ==cos(tilt) or w2/w1 for raw singularValueDecompose()
@@ -90,7 +90,7 @@ public class SingularValueDecomposition {
 		double beta =  (g_p_b - g_m_b)/2; 
 		SingularValueDecomposition svd= new SingularValueDecomposition (beta,gamma,w1,w2,g_p_b);
 		svd.scale = Math.sqrt(svd.w1*svd.w2);
-		svd.ratio = svd.w2/svd.w1;
+		svd.ratio = svd.w2/svd.w1; // <= 1.0;
 		return svd;
 	}
 
@@ -112,7 +112,7 @@ public class SingularValueDecomposition {
 		if (A[0].length < 3) {
 			return AR;
 		}
-		A=A.clone();
+		A = new double [][] {A[0].clone(),A[1].clone()};
 		if (removeOffset) {
 			for (int i = 0; i < 2; i++) {
 				A[i][2] = 0;	
@@ -162,7 +162,8 @@ public class SingularValueDecomposition {
 		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});
+		return new double[] {w2,w1}; // because w1_m_w2_2 >=0 so w1 >= w2 
+//		return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});
 
 	}
 	
@@ -209,7 +210,7 @@ public class SingularValueDecomposition {
 		// (and source image coordinates), while X should correspond to
 		// the axis of rotation, and scale is just scale caused by error in
 		// altitude.
-		scale = Math.min(w1, w2);
+		scale = Math.min(w1, w2); // w1 >= w2 after singularValueDecompose() !
 		ratio = w1/w2; // <=1.0, ==cos(tilt)
 		if (w1 > w2) { // rotate tilt by PI/2
 			ratio = w2/w1;