Debugging LY adjustment, tested semi-manual OK

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

@@ -292,6 +292,8 @@ public class CLTParameters {
 	public double     ly_norm_disp =    5.0;     // Reduce weight of higher disparity tiles
 
 	// Lazy eye multi-step fitting
+	public String     lym_dbg_path =   "";      // read saved extrinsics_bgnd_combo.tif to bypass long preparation
+	
 	public double     lym_overexp     = 0.0001;  // Any (near) saturated pixels - discard tile (see sat_level also)
 	public boolean    lym_update_disp = true;    // Update target disparity after each step
 	public int        lym_iter =        25;      // Maximal number of iterations
@@ -1188,6 +1190,9 @@ public class CLTParameters {
 		properties.setProperty(prefix+"ly_disp_rvar_gt",            this.ly_disp_rvar_gt +"");
 		properties.setProperty(prefix+"ly_norm_disp",               this.ly_norm_disp +"");
 		properties.setProperty(prefix+"lym_overexp",                this.lym_overexp +"");
+
+		properties.setProperty(prefix+"lym_dbg_path",                this.lym_dbg_path +"");
+		
 		properties.setProperty(prefix+"lym_update_disp",            this.lym_update_disp+"");
 		properties.setProperty(prefix+"lym_iter",                   this.lym_iter+"");
 		properties.setProperty(prefix+"lym_change",                 this.lym_change +"");
@@ -1997,6 +2002,7 @@ public class CLTParameters {
 		if (properties.getProperty(prefix+"ly_disp_rvar_gt")!=null)               this.ly_disp_rvar_gt=Double.parseDouble(properties.getProperty(prefix+"ly_disp_rvar_gt"));
 		if (properties.getProperty(prefix+"ly_norm_disp")!=null)                  this.ly_norm_disp=Double.parseDouble(properties.getProperty(prefix+"ly_norm_disp"));
 		if (properties.getProperty(prefix+"lym_overexp")!=null)                   this.lym_overexp=Double.parseDouble(properties.getProperty(prefix+"lym_overexp"));
+		if (properties.getProperty(prefix+"lym_dbg_path")!=null)                  this.lym_dbg_path= (String) properties.getProperty(prefix+"lym_dbg_path");
 		if (properties.getProperty(prefix+"lym_update_disp")!=null)               this.lym_update_disp=Boolean.parseBoolean(properties.getProperty(prefix+"lym_update_disp"));
 		if (properties.getProperty(prefix+"lym_iter")!=null)                      this.lym_iter=Integer.parseInt(properties.getProperty(prefix+"lym_iter"));
 		if (properties.getProperty(prefix+"lym_change")!=null)                    this.lym_change=Double.parseDouble(properties.getProperty(prefix+"lym_change"));
@@ -2895,6 +2901,8 @@ public class CLTParameters {
 				"Full allowed mismatch is a sum of absolute and disparity times relative (relaxed when ground truth is available)");
 		gd.addNumericField("Reduce weight of higher disparity tiles",                                           this.ly_norm_disp, 5,8,"");
 		gd.addMessage     ("--- Lazy eye multi-step fitting ---");
+
+		gd.addStringField ("Read extrinsics_bgnd_combo instead of building it",                                 this.lym_dbg_path, 80);
 		gd.addNumericField("Any (near) saturated pixels - discard tile (see sat_level also)",                   this.lym_overexp,  10,12,"");
 		gd.addCheckbox    ("Update target disparity after each step",                                           this.lym_update_disp);
 		gd.addNumericField("Maximal number of iterations",                                                      this.lym_iter,  0);
@@ -3833,6 +3841,8 @@ public class CLTParameters {
 		this.ly_disp_var_gt=        gd.getNextNumber();
 		this.ly_disp_rvar_gt=       gd.getNextNumber();
 		this.ly_norm_disp=          gd.getNextNumber();
+		
+		this.lym_dbg_path =         gd.getNextString();
 		this.lym_overexp=           gd.getNextNumber();
 		this.lym_update_disp=       gd.getNextBoolean();
 		this.lym_iter=        (int) gd.getNextNumber();

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

@@ -902,8 +902,10 @@ public class CLTPass3d{
 		 */
 		public void updateDisparity()
 		{
+			if (disparity_map != null) {
 				setTileOpDisparity(null, getDisparity(0));
 			}
+		}
 
 
 		public double [] getSecondMaxDiff (

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

@@ -1101,8 +1101,12 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 				}
 			}
 			if (par_list.size() < par_mask.length) {
-				int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
-				from_sym = from_sym.getMatrix(0, par_mask.length,sub_pars); // remove masked columns
+				int[] sub_pars = new int [par_list.size()];
+				for (int i = 0; i < sub_pars.length; i++) {
+					sub_pars[i] = par_list.get(i); 
+				}
+//				int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
+				from_sym = from_sym.getMatrix(0, par_mask.length-1,sub_pars); // remove masked columns
 			}
 		}
 		Matrix pcd = new Matrix (port_coord_deriv); // rows: px0,py0,... px[n-1], py[n-1], columns: tar
@@ -1187,7 +1191,7 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 			}
 			if (par_list.size() < par_mask.length) {
 				int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
-				to_sym = to_sym.getMatrix(sub_pars, 0, par_mask.length); // remove masked rows
+				to_sym = to_sym.getMatrix(sub_pars, 0, par_mask.length-1); // remove masked rows
 			}
 		}
 		Matrix tar = new Matrix(tar_array,tar_array.length);
@@ -1209,7 +1213,7 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 			}
 			if (par_list.size() < par_mask.length) {
 				int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
-				from_sym = from_sym.getMatrix(0, par_mask.length,sub_pars); // remove masked columns
+				from_sym = from_sym.getMatrix(0, par_mask.length-1,sub_pars); // remove masked columns
 			}
 		}
 		Matrix sym = new Matrix(sym_array,sym_array.length);

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

