Commit 1d564879 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

working snapshot

parent 7de7fe06

src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java

+1 −1
Original line number Diff line number Diff line
@@ -5437,7 +5437,7 @@ public class OpticalFlow { 
	    	}

	    	double [] rms = new double[5];

	    	double [] quat = new double[4];

	    	int quat_lma_mode = 2; // 1;

	    	int quat_lma_mode = 3; //  2; // 1;

	    	int debug_lev = 3;

	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		

	    	double  translation_weight =   1.0 / (avg_z + 1.0);

src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java

+3 −1
Original line number Diff line number Diff line
@@ -274,6 +274,7 @@ public class QuadCLTCPU { 
				vect_y[nscene] = xyzatr[nscene];

			}

			quaternionLma.prepareLMA(

					quat_lma_mode,      // int           mode,

					avg_height,         // double        avg_height,

					vect_x,             // double [][][] vect_x,

					vect_y,             // double [][][] vect_y,
@@ -281,7 +282,7 @@ public class QuadCLTCPU { 
					translation_weight, // double        translation_weight, // 0.0 ... 1.0;

					quat0,              // double []     quat0,

					debugLevel);        // int           debug_level)

		} else if (quat_lma_mode == 1) {

		} else if ((quat_lma_mode == 1) || (quat_lma_mode == 3)) {

			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR

			double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR

			for (int nscene = early_index; nscene <= last_index; nscene++) {
@@ -289,6 +290,7 @@ public class QuadCLTCPU { 
				vect_y[nscene] = xyzatr[nscene];

			}

			quaternionLma.prepareLMA(

					quat_lma_mode,      // int           mode,

					vect_x,             // double [][][] vect_x,

					vect_y,             // double [][][] vect_y,

					null,               // double []     vect_w, all same weight

src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java

+277 −15
Original line number Diff line number Diff line
@@ -47,6 +47,7 @@ public class QuaternionLma { 
	private double []         parameters_vector = null;

	private double []         x_vector =        null;

	private double []         y_vector =        null;

	private double []         y_inv_vector =    null;

	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization

	private double            pure_weight; // weight of samples only	

	private double []         last_ymfx =       null;
@@ -106,6 +107,7 @@ public class QuaternionLma { 
	

	

	public void prepareLMA(

			int           mode,

			double [][][] vect_x, // []{{x,y,z},{a,t,r}}

			double [][][] vect_y, // []{{x,y,z},{a,t,r}}

			double []     vect_w, // one per scene
@@ -113,8 +115,20 @@ public class QuaternionLma { 
			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

			double []     quat0,

			final int     debug_level) {

		if (mode != 1) {

			prepareLMAMode3(

					mode, // int           mode,

					vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}

					vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}

					vect_w, // double []     vect_w, // one per scene

					translation_weight, // double        translation_weight, // 0.0 ... 1.0;

					reg_w,              // double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

					quat0,              // double []     quat0,

					debug_level);       // final int     debug_level);

			return;

		}

		N = vect_x.length;

		mode = 1;

		this.mode = mode;

		samples = 7;

		samples_x = 7;

		pure_weight = 1.0 - reg_w;
@@ -122,8 +136,6 @@ public class QuaternionLma { 
		y_vector = new double [samples * N + REGLEN];

		weights =  new double [samples * N + REGLEN];

		parameters_vector = quat0.clone();

		

		double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};

		double sw = 0;

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

			if ((vect_x[i]== null) || (vect_y[i]== null)) {
@@ -135,17 +147,18 @@ public class QuaternionLma { 
			} else {

				// xyz

				double w = translation_weight*((vect_w != null)?  vect_w[i] : 1.0);

				// quaternions

				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);

				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);

				for (int j = 0; j < 3; j++) {

					x_vector[samples * i + j] = vect_x[i][0][j];

					

					y_vector[samples * i + j] = vect_y[i][0][j];

					weights[samples * i +  j] = w;

					sw += w;

				}

				// quaternions

				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);

				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);

				x_vector[samples * i + 3] = rot_x.getQ0();

				x_vector[samples * i + 4] = rot_x.getQ1();

				x_vector[samples * i + 5] = rot_x.getQ2();
@@ -175,7 +188,100 @@ public class QuaternionLma { 
	}





	

	public void prepareLMAMode3(

			int           mode,

			double [][][] vect_x, // []{{x,y,z},{a,t,r}}

			double [][][] vect_y, // []{{x,y,z},{a,t,r}}

			double []     vect_w, // one per scene

			double        translation_weight, // 0.0 ... 1.0;

			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

			double []     quat0,

			final int     debug_level) {

		N = vect_x.length;

		this.mode = mode;

		samples = 7;

		samples_x = 7;

		pure_weight = 1.0 - reg_w;

		x_vector =     new double [samples *   N];

		y_vector =     new double [samples *   N + REGLEN];

		y_inv_vector = new double [samples_x * N + REGLEN]; 		

		weights =      new double [samples *   N + REGLEN];

		parameters_vector = quat0.clone();

		

		double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};

		double sw = 0;

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

			if ((vect_x[i]== null) || (vect_y[i]== null)) {

				for (int j = 0; j < samples; j++) {

					x_vector    [samples * i + j] = 0.0;

					y_inv_vector[samples * i + j] = 0.0;

					weights     [samples * i + j] = 0.0;

				}				

			} else {

				// xyz

				for (int j = 0; j < 3; j++) {

					x_vector[samples_x * i + j] = vect_x[i][0][j];

				}

				// quaternions

				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);

				x_vector[samples_x * i + 3] = rot_x.getQ0();

				x_vector[samples_x * i + 4] = rot_x.getQ1();

				x_vector[samples_x * i + 5] = rot_x.getQ2();

				x_vector[samples_x * i + 6] = rot_x.getQ3();

				

				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);

				// just testing - y should be all 0

				double [] xyz_y = new double [] {

						vect_y[i][0][0],

						vect_y[i][0][1],

						vect_y[i][0][2]}; 

				double [] quat_y = new double [] {

						rot_y.getQ0(),

						rot_y.getQ1(),

						rot_y.getQ2(),

						rot_y.getQ3()};

				double [][] inv_y = invertTransRot(

						xyz_y,   // double [] xyz_src,     // transformation to apply to (was reference_xyz)

						quat_y); // double [] quat_src);

				double [][] comb_y = combineTransRot( // just to verify it is 0; 

						inv_y[0], // double [] xyz_src,     // transformation to apply to (was reference_xyz)

						inv_y[1], // double [] quat_src,    // transformation to apply to (was reference_atr)

						xyz_y,    // double [] xyz_target,  // to which is applied (was scene_xyz)

						quat_y);  // double [] quat_target  // to which is applied (was scene_atr)		

				System.arraycopy(inv_y[0], 0, y_inv_vector, samples_x * i,      3);

				System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3,  4);

				

				double wt = translation_weight*((vect_w != null)?  vect_w[i] : 1.0);

				double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);

				for (int j = 0; j < 3; j++) {

					weights[samples * i +  j] = wt;

					sw += wt;

				}

				for (int j = 0; j < 4; j++) {

					weights[samples * i + 3 + j] = wr;

					sw += wr;

				}

			}

		}

		double k = (pure_weight)/sw;

		for (int i = 0; i < weights.length; i++) weights[i] *= k;

		weights [samples * N] = 1.0 - pure_weight;

		y_inv_vector[samples * N] = 1.0;

		last_jt = new double [parameters_vector.length][];		

		if (debug_level > 0) {

			 debugYfX ( "Y-INV-",   // String pfx,

					 y_inv_vector); // double [] data)

			 debugYfX ( "PIMU-",   // String pfx,

						x_vector); // double [] data)

		}

	}

	

	

	public void prepareLMA(

			int           mode,

			double        avg_height,         // 

			double [][][] vect_x, // []{{x,y,z},{a,t,r}}

			double [][][] vect_y, // []{{x,y,z},{a,t,r}}
@@ -184,7 +290,7 @@ public class QuaternionLma { 
			double []     quat0,

			final int     debug_level) {

		N = vect_x.length;

		mode = 2;

		this.mode = mode; // 2;

		samples = 4;

		samples_x = 7;

		height = avg_height;
@@ -560,6 +666,10 @@ public class QuaternionLma { 
				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

		case 3:return  getFxDerivs6DofMode3(

				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

		}

		double [] fx = new double [weights.length];

		final double q0 = vector[0];
@@ -599,7 +709,6 @@ public class QuaternionLma { 
		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

//		final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal

		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {
@@ -635,11 +744,9 @@ public class QuaternionLma { 
			}

			// rotations

			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};

//			quat_rot = compose(vector, quat_r);

			quat_rot = composeQR_Q(vector_r, quat_r);

			System.arraycopy(quat_rot, 0, fx, i7+3, 4);

			if (jt != null) {

//				quat_dq = composeDQ(quat_r);

				quat_dq = composeQR_QdQ(vector_r,quat_r);

				for (int j = 0; j < 4; j++) {

					System.arraycopy(quat_dq[j], 0, jt[j], i7+3, 4);
@@ -649,6 +756,92 @@ public class QuaternionLma { 
		return fx;

	}



	private double [] getFxDerivs6DofMode3(

			double []         vector,

			final double [][] jt, // should be null or initialized with [vector.length][]

			final int         debug_level) {

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});

		if (jt != null) {

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

				jt[i] = new double [weights.length];

				jt[i][samples * N] = 2 * vector[i];

			}

		}

		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;

		double [] xyz_rot;

		double [] quat_rot;

		double [][] xyz_dq;

		double [][] quat_dq;

		double [][] inv_y = new double [][] {new double[3],new double[4]}; 			

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

			int i7 = samples * i;

			has_data:{

				for (int j = 0; j < samples; j++) {

					if (weights[i7+j] > 0) {

						break has_data;

					}

				}

				continue; // nothing to process for this scene 

			}

			

			// translations

			final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};

			// rotations

			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};

			xyz_rot = applyTo(vector, xyz);

			quat_rot = composeQR_Q(vector_r, quat_r);

			System.arraycopy(y_inv_vector, i7,   inv_y[0], 0, 3);

			System.arraycopy(y_inv_vector, i7+3, inv_y[1], 0, 4);

			

		    double [][] comb_y = combineTransRot( // 

		            inv_y[0],   // double [] xyz_src,     // transformation to apply to (was reference_xyz)

		            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)

		            xyz_rot,    // double [] xyz_target,  // to which is applied (was scene_xyz)

		            quat_rot);  // double [] quat_target  // to which is applied (was scene_atr)		

			System.arraycopy(comb_y[0], 0, fx, i7,   3);

			System.arraycopy(comb_y[1], 0, fx, i7+3, 4);



			

			if (jt != null) {

				xyz_dq =  applyToDQ(vector, xyz);

				double [][] xyz_dq_local = new double [xyz_dq.length][];

				for (int j = 0; j < xyz_dq.length; j++) {

					xyz_dq_local[j] =combineTransRot(

				            null,       // double [] xyz_src,     // transformation to apply to (was reference_xyz)

				            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)

				            xyz_dq[j],  // double [] xyz_target,  // to which is applied (was scene_xyz)

				            null)[0];   // double [] quat_target  // to which is applied (was scene_atr)		

				}

				for (int j = 0; j < 4; j++) {

					System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);

				}

				

				quat_dq = composeQR_QdQ(vector_r,quat_r);

				double [][] invy_mat = qMat(inv_y[1]);

				double [][] quat_dq_local = mulMat(invy_mat, quat_dq);

				

				double [][]dcomp_dsecond = composeDR(inv_y[1]);

				double [][] quat_dq_local1 = new double [4][];

				for (int j = 0; j < 4; j++) {

//					quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);

					quat_dq_local1[j] = mulMat(invy_mat, quat_dq[j]);

				}

				

				for (int j = 0; j < 4; j++) {

					System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4);

				}

			}			

		}

		return fx;

	}

	

	

	

	private double compareJT(

			double [] vector,

			double    delta) {
@@ -665,7 +858,7 @@ public class QuaternionLma { 
				-1); // final int         debug_level)

		for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {

			System.out.println(String.format(

					"%3d\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f",

					"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",

					n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],

					jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],

					jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
@@ -674,7 +867,7 @@ public class QuaternionLma { 
			}

		}

		System.out.println(String.format(

				"-\t-\t-\t-\t-\t-\t-\t-\t-\t%10.7f\t%10.7f\t%10.7f\t%10.7f",

				"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",

				errors[0], errors[1], errors[2], errors[3]));

		double err=0;

		for (int i = 0; i < vector.length; i++) {
@@ -1127,7 +1320,8 @@ public class QuaternionLma { 
	public void debugYfX (

			String pfx,

			double [] data) {

		if ((mode == 1) || ((mode == 2) && (data.length == x_vector.length))) { // different data size data[3*nscene+...]

//		if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]

		if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]

			System.out.println(String.format("%3s"+

					"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r

					"\t%9s\t%9s\t%9s",
@@ -1166,6 +1360,27 @@ public class QuaternionLma { 
		return new String[] {};

	}

	

	public static double [][] combineTransRot(

			double [] xyz_src,     // transformation to apply to (was reference_xyz)

			double [] quat_src,    // transformation to apply to (was reference_atr)

			double [] xyz_target,  // to which is applied (was scene_xyz)

			double [] quat_target  // to which is applied (was scene_atr)		

			) {

		double [] offs = applyTo(quat_src, xyz_target);

		double [] xyz = (xyz_src==null)? offs : addTo(xyz_src,offs);

		double [] quat = (quat_target==null)? null: compose(quat_src,quat_target); // includes normSign()

		return new double [][] {xyz,quat}; 

	}



	public static double [][] invertTransRot(

			double [] xyz_src,     // transformation to apply to (was reference_xyz)

			double [] quat_src){    // transformation to apply to (was reference_atr)

		double [] quat = normSign(new double[] {-quat_src[0],quat_src[1],quat_src[2],quat_src[3]});

		double [] xyz = applyTo(quat,new double [] {-xyz_src[0],-xyz_src[1],-xyz_src[2]});

		return new double [][] {xyz,quat};

	}



	

	/**

	 * Apply quaternion q to quaternion r

	 * @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
@@ -1359,6 +1574,16 @@ public class QuaternionLma { 
			{-q[3],-q[2], q[1], q[0]}};

	}



	public static double [] addTo(

			double [] offs,

			double [] xyz) {

		return new double [] {

				offs[0]+xyz[0],

				offs[1]+xyz[1],

				offs[2]+xyz[2]};

	}



	

	/**

	 * Apply quaternion to a 3D vector 

	 * @param q   4 components (scalar, vector) of the quaternion being applied
@@ -1407,4 +1632,41 @@ public class QuaternionLma { 
			{-2*xyz[2]*q[0]+2*xyz[1]*q[1],               2*s +         2*xyz[1]*q[2],              2*xyz[0]*q[0]+2*xyz[1]*q[3]}, 

			{ 2*xyz[1]*q[0]+2*xyz[2]*q[1],              -2*xyz[0]*q[0]+2*xyz[2]*q[2],              2*s          +2*xyz[2]*q[3]}};

	}

	

	public static double [] mulMat(

			double [][] mat,

			double [] vect) {

		double [] rslt = new double[mat.length];

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

			for (int j = 0; j < vect.length; j++) {

				rslt[i]+= mat[i][j]* vect[j];

			}

		}

		return rslt;

		

	}

	

	public static double [][] mulMat(

			double [][] mat,

			double [][] mat1) {

		double [][] rslt = new double[mat.length][mat1[0].length];

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

			for (int j = 0; j < rslt[0].length; j++) {

				for (int k = 0; k < mat[0].length; k++) {

					rslt[i][j] += mat[i][k]* mat1[k][j];

				}

			}

		}

		return rslt;

	}

	

	public static double [][] qMat(

			double [] q){

		return new double [][] {

			{q[0],-q[1],-q[2],-q[3]},

			{q[1], q[0], q[3],-q[2]},

			{q[2],-q[3], q[0], q[1]},

			{q[3], q[2],-q[1], q[0]}};

	}



}