Commit 203fad06 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

quaternions-affines

parent d3389b58

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

+86 −12
Original line number Diff line number Diff line
@@ -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,7 +348,7 @@ 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));

			        // 0, 1 and simulated 2?
@@ -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/QuatUtils.java

+227 −1
Original line number Diff line number Diff line
@@ -23,6 +23,8 @@ 


package com.elphel.imagej.orthomosaic;



import com.elphel.imagej.common.GenericJTabbedDialog;



public final class QuatUtils { // static class

	public static final double []   UNIT_QUAT =   {1,0,0,0};

	public static final double [][] UNIT_AFFINE = {{1,0},{0,1}};
@@ -209,6 +211,8 @@ public final class QuatUtils { // static class 
	public static String affinesToString(

			double [][] a,

			String      name) {

		boolean max_is_scale = false;

		boolean show_diff_unity=true;

		int flen = name.length();

		String fmt1 = String.format("%%%ds = [[",flen);

		String fmt2 = String.format("%%%ds    [",flen);
@@ -229,9 +233,40 @@ public final class QuatUtils { // static class 
				}

			}

		}

		if (show_diff_unity) {

			double [][] a_noS = SingularValueDecomposition.removeTiltRotScale(

					a,             // double [][] A,

					false,         // boolean removeTilt,

					false,         // boolean removeRot,

					true,          // boolean removeScale,

					false,         // boolean removeOffset,

					max_is_scale); // boolean max_is_scale);

			double [][] a_noRS = SingularValueDecomposition.removeTiltRotScale(

					a,             // double [][] A,

					false,         // boolean removeTilt,

					true,          // boolean removeRot,

					true,          // boolean removeScale,

					false,         // boolean removeOffset,

					max_is_scale); // boolean max_is_scale);



			double mag =      diffUnity(a);



			double mag_noS =  diffUnity(a_noS);

			double mag_noRS = diffUnity(a_noRS);

			s+=String.format(", magnitude=%12.9f, mag_no_scale=%12.9f mag_tilts=%12.9f", mag, mag_noS, mag_noRS);

		}

		

		