@@ -102,11 +102,11 @@ public class ExtrinsicAdjustment {
 	private CorrVector corr_vector = null;
 	private boolean []        par_mask =        null;
 	private boolean           use_rig_offsets = false;
-	private double [][]       measured_dsxy =   null;
+	private double [][]       measured_dsxy =   null; // only set in solveCorr()
 //	private double [][]       dy_ddisparity =   null; // conveniently extracted from  dsdn
-	private double [][]       pY_offset =    null; // conveniently extracted from  dsdn - per-sensor average pY to calculate ERS offset 
+	private double [][]       pY_offset =       null; // conveniently extracted from  dsdn - per-sensor average pY to calculate ERS offset (set in getXYNondistorted())
 	private double [][]       x0y0 =            null; //
-	private double[][]        world_xyz =       null;
+	private double[][]        world_xyz =       null; // only set in solveCorr()
 	private double []         weight_window =   null; // center area is more reliable
 
 	public final GeometryCorrection geometryCorrection;
@@ -296,7 +296,13 @@ public class ExtrinsicAdjustment {
 	*/
 	
 	private void showX0Y0(double [][] xy0, String title) {
-		String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"};
+		int num_sensors = this.geometryCorrection.getNumSensors();
+//		String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"};
+		String [] titles = new String [ 2 * num_sensors];
+		for (int i = 0; i < num_sensors; i++) {
+			titles[2*i + 0] = "xnd-"+i;
+			titles[2*i + 1] = "ynd-"+i;
+		}
 		int clusters = clustersX * clustersY;
 		double [][] pixels = new double [titles.length][clusters];
 		for (int cluster = 0; cluster < clusters; cluster++) {
@@ -392,7 +398,7 @@ public class ExtrinsicAdjustment {
 		world_xyz =  getWorldXYZ(); // freeze world coordinates for measured pX,pY and disparity
 		// calculate x,y non-distorted offsets for current correction vectors (to subtract from the new (modified) values
 		// 0/0 in the center (in the optical center), in pixels 
-		x0y0 = getXYNondistorted(
+		x0y0 = getXYNondistorted( // Sets pY_offset[], needed for .getPortsDDNDDerivativesNew() (in getJacobianTransposed)
 				corr_vector,
 				true); // boolean set_dydisp)
 
@@ -418,7 +424,7 @@ public class ExtrinsicAdjustment {
 		
 		double [] dfe = null;
 		if (max_dfe > 0) {
-			dfe = distanceFromEdge (
+			dfe = distanceFromEdge ( // all 0? too little, non-continuous
 					force_disparity,
 					measured_dsxy,
 					min_dfe, // 1.75
@@ -434,7 +440,7 @@ public class ExtrinsicAdjustment {
 					force_disparity,    // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
 					dfe,                // double [] distance_from_edge,
 					en_infinity_tilt,   // boolean en_tilt,  // consider right/left infinity tilt (will be disabled if more than *abs difference over width)
-					min_num_forced,     // int min_in_half,
+					min_num_forced/4,   // int min_in_half,
 					inf_min_disp_abs,   // double min_infinity_abs,
 					inf_max_disp_abs,   // double max_infinity_abs,
 					inf_min_disparity,  // double min_infinity,
@@ -467,7 +473,7 @@ public class ExtrinsicAdjustment {
 				 max_disparity_far,   // 					double max_disparity_far)     // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
 				 max_disparity_use);
 		if (use_disparity) {
-			if (inf_stat[0] < min_num_forced) {
+			if (inf_stat[0] < min_num_forced) { // 4
 				System.out.println("Too few infinity tiles ("+inf_stat[0]+"<"+(min_num_forced)+") to adjust disparity");
 				// disable parameters...all extrinsic
 				use_disparity =    false;
@@ -475,7 +481,7 @@ public class ExtrinsicAdjustment {
 				use_diff_rolls =   false;
 				common_roll =      false;
 				corr_focalLength = false;
-			} else if (inf_stat[1] < min_num_forced/2) {
+			} else if ((inf_stat[1] < min_num_forced/4) || (inf_stat[1] < 1)) { 
 				System.out.println("Too few infinity tiles ("+inf_stat[1]+"<"+(min_num_forced/2)+") in one half to balance right/left");
 				// disable parameters: all extrinsic but disparity (S0)
 				use_aztilts =      false;
@@ -821,7 +827,7 @@ public class ExtrinsicAdjustment {
 	private boolean [] filterInfinity(
 			double  [][] measured_dsxy,
 			boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
-			double [] distance_from_edge,
+			double [] distance_from_edge, // all 0?
 			boolean en_tilt,  // consider right/left infinity tilt (will be disabled if more than *abs difference over width)
 			int min_in_half,
 			double min_infinity_abs,
@@ -829,6 +835,9 @@ public class ExtrinsicAdjustment {
 			double min_infinity,
 			double max_infinity
 			) {
+		if (min_in_half < 1) {
+			min_in_half = 1;
+		}
 		int clusters = clustersX * clustersY;
 	    double half_width = 0.5 * clustersX;
 		boolean [] true_infinity = new boolean[clusters];
@@ -1584,17 +1593,18 @@ public class ExtrinsicAdjustment {
 		}
 		if (graphic) {
 			String [] titles3 = new String[num_pars * 3];
-
-			int width  = 3 * clustersX + 2 * gap;
-			int height = 3 * clustersY + 2 * gap;
+			int rows = getRowsCols()[0];
+			int cols = getRowsCols()[1];
+			int width  = cols * (clustersX + gap) - gap;
+			int height = rows * (clustersY + gap) - gap;
 			double [][] dbg_img = new double [num_pars * 3][width*height];
 			for (int par = 0; par < num_pars; par++) {
 				titles3[3 * par + 0] = titles[par]+"";
 				titles3[3 * par + 1] = titles[par]+"_delta";
 				titles3[3 * par + 2] = titles[par]+"_diff";
 				for (int mode = 0; mode < points_sample; mode++) {
-					int x0 = (mode % 3) * (clustersX + gap);
-					int y0 = (mode / 3) * (clustersY + gap);
+					int x0 = (mode % cols) * (clustersX + gap);
+					int y0 = (mode / cols) * (clustersY + gap);
 					for (int cluster = 0; cluster < clusters;  cluster++) {
 						int x = x0 + (cluster % clustersX);
 						int y = y0 + (cluster / clustersX);
@@ -1624,8 +1634,8 @@ public class ExtrinsicAdjustment {
 				dbg_img = new double [num_pars][width*height];
 				for (int par = 0; par < num_pars; par++) {
 					for (int mode = 0; mode < points_sample; mode++) {
-						int x0 = (mode % 3) * (clustersX + gap);
-						int y0 = (mode / 3) * (clustersY + gap);
+						int x0 = (mode % cols) * (clustersX + gap);
+						int y0 = (mode / cols) * (clustersY + gap);
 						for (int cluster = 0; cluster < clusters;  cluster++) {
 							int x = x0 + (cluster % clustersX);
 							int y = y0 + (cluster / clustersX);
@@ -1659,12 +1669,14 @@ public class ExtrinsicAdjustment {
 		int gap = 10; //pix
 		int clusters = clustersX * clustersY;
 		String [] titles = {"Y", "-fX", "Y+fx"};
-		int width  = 3 * clustersX + 2 * gap;
-		int height = 3 * clustersY + 2 * gap;
+		int rows = getRowsCols()[0];
+		int cols = getRowsCols()[1];
+		int width  = cols * (clustersX + gap) - gap;
+		int height = rows * (clustersY + gap) - gap;
 		double [][] dbg_img = new double [3][width*height];
 		for (int mode = 0; mode < points_sample; mode++) {
-			int x0 = (mode % 3) * (clustersX + gap);
-			int y0 = (mode / 3) * (clustersY + gap);
+			int x0 = (mode % cols) * (clustersX + gap);
+			int y0 = (mode / cols) * (clustersY + gap);
 			for (int cluster = 0; cluster < clusters;  cluster++) {
 				int x = x0 + (cluster % clustersX);
 				int y = y0 + (cluster / clustersX);
@@ -1998,6 +2010,14 @@ public class ExtrinsicAdjustment {
 	
 	
 	
+	public int [] getRowsCols() {
+		int rows = (int) Math.round(Math.sqrt(points_sample));
+		int cols = points_sample/rows;
+		if (rows*cols < points_sample) {
+			cols++;
+		}
+		return new int [] {rows,cols};
+	}
 
 	private void dbgYminusFxWeight(
 			double []   fx,
@@ -2009,12 +2029,14 @@ public class ExtrinsicAdjustment {
 		int gap = 10; //pix
 		int clusters = clustersX * clustersY;
 		String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"};
-		int width  = 3 * clustersX + 2 * gap;
-		int height = 3 * clustersY + 2 * gap;
+		int rows = getRowsCols()[0];
+		int cols = getRowsCols()[1];
+		int width  = cols * (clustersX + gap) - gap;
+		int height = rows * (clustersY + gap) - gap;
 		double [][] dbg_img = new double [titles.length][width*height];
 		for (int mode = 0; mode < points_sample; mode++) {
-			int x0 = (mode % 3) * (clustersX + gap);
-			int y0 = (mode / 3) * (clustersY + gap);
+			int x0 = (mode % cols) * (clustersX + gap);
+			int y0 = (mode / cols) * (clustersY + gap);
 			for (int cluster = 0; cluster < clusters;  cluster++) {
 				int x = x0 + (cluster % clustersX);
 				int y = y0 + (cluster / clustersX);
@@ -2053,7 +2075,12 @@ public class ExtrinsicAdjustment {
 						}
 					}
 				} else {
-					dbg_img[0][pix] =  Double.NaN;
+					if (pix >= dbg_img[0].length) {
+						System.out.println("pix="+pix+" >= "+dbg_img[0].length);
+						System.out.println("pix="+pix+" >= "+dbg_img[0].length);
+						System.out.println("pix="+pix+" >= "+dbg_img[0].length);
+					}
+					dbg_img[0][pix] =  Double.NaN; //Index 16240 out of bounds for length 16240
 					dbg_img[1][pix] =  Double.NaN;
 					dbg_img[2][pix] =  Double.NaN;
 					dbg_img[3][pix] =  Double.NaN;
@@ -2078,15 +2105,17 @@ public class ExtrinsicAdjustment {
 		int gap = 10; //pix
 		int clusters = clustersX * clustersY;
 		String [] titles = {"meas", "correction", "diff"};
-		int width  = 3 * clustersX + 2 * gap;
-		int height = 3 * clustersY + 2 * gap;
+		int rows = getRowsCols()[0];
+		int cols = getRowsCols()[1];
+		int width  = cols * (clustersX + gap) - gap;
+		int height = rows * (clustersY + gap) - gap;
 		double [][] xy = getXYNondistorted(corr_vector, false);
 
 		double [][] dbg_img = new double [3][width*height];
 		double [][] moving_objects = new double [3][clusters];
 		for (int mode = 0; mode < 2 * num_sensors + 1; mode++) {
-			int x0 = (mode % 3) * (clustersX + gap);
-			int y0 = (mode / 3) * (clustersY + gap);
+			int x0 = (mode % cols) * (clustersX + gap);
+			int y0 = (mode / cols) * (clustersY + gap);
 			for (int cluster = 0; cluster < clusters;  cluster++) {
 				int x = x0 + (cluster % clustersX);
 				int y = y0 + (cluster / clustersX);
@@ -2142,6 +2171,7 @@ public class ExtrinsicAdjustment {
 //			Mismatch mm = mismatch_list.get(indx);
 //			double [] pXY = mm.getPXY();
 			// will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters
+			// Now 2*num_sensors+1
 			double [][] deriv = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir
 					geometryCorrection,     // GeometryCorrection gc_main,
 					use_rig_offsets,        // boolean     use_rig_offsets,
@@ -2156,7 +2186,7 @@ public class ExtrinsicAdjustment {
 ///			int dbg_index = cluster; // dbg_index (pXY, dbg_decimate);
 			// convert to symmetrical coordinates
 			// derivatives of each port coordinates (in pixels) for each of selected symmetric all parameters (sym0 is convergence for disparity)
-			 double [][] jt_partial = corr_vector.getJtPartial(
+			 double [][] jt_partial = corr_vector.getJtPartial( // extract common?
 						deriv, // double [][] port_coord_deriv,
 						par_mask); // boolean [] par_mask
 
@@ -2286,7 +2316,7 @@ public class ExtrinsicAdjustment {
 		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
 			this.last_jt =  getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
 			this.last_ymfx = getFx(corr_vector);
-			if (debug_level > -1) { // temporary
+			if (debug_level > -2) { // temporary
 				dbgYminusFxWeight(
 						this.last_ymfx,
 						this.weights,
@@ -2300,8 +2330,14 @@ public class ExtrinsicAdjustment {
 					this.last_ymfx); // modifies this.last_ymfx (weights and subtracts fx)
 			this.initial_rms = this.last_rms.clone();
 			this.good_or_bad_rms = this.last_rms.clone();
+			if (debug_level > -1) {
+				showDerivatives(0);
+				showDerivatives(1);
+				showDerivatives(2);
+				showDerivatives(3);
+			}
 			// TODO: Restore/implement
-			if (debug_level > 3) {
+			if (debug_level > 3) { // compares true jacobians and calculated with delta (same for all parameters)
 				 dbgJacobians(
 							corr_vector, // CorrVector corr_vector,
 							1E-5, // double delta,
@@ -2409,4 +2445,101 @@ public class ExtrinsicAdjustment {
 		}
 		return rslt;
 	}
+	
+	public void showDerivatives(int typ4) {
+		// typ == 0 -> DDND, 1 - XT
+		int typ = typ4 % 2;
+		boolean use_sym = typ4 > 1;
+		int gap = 10;
+		int clusters = clustersX * clustersY;
+		//		int num_pars = getNumPars(); // only used
+		int num_pars = corr_vector.toArray().length; // all parameters
+		String [] titles = new String [num_pars]; //ea.getSymNames();
+		for (int i = 0; i < num_pars; i++) {
+			titles[i] = "S"+i;
+		}
+		Matrix []   corr_rots =  corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
+		Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
+		Matrix from_sym = corr_vector.symmVectorsSet.from_sym;
+		double [] imu = corr_vector.getIMU(); //
+		int rows = getRowsCols()[0];
+		int cols = getRowsCols()[1];
+		int width  = cols * (clustersX + gap) - gap;
+		int height = rows * (clustersY + gap) - gap;
+		double [][] dbg_img = new double [num_pars][width*height];
+		//		double [][][] derivs = new double [points_sample][][];
+		double [][][] derivs = new double [clusters][][];
+		for (int cluster = 0; cluster < clusters;  cluster++) if (measured_dsxy[cluster] != null){
+			//			Mismatch mm = mismatch_list.get(indx);
+			//			double [] pXY = mm.getPXY();
+			// will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters
+			// Now 2*num_sensors+1
+			if (typ == 0) {
+				derivs[cluster] = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir
+						this.geometryCorrection,     // GeometryCorrection gc_main,
+						this.use_rig_offsets,        // boolean     use_rig_offsets,
+						corr_rots,              // Matrix []   rots,
+						deriv_rots,             // Matrix [][] deriv_rots,
+						this.pY_offset[cluster],     // double []   py_offset,  // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
+						imu,                    // double []   imu,
+						this.world_xyz[cluster],                  // double []   xyz, // world XYZ for ERS correction
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double      px,
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double      py,
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double      disparity);
+			} else {
+				derivs[cluster] = geometryCorrection.getPortsXYDerivativesNew( // USED in lwir
+						this.geometryCorrection,     // GeometryCorrection gc_main,
+						this.use_rig_offsets,        // boolean     use_rig_offsets,
+						corr_rots,              // Matrix []   rots,
+						deriv_rots,             // Matrix [][] deriv_rots,
+						this.pY_offset[cluster],     // double []   py_offset,  // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
+						imu,                    // double []   imu,
+						this.world_xyz[cluster],                  // double []   xyz, // world XYZ for ERS correction
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double      px,
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double      py,
+						this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double      disparity);
+				
+			}
+			// optionally convert to use sym vectors
+			if (use_sym) {
+				Matrix derivs_tarz = new Matrix(derivs[cluster]);
+				Matrix derivs_sym = derivs_tarz.times(from_sym);
+				derivs[cluster] = derivs_sym.getArray();
+			}
+		}
+		for (int par = 0; par < num_pars; par++) {
+			for (int mode = 0; mode < points_sample; mode++) {
+				int x0 = (mode % cols) * (clustersX + gap);
+				int y0 = (mode / cols) * (clustersY + gap);
+				for (int cluster = 0; cluster < clusters;  cluster++) {
+					int x = x0 + (cluster % clustersX);
+					int y = y0 + (cluster / clustersX);
+					int pix = x + y * width;
+//					int indx = (mode == 0) ? INDX_DIFF : (indx_dd0 + mode - 1);
+					if (typ == 0) {
+						double d = (derivs[cluster] == null)? Double.NaN : derivs[cluster][mode][par];
+						if (mode == 0) {
+							dbg_img[par][pix] = -d;
+						} else {
+							dbg_img[par][pix] = d;
+						}
+					} else {
+						if (mode == 0) {
+							dbg_img[par][pix] = Double.NaN; // skip
+						} else {
+							dbg_img[par][pix] = (derivs[cluster] == null)? Double.NaN : derivs[cluster][mode-1][par];
+						}
+					}
+				}
+			}
+		}
+		String title = ((typ == 0)?"dDND_dpar":"dXY_dpar") + (use_sym? "_sym":"_tarz");
+		(new ShowDoubleFloatArrays()).showArrays(
+				dbg_img,
+				width,
+				height,
+				true,
+				title, // +(update_disparity?"U":""),
+				(use_sym? titles: corr_vector.getCorrNames())); // titles);
+	}
 }

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

@@ -2806,6 +2806,35 @@ public class GeometryCorrection {
 		return jt;
 	}
 
+	public double [][] getPortsXYDerivativesNew( // USED in lwir
+			GeometryCorrection gc_main,
+			boolean     use_rig_offsets,
+			Matrix []   rots,
+			Matrix [][] deriv_rots,
+			double []   py_offset,  // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
+			double []   imu, // may be null
+			double []   xyz, // world XYZ for ERS correction
+			double      px,
+			double      py,
+			double      disparity)
+	{
+		// make sure initPrePostMatrices(true) already ran (in constructor). (true) means that minus sign is already incorporated
+		double [][] pXYNDderiv = new double [2*numSensors][]; // CorrVector.LENGTH][];
+		getPortsNonDistortedCoordinatesAndDerivativesNew( // USED in lwir
+				gc_main,
+				use_rig_offsets,
+				rots,
+				deriv_rots,
+				pXYNDderiv, // if not null, should be double[8][]
+				py_offset,   //  // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
+				imu,
+				xyz,
+				px,
+				py,
+				disparity);
+		return(new Matrix(pXYNDderiv)).getArray();
+	}
+
 	
 
 

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

@@ -4042,6 +4042,28 @@ public class ImageDttCPU {
 										disparity_map[DISPARITY_INDEX_POLY+1][tIndex] = ds[0][1];
 										if (debugTile0) {
 											lma2.printStats(ds,1);
+											double [][] ddnd = lma2.getDdNd();
+											if (ddnd != null) {
+												double [][] dxy= new double [ddnd.length][2];
+												for (int i = 0; i < dxy.length; i++) {
+													dxy[i][0] = ddnd[i][0] * rXY[i][0] - ddnd[i][1] * rXY[i][1];
+													dxy[i][1] = ddnd[i][0] * rXY[i][1] + ddnd[i][1] * rXY[i][0];
+												}
+												System.out.print("       Port:  ");
+												for (int i = 0; i < dxy.length; i++) System.out.print(String.format("   %2d   ", i)); System.out.println();
+												System.out.print("Radial_in =  [");
+												for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", ddnd[i][0])); System.out.println("]");
+												System.out.print("Tangent_CW = [");
+												for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", ddnd[i][1])); System.out.println("]");
+												System.out.print("X =          [");
+												for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", dxy[i][0])); System.out.println("]");
+												System.out.print("Y =          [");
+												for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", dxy[i][1])); System.out.println("]");
+												System.out.println();
+											}
+											
+											
+											
 										}
 									}
 								}

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

@@ -6028,9 +6028,9 @@ public class QuadCLTCPU {
 			  float [][] rgba = new float [num_slices][];
 			  for (int i = 0; i < 3; i++) rgba[i] = new float [iclt_data[green_index].length];
 			  for (int i = 0; i < rbg_in[green_index].length; i++) {
-				  if (i == 700) {
-					  System.out.println("linearStackToColor(): i="+i);
-				  }
+//				  if (i == 700) {
+//					  System.out.println("linearStackToColor(): i="+i);
+//				  }
 				  float [] rgb = tc.getRGB(iclt_data[green_index][i]);
 				  rgba[0][i] = rgb[0]; // red
 				  rgba[1][i] = rgb[1]; // green
@@ -6147,9 +6147,9 @@ public class QuadCLTCPU {
 			  double [][] rgba = new double [num_slices][];
 			  for (int i = 0; i < 3; i++) rgba[i] = new double [iclt_data[green_index].length];
 			  for (int i = 0; i < rbg_in[green_index].length; i++) {
-				  if (i == 700) {
-					  System.out.println("linearStackToColor(): i="+i);
-				  }
+//				  if (i == 700) {
+//					  System.out.println("linearStackToColor(): i="+i);
+//				  }
 				  double [] rgb = tc.getRGB(iclt_data[green_index][i]);
 				  rgba[0][i] = rgb[0]; // red
 				  rgba[1][i] = rgb[1]; // green
@@ -8158,8 +8158,78 @@ public class QuadCLTCPU {
 			  double inf_max, //  =  1.0;
 			  final int        threadsMax,  // maximal number of threads to launch
 			  final boolean    updateStatus,
+			  final int        debugLevel) {
+		  return extrinsicsCLT( 
+				  clt_parameters,
+				  null, // String           dbg_path, // if not null - read extrinsics_bgnd_combo file instead of extrinsics_prepare
+				  adjust_poly,
+				  inf_min, //  = -1.0;
+				  inf_max, //  =  1.0;
+				  threadsMax,  // maximal number of threads to launch
+				  updateStatus,
+				  debugLevel);
+	  }
+	  
+	  public boolean getPreparedExtrinsics(String path) {
+		  String [] titles =  {"bgnd_disp","bgnd_str","combo_disp","combo_str","bg_sel","bg_use","combo_use"};
+		  int exp_slices = titles.length;
+		  if (path == null) {return false;}
+		  ImagePlus img_extrinsics_bgnd_combo = new ImagePlus(path);
+		  ImageStack stack_extrinsics_bgnd_combo = img_extrinsics_bgnd_combo.getStack();
+		  int nSlices=stack_extrinsics_bgnd_combo.getSize();
+		  int width = img_extrinsics_bgnd_combo.getWidth();
+		  int height = img_extrinsics_bgnd_combo.getHeight();
+		  if (nSlices != exp_slices) {
+			  throw new IllegalArgumentException ("getPreparedExtrinsics(): Expected "+exp_slices+" in "+path+", got "+nSlices);
+		  }
+		  double [][] data = new double [nSlices][width*height];
+		  for (int slice = 0; slice < nSlices; slice ++) {
+			  float [] pixels = (float []) stack_extrinsics_bgnd_combo.getPixels(slice+1);
+			  for (int i = 0; i < pixels.length; i++) {
+				  data[slice][i] = pixels[i];
+			  }
+		  }
+		  boolean [] bg_sel =    new boolean [width*height];
+		  boolean [] bg_use =    new boolean [width*height];
+		  boolean [] combo_use = new boolean [width*height];
+		  for (int i = 0; i < bg_sel.length; i++) {
+			  bg_sel[i] = data[4][i] > 0; // NaN OK
+			  bg_use[i] = data[5][i] > 0; // NaN OK
+			  combo_use[i] = data[6][i] > 0; // NaN OK
+		  }
+		  
+		  CLTPass3d bg_scan =    new CLTPass3d(tp, 0 );
+		  CLTPass3d combo_scan = new CLTPass3d(tp, 0 );
+		  int op = ImageDtt.setImgMask(0, 0xf);
+		  op =     ImageDtt.setPairMask(op,0xf);
+		  op =     ImageDtt.setForcedDisparity(op,true);
+		  for (int ty = 0; ty < height; ty++) {
+			  for (int tx = 0; tx < width; tx++) {
+				  int indx = ty*width+tx;
+				  bg_scan.tile_op[ty][tx] =      bg_use[indx]? op: 0;
+				  combo_scan.tile_op[ty][tx] =   combo_use[indx]? op: 0;
+//				  bg_scan.disparity[ty][tx] =    bg_use[indx]? data[0][indx]: Double.NaN;
+				  bg_scan.disparity[ty][tx] =    bg_use[indx]? 0.0: Double.NaN;
+				  combo_scan.disparity[ty][tx] = combo_use[indx]? data[2][indx]: Double.NaN;
+			  }
+		  }
+		  tp.clt_3d_passes.add(bg_scan);
+		  tp.clt_3d_passes.add(combo_scan);
+		  return true;
+	  }
+	  
+	  public boolean extrinsicsCLT( 
+			  CLTParameters           clt_parameters,
+			  String           dbg_path, // if not null - read extrinsics_bgnd_combo file instead of extrinsics_prepare
+			  boolean 		   adjust_poly,
+			  double inf_min, //  = -1.0;
+			  double inf_max, //  =  1.0;
+			  final int        threadsMax,  // maximal number of threads to launch
+			  final boolean    updateStatus,
 			  final int        debugLevel)
 	  {
+		  boolean got_saved = getPreparedExtrinsics(dbg_path);
+		  if (!got_saved) {
 			  extrinsics_prepare( 
 					  clt_parameters,
 					  inf_min, //  = -1.0;
@@ -8167,9 +8237,9 @@ public class QuadCLTCPU {
 					  threadsMax,  // maximal number of threads to launch
 					  updateStatus,
 					  debugLevel);
+		  }
 		  
-		  
-		  final boolean    batch_mode = clt_parameters.batch_run;
+		  final boolean    batch_mode = false; // clt_parameters.batch_run;
 		  int debugLevelInner =  batch_mode ? -5: debugLevel;
 		  boolean update_disp_from_latest = clt_parameters.lym_update_disp ; // true;
 		  int max_tries =                   clt_parameters.lym_iter; // 25;
@@ -8254,7 +8324,7 @@ public class QuadCLTCPU {
 							  false, // boolean                 update_disparity, // re-measure disparity before measuring LY
 							  threadsMax, // final int        threadsMax,  // maximal number of threads to launch
 							  updateStatus, //final boolean    updateStatus,
-							  1E-3, // double           delta,
+							  1E-2, // 1E-3, // double           delta,
 							  debugLevel); // final int        debugLevel)				  
 				  }
 				  
@@ -12465,31 +12535,62 @@ public class QuadCLTCPU {
 			  double                  delta,
 			  final int               debugLevel)
 	  {   
-		  double [] parameter_scales = { // multiply delay for each parameter
-				  0.3,  // 0.014793657667505566, // 00 10
-				  0.3,  // 0.015484017460841183, // 01 10
-				  0.3,  // 0.02546712771769517,  // 02 10
-				  0.3,  // 0.02071573747995167,  // 03 10
-				  0.3,  // 0.026584237444512468, // 04 10
-				  0.3,  // 0.014168012698804967, // 05 10
-				  2.0,  // 1.8554483718240792E-4, // 06
-				  0.3, //2.3170738149889717E-4, // 07
-				  0.3, //3.713239026512266E-4,  // 08
-				  0.3, //2.544834643007531E-4,  // 09
-				  0.3, // 2.5535557646736286E-4, // 10 
-				  0.3, // 1.98531249109261E-4,   // 11
-				  0.3, // 2.1802727086879284E-4, // 12
-				  150, // 8.814346720176489E-1,  // 5,  // 13 10000x
-				  150, // 7.071297501674136E-1,  // 5,  // 14 10000x
-				  150, // 1.306306793587865E-0,  // 4,  // 15 10000x
-				  300, // 2.8929916645453735E-0, // 4, // 16 10000x
-				  300, // 2.943408022525927E-0,  // 4,  // 17 10000x
-				  500.0}; // 390.6185365641268};    //4};  // 18 100000x 
+		  delta = 0.0003;
+		  /*double [] parameter_scales4 = { // multiply delay for each parameter
+				  0.3,  // 0.014793657667505566, // 00 10 tilt0
+				  0.3,  // 0.015484017460841183, // 01 10 tilt1
+				  0.3,  // 0.02546712771769517,  // 02 10 tilt2
+				  
+				  0.3,  // 0.02071573747995167,  // 03 10 az0
+				  0.3,  // 0.026584237444512468, // 04 10 az1
+				  0.3,  // 0.014168012698804967, // 05 10 az2
+				  
+				  2.0,  // 1.8554483718240792E-4,// 06 roll0
+				  0.3, //2.3170738149889717E-4,  // 07 roll1
+				  0.3, //3.713239026512266E-4,   // 08 roll2
+				  0.3, //2.544834643007531E-4,   // 09 roll3
+				  0.3, // 2.5535557646736286E-4, // 10 zoom0 
+				  0.3, // 1.98531249109261E-4,   // 11 zoom1
+				  0.3, // 2.1802727086879284E-4, // 12 zoom2
+				  
+				  150, // 8.814346720176489E-1,  // 5,  // 13 10000x omega-tilt
+				  150, // 7.071297501674136E-1,  // 5,  // 14 10000x omega az
+				  150, // 1.306306793587865E-0,  // 4,  // 15 10000x omega roll
+				  300, // 2.8929916645453735E-0, // 4,  // 16 10000x vx
+				  300, // 2.943408022525927E-0,  // 4,  // 17 10000x vy
+				  500.0}; // 390.6185365641268};    //4};  // 18 100000x vz
+				  */
+		  double scale_tl = 0.3;
+		  double scale_az = 0.3;
+		  double scale_rl0 = 2.0;
+		  double scale_rl = 0.3;
+		  double scale_zoom = 0.3;
+		  double [] scales_imu = {
+				  150, // 8.814346720176489E-1,  // 5,  // 13 10000x omega-tilt
+				  150, // 7.071297501674136E-1,  // 5,  // 14 10000x omega az
+				  150, // 1.306306793587865E-0,  // 4,  // 15 10000x omega roll
+				  300, // 2.8929916645453735E-0, // 4,  // 16 10000x vx
+				  300, // 2.943408022525927E-0,  // 4,  // 17 10000x vy
+				  500.0}; // 390.6185365641268};    //4};  // 18 100000x vz
 //		  delta = 0.001; // should be 0.001
 		  boolean debug_img = false;
 		  int debugLevelInner = -5;
 		  CLTPass3d scan = tp.clt_3d_passes.get(scanIndex);
 		  CorrVector corr_vector = geometryCorrection.getCorrVector().clone();
+//		  String [] corr_names = corr_vector.getCorrNames();
+		  int num_sensors=getNumSensors();
+		  double [] parameter_scales = new double [corr_vector.getLength()];	
+		  for (int i = 0; i < num_sensors; i++) {
+			  parameter_scales    [corr_vector.getRollIndex()+   i] = (i > 0) ? scale_rl : scale_rl0;
+			  if (i < num_sensors - 1) {
+				  parameter_scales[corr_vector.getTiltIndex()+   i]=scale_tl;
+				  parameter_scales[corr_vector.getAzimuthIndex()+i]=scale_az;
+				  parameter_scales[corr_vector.getZoomIndex()+   i]=scale_zoom;
+			  }
+		  }
+		  for (int i = 0; i < scales_imu.length; i++) {
+			  parameter_scales[corr_vector.getIMUIndex()+   i] = scales_imu[i];
+		  }		  
 		  double [] curr_corr_arr = corr_vector.toArray();
 		  int clusters = ea.clustersX * ea.clustersY;
 		  int num_ly = ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); // scan.getLazyEyeData().length;
@@ -12514,6 +12615,10 @@ public class QuadCLTCPU {
 					  ly_initial, // double[][] data,
 					  "drv_reference");// String title);
 		  }
+		  String [] titles = corr_vector.getCorrNames(); // new String [num_pars]; //ea.getSymNames(); // why "S" here, while it is tarz???
+//		  for (int i = 0; i < num_pars; i++) {
+//			  titles[i] = "S"+i;
+//		  }
 		  
 		  for (int npar = 0; npar < num_pars; npar++) {
 			  // perform asymmetric delta
@@ -12524,7 +12629,8 @@ public class QuadCLTCPU {
 			  corr_vectorp.incrementVector(corr_delta,  1.0); // 0.5 for p/m
 			  geometryCorrection.setCorrVector(corr_vectorp) ;
 			  double rdelta = 1.0/ par_inc[npar];
-			  System.out.println("S"+npar+" scale="+rdelta+"\n"+(geometryCorrection.getCorrVector().toString()));
+//			  System.out.println("S"+npar+" scale="+rdelta); // +"\n"+(geometryCorrection.getCorrVector().toString()));
+			  System.out.println(npar+": "+ titles[npar]+", scale="+rdelta); // +"\n"+(geometryCorrection.getCorrVector().toString()));
 			  gpuResetCorrVector();
 			  if (update_disparity) {
 				  CLTMeasureCorr( // perform single pass according to prepared tiles operations and disparity
@@ -12585,12 +12691,12 @@ public class QuadCLTCPU {
 		  }
 		  */
 		  int gap = 10;
-		  String [] titles = new String [num_pars]; //ea.getSymNames();
-		  for (int i = 0; i < num_pars; i++) {
-			  titles[i] = "S"+i;
-		  }
-		  int width  = 3 * ea.clustersX + 2 * gap;
-		  int height = 3 * ea.clustersY + 2 * gap;
+//		  int width  = 3 * ea.clustersX + 2 * gap;
+//		  int height = 3 * ea.clustersY + 2 * gap;
+		  int rows = ea.getRowsCols()[0];
+		  int cols = ea.getRowsCols()[1];
+		  int width  = cols * (ea.clustersX + gap) - gap;
+		  int height = rows * (ea.clustersY + gap) - gap;
 		  double [][] dbg_img = new double [num_pars][width*height];
 		  /*
 		  for (int par = 0; par < num_pars; par++) {
@@ -12617,8 +12723,8 @@ public class QuadCLTCPU {
 		  dbg_img = new double [num_pars][width*height];
 		  for (int par = 0; par < num_pars; par++) {
 			  for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
-				  int x0 = (mode % 3) * (ea.clustersX + gap);
-				  int y0 = (mode / 3) * (ea.clustersY + gap);
+				  int x0 = (mode % cols) * (ea.clustersX + gap);
+				  int y0 = (mode / cols) * (ea.clustersY + gap);
 				  for (int cluster = 0; cluster < clusters;  cluster++) {
 					  int x = x0 + (cluster % ea.clustersX);
 					  int y = y0 + (cluster / ea.clustersX);
@@ -12643,8 +12749,8 @@ public class QuadCLTCPU {
 		  dbg_img = new double [num_pars][width*height];
 		  for (int par = 0; par < num_pars; par++) {
 			  for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
-				  int x0 = (mode % 3) * (ea.clustersX + gap);
-				  int y0 = (mode / 3) * (ea.clustersY + gap);
+				  int x0 = (mode % cols) * (ea.clustersX + gap);
+				  int y0 = (mode / cols) * (ea.clustersY + gap);
 				  for (int cluster = 0; cluster < clusters;  cluster++) {
 					  int x = x0 + (cluster % ea.clustersX);
 					  int y = y0 + (cluster / ea.clustersX);

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

@@ -103,7 +103,7 @@ public class SymmVector {
 				full_type2,
 				debug_level);
 		rvs.xy =     sv.exportXY();
-		rvs.rt =     sv.exportRT();
+		rvs.rt =     sv.exportRT(); // radius/tangential (out, cw, out, cw
 		rvs.dir_rt = sv.exportDirRT();
 		rvs.rots =   sv.exportRZ(false); // include common roll
 		rvs.zooms =  sv.exportRZ(true);  // no common zoom
@@ -1153,7 +1153,8 @@ public class SymmVector {
 			xy2ta.set(    i, 2* i + 1, TILT_SIGN);
 			xy2ta.set(N-1+i, 2 *i + 0, AZIMUTH_SIGN);
 		}
-		return xy2ta.times(sym2xy);
+		Matrix sym2ta = xy2ta.times(sym2xy);
+		return sym2ta;
 	}
 	
 	/**
@@ -1195,6 +1196,7 @@ public class SymmVector {
 		to_sym.setMatrix(i1,i2-1,i1,i2-1, sym2roll.inverse());
 		to_sym.setMatrix(i2,i3-1,i2,i3-1, sym2zoom.inverse());
 		to_sym.setMatrix(i3,i4-1,i3,i4-1, sym2ers.inverse());
+//		Matrix dbg = from_sym.times(to_sym);
 		return new Matrix[] {from_sym,to_sym};
 	}
 	//getColumnDimension

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

@@ -9495,6 +9495,12 @@ if (debugLevel > -100) return true; // temporarily !
 					System.out.println("Building basic  DSI for the AUX camera image set "+quadCLT_main.image_name+
 							" using main camera DSI, pass "+(num_adjust_aux+1)+" of "+adjust_aux);
 				}
+			    String dbg_path = clt_parameters.lym_dbg_path; // /home/elphel/lwir16-proc/proc1/results_cuda/25/extrinsics_bgnd_combo.tif
+			    if (dbg_path.length()==0) {
+			    	dbg_path = null;
+			    }
+//			    dbg_path = "/home/elphel/lwir16-proc/proc1/results_cuda/25/extrinsics_bgnd_combo.tif";
+			    if (dbg_path == null) {
 			    	quadCLT_aux.preExpandCLTQuad3d( // returns ImagePlus, but it already should be saved/shown
 			    			imp_srcs_aux, // [srcChannel], // should have properties "name"(base for saving results), "channel","path"
 			    			saturation_imp_aux, // boolean [][] saturation_imp, // (near) saturated pixels or null
@@ -9513,23 +9519,25 @@ if (debugLevel > -100) return true; // temporarily !
 			    		System.out.println("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
 			    				", pass "+(num_adjust_aux+1)+" of "+adjust_aux);
 			    	}
+			    }
 				if (quadCLT_aux.ds_from_main == null) {
 					System.out.println("BUG: quadCLT_aux.ds_from_main should be not null here!");
 				    double inf_min = -1.0;
 				    double inf_max =  1.0;
 				    if (num_adjust_aux >= (adjust_aux/2)) {
 				        inf_min = -0.2;
-				        inf_max = 0.05;
+				        inf_max = 0.2;  // 0.05; Changed for LWIR16
 				    }
 					// adjust w/o main camera - maybe will be used in the future
 					boolean ok = quadCLT_aux.extrinsicsCLT(
 							clt_parameters, // EyesisCorrectionParameters.CLTParameters           clt_parameters,
+							dbg_path,
 							false, // adjust_poly,
 				            inf_min, // double inf_min,
 				            inf_max,  // double inf_max,
 							threadsMax,  //final int        threadsMax,  // maximal number of threads to launch
 							updateStatus,// final boolean    updateStatus,
-							debugLevelInner); // final int        debugLevel)
+							1); // debugLevelInner); // final int        debugLevel)
 					if (!ok) break;
 				} else {
 					boolean ok = quadCLT_aux.extrinsicsCLTfromGT(