Debugged QuaternionLma.MODE_COMBO_LOCAL

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

@@ -257,6 +257,7 @@ public class QuadCLTCPU {
 			double []     rms, // null or double[5];
 			int           debugLevel
     		) {
+    	boolean use_scale_y = false;
 //    	final boolean use3 = (quat_lma_mode == 3); // true;
 		double lambda =            clt_parameters.imp.quat_lambda; // 0.1;
 		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
@@ -276,20 +277,44 @@ public class QuadCLTCPU {
 			vect_x[nscene] = ims_xyzatr[nscene];
 			vect_y[nscene] = xyzatr[nscene];
 		}
-		double [][][] vect_y_scaled = QuaternionLma.scaleXYZ(
+		// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
+		double [][][] vect_y_scaled;
+		if (use_scale_y) {
+			vect_y_scaled= QuaternionLma.scaleXYZ(
 					vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
 					vect_y, // double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
 					new int [] {early_index, last_index}); // int []    first_last){
-		if ((quat_lma_mode == quaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
+		} else {
+			vect_y_scaled = vect_y;
+		}
+		if ((quat_lma_mode == QuaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
 			quaternionLma.prepareLMA(
 					quat_lma_mode,      // int           mode,
 					avg_height,         // double        avg_height,
 					vect_x,             // double [][][] vect_x,
 					vect_y_scaled,      // vect_y,             // double [][][] vect_y,
 					null,               // double []     vect_w, all same weight
-					translation_weight, // double        translation_weight, // 0.0 ... 1.0;
+					0.0, // reg_w,              // double        reg_w,      // regularization weight [0..1)  
+					quat0,              // double []     quat0,
+					debugLevel);        // int           debug_level)
+			double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
+			if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
+				double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
+				reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
+				if (debugLevel > -1) {
+					System.out.println("==== Calculated reg_w = "+reg_w);
+				}
+				quaternionLma.prepareLMA(
+						quat_lma_mode,      // int           mode,
+						avg_height,         // double        avg_height,
+						vect_x,             // double [][][] vect_x,
+						vect_y_scaled,      // vect_y,             // double [][][] vect_y,
+						null,               // double []     vect_w, all same weight
+						reg_w,              // double        reg_w,      // regularization weight [0..1)  
 						quat0,              // double []     quat0,
 						debugLevel);        // int           debug_level)
+			}			
+			
 		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
 				(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
 				(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
@@ -320,14 +345,6 @@ public class QuadCLTCPU {
 						debugLevel);        // int           debug_level)
 			}
 		} else {
-			/*
-			double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
-			double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
-			for (int nscene = early_index; nscene <= last_index; nscene++) {
-				vect_x[nscene] = ims_xyzatr[nscene][0];
-				vect_y[nscene] = xyzatr[nscene][0];
-			}
-*/
 			quaternionLma.prepareLMA(
 					vect_x,        // quat_lma_xyz,     // double [][] vect_x,
 					vect_y_scaled, // vect_y,             // double [][][] vect_y,
@@ -493,7 +510,8 @@ public class QuadCLTCPU {
     	}
     	double [] rms = new double[5];
     	double [] quat = new double[4];
-    	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; //  2; // 1;
+//    	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; //  2; // 1;
+    	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; //  2; // 1;
     	int debug_lev = debugLevel; // 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/QuaternionLma.java

@@ -479,7 +479,7 @@ public class QuaternionLma {
 			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        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
+			double        reg_w,      // regularization weight [0..1)  
 			double []     quat0,
 			final int     debug_level) {
 		if (mode != MODE_COMBO) {
@@ -501,9 +501,10 @@ public class QuaternionLma {
 		samples_x = 7;
 		height = avg_height;
 		pure_weight = 1.0 - reg_w;
+		int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
 		x_vector = new double [samples_x * N];
-		y_vector = new double [samples *   N + REGLEN];
-		weights =  new double [samples *   N + REGLEN];
+		y_vector = new double [samples *   N + extra_samples];
+		weights =  new double [samples *   N + extra_samples];
 		parameters_vector = quat0.clone();
 		double sw = 0;
 		for (int i = 0; i < N; i++) {
@@ -545,8 +546,15 @@ public class QuaternionLma {
 		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
-		weights [samples * N] = 1.0 - pure_weight;
-		y_vector[samples * N] = 1.0;
+		if (extra_samples > 0) {
+			double w = (1.0 - pure_weight)/parameters_vector.length;
+			for (int i = 0; i < parameters_vector.length; i++) {
+				weights [samples * N + i] = w;
+				y_vector[samples * N] = 0.0; // or target value
+			}
+		}
+//		weights [samples * N] = 1.0 - pure_weight;
+//		y_vector[samples * N] = 1.0;
 		last_jt = new double [parameters_vector.length][];		
 		if (debug_level > 0) {
 			 debugYfX ( "",   // String pfx,
@@ -556,7 +564,7 @@ public class QuaternionLma {
 		}
 	}
 
-	public void prepareLMAMode4(
+	public void prepareLMAMode4( // MODE_COMBO_LOCAL
 			int           mode,
 			double        avg_height,         // 
 			double [][][] vect_x, // []{{x,y,z},{a,t,r}}
@@ -572,10 +580,11 @@ public class QuaternionLma {
 		samples_x = 7;
 		height = avg_height;
 		pure_weight = 1.0 - reg_w;
+		int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
 		x_vector =     new double [samples_x * N];
-		y_vector = new double [samples *   N + REGLEN];
-		y_inv_vector = new double [samples_x * N + REGLEN]; 		
-		weights =  new double [samples *   N + REGLEN];
+		y_vector =     new double [samples *   N + extra_samples];
+		y_inv_vector = new double [samples_x * N + extra_samples]; 		
+		weights =      new double [samples *   N + extra_samples];
 		parameters_vector = quat0.clone();
 		double sw = 0;
 		for (int i = 0; i < N; i++) {
@@ -627,8 +636,15 @@ public class QuaternionLma {
 		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
-		weights [samples * N] = 1.0 - pure_weight;
-		y_vector[samples * N] = 1.0;
+		if (extra_samples > 0) {
+			double w = (1.0 - pure_weight)/parameters_vector.length;
+			for (int i = 0; i < parameters_vector.length; i++) {
+				weights [samples * N + i] = w;
+				y_vector[samples * N] = 0.0; // or target value
+			}
+		}
+//		weights [samples * N] = 1.0 - pure_weight;
+//		y_vector[samples * N] = 1.0;
 		last_jt = new double [parameters_vector.length][];		
 		if (debug_level > 0) {
 			 debugYfX ( "Y-INV-",   // String pfx,
@@ -640,6 +656,7 @@ public class QuaternionLma {
 	
 	// TODO: Consider adding differences between x and y for regularization (or it won't work)
 	// goal - to minimize "unneeded" rotation along the common axis
+	// updated for MODE_XYZQ/3, MODE_COMBO_LOCAL/3 ( /3 - only 3 quaternion components
 	private double [] getFxDerivs(
 			double []         vector,
 			final double [][] jt, // should be null or initialized with [vector.length][]
@@ -678,10 +695,17 @@ public class QuaternionLma {
 						debug_level); // final int         debug_level)
 			}
 		case MODE_COMBO_LOCAL: 
+			if (parameters_vector.length < 4) { // updated for [3]
+				return  getFxDerivsVisualMode43( // fill change
+						vector,       // double []         vector,
+						jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+						debug_level); // final int         debug_level)
+			} else {
 				return  getFxDerivsVisualMode4( // fill change
 						vector,       // double []         vector,
 						jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 						debug_level); // final int         debug_level)
+			}
 		case MODE_COMPASS:return  getFxDerivsCompass( // fill change
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
@@ -1307,23 +1331,23 @@ public class QuaternionLma {
 		return fx;
 	}
 	
-	private double [] getFxDerivsVisualMode4(
+	private double [] getFxDerivsVisualMode4(  // seems rotation is opposite sign
 			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 q0 = vector[0]; 
+//		final double q1 = vector[1]; 
+//		final double q2 = vector[2]; 
+//		final double q3 = vector[3];
 		final double [] vector_r = normSign(vector);
 		if (jt != null) {
 			for (int i = 0; i < vector.length; i++) {
 				jt[i] = new double [weights.length];
-				jt[i][samples * N] = 2 * vector[i];
+//				jt[i][samples * N] = 2 * vector[i];
 			}
 		}
-		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
+//		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
 		double [] xyz_rot;
 		double [] quat_rot;
 		double [][] xyz_dq;
@@ -1393,9 +1417,144 @@ public class QuaternionLma {
 				jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
 			}			
 		}
+		if (weights.length > N*samples) {
+			for (int i = 0; i < vector.length; i++) {
+				fx[samples*N + i] = vector[i];
+				if (jt != null) {
+					jt[i][samples*N + i] = 1.0;
+				}				
+			}	
+		}
 		return fx;
 	}
 	
+	private double [] getFxDerivsVisualMode43(
+			double []         vector3,
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
+		double [] fx = new double [weights.length];
+		double [] qn = new double[4];
+		final double [] vector_r = normSign(vector).clone(); // should be already q0>0 
+		qNorm (vector_r, qn); // calculates qn // normalized
+		final double [][] dQn_dQ123 = dQ_dQ123(vector3);
+		if (jt != null) {
+			for (int i = 0; i < jt.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+		}
+		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 i4 = samples * i;
+			int i7 = samples_x * i;
+			has_data:{
+				for (int j = 0; j < samples; j++) {
+					if (weights[i4+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)
+			fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height;                   // Z
+			fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3];                       // 2 * Q3
+			fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
+			fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
+		    /*
+			fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height;                   // Z
+			fx[i4 + 1] = -2 * comb_y[1][3]; // quat_rot[3];                       // 2 * Q3
+			fx[i4 + 2] = -2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
+			fx[i4 + 3] = -2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
+			*/
+			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)		
+				}
+				quat_dq = composeQR_QdQ(vector_r,quat_r, false); // true);
+				double [][] invy_mat = qMat(inv_y[1]);
+				double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
+				double [][] d_dQn = new double[4][4];
+				
+				// Z
+				d_dQn[0][0] = xyz_dq_local[0][2] / height;
+				d_dQn[1][0] = xyz_dq_local[1][2] / height;
+				d_dQn[2][0] = xyz_dq_local[2][2] / height;
+				d_dQn[3][0] = xyz_dq_local[3][2] / height;
+                /*				
+				// why all "-2"?
+				// 2 * Q3
+				d_dQn[0][1] = -2 * quat_dq_local[0][3];
+				d_dQn[1][1] = -2 * quat_dq_local[1][3]; 
+				d_dQn[2][1] = -2 * quat_dq_local[2][3];
+				d_dQn[3][1] = -2 * quat_dq_local[3][3];
+				// 2 * Q2 - X / height                
+				d_dQn[0][2] = -2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
+				d_dQn[1][2] = -2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
+				d_dQn[2][2] = -2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
+				d_dQn[3][2] = -2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
+				// 2 * Q1 + Y / height                
+				d_dQn[0][3] = -2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
+				d_dQn[1][3] = -2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
+				d_dQn[2][3] = -2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
+				d_dQn[3][3] = -2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
+				*/
+				
+				// 2 * Q3
+				d_dQn[0][1] = 2 * quat_dq_local[0][3];
+				d_dQn[1][1] = 2 * quat_dq_local[1][3]; 
+				d_dQn[2][1] = 2 * quat_dq_local[2][3];
+				d_dQn[3][1] = 2 * quat_dq_local[3][3];
+				// 2 * Q2 - X / height                
+				d_dQn[0][2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
+				d_dQn[1][2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
+				d_dQn[2][2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
+				d_dQn[3][2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
+				// 2 * Q1 + Y / height                
+				d_dQn[0][3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
+				d_dQn[1][3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
+				d_dQn[2][3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
+				d_dQn[3][3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
+								
+				//dQn_dQ123
+				double [][] d_dq = mulMat(dQn_dQ123, d_dQn);
+				for (int j = 0; j < d_dq.length; j++) { // 3
+					System.arraycopy(d_dq[j], 0, jt[j], i4, samples);
+				}
+			}			
+		}
+		if (weights.length > N*samples) {
+			for (int i = 0; i < vector3.length; i++) {
+				fx[samples*N + i] = vector3[i];
+				if (jt != null) {
+					jt[i][samples*N + i] = 1.0;
+				}				
+			}	
+		}
+		
+		return fx;
+	}
 	
 	private double [] getYminusFxWeighted(
 			final double []   fx,