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