Improving mount correction, implementing gyro (omegas) correction

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

@@ -4659,7 +4659,10 @@ public class OpticalFlow {
 		// skip completely if no color or mono, tiff or video
 		
 		boolean adjust_imu_orient =          clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS
+		
 		boolean calc_quat_corr =             clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
+		
+		
 		// apply_quat_corr used inside getGroundIns() - used when generating output
 //		boolean apply_quat_corr =            clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
 		boolean use_ims_rotation =           clt_parameters.imp.use_quat_corr;   // use internally (probably deprecated

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

@@ -221,12 +221,13 @@ public class QuaternionLma {
 					debug_level);       // final int     debug_level);
 			return;
 		}
+		//MODE_XYZQ
 		N = vect_x.length;
 		this.mode = mode;
 		samples = 3+ quat0.length;
 		samples_x = 7;
 		pure_weight = 1.0 - reg_w;
-		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
+		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 + extra_samples];
 		weights =  new double [samples *   N + extra_samples];
@@ -285,9 +286,12 @@ public class QuaternionLma {
 		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
-		if (extra_samples>0) {
-			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
+			}
 		}
 		last_jt = new double [parameters_vector.length][];		
 		if (debug_level > 0) {
@@ -387,7 +391,7 @@ public class QuaternionLma {
 		samples = 7;
 		samples_x = 7;
 		pure_weight = 1.0 - reg_w;
-		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
+		int extra_samples = 0; // (reg_w > 0)? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
 		x_vector =     new double [samples *   N];
 		y_vector =     new double [samples *   N + extra_samples];
 		y_inv_vector = new double [samples_x * N + extra_samples]; 		
@@ -1130,6 +1134,14 @@ public class QuaternionLma {
 				}
 			}			
 		}
+		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;
 	}
 	
@@ -1563,6 +1575,26 @@ public class QuaternionLma {
 		return rslt[0]? iter : -1;
 	}
 	
+	public double [] getMinMaxDiag(
+			int debug_level){
+		double [][] jt = new double [parameters_vector.length][];    
+		double [] fx = getFxDerivs(
+				parameters_vector, // double []         vector,
+				jt,                // final double [][] jt, // should be null or initialized with [vector.length][]
+				debug_level);      // final int         debug_level)
+		Matrix wjtjlambda = new Matrix(getWJtJlambda(
+				0, // *10, // temporary
+				jt)); // double [][] jt)
+		double [] mn_mx= {Double.NaN,Double.NaN};
+		for (int i = 0; i < parameters_vector.length; i++) {
+			double d = wjtjlambda.get(i,i);
+			if (!(d > mn_mx[0])) mn_mx[0] = d;
+			if (!(d < mn_mx[1])) mn_mx[1] = d;
+			
+		}
+		return mn_mx;
+		
+	}
 	private boolean [] lmaStep(
 			double lambda,
 			double rms_diff,