Added linear velocities scale adjustment

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

@@ -2101,7 +2101,8 @@ public class Interscene {
 		    		);
 	    	double [] rms = new double[5];
 	    	double [] quat = new double[4];
-	    	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
+//	    	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
+	    	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // 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/QuadCLTCPU.java

@@ -467,6 +467,10 @@ public class QuadCLTCPU {
 		boolean apply_imu_orient =  clt_parameters.imp.apply_imu_orient; // apply  IMU misalignment to the camera if adjusted
 		boolean adjust_gyro =       clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
 		boolean apply_gyro =        clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
+		boolean adjust_accl =       clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
+		boolean apply_accl =        clt_parameters.imp.apply_accl; // apply IMU velocities scales
+		double  quat_max_change =   clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
+		double  quat_min_lin =   clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale 
 		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
 				clt_parameters, // final CLTParameters clt_parameters,
 				quadCLTs,       // final QuadCLT[]     quadCLTs,
@@ -486,7 +490,7 @@ public class QuadCLTCPU {
     	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // 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);
+    	double  translation_weight =  0.0; //  1.0 / (avg_z + 1.0);
     	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
     			clt_parameters,     // CLTParameters clt_parameters,
     			quat_lma_mode,      // int quat_lma_mode,
@@ -519,6 +523,17 @@ public class QuadCLTCPU {
         			ims_mount_atr, // double [] ims_atr,
         			quat); // double [] corr_q)
         	if (apply_imu_orient) {
+        		for (int i = 0; i < new_ims_mount_atr.length; i++) {
+        			if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
+        				apply_imu_orient = false;
+        				if (debugLevel > -3) {
+        					System.out.println ("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+" exceeds the specified limit ("+quat_max_change+")");
+        					System.out.println ("*** Orientation update is disabled.");
+        				}
+        			}
+        		}
+        	}        		
+        	if (apply_imu_orient) { 
         		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
         		if (debugLevel > -3) {
         			System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
@@ -575,11 +590,133 @@ public class QuadCLTCPU {
 
         		}
         	}
+        	double [] acc_corr = null;
+        	if (adjust_accl) {
+        		acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
+        				clt_parameters,     // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+        				rotated_xyzatr,     // double [][][] rotated_xyzatr,
+        				quadCLTs,           // QuadCLT[]     quadCLTs,
+        				ref_index,          // int           ref_index,
+        				earliest_scene,     // int           earliest_scene,
+        				last_index,         // int           last_index,
+        				quat_min_lin, // double        min_range,
+        				debugLevel);        // int debugLevel
+        		
+        		if (acc_corr != null) {
+    				double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
+    				double [] new_accl_corr =  used_accl_corr.clone();
+    				int num_corr=0;
+    				for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
+    					new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
+    					num_corr++;
+    				}
+        			if (debugLevel > -1) {
+        				System.out.println(String.format(
+        						"Used velocities scales = [%9f, %9f, %9f]", used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
+        				System.out.println(String.format(
+        						"Diff.velocities scales = [%9f, %9f, %9f]", acc_corr[0], acc_corr[1], acc_corr[2]));
+        				System.out.println(String.format(
+        						"New  velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
+        			}        				
+        			if (apply_accl && (num_corr>0)) {
+        				clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
+            			if (debugLevel > -3) {
+            				System.out.println(String.format(
+            						"Applied new velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
+                			System.out.println ("*** Need to save the main configuration file ***");
+            			}
+        			} else {
+            			if (debugLevel > -3) {
+            				System.out.println(String.format(
+            						"New velocities scales are not applied = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
+            			}
+        			}
+        		} else {
+        			System.out.println("*** Adjustment of the gyro omegas failed ***");
+
+        		}
+        	}
     	} else {
     		System.out.println ("*** Failed to calculate IMS mount correction! ***");
     	}
     }
     
+    public static double [] getVelocitiesCorrections(
+    		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+    		double [][][] rotated_xyzatr,
+    		QuadCLT[]     quadCLTs,
+    		int           ref_index,
+    		int           early_index,
+    		int           last_index,
+    		double        min_range,
+    		int           debugLevel
+    		) {
+		double [][][] xyzatr = new double [quadCLTs.length][][];
+		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
+    	double [] timestamps = new double [quadCLTs.length];
+    	double [][][] data = new double[3][last_index-early_index+1][2];
+//    	double ts_ref = quadCLTs[ref_index].getTimeStamp();
+    	double [][] xyz_minmax = new double [3][2]; // zeros OK as it is for ref scene
+    	for (int nscene = early_index; nscene <= last_index; nscene++) {
+    		timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
+        	xyzatr[nscene] = ers_ref.getSceneXYZATR(quadCLTs[nscene].getImageName());
+        	for (int i = 0; i < 3; i++) {
+//        		double d = xyzatr[nscene][0][i] - rotated_xyzatr[nscene][0][i];
+        		xyz_minmax[i][0] = Math.min(xyz_minmax[i][0], xyzatr[nscene][0][i]);
+        		xyz_minmax[i][1] = Math.max(xyz_minmax[i][1], xyzatr[nscene][0][i]);
+        	}
+    	}
+    	boolean [] dir_ok = new boolean[3];
+    	for (int i = 0; i < 3; i++) {
+    		dir_ok[i] = (xyz_minmax[i][1]-xyz_minmax[i][0]) > min_range;
+    	}    	
+    	for (int nscene = early_index; nscene <= last_index; nscene++) {
+//    		double rel_ts = timestamps[nscene] - ts_ref;
+    		for (int i = 0; i < 3; i++) if (dir_ok[i] ){
+    			data[i][nscene-early_index][0] = xyzatr[nscene][0][i]; // for all xyz the same
+    			data[i][nscene-early_index][1] = rotated_xyzatr[nscene][0][i];
+    		}
+    	}
+    	PolynomialApproximation pa= new PolynomialApproximation(-1); // debugLevel);
+    	double [][] coeffs = new double [3][2];
+    	for (int i = 0; i < 3; i++)  {
+    		if (dir_ok[i] ){
+    			coeffs[i] = pa.polynomialApproximation1d(data[i], 1); // linear
+    		} else {
+    			coeffs[i] = new double [] {Double.NaN,Double.NaN};
+    		}
+    	}
+       	if (debugLevel > -1) {
+    		System.out.println("Velocity X correction: " +coeffs[0][1]+" ("+coeffs[0][0]+")");
+    		System.out.println("Velocity Y correction: " +coeffs[1][1]+" ("+coeffs[1][0]+")");
+    		System.out.println("Velocity Z correction: " +coeffs[2][1]+" ("+coeffs[2][0]+")");
+    		if (debugLevel > 0) {
+        		System.out.println(String.format("%3s"+
+        				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
+        				"N","X","Y","Z","IMU-X","IMU-Y","IMU-Z","lX","lY","lZ"));
+            	for (int nscene = early_index; nscene <= last_index; nscene++) {
+            		System.out.println(String.format("%3d"+
+            				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
+            				nscene,
+            				xyzatr[nscene][0][0],
+            				xyzatr[nscene][0][1],
+            				xyzatr[nscene][0][2],
+            				rotated_xyzatr[nscene][0][0],
+            				rotated_xyzatr[nscene][0][1],
+            				rotated_xyzatr[nscene][0][2],
+            				coeffs[0][0]+coeffs[0][1] * xyzatr[nscene][0][0], 
+            				coeffs[1][0]+coeffs[1][1] * xyzatr[nscene][0][1], 
+            				coeffs[2][0]+coeffs[2][1] * xyzatr[nscene][0][2]));
+            	}    			
+    		}
+    	}
+    	
+    	
+    	
+    	return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};
+
+    }
+    
     public static double [] getOmegaCorrections(
     		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
     		double [][][] rotated_xyzatr,
@@ -881,7 +1018,7 @@ public class QuadCLTCPU {
     	double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
     	if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?
     		for (int i = 0; i < 3; i++) {
-    			dxyzatr[0][i] -= pimu_offsets[0][i];
+    			dxyzatr[0][i] /= pimu_offsets[0][i];
     			dxyzatr[1][i] -= pimu_offsets[1][i];
     		}
     	}