Commit 0abd52b8 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

fixing quaternions-affines

parent d9f79276

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

+40 −10
Original line number Diff line number Diff line
@@ -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

+269 −28
Original line number Diff line number Diff line
@@ -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

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

		}