working snapshot

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

@@ -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

@@ -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

@@ -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();
@@ -173,9 +186,102 @@ public class QuaternionLma {
 						x_vector); // double [] data)
 		}
 	}
+
+
+	
+	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]}};
+	}
+
 }