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/IntersceneMatchParameters.java

@@ -46,6 +46,7 @@ public class IntersceneMatchParameters {
 	// Tilt ->    -pimu_omega[1] rad/s, average did_pimu.theta[1]/did_pimu.dt
 	// Roll ->    +pimu_omega[0] rad/s, average did_pimu.theta[0]/did_pimu.dt
 	public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418}; 
+	public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1)  
 	
 	
 	@Deprecated
@@ -117,6 +118,8 @@ public class IntersceneMatchParameters {
 	public  boolean apply_imu_orient =   true; // apply  IMU misalignment to the camera if adjusted
 	public  boolean adjust_gyro =        false;// adjust qyro omegas offsets
 	public  boolean apply_gyro =         true; // apply adjusted qyro omegas offsets
+	public  boolean adjust_accl =        false;// adjust IMU velocities scales
+	public  boolean apply_accl =         true; // apply IMU velocities scales
 	
 	public  boolean calc_quat_corr =     true; // calculate camera orientation correction from predicted by IMS 
     public  boolean apply_quat_corr =    true; // apply camera orientation correction from predicted by IMS
@@ -459,6 +462,7 @@ public class IntersceneMatchParameters {
     public  double  quat_max_change =        0.1; // radians
     public  double  quat_min_transl =        5.0; // meters to adjust by translation
     public  double  quat_min_rot =           0.1; // radians to adjust by rotation
+    public  double  quat_min_lin =          10.0; // meters, minimal distance per axis to adjust IMS velocity scale 
 	
 	public  boolean stereo_merge =       true;
 	public  int     stereo_gap =         32;    // pixels between right and left frames
@@ -528,7 +532,8 @@ public class IntersceneMatchParameters {
 
 	public double [][] get_pimu_offs(){
 		// TODO: use ims_ortho, ims_mount_atr
-		return new double [][] {new double[3],
+		return new double [][] {
+			{ pimu_acc_offs[0] + 1,  pimu_acc_offs[1] + 1, pimu_acc_offs[2] + 1},
 			{-pimu_gyr_offs[2],     -pimu_gyr_offs[1],     pimu_gyr_offs[0]}}; // {XYZ,ATR}
 	}
 	
@@ -537,6 +542,14 @@ public class IntersceneMatchParameters {
 		pimu_gyr_offs[1] = - atr[1];
 		pimu_gyr_offs[0] =   atr[2];
 	}
+	
+	public void set_pimu_velocities_scales(double [] vxyz) {
+		pimu_acc_offs[0] = vxyz[0] - 1;
+		pimu_acc_offs[1] = vxyz[1] - 1;
+		pimu_acc_offs[2] = vxyz[2] - 1;
+	}
+	
+	
 	public void dialogQuestions(GenericJTabbedDialog gd) {
 		//		gd.addMessage  ("Scene parameters selection");
 //		gd.addTab         ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
@@ -560,6 +573,9 @@ public class IntersceneMatchParameters {
 		gd.addStringField ("IMU angular corrections (rad/s)",  IntersceneMatchParameters.doublesToString(pimu_gyr_offs), 80,
 				"Average  from did_pimu.theta[]/did_pimu.dt when not moving.");
 
+		gd.addStringField ("IMU accelerators gain",  IntersceneMatchParameters.doublesToString(pimu_acc_offs), 80,
+				"IMU accelerators gain difference from 1. Negative values when IMU-reported velocities are smaller than actual");
+
 		gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
 				"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
 		gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
@@ -661,6 +677,12 @@ public class IntersceneMatchParameters {
 				"Adjust qyro omegas offsets.");
 		gd.addCheckbox ("Apply gyro offsets",                        this.apply_gyro,
 				"Apply adjusted qyro omegas offsets.");
+		
+		gd.addCheckbox ("Adjust accelerometers scales",              this.adjust_accl,
+				"Adjust IMU linear velocities scales.");
+		gd.addCheckbox ("Apply accelerometers scales",               this.apply_accl,
+				"Apply IMU linear velocities scales.");
+		
 		gd.addCheckbox ("Calculate IMS orientation correction",      this.calc_quat_corr,
 				"Calculate camera orientation correction from predicted by IMS .");
 		gd.addCheckbox ("Apply IMS orientation correction ",         this.apply_quat_corr,
@@ -1304,6 +1326,8 @@ public class IntersceneMatchParameters {
 				"Do not use translation for IMS mount adjustment if it is too small.");
 		gd.addNumericField("Minimal rotation for mount calibration",      this.quat_min_rot, 3,5,"rad",
 				"Do not use rotations for IMS mount adjustment if it is too small.");
+		gd.addNumericField("Minimal per-axis travel",                     this.quat_min_lin, 3,5,"m",
+				"Minimal per-axis travel to adjust accelerometer sensitivity.");
 	
 		gd.addTab("Stereo/Video","Parameters for stereo video generation");
 		gd.addMessage  ("Stereo");
@@ -1409,6 +1433,7 @@ public class IntersceneMatchParameters {
 		this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
+		this.pimu_acc_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.fmg_initial_en =                 gd.getNextBoolean();
@@ -1457,6 +1482,10 @@ public class IntersceneMatchParameters {
 		this.apply_imu_orient =               gd.getNextBoolean();
 		this.adjust_gyro =                    gd.getNextBoolean();
 		this.apply_gyro =                     gd.getNextBoolean();
+		this.adjust_accl =                    gd.getNextBoolean();
+		this.apply_accl =                     gd.getNextBoolean();
+		
+		
 		this.calc_quat_corr =                 gd.getNextBoolean();
 		this.apply_quat_corr =                gd.getNextBoolean();
 		this.use_quat_corr =                  gd.getNextBoolean();
@@ -1752,6 +1781,7 @@ public class IntersceneMatchParameters {
 		this.quat_max_change =          gd.getNextNumber();
 		this.quat_min_transl =          gd.getNextNumber();
 		this.quat_min_rot =             gd.getNextNumber();
+		this.quat_min_lin =             gd.getNextNumber();
 
 		if (stereo_views.length > 0) {
 			int i =                     gd.getNextChoiceIndex();
@@ -1849,6 +1879,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"ims_mount_atr",                 IntersceneMatchParameters.doublesToString(this.ims_mount_atr));		
 		properties.setProperty(prefix+"ims_mount_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
 		properties.setProperty(prefix+"pimu_gyr_offs",                 IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs));
+		properties.setProperty(prefix+"pimu_acc_offs",                 IntersceneMatchParameters.doublesToString(this.pimu_acc_offs));
 		properties.setProperty(prefix+"ims_scale_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
 		properties.setProperty(prefix+"ims_scale_atr",                 IntersceneMatchParameters.doublesToString(this.ims_scale_atr));		
 		properties.setProperty(prefix+"fmg_initial_en",                this.fmg_initial_en + "");              // boolean
@@ -1896,6 +1927,8 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"apply_imu_orient",              this.apply_imu_orient+"");              // boolean
 		properties.setProperty(prefix+"adjust_gyro",                   this.adjust_gyro+"");                   // boolean
 		properties.setProperty(prefix+"apply_gyro",                    this.apply_gyro+"");                    // boolean
+		properties.setProperty(prefix+"adjust_accl",                   this.adjust_accl+"");                   // boolean
+		properties.setProperty(prefix+"apply_accl",                    this.apply_accl+"");                    // boolean
 		properties.setProperty(prefix+"calc_quat_corr",                this.calc_quat_corr+"");                // boolean
 		properties.setProperty(prefix+"apply_quat_corr",               this.apply_quat_corr+"");               // boolean
 		properties.setProperty(prefix+"use_quat_corr",                 this.use_quat_corr+"");                 // boolean
@@ -2201,6 +2234,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"quat_max_change",      this.quat_max_change+"");     // double
 		properties.setProperty(prefix+"quat_min_transl",      this.quat_min_transl+"");     // double
 		properties.setProperty(prefix+"quat_min_rot",         this.quat_min_rot+"");        // double
+		properties.setProperty(prefix+"quat_min_lin",         this.quat_min_lin+"");        // double
 		
 		properties.setProperty(prefix+"stereo_merge",         this.stereo_merge+"");        // boolean
 		properties.setProperty(prefix+"stereo_gap",           this.stereo_gap+"");          // int
@@ -2253,6 +2287,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"ims_mount_atr")!=null) 	   this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
 		if (properties.getProperty(prefix+"ims_mount_xyz")!=null) 	   this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
 		if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) 	   this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3);
+		if (properties.getProperty(prefix+"pimu_acc_offs")!=null) 	   this.pimu_acc_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_acc_offs"),3);
 		if (properties.getProperty(prefix+"ims_scale_xyz")!=null) 	   this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
 		if (properties.getProperty(prefix+"ims_scale_atr")!=null) 	   this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
 		if (properties.getProperty(prefix+"fmg_initial_en")!=null)     this.fmg_initial_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_initial_en"));		
@@ -2300,6 +2335,11 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"apply_imu_orient")!=null)              this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
 		if (properties.getProperty(prefix+"adjust_gyro")!=null)                   this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
 		if (properties.getProperty(prefix+"apply_gyro")!=null)                    this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
+
+		if (properties.getProperty(prefix+"adjust_accl")!=null)                   this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
+		if (properties.getProperty(prefix+"apply_accl")!=null)                    this.apply_accl=Boolean.parseBoolean(properties.getProperty(prefix+"apply_accl"));
+		
+		
 		if (properties.getProperty(prefix+"calc_quat_corr")!=null)                this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
 		if (properties.getProperty(prefix+"apply_quat_corr")!=null)               this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
 		if (properties.getProperty(prefix+"use_quat_corr")!=null)                 this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
@@ -2612,6 +2652,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"quat_max_change")!=null)      this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
 		if (properties.getProperty(prefix+"quat_min_transl")!=null)      this.quat_min_transl=Double.parseDouble(properties.getProperty(prefix+"quat_min_transl"));
 		if (properties.getProperty(prefix+"quat_min_rot")!=null)         this.quat_min_rot=Double.parseDouble(properties.getProperty(prefix+"quat_min_rot"));
+		if (properties.getProperty(prefix+"quat_min_lin")!=null)         this.quat_min_lin=Double.parseDouble(properties.getProperty(prefix+"quat_min_lin"));
 		
 		if (properties.getProperty(prefix+"stereo_merge")!=null)         this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
 		if (properties.getProperty(prefix+"stereo_gap")!=null)           this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
@@ -2684,6 +2725,7 @@ public class IntersceneMatchParameters {
  		imp.ims_mount_atr                 = this.ims_mount_atr.clone();
  		imp.ims_mount_xyz                 = this.ims_mount_xyz.clone();
  		imp.pimu_gyr_offs                 = this.pimu_gyr_offs.clone();
+ 		imp.pimu_acc_offs                 = this.pimu_acc_offs.clone();
  		imp.ims_scale_xyz                 = this.ims_scale_xyz.clone();
  		imp.ims_scale_atr                 = this.ims_scale_atr.clone();
  		imp.fmg_initial_en                = this.fmg_initial_en;
@@ -2731,6 +2773,8 @@ public class IntersceneMatchParameters {
 		imp.apply_imu_orient              = this.apply_imu_orient;
 		imp.adjust_gyro                   = this.adjust_gyro;
 		imp.apply_gyro                    = this.apply_gyro;
+		imp.adjust_accl                   = this.adjust_accl;
+		imp.apply_accl                    = this.apply_accl;
 		imp.calc_quat_corr                = this.calc_quat_corr;
 		imp.apply_quat_corr               = this.apply_quat_corr;
 		imp.use_quat_corr                 = this.use_quat_corr;
@@ -3032,6 +3076,7 @@ public class IntersceneMatchParameters {
 		imp.quat_max_change       = this.quat_max_change;
 		imp.quat_min_transl       = this.quat_min_transl;
 		imp.quat_min_rot          = this.quat_min_rot;
+		imp.quat_min_lin          = this.quat_min_lin;
 
 		imp.stereo_merge          = this.stereo_merge;
 		imp.stereo_gap            = this.stereo_gap;

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,
@@ -518,6 +522,17 @@ public class QuadCLTCPU {
         	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
         			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) {
@@ -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];
     		}
     	}