Commit ace24186 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

fixing, but so far scale > 1 for stretch, while affines are image coord 

from map coord
parent 6a6d0c40

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

+22 −7
Original line number Original line Diff line number Diff line
@@ -314,6 +314,11 @@ public class OrthoAltitudeMatch { 
		    			true,             //   boolean   stretch,

		    			true,             //   boolean   stretch,

		    			make__pure_tilt, // boolean   make__pure_tilt)

		    			make__pure_tilt, // boolean   make__pure_tilt)

		    			y_down_ccw); // boolean y_down_ccw);

		    			y_down_ccw); // boolean y_down_ccw);

		        double [][] aff1_shrink = QuatUtils.quatToAffine(

		        		quats01[0],       // double [] quat,

		    			false,            //   boolean   stretch,

		    			make__pure_tilt, // boolean   make__pure_tilt)

		    			y_down_ccw); // boolean y_down_ccw);

		        double [][] aff2_shrink = QuatUtils.quatToAffine(

		        double [][] aff2_shrink = QuatUtils.quatToAffine(

		        		quats01[2],       // double [] quat,

		        		quats01[2],       // double [] quat,

		    			false,             //   boolean   stretch,

		    			false,             //   boolean   stretch,
@@ -325,27 +330,37 @@ public class OrthoAltitudeMatch { 
		    			make__pure_tilt, // boolean   make__pure_tilt)

		    			make__pure_tilt, // boolean   make__pure_tilt)

		    			y_down_ccw); // boolean y_down_ccw);

		    			y_down_ccw); // boolean y_down_ccw);

		        

		        

		        double [][] qaffd_pm =    QuatUtils.affineToQuatScaled(affine_pair, 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

		        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[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[1]=   ["+qaffd_pm[1][0]+ ","+qaffd_pm[1][1]+ ","+qaffd_pm[1][2]+ ","+qaffd_pm[1][3]+ "] scale="+sqaffd_pm[1]);

		        double [][] qaff1_pm =    QuatUtils.affineToQuatScaled(aff1_stretch, y_down_ccw);

		        

		        double []   sqaff1_pm =   {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaff1_pm[1])}; // normalizes

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

		        

		        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[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[1]=   ["+qaff1_pm[1][0]+ ","+qaff1_pm[1][1]+ ","+qaff1_pm[1][2]+ ","+qaff1_pm[1][3]+ "] scale="+sqaff1_pm[1]);

		        double [][] qaff2a_pm =   QuatUtils.affineToQuatScaled(aff2_shrink,  y_down_ccw);

		        

		        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

		        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[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[1]=  ["+qaff2a_pm[1][0]+","+qaff2a_pm[1][1]+","+qaff2a_pm[1][2]+","+qaff2a_pm[1][3]+"] scale="+sqaff2a_pm[1]);

		        double [][] qaff2_pm =    QuatUtils.affineToQuatScaled(aff2_stretch, y_down_ccw);

		        

		        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

		        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[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[1]=   ["+qaff2_pm[1][0]+ ","+qaff2_pm[1][1]+ ","+qaff2_pm[1][2]+ ","+qaff2_pm[1][3]+ "] scale="+sqaff2_pm[1]);

		        double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], y_down_ccw);

		        

		        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

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

		        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 [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1],  y_down_ccw);

		        

		        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

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

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

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

+8 −5
Original line number Original line Diff line number Diff line
@@ -211,23 +211,26 @@ public final class QuatUtils { // static class 
	 */

	 */

	public static double [][] affineToQuatScaled (

	public static double [][] affineToQuatScaled (

			double [][] affine,

			double [][] affine,

			boolean     stretch,

			boolean     y_down_ccw) {

			boolean     y_down_ccw) {

		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(

		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(

				affine,

				affine,

				y_down_ccw);

				y_down_ccw);

		double scale = svd.getMinScale();

//		double scale = stretch? svd.getMinScale(): (1/svd.getMinScale());

		double scale = stretch? svd.getMinScale(): svd.getMaxScale();

		double rot = svd.getRotAngle(); // TODO: check sign !

		double rot = svd.getRotAngle(); // TODO: check sign !

		// Now beta,rot correspond to Y - up

		// Now beta,rot correspond to Y - up

		double chalfrot = Math.cos(rot/2); 

		double chalfrot = Math.cos(rot/2); 

		double shalfrot = Math.sin(rot/2); 

		double shalfrot = Math.sin(rot/2); 

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

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

		double cbeta = Math.cos(svd.beta);

		// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)

		double sbeta = Math.sin(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 tiltAngle = svd.getTiltAngle(); // >0

		double tiltAngle = svd.getTiltAngle(); // >0

		double ctilt = Math.cos(tiltAngle/2);

		double ctilt = Math.cos(tiltAngle/2);

		double stilt = Math.sin(tiltAngle/2);

		double stilt = Math.sin(tiltAngle/2);

		double [] q_plus =  {ctilt,  stilt*cbeta,  stilt*sbeta, 0};

		double [] q_plus =  {ctilt,  stilt*cmbeta,  stilt*smbeta, 0};

		double [] q_minus = {ctilt, -stilt*cbeta, -stilt*sbeta, 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)};

		return quats_pm;

		return quats_pm;

	}

	}

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

+1 −1
Original line number Original line Diff line number Diff line
@@ -186,7 +186,7 @@ public class SingularValueDecomposition { 
		while (gamma >= Math.PI/2) {

		while (gamma >= Math.PI/2) {

			gamma -= Math.PI;

			gamma -= Math.PI;

		}

		}

		while (gamma < Math.PI/2) {

		while (gamma < -Math.PI/2) {

			gamma += Math.PI;

			gamma += Math.PI;

		}

		}