		return s;

	}

	

	public static double diffUnity(

			double [][] a) {

		return Math.sqrt((a[0][0]-1)*(a[0][0]-1) + (a[1][1]-1)*(a[1][1]-1) + a[0][1]*a[0][1] + a[1][0]*a[1][0]);

	}

	

	

	

	public static double [] quatRotDirTiltScale(

			double rot,

			double dir,
@@ -369,11 +404,202 @@ public final class QuatUtils { // static class 
		double shalfrot = Math.sin(rot/2); 

		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis

		double [] qinv_tilts = invert(qtilts); //scene to ground

		double [] qrt=  multiply(qrot,qinv_tilts);

		double [] qrt=  multiply(qrot,qinv_tilts); // just for debugging

//		double [] quat= scale(multiply(qrot,qinv_tilts),scale);

		double [] quat= scale(multiply(qinv_tilts,qrot),scale);

		return    quat;

	}	

	/**

	 * Calculate relative affine transform from scene0 to scene1

	 * from tilts of the first scene and differential tilt from 

	 * scene0 to scene1 (tilt1 - tilt0). Individual scene tilt

	 * measurement depends on finding ground planes that may be 

	 * inacurate due to non-flat surfaces, so differential tilt

	 * is much better. Tilts are calculated after scene rotations

	 * are corrected, so the result affine does not include rotation

	 * and scale (they should be removed from the differential

	 * affine when comparing, This method is intended for fitting

	 * tilts0 (first manually, then with LMA) to get the best fit

	 * between the differential tilt and differential affine

	 * transform and so resolving ambiguity of the sign of affine

	 * tilt. 

	 * @param tilts0 first scene tilts (tiltX, tiltY, optional offset)

	 * @param tilts_diff differential tilt (scene1 - scene0)

	 * @param invert_q2a invert affines from quternions to match "usual" ones

	 * @return differential affine transform (no scale and rotation)

	 */

	public static double[][] diffAffineFromTilts(

			double [] tilts0,

			double [] tilts_diff,

			boolean invert_q2a,  // invert result affines (to match "usual")

			boolean debug){

		return diffAffineFromTilts(

				tilts0,

				tilts_diff,

				invert_q2a,  // invert result affines (to match "usual")

				debug,

				false, // boolean invert_order,

				false); // boolean invert_y)

	}

	public static double[][] diffAffineFromTilts(

			double [] tilts0,

			double [] tilts_diff,

			boolean invert_q2a,  // invert result affines (to match "usual")

			boolean debug,

			boolean invert_order,

			boolean invert_y) {

		boolean	 y_down_ccw = true;

		

		double [] tilts1 = tilts0.clone();

		for (int i = 0; i < tilts_diff.length;i++) {

			tilts1[i] += tilts_diff[i];

		}

		double [][] tilts = {tilts0, tilts1};

		double [][][] affines= new double [2][][];

		double [] aff_tilts = new double[2];

		for (int nscene = 0; nscene < 2; nscene++) {

			double [] quat = sceneRelLocalGround(

	    			tilts[nscene], // double []   txy,

	    			null, // double [][] affine,

	    			y_down_ccw); // boolean y_down_ccw)

			affines[nscene] = QuatUtils.quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);

			if (debug) {

				double [] w_min_max = SingularValueDecomposition.getMinMaxEigenValues(affines[nscene]);

				aff_tilts[nscene] = w_min_max[1]/w_min_max[0]-1.0; // >=0.0

			}

		}

//		double [][] iaffines0 = matInverse2x2(affines[0]);

		double [][] affine_diff =  matMult2x2(affines[1], matInverse2x2(affines[0]));

		if (invert_order) {

			 affine_diff =         matMult2x2(matInverse2x2(affines[0]), affines[1]);

		}

		if (debug) {		

			double [] w_min_max_diff = SingularValueDecomposition.getMinMaxEigenValues(affine_diff);

			double aff_tilt_diff = w_min_max_diff[1]/w_min_max_diff[0] - 1.0; // >=0.0

			System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0], aff_tilts[1], aff_tilt_diff));

		}

		/*

		SingularValueDecomposition[] svds = new SingularValueDecomposition[2];

		for (int nscene = 0; nscene < 2; nscene++) {

			svds[nscene] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[nscene], y_down_ccw ^ invert_y);

		}

		SingularValueDecomposition svds_diff =SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_diff, y_down_ccw);

		 */

		return affine_diff;

	}	

	

	

	public static double [] manualFitTilt(

			double [] tilts0,

			double [] tilts_diff,

			double [][] affine_pair_nors,

			boolean invert_q2a, // invert result affines (to match "usual")

			boolean invert_order,

			boolean invert_y) {

		boolean	 y_down_ccw = true;

		boolean use_degrees = true;

		boolean debug_tilts = true;

		double [] tilt0_delta = new double[3]; // [2] - not used

		double [] tilts0_mod = tilts0.clone();

		double [] prev_tilts=new double [3];

		double [] prev2_tilts=new double [3];

		double last_err = Double.NaN;

		double prev_err = Double.NaN;

		double prev2_err = Double.NaN;

		double err_scale = 1e6;

		while(true) {

			double last_tilt=Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);

			double prevt_tilt=Math.sqrt(prev_tilts[0]*prev_tilts[0]+prev_tilts[1]*prev_tilts[1]);

			GenericJTabbedDialog gd = new GenericJTabbedDialog("Select tilt correction, TILT="+

					String.format("TILT=%.3f%% (%.3f%%)",100*last_tilt, 100*prevt_tilt),450,150);

			

			gd.addNumericField(String.format("tiltX (%8.5f)",tilts0[0]),  tilt0_delta[0],  6,10, String.format("was %f, %f", prev_tilts[0],prev2_tilts[0]),

					"Set correction to tilts0[0]");

			gd.addNumericField(String.format("tiltY (%8.5f)",tilts0[1]),  tilt0_delta[1],  6,10, String.format("was %f, %f", prev_tilts[1],prev2_tilts[1]),

					"Set correction to tilts0[1]");

			gd.addMessage(String.format("Last error\u00D7%.0e = %.4f (previous was %.4f, %4f)", err_scale, err_scale*last_err, err_scale*prev_err, err_scale*prev2_err));

			gd.showDialog();

			if (gd.wasCanceled()) break;

	        prev2_tilts = prev_tilts.clone();			

	        prev_tilts = tilt0_delta.clone();			

			tilt0_delta[0] = gd.getNextNumber();

			tilt0_delta[1] = gd.getNextNumber();

			double [] tilts1_mod = tilts0_mod.clone();



			for (int i = 0; i < 2; i++) {

				tilts0_mod[i] = tilts0[i]+tilt0_delta[i];

				tilts1_mod[i] += tilts_diff[i];



//				System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",

//						i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));

			}

			double tilt0_delta_len = Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);

			double tilt0_len = Math.sqrt(tilts0_mod[0]*tilts0_mod[0]+tilts0_mod[1]*tilts0_mod[1]);

			double tilt1_len = Math.sqrt(tilts1_mod[0]*tilts1_mod[0]+tilts1_mod[1]*tilts1_mod[1]);

			double tilts_diff_len = Math.sqrt(tilts_diff[0]*tilts_diff[0]+tilts_diff[1]*tilts_diff[1]);

			double tilt01_dot = tilts0_mod[0]*tilts1_mod[0]+tilts0_mod[1]*tilts1_mod[1];

			double tilt01_acos = Math.acos(tilt01_dot/tilt0_len/tilt1_len)*180/Math.PI; 



