Changed disp_dist format to [quad][4], implemented and connected new LMA

src/main/java/com/elphel/imagej/gpu/GPUTileProcessor.java

@@ -733,8 +733,8 @@ public class GPUTileProcessor {
 						// TODO: move port coordinates out of color channel loop
 						double [][] centersXY_main = null;
 						double [][] centersXY_aux =  null;
-						double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
-						double [][] disp_dist_aux =  new double[2 * quad_aux][]; // used to correct 3D correlations
+						double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
+						double [][] disp_dist_aux =  new double[quad_aux][]; // used to correct 3D correlations
 
 						if (geometryCorrection_main != null) {
 							centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(

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

@@ -3091,7 +3091,7 @@ B = |+dy0   -dy1      -2*dy3 |
 			double [] pXY = mm.getPXY();
 			double [][] deriv = new double [2 * NUM_SENSORS][];
 			int dbg_index =dbg_index (pXY, dbg_decimate);
-			double [][] disp_dist = new double[dbg_titles_xy.length][]; // used to correct 3D correlations
+			double [][] disp_dist = new double[NUM_SENSORS][]; // used to correct 3D correlations
 			geometryCorrection.getPortsCoordinatesAndDerivatives(
 					geometryCorrection, //			GeometryCorrection gc_main,
 					false,       // boolean use_rig_offsets,

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

@@ -114,7 +114,7 @@ public class Corr2dLMA {
     private int ncam = 0; // number of used cameras
     private int npairs=0; // number of used pairs
     private int     last_cam; // index of the last camera (special treatment for disparity correction)
-    private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
+//    private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
     private final Matrix [][] m_pairs =      new Matrix[NUM_CAMS][NUM_CAMS];
     private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
     private final int [][] pindx =           new int [NUM_CAMS][NUM_CAMS];
@@ -206,13 +206,13 @@ public class Corr2dLMA {
 		}
 	}
 	public void initVector( // USED in lwir
-			boolean adjust_width,     // adjust width of the maximum
-			boolean adjust_scales,    // adjust 2D correlation scales
-			boolean adjust_ellipse,   // allow non-circular correlation maximums
-			boolean adjust_lazyeye,   // adjust disparity corrections and orthogonal disparities
+			boolean adjust_width,     // adjust width of the maximum -                                           lma_adjust_wm
+			boolean adjust_scales,    // adjust 2D correlation scales -                                          lma_adjust_ag
+			boolean adjust_ellipse,   // allow non-circular correlation maximums                                 lma_adjust_wy
+			boolean adjust_lazyeye,   // adjust disparity corrections and orthogonal disparities                 lma_adjust_wxy
 			double  disp0,            // initial value of disparity
-			double  half_width,       // A=1/(half_widh)^2
-			double  cost_lazyeye     // cost for each of the non-zero disparity corrections and ortho disparity
+			double  half_width,       // A=1/(half_widh)^2                                                       lma_half_width
+			double  cost_lazyeye     // cost for each of the non-zero disparity corrections and ortho disparity  lma_cost_wy
 			) {
 //		int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
 		for (int f = 0; f < NUM_CAMS; f++) {
@@ -231,6 +231,7 @@ public class Corr2dLMA {
 		for (Sample s:samples) { // ignore zero-weight samples
 			used_cameras[s.fcam]=true;
 			used_cameras[s.scam]=true;
+			/*
 			if (s.fcam > last_cam) {
 				second_last = false;
 				last_cam = s.fcam;
@@ -239,6 +240,7 @@ public class Corr2dLMA {
 				second_last = true;
 				last_cam = s.scam;
 			}
+			*/
 			used_pairs[pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s
 			used_pairs_dir[s.fcam][s.scam] = true;
 		}
@@ -335,8 +337,6 @@ public class Corr2dLMA {
 	}
 
 	public void initMatrices() { // should be called after initVector and after setMatrices
-//    private final Matrix [][] m_pairs =      new Matrix[NUM_CAMS][NUM_CAMS];
-//    private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
 		for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) {
 			m_pairs[f][s] =      null;
 			m_pairs_last[f][s] = null;
@@ -389,7 +389,7 @@ public class Corr2dLMA {
 
 		int num_samples = samples.size();
 		double [] fx= new double [num_samples + 2 * NUM_CAMS];
-		double sqrt2 = Math.sqrt(2.0);
+//		double sqrt2 = Math.sqrt(2.0);
 		double A = av[A_INDEX];
 		double B = av[B_INDEX];
 		double C = A + av[CMA_INDEX];

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

 package com.elphel.imagej.tileprocessor;
 import java.util.ArrayList;
 
+import com.elphel.imagej.common.DoubleGaussianBlur;
 import com.elphel.imagej.common.PolynomialApproximation;
 import com.elphel.imagej.common.ShowDoubleFloatArrays;
 
@@ -1751,7 +1752,103 @@ public class Correlation2d {
     	return    rslt;
     }
 
+    public Corr2dLMA corrLMA2( // USED in lwir
+    		ImageDttParameters  imgdtt_params,
+    		double [][]         corrs,
+    		double [][]         disp_dist, // per camera disparity matrix as a 1d (linescan order)
+    		int                 pair_mask, // which pairs to process
+    		boolean             run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
+    		double              sigma, // low-pass sigma to find maximum (and convex too
+    		double              xcenter,   // preliminary center x in pixels for largest baseline
+    		double              vasw_pwr,  // value as weight to this power,
+    		int                 debug_level,
+    		int                 tileX, // just for debug output
+    		int                 tileY
+    		)
+    {
+    	// corrs are organized as PAIRS, some are null if not used
+    	// for each enabled and available pair find a maximum, filter convex and create sample list
+    	DoubleGaussianBlur gb = null;
+    	if (sigma > 0) gb = new DoubleGaussianBlur();
+    	int center =       transform_size - 1;
+    	int corr_size = 2 * transform_size - 1;
+    	Corr2dLMA lma = new Corr2dLMA(transform_size);
+    	for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (((pair_mask >> npair) & 1) !=0)){
+    		double[] corr = corrs[npair].clone();
+    		if (sigma > 0) {
+    			gb.blurDouble(corr, corr_size, corr_size, sigma, sigma, 0.01);
+    		}
+    		int imx = 0;
+    		for (int i = 1; i < corr.length; i++) if (corr[i] > corr[imx]) imx = i;
+    		// filter convex
+    		int ix0 = (imx % corr_size) - center; // signed, around center to match filterConvex
+    		int iy0 = (imx / corr_size) - center; // signed, around center to match filterConvex
+    	    double [] filtWeight =  filterConvex(
+    	            corr,                         // double [] corr_data,
+    	            imgdtt_params.cnvx_hwnd_size, // int       hwin,
+    	            ix0,                          // int       x0,
+    	            iy0,                          // int       y0,
+    	            imgdtt_params.cnvx_add3x3,    // boolean   add3x3,
+    	            imgdtt_params.cnvx_weight,    // double    nc_cost,
+    	            (debug_level > 2));           // boolean   debug);
+    	    int fcam = PAIRS[npair][0];
+    	    int scam = PAIRS[npair][1];
+    	    for (int i = 1; i < filtWeight.length; i++) if (filtWeight[i] > 0.0) {
+    	    	int ix = i % corr_size; // >=0
+    	    	int iy = i / corr_size; // >=0
+    	    	double v = corrs[npair][i]; // not blurred
+    	    	double w = filtWeight[i];
+    	        if (vasw_pwr != 0) {
+    	            w *= Math.pow(Math.abs(v), vasw_pwr);
+    	        }
+    	    	lma.addSample( // x = 0, y=0 - center
+    	    			fcam, // int    fcam, // first  camera index
+    	    			scam, // int    scam, // second camera index
+    	    			ix, // int    x,      // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
+    	    			iy, // int    y,      // y coordinate (0 - disparity axis)
+    	    			v, // double v,       // correlation value at that point
+    	    			w); //double w){      // sample weight
+    	    }
+    	}
+    	lma.initVector( // USED in lwir
+    			imgdtt_params.lma_adjust_wm,  // boolean adjust_width,     // adjust width of the maximum - lma_adjust_wm
+    			imgdtt_params.lma_adjust_ag,  // boolean adjust_scales,    // adjust 2D correlation scales - lma_adjust_ag
+    			imgdtt_params.lma_adjust_wy,  // boolean adjust_ellipse,   // allow non-circular correlation maximums lma_adjust_wy
+    			imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye,   // adjust disparity corrections and orthogonal disparities lma_adjust_wxy
+    			xcenter,                      // double  disp0,            // initial value of disparity
+    			imgdtt_params.lma_half_width, // double  half_width,       // A=1/(half_widh)^2   lma_half_width
+    			imgdtt_params.lma_cost_wy     // double  cost_lazyeye     // cost for each of the non-zero disparity corrections and ortho disparity lma_cost_wy
+    			);
+    	lma.setMatrices(disp_dist);
+    	lma.initMatrices(); // should be called after initVector and after setMatrices
+    	boolean lmaSuccess = false;
+    	if (debug_level > 1) {
+    		System.out.println("Input data:");
+    		lma.printInputDataFx(false);
+    	}
+
+    	lmaSuccess = 	lma.runLma(
+    			imgdtt_params.lma_lambda_initial,     // double lambda,           // 0.1
+    			imgdtt_params.lma_lambda_scale_good,  // double lambda_scale_good,// 0.5
+    			imgdtt_params.lma_lambda_scale_bad,   // double lambda_scale_bad, // 8.0
+    			imgdtt_params.lma_lambda_max,         // double lambda_max,       // 100
+    			imgdtt_params.lma_rms_diff,           // double rms_diff,         // 0.001
+    			imgdtt_params.lma_num_iter,           // int    num_iter,         // 20
+    			debug_level);       // int    debug_level)
+
+    	lma.updateFromVector();
+    	double [] rms = lma.getRMS();
+    	if (debug_level > 0) {
+    		System.out.println("LMA ->"+lmaSuccess+" RMS="+rms[0]+", pure RMS="+rms[1]);
+    		lma.printParams();
+    	}
 
+    	if (debug_level > 1) {
+    		System.out.println("Input data and approximation:");
+    		lma.printInputDataFx(true);
+    	}
+    	return lmaSuccess? lma: null;
+    }
 
     public Correlations2dLMA corrLMA( // USED in lwir
     		ImageDttParameters  imgdtt_params,
@@ -1824,7 +1921,6 @@ public class Correlation2d {
         		lma,                          // Correlations2dLMA lma,
         		imgdtt_params.cnvx_add3x3,    // boolean   add3x3,
         		imgdtt_params.cnvx_weight,    // double    nc_cost,
-
         		debug_level);    // int               debug_level
         boolean lmaSuccess;
         if (run_poly_instead) { // not used in lwir

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

@@ -2336,7 +2336,7 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 	 * @param rots misalignment correction (now includes zoom in addition to rotations
 	 * @param deriv_rots derivatives by d_az, f_elev, d_rot, d_zoom
 	 * @param pXYderiv - null or double[2 * number_of_cameras][] array to accommodate derivatives of px, py by each of the parameters
-	 * @param disp_dist - null or double[2 * number_of_cameras][] array to accommodate X,Y derivatives by disp and CCW90 of disp
+	 * @param disp_dist - null or double[number_of_cameras][4] array to accommodate X,Y (rows) derivatives by disp and CCW90 of disp (cols)
 	 * @param px pixel X coordinate
 	 * @param py pixel Y coordinate
 	 * @param disparity disparity (for non-distorted image space)
@@ -2452,8 +2452,7 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 			}
 
 			if (disp_dist != null) {
-				disp_dist[2 * i] =   new double [2]; // dx/d_disp, dx_d_ccw_disp
-				disp_dist[2 * i+1] = new double [2]; // dy/d_disp, dy_d_ccw_disp
+				disp_dist[i] =   new double [4]; // dx/d_disp, dx_d_ccw_disp
 				// Not clear - what should be in Z direction before rotation here?
 				double [][] add0 = {
 						{-rXY[i][0],  rXY[i][1], 0.0},
@@ -2484,10 +2483,10 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 
 				Matrix dd2 = rot2.transpose().times(scale_distort).times(rot2).times(dd1);
 
-				disp_dist[2 * i  ][0] =   dd2.get(0, 0);
-				disp_dist[2 * i  ][1] =   dd2.get(0, 1);
-				disp_dist[2 * i+1][0] =   dd2.get(1, 0);
-				disp_dist[2 * i+1][1] =   dd2.get(1, 1);
+				disp_dist[i][0] =   dd2.get(0, 0);
+				disp_dist[i][1] =   dd2.get(0, 1);
+				disp_dist[i][2] =   dd2.get(1, 0);
+				disp_dist[i][3] =   dd2.get(1, 1);
 
 			}
 
@@ -3008,7 +3007,7 @@ matrix([[-0.125, -0.125,  0.125,  0.125, -0.125,  0.125, -0.   , -0.   ,   -0.
 					break; // too high distortion
 				}
 				if (Math.abs(rD-rDist)<delta) {
-					System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
+					if (debugThis) System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
 					break; // success
 				}
 				r+=(rDist-rD)/drDistDr;

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

@@ -1825,7 +1825,7 @@ public class ImageDtt {
 						centerY = tileY * transform_size + transform_size/2 - shiftY;
 						// TODO: move port coordinates out of color channel loop
 						double [][] centersXY;
-						double [][] disp_dist = new double[2 * quad][]; // used to correct 3D correlations
+						double [][] disp_dist = new double[quad][]; // used to correct 3D correlations
 
 						if ((disparity_array == null) || (disparity_array[tileY] == null) || (Double.isNaN(disparity_array[tileY][tileX]))) {
 							System.out.println("Bug with disparity_array !!!");
@@ -1891,10 +1891,10 @@ public class ImageDtt {
 							}
 							// save disparity distortions for visualization:
 							for (int cam = 0; cam <quad; cam++) {
-								dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[ 2* cam + 0][0];
-								dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[ 2* cam + 0][1];
-								dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[ 2* cam + 1][0];
-								dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[ 2* cam + 1][1];
+								dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[cam][0];
+								dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[cam][1];
+								dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[cam][2];
+								dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[cam][3];
 							}
 
 							// TODO: use correction after disparity applied (to work for large disparity values)
@@ -2156,6 +2156,7 @@ public class ImageDtt {
 						    		col_weights,     // double []           col_weights,
 						    		corr_fat_zero);  // double              fat_zero)
 
+
 						    // calculate interpolated "strips" to match different scales and orientations (ortho/diagonal) on the
 						    // fine (0.5 pix) grid. ortho for scale == 1 provide even/even samples (1/4 of all), diagonal ones -
 						    // checkerboard pattern
@@ -2295,6 +2296,26 @@ public class ImageDtt {
 								if (tile_lma_debug_level > 0) {
 									System.out.println("Will run getMaxXSOrtho( ) for tileX="+tileX+", tileY="+tileY);
 								}
+
+								// debug new LMA correlations
+								if (debugTile) {
+									System.out.println("Will run new LMA for tileX="+tileX+", tileY="+tileY);
+
+							    	Corr2dLMA lma2 = corr2d.corrLMA2(
+							    			imgdtt_params,                // ImageDttParameters  imgdtt_params,
+							    			corrs,                        // double [][]         corrs,
+							    			disp_dist,
+							    			imgdtt_params.dbg_pair_mask,  // int                 pair_mask, // which pairs to process
+							        		false,                        // boolean             run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
+							        		0.5,                          // double              sigma, // low-pass sigma to find maximum (and convex too
+							    			corr_stat[0],                 // double    xcenter,   // preliminary center x in pixels for largest baseline
+							    			imgdtt_params.ortho_vasw_pwr, // double    vasw_pwr,  // value as weight to this power,
+							    			tile_lma_debug_level,         // int                 debug_level,
+							        		tileX,                        // int                 tileX, // just for debug output
+							        		tileY );                      // int                 tileY
+
+								}
+
 //								disparity_map[DISPARITY_INDEX_CM + 1][tIndex] = // y not available here
 								// calculate/fill out hor and vert
 								// convert to multi-baseline combining results from several integer scales
@@ -8246,8 +8267,8 @@ public class ImageDtt {
 						if (disparity_bimap != null){
 							disparity_bimap[BI_TARGET_INDEX][tIndex] = disparity_main;
 						}
-						double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
-						double [][] disp_dist_aux =  new double[2 * quad_aux][]; // used to correct 3D correlations
+						double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
+						double [][] disp_dist_aux =  new double[quad_aux][]; // used to correct 3D correlations
 
 						centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(
 								geometryCorrection_main, //			GeometryCorrection gc_main,
@@ -9049,8 +9070,8 @@ public class ImageDtt {
 						if (disparity_bimap != null){
 							disparity_bimap[BI_TARGET_INDEX][tIndex] = disparity_main;
 						}
-						double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
-						double [][] disp_dist_aux =  new double[2 * quad_aux][]; // used to correct 3D correlations
+						double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
+						double [][] disp_dist_aux =  new double[quad_aux][]; // used to correct 3D correlations
 						if (calc_main) {// not used in lwir
 							centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(
 									geometryCorrection_main, //			GeometryCorrection gc_main,

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

@@ -94,10 +94,13 @@ public class ImageDttParameters {
 	public double  corr_wndx_blur =         5.0;   // 100% to 0 % vertical transition range
 
 // LMA parameters
-	public boolean lma_adjust_wm =          true;
-	public boolean lma_adjust_wy =          false;
-	public boolean lma_adjust_wxy =         true;
-	public boolean lma_adjust_ag =          true;
+	public boolean lma_adjust_wm =          true;  // used in new for width
+	public boolean lma_adjust_wy =          false; // used in new for ellipse
+	public boolean lma_adjust_wxy =         true; // used in new for lazy eye
+	public boolean lma_adjust_ag =          true;// used in new for gains
+
+// new LMA parameters
+
 
 	public double  lma_half_width =         2.0;   //
 	public double  lma_cost_wy =            0.1;   //
@@ -241,12 +244,12 @@ public class ImageDttParameters {
 			gd.addTab("Corr LMA","Parameters for LMA fitting of the correlation maximum parameters");
 			gd.addCheckbox    ("Fit correlation defined half-width",                              this.lma_adjust_wm,
 					"Allow fitting of the half-width common for all pairs, defined by the LPF filter of the phase correlation");
-			gd.addCheckbox    ("Fit extra vertical half-width",                                   this.lma_adjust_wy,
-					"Fit extra perpendicular to disparity half-width (not used? and only possible with multi-baseline cameras)");
-			gd.addCheckbox    ("Fit extra half-width along disparity direction",                  this.lma_adjust_wxy,
+			gd.addCheckbox    ("Adjust ellipse parameters (was Fit extra vertical half-width)",   this.lma_adjust_wy,
+					"Adjust ellipse (non-circular) of the correlation maximum (was Fit extra perpendicular to disparity half-width (not used? and only possible with multi-baseline cameras))");
+			gd.addCheckbox    ("Adjust \"lazy eye\" paramdeters (was Fit extra half-width along disparity direction)", this.lma_adjust_wxy,
 					"Increased width in disparity direction caused by multi-distance objects in the tile");
-			gd.addCheckbox    ("Adjust per-group amplitudes",                                     this.lma_adjust_ag,
-					"Each correlation type's amplitude (now always needed)");
+			gd.addCheckbox    ("Adjust per-pair scale (was Adjust per-group amplitudes)",         this.lma_adjust_ag,
+					"Each correlation pair gain (was Each correlation type's amplitude (now always needed))");
 
 			gd.addNumericField("Initial/expected half-width of the correlation maximum in both directions", this.lma_half_width,  3, 6, "pix",
 					"With LPF sigma = 0.9 it seems to be ~= 2.0. Used both as initial parameter and the fitted value difference from this may be penalized");