tested new extrinsic adjustment including ERS rotation

src/main/java/com/elphel/imagej/cameras/CLTParameters.java

@@ -179,8 +179,12 @@ public class CLTParameters {
 	public boolean    ly_diff_roll_en = true;    // Adjust differential rolls (3 of 4 angles)
 	public boolean    ly_focalLength=   true;    // Correct scales (focal length temperature? variations)
 	public boolean    ly_com_roll=      false;   // Enable common roll (valid for high disparity range only)
+	public boolean    ly_ers_rot=       true;    // Enable ERS correction of the camera rotation
+	public boolean    ly_ers_lin=       false;   // Enable ERS correction of the camera linear movement
 	public int        ly_par_sel   =    0;       // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use checkbox selections above)
 
+	public int        ly_debug_level =  0;       // LY debug level
+
 	public boolean    ly_right_left=    false;   // equalize weights of right/left FoV (use with horizon in both halves and gross infinity correction)
 
 	public int        ly_per_quad =     10;      // minimal tiles per quadrant (not counting the worst) tp proceed
@@ -966,8 +970,12 @@ public class CLTParameters {
 		properties.setProperty(prefix+"ly_diff_roll_en",            this.ly_diff_roll_en+"");
 		properties.setProperty(prefix+"ly_focalLength",             this.ly_focalLength+"");
 		properties.setProperty(prefix+"ly_com_roll",                this.ly_com_roll+"");
-
+		properties.setProperty(prefix+"ly_ers_rot",                 this.ly_ers_rot+"");
+		properties.setProperty(prefix+"ly_ers_lin",                 this.ly_ers_lin+"");
 		properties.setProperty(prefix+"ly_par_sel",                 this.ly_par_sel+"");
+		properties.setProperty(prefix+"ly_debug_level",             this.ly_debug_level+"");
+
+
 		properties.setProperty(prefix+"ly_right_left",              this.ly_right_left+"");
 
 		properties.setProperty(prefix+"ly_per_quad",                this.ly_per_quad +"");
@@ -1683,8 +1691,11 @@ public class CLTParameters {
 		if (properties.getProperty(prefix+"ly_diff_roll_en")!=null)               this.ly_diff_roll_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_diff_roll_en"));
 		if (properties.getProperty(prefix+"ly_focalLength")!=null)                this.ly_focalLength=Boolean.parseBoolean(properties.getProperty(prefix+"ly_focalLength"));
 		if (properties.getProperty(prefix+"ly_com_roll")!=null)                   this.ly_com_roll=Boolean.parseBoolean(properties.getProperty(prefix+"ly_com_roll"));
-
+		if (properties.getProperty(prefix+"ly_ers_rot")!=null)                    this.ly_ers_rot=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_rot"));
+		if (properties.getProperty(prefix+"ly_ers_lin")!=null)                    this.ly_ers_lin=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_lin"));
 		if (properties.getProperty(prefix+"ly_par_sel")!=null)                    this.ly_par_sel=Integer.parseInt(properties.getProperty(prefix+"ly_par_sel"));
+		if (properties.getProperty(prefix+"ly_debug_level")!=null)                this.ly_debug_level=Integer.parseInt(properties.getProperty(prefix+"ly_debug_level"));
+
 		if (properties.getProperty(prefix+"ly_right_left")!=null)                 this.ly_right_left=Boolean.parseBoolean(properties.getProperty(prefix+"ly_right_left"));
 
 		if (properties.getProperty(prefix+"ly_per_quad")!=null)                   this.ly_per_quad=Integer.parseInt(properties.getProperty(prefix+"ly_per_quad"));
@@ -2436,7 +2447,7 @@ public class CLTParameters {
 		gd.addTab         ("Lazy eye", "Lazy eye parameters");
 
 		gd.addMessage     ("--- main-to-aux depth map parameters ---");
-		gd.addNumericField("Minimal reference (main) channel orrelation strength",                              this.ly_gt_strength,        3);
+		gd.addNumericField("Minimal reference (main) channel correlation strength",                              this.ly_gt_strength,        3);
 		gd.addCheckbox    ("Use window for AUX tiles to reduce weight of the hi-res tiles near low-res tile boundaries", this.ly_gt_use_wnd);
 		gd.addNumericField("Aux disparity thershold to split FG and BG (and disable AUX tile for adjustment)",  this.ly_gt_rms,        3);
 		gd.addMessage     ("--- others ---");
@@ -2446,9 +2457,15 @@ public class CLTParameters {
 		gd.addCheckbox    ("Adjust differential rolls",                                                         this.ly_diff_roll_en,"Adjust differential rolls (3 of 4 rolls, keeping average roll)");
 		gd.addCheckbox    ("Correct scales (focal length temperature? variations)",                             this.ly_focalLength);
 		gd.addCheckbox    ("Enable common roll adjustment (valid for high disparity range scans only)",         this.ly_com_roll);
-
+		gd.addCheckbox    ("Enable ERS correction of the camera rotation",                                      this.ly_ers_rot);
+		gd.addCheckbox    ("Enable ERS correction of the camera linear movement",                               this.ly_ers_lin);
 		gd.addNumericField("Manual parameter mask selection (0 use checkboxes above)",                          this.ly_par_sel,  0, 5,"",
 				"bit 0 - sym0, bit1 - sym1, ...");
+		gd.addNumericField("Debug level for lazy eye/ers processing",                                           this.ly_debug_level,  0, 5,"",
+				"Active when global debug level > -1");
+
+
+
 		gd.addCheckbox    ("Equalize weights of right/left FoV",           this.ly_right_left,
 				"Use this mode use with horizon visible in both FoV halves when gross infinity correction is needed");
 
@@ -3273,7 +3290,10 @@ public class CLTParameters {
 		this.ly_diff_roll_en=       gd.getNextBoolean();
 		this.ly_focalLength=        gd.getNextBoolean();
 		this.ly_com_roll=           gd.getNextBoolean();
+		this.ly_ers_rot=            gd.getNextBoolean();
+		this.ly_ers_lin=            gd.getNextBoolean();
 		this.ly_par_sel=      (int) gd.getNextNumber();
+		this.ly_debug_level=  (int) gd.getNextNumber();
 
 		this.ly_right_left=         gd.getNextBoolean();
 

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

@@ -709,7 +709,9 @@ private Panel panel1,
 			addButton("LWIR_TEST",                  panelClt_GPU, color_conf_process);
 			addButton("LWIR_ACQUIRE",               panelClt_GPU, color_conf_process);
 			addButton("IMU main",                   panelClt_GPU, color_conf_process);
+			addButton("ERS main",                   panelClt_GPU, color_process);
 			addButton("IMU aux",                    panelClt_GPU, color_conf_process_aux);
+			addButton("ERS aux",                    panelClt_GPU, color_process_aux);
 
 			plugInFrame.add(panelClt_GPU);
 		}
@@ -4318,7 +4320,8 @@ private Panel panel1,
     } else if (label.equals("CLT 4 images") ||
     		label.equals("CLT apply fine corr") ||
     		label.equals("CLT infinity corr") ||
-    		label.equals("CORR TEST" )) {
+    		label.equals("CORR TEST" ) ||
+    		label.equals("ERS main")) {
     	boolean apply_corr = label.equals("CLT apply fine corr");
     	boolean infinity_corr = label.equals("CLT infinity corr");
     	DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
@@ -4377,7 +4380,19 @@ private Panel panel1,
         int num_infinity_corr = infinity_corr? CLT_PARAMETERS.inf_repeat : 1;
         if ( num_infinity_corr < 1) num_infinity_corr = 1;
         for (int i_infinity_corr = 0;  i_infinity_corr < num_infinity_corr; i_infinity_corr++) {
-        	if (label.equals("CORR TEST")) {
+        	if (label.equals("ERS main")) {
+        		QUAD_CLT.processCLTQuadCorrsTestERS(
+        				CLT_PARAMETERS,  // EyesisCorrectionParameters.DCTParameters           dct_parameters,
+        				DEBAYER_PARAMETERS, //EyesisCorrectionParameters.DebayerParameters     debayerParameters,
+        				COLOR_PROC_PARAMETERS, //EyesisCorrectionParameters.ColorProcParameters colorProcParameters,
+        				CHANNEL_GAINS_PARAMETERS, //CorrectionColorProc.ColorGainsParameters     channelGainParameters,
+        				RGB_PARAMETERS, //EyesisCorrectionParameters.RGBParameters             rgbParameters,
+        				apply_corr,
+        				infinity_corr, // calculate and apply geometry correction at infinity
+        				THREADS_MAX, //final int          threadsMax,  // maximal number of threads to launch
+        				UPDATE_STATUS, //final boolean    updateStatus,
+        				DEBUG_LEVEL); //final int        debugLevel);
+        	} else if (label.equals("CORR TEST")) {
         		QUAD_CLT.processCLTQuadCorrsTest(
         				CLT_PARAMETERS,  // EyesisCorrectionParameters.DCTParameters           dct_parameters,
         				DEBAYER_PARAMETERS, //EyesisCorrectionParameters.DebayerParameters     debayerParameters,

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

@@ -100,6 +100,7 @@ public class Corr2dLMA {
 	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization terms
 	private double            pure_weight; // weight of samples only
 	private double []         values;
+
 	// next values are only updated after success
 	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
 	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
@@ -763,7 +764,7 @@ public class Corr2dLMA {
 		return dbg_img;
 	}
 
-	private double [] getFxJt( // USED in lwir
+	private double [] getFxJt(
 			double []   vector,
 			double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
 		if (this.gaussian_mode) return getFxJt_gaussian(vector, jt);
@@ -1318,7 +1319,7 @@ public class Corr2dLMA {
 
 ////////////////////////////////////////////////////////////////////////
 
-	private boolean debugJt( // not used in lwir
+	private boolean debugJt(
     		double      delta,
     		double []   vector) {
     	int num_points = this.values.length;
@@ -1653,7 +1654,7 @@ public class Corr2dLMA {
 		return wjtymfx;
 	}
 
-	public boolean runLma( // USED in lwir
+	public boolean runLma(
 			double lambda,           // 0.1
 			double lambda_scale_good,// 0.5
 			double lambda_scale_bad, // 8.0
@@ -1713,7 +1714,7 @@ public class Corr2dLMA {
 
 
 	// returns {success, done}
-	public boolean [] lmaStep( // USED in lwir
+	public boolean [] lmaStep(
 			double lambda,
 			double rms_diff,
 			int debug_level) {
@@ -1821,5 +1822,4 @@ public class Corr2dLMA {
 		}
 		return rslt;
 	}
-
 }

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

@@ -1808,6 +1808,8 @@ public class ImageDtt {
 						double []     strength =  new double [clustSize];
 //						double []     disparity = new double [clustSize];
 						double [][]   disp_str =  new double [clustSize][];
+						double [][]   dY_dD =     new double [clustSize][quad];
+						double [][]   pxpy =      new double [clustSize][2];
 
 						boolean debugCluster =  (clustX == debug_clustX) && (clustY == debug_clustY);
 						boolean debugCluster1 = (Math.abs(clustX - debug_clustX) < 10) && (Math.abs(clustY - debug_clustY) < 10);
@@ -1947,6 +1949,8 @@ public class ImageDtt {
 													centerY,
 													disparity_array[tileY][tileX] + disparity_corr);
 										}
+										pxpy[cTile][0] = centerX;
+										pxpy[cTile][1] = centerY;
 
 										if (((globalDebugLevel > 0) || debug_distort) || (debugTile && (globalDebugLevel > -2))) {
 											for (int i = 0; i < quad; i++) {
@@ -2257,7 +2261,8 @@ public class ImageDtt {
 								double [][] extra_stats = lma2.getTileStats();
 								//		final double [][] lazy_eye_data = new double [clustersY*clustersX][];
 								// calculate average disparity per cluster using a sum of the disparity_array and the result of the LMA
-								double sum_wd = 0, sum_w = 0;
+								lazy_eye_data[nCluster] = new double [ExtrinsicAdjustment.INDX_LENGTH];
+								double sum_w = 0;
 								for (int cTileY = 0; cTileY < tileStep; cTileY++) {
 									tileY = clustY * tileStep + cTileY ;
 									if (tileY < tilesY) {
@@ -2267,9 +2272,15 @@ public class ImageDtt {
 												cTile = cTileY * tileStep + cTileX;
 												tIndex =    tileY * tilesX + tileX;
 												if ((lma_ds[cTile] != null) && (lma_ds[cTile][1]> 0.0)) {
-													double d = lma_ds[cTile][0] + disparity_array[tileY][tileX] + disparity_corr;
 													double w = lma_ds[cTile][1];
-													sum_wd += w * d;
+													lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP] += (lma_ds[cTile][0] + disparity_array[tileY][tileX] + disparity_corr) * w;
+													lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] += (disparity_array[tileY][tileX] + disparity_corr) * w;
+													lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF] += lma_ds[cTile][0] * w;
+													lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] += pxpy[cTile][0] * w;
+													lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] += pxpy[cTile][1] * w;
+													for (int cam = 0; cam < quad; cam++) {
+														lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] += disp_dist[cTile][cam][2] * w;
+													}
 													sum_w += w;
 												}
 											}
@@ -2277,20 +2288,34 @@ public class ImageDtt {
 									}
 								}
 								if (sum_w > 0.0) {
-									lazy_eye_data[nCluster] = new double [2+ 2 * ddnd.length];
-									lazy_eye_data[nCluster][0] = sum_wd / sum_w;
-									lazy_eye_data[nCluster][1] = stats[0];
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_STRENGTH] = stats[0];
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP]   /= sum_w;
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] /= sum_w;
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF]   /= sum_w;
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] /= sum_w;
+									lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] /= sum_w;
+									for (int cam = 0; cam < quad; cam++) {
+										lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] /= sum_w;
+									}
+
 									for (int cam = 0; cam < ddnd.length; cam++) {
 										if (ddnd[cam] != null) { //convert to x,y from dd/nd
-											lazy_eye_data[nCluster][2 * cam + 2] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
-											lazy_eye_data[nCluster][2 * cam + 3] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
+											lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
+											lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
+											lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = ddnd[cam][0];
+											lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = ddnd[cam][1];
 										} else {
-											lazy_eye_data[nCluster][2 * cam + 2] = Double.NaN;
-											lazy_eye_data[nCluster][2 * cam + 3] = 0.0;
+											lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
+											lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
+											lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = Double.NaN;
+											lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = Double.NaN;
 										}
 									}
+								} else {
+									lazy_eye_data[nCluster] = null;
 								}
-
+// just for debugging, can be removed
+								/*
 								double [][] lma2_ds = lma2.lmaDisparityStrength(
 					    				imgdtt_params.lma_max_rel_rms,  // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
 					    				imgdtt_params.lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
@@ -2355,6 +2380,7 @@ public class ImageDtt {
 										}
 									}
 								}
+								*/
 							}
 						}
 					}