//			double 

			System.out.println("---------------------------------------: "+

					String.format("TILT_CORR=%.3f%% (TILT0=%.3f%%, TILT1=%.3f%%, TILTS_COS=%.1f\u00B0, TILT_DIFF=%.3f%%)",

							100*tilt0_delta_len,

							100*tilt0_len,

							100*tilt1_len,

							tilt01_acos,

							100*tilts_diff_len));

			for (int i = 0; i < 2; i++) {

				System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",

						i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));

			}



        	double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(

        			tilts0_mod, // double [] tilts0,

        			tilts_diff,     // 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);

	        //affine_pair_nors

	        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();

	        

        	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));

			double [] w_min_max_err = SingularValueDecomposition.getMinMaxEigenValues(adq_err);

			double aff_tilt_err = w_min_max_err[1]/w_min_max_err[0]-1.0; // >=0.0

//			System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0]-1.0, aff_tilts[1]-1.0, aff_tilt_diff-1.0));



	        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));

	        /*

	        double err = Math.sqrt(

	        		(adq_err[0][0]-1.0)*(adq_err[0][0]-1.0)+

	        		(adq_err[1][1]-1.0)*(adq_err[1][1]-1.0) +

	        		adq_err[0][1]*adq_err[0][1]+adq_err[1][0]*adq_err[1][0]);

	        System.out.println(String.format("err=%12.10f",err));

	        */

	        System.out.println(String.format("AFF_TILT_ERR\u00D7%.0e=%.4f (previous were %.4f, %.4f)",

	        		err_scale, err_scale*aff_tilt_err, err_scale*last_err, err_scale*prev_err));

	        System.out.println();

	        prev2_err=prev_err;

	        prev_err=last_err;

	        last_err=aff_tilt_err;

		}

		return tilts0_mod; // tilt0_delta;

	}	

	

	

	

	

	/**

	 * Remove rotation around Z-axis, keep only tilt around axis in XY plane

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

+6 −5
Original line number Diff line number Diff line
@@ -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;