Implemented 2d maximum modelled as Gaussian in addition to parabola

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

@@ -130,6 +130,7 @@ public class Corr2dLMA {
     private final int [][] pindx =           new int [NUM_CAMS][NUM_CAMS];
     private int            numTiles = 1;
 
+    public boolean gaussian_mode = true;
 
 	public class Sample{ // USED in lwir
 		int    tile;   // tile in a cluster
@@ -162,8 +163,10 @@ public class Corr2dLMA {
 	public Corr2dLMA (
 				int numTiles,
 				int ts, // null - use default table
-				double [][] corr_wnd // may be null
+				double [][] corr_wnd, // may be null
+				boolean gaussian_mode
 				) {
+		this.gaussian_mode = gaussian_mode;
 		for (int f = 0; f < NUM_CAMS; f++) {
 			pindx[f][f]=-1;
 			for (int s = f+1; s < NUM_CAMS; s++) {
@@ -495,10 +498,16 @@ public class Corr2dLMA {
 			}
 		}
 	}
-
 	public double [] getFxJt( // USED in lwir
 			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);
+		else                    return getFxJt_parabola(vector, jt);
+	}
+
+	public double [] getFxJt_parabola( // USED in lwir
+			double []   vector,
+			double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
 		if (vector == null) return null;
 		double [] av = fromVector(vector);
 		Matrix [][] xcam_ycam = new Matrix[numTiles][NUM_CAMS];
@@ -635,6 +644,147 @@ public class Corr2dLMA {
 		return fx;
 	}
 
+	public double [] getFxJt_gaussian( // USED in lwir
+			double []   vector,
+			double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
+		if (vector == null) return null;
+		double [] av = fromVector(vector);
+		Matrix [][] xcam_ycam = new Matrix[numTiles][NUM_CAMS];
+		double [][][][] xp_yp = new double[numTiles][NUM_CAMS][NUM_CAMS][];
+		double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
+		Matrix xc_yc = new Matrix(axc_yc, 2);
+		double [] AT = new double [numTiles]; // av[A_INDEX];
+		double [] BT = new double [numTiles]; // av[B_INDEX];
+		double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
+		for (int nTile = 0; nTile < numTiles; nTile++) {
+			for (int i = 0; i < NUM_CAMS; i++) if (used_cameras[i]) {
+				double [] add_dnd = {av[DISP_INDEX+ nTile * TILE_PARAMS]+ av[DDISP_INDEX + i],  av[NDISP_INDEX + i]};
+				xcam_ycam[nTile][i] = m_disp[nTile][i].times(new Matrix(add_dnd,2));
+			}
+			for (int f = 0; f < NUM_CAMS; f++) if (used_cameras[f]) {
+				for (int s = 0; s < NUM_CAMS; s++) if (used_cameras[s]) {
+					xp_yp[nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
+				}
+			}
+			AT[nTile] = av[A_INDEX + nTile * TILE_PARAMS];
+			BT[nTile] = av[B_INDEX + nTile * TILE_PARAMS];
+			CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * TILE_PARAMS];
+		}
+
+		int num_samples = samples.size();
+		double [] fx= new double [num_samples + 2 * NUM_CAMS];
+//corr_wnd
+		for (int ns = 0; ns < num_samples; ns++) {
+			Sample s = samples.get(ns);
+			int pair = pindx[s.fcam][s.scam]; // all pairs, noit just used?
+			double A = AT[s.tile];
+			double B = BT[s.tile];
+			double C = CT[s.tile];
+
+			double Gp = av[G0_INDEX + pair + s.tile * TILE_PARAMS];
+			double Wp = corr_wnd[s.ix][s.iy];
+			double WGp = Wp * Gp;
+			double xmxp = s.ix - xp_yp[s.tile][s.fcam][s.scam][0];
+			double ymyp = s.iy - xp_yp[s.tile][s.fcam][s.scam][1];
+			double xmxp2 = xmxp * xmxp;
+			double ymyp2 = ymyp * ymyp;
+			double xmxp_ymyp = xmxp * ymyp;
+////			double comm = Wp*(1.0 - (A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
+			double exp = Math.exp(-(A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
+			double comm = exp * Wp;
+			double WGpexp = WGp*exp;
+
+			fx[ns] = comm * Gp;
+			if (Double.isNaN(fx[ns])) {
+				System.out.println("fx["+ns+"]="+fx[ns]);
+			}
+			if (s.tile > 0) {
+				System.out.print("");
+			}
+			if (jt != null) {
+				if (par_map[DISP_INDEX + s.tile*TILE_PARAMS] >= 0)  jt[par_map[DISP_INDEX + s.tile*TILE_PARAMS]][ns] = 2 * WGpexp *
+						((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
+						 (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
+
+
+				if (par_map[A_INDEX + s.tile*TILE_PARAMS] >= 0)     jt[par_map[A_INDEX + s.tile*TILE_PARAMS]][ns] = -WGpexp*(xmxp2 + ymyp2);
+				if (par_map[B_INDEX + s.tile*TILE_PARAMS] >= 0)     jt[par_map[B_INDEX + s.tile*TILE_PARAMS]][ns] = -WGpexp* 2 * xmxp_ymyp;
+				if (par_map[CMA_INDEX + s.tile*TILE_PARAMS] >= 0)   jt[par_map[CMA_INDEX + s.tile*TILE_PARAMS]][ns] = -WGp* ymyp2 * exp;
+				for (int p = 0; p < npairs[s.tile]; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0
+					if (par_map[G0_INDEX + p + s.tile*TILE_PARAMS] >= 0) jt[par_map[G0_INDEX + p + s.tile*TILE_PARAMS]][ns] = (p== pair)? comm : 0.0; // (par_mask[G0_INDEX + pair])? d;
+				}
+				// process ddisp (last camera not used, is equal to minus sum of others to make a sum == 0)
+
+				for (int f = 0; f < NUM_CAMS; f++) if (par_map[DDISP_INDEX + f] >= 0) { // -1 for the last_cam
+					jt[par_map[DDISP_INDEX + f]][ns] = 0.0;
+				}
+				if (par_map[DDISP_INDEX + s.fcam] >= 0){ // par_map[DDISP_INDEX + last_cam] always <0
+						jt[par_map[DDISP_INDEX + s.fcam]][ns] += 2 * WGpexp *
+									((A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 0)+
+									 (B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 0));
+				} else if (s.fcam == last_cam) {
+					for (int c = 0; c < NUM_CAMS; c++) if ((c != last_cam) && (par_map[DDISP_INDEX + c] >=0)) {
+						jt[par_map[DDISP_INDEX + c]][ns] -= 2 * WGpexp *
+								(       (A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 0)+
+										(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 0));
+					}
+				}
+				if (par_map[DDISP_INDEX + s.scam]>= 0){ // par_map[DDISP_INDEX + last_cam] always <0
+					 jt[par_map[DDISP_INDEX + s.scam]][ns] -= 2 * WGpexp *
+								((A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 0)+
+								 (B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 0));
+
+				} else if (s.scam == last_cam) {
+					for (int c = 0; c < NUM_CAMS; c++) if ((c != last_cam) && (par_map[DDISP_INDEX + c] >= 0)) {
+						 jt[par_map[DDISP_INDEX + c]][ns] += 2 * WGpexp *
+							        ((A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 0)+
+									 (B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 0));
+					}
+				}
+
+				// process ndisp
+				for (int f = 0; f < ncam; f++) if (par_map[NDISP_INDEX + f] >= 0) {
+					jt[par_map[NDISP_INDEX + f]][ns] = 0.0;
+				}
+				if (par_map[NDISP_INDEX + s.fcam] >=0){
+					jt[par_map[NDISP_INDEX + s.fcam]][ns] += 2 * WGpexp *
+							(       (A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 1)+
+									(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 1));
+				}
+
+				if (par_map[NDISP_INDEX + s.scam] >= 0) {
+
+					jt[par_map[NDISP_INDEX + s.scam]][ns] -= 2 * WGpexp *
+							(       (A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 1)+
+									(B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 1));
+				}
+			}
+		}
+		for (int n = 0; n < NUM_CAMS; n++) { // av[DDISP_INDEX +last_cam] is already populated
+			fx[num_samples +            n] = av[DDISP_INDEX + n];
+			fx[num_samples + NUM_CAMS + n] = av[NDISP_INDEX + n];
+		}
+
+// and derivatives
+		if (jt != null) {
+			for (int i = 0; i < NUM_CAMS; i++) {
+				if ((i != last_cam) && (par_map[DDISP_INDEX + i] >= 0)) {
+					for (int j = 0; j < NUM_CAMS; j++) { // j - column
+						jt[par_map[DDISP_INDEX + i]][num_samples + j] = (i==j)? 1.0 : 0.0;
+					}
+					jt[par_map[DDISP_INDEX + i]][num_samples + last_cam] = -1.0;
+				}
+			}
+			for (int i = 0; i < NUM_CAMS; i++) {
+				if (par_map[NDISP_INDEX + i] >= 0) {
+					for (int j = 0; j < NUM_CAMS; j++) { // j - column
+						jt[par_map[NDISP_INDEX + i] ][num_samples + NUM_CAMS + j] = (i==j)? 1.0 : 0.0;
+					}
+				}
+			}
+		}
+		return fx;
+	}
 
 	public void printParams() { // not used in lwir
 		for (int np = 0; np < all_pars.length; np++) {

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

@@ -1823,14 +1823,19 @@ public class Correlation2d {
     	// 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
     	boolean debug_graphic = (debug_level > -1);
-    	boolean debug_second_all = true;
+    	boolean debug_second_all = false; // true;
 		int  clust_height = corrs.length/clust_width;
     	int ntiles = corrs.length;
     	DoubleGaussianBlur gb = null;
     	if (imgdtt_params.lma_sigma > 0) gb = new DoubleGaussianBlur();
     	int center =       transform_size - 1;
     	int corr_size = 2 * transform_size - 1;
-    	Corr2dLMA lma = new Corr2dLMA(corrs.length,transform_size, corr_wnd);
+    	Corr2dLMA lma = new Corr2dLMA(
+    			corrs.length,
+    			transform_size,
+    			corr_wnd,
+    			imgdtt_params.lma_gaussian//boolean gaussian_mode
+    			);
 
     	double [][][] dbg_corr =    debug_graphic ? new double [corrs.length][][] : null;
     	double [][][] dbg_weights = debug_graphic ? new double [corrs.length][][] : null;
@@ -2082,24 +2087,37 @@ public class Correlation2d {
     					true,
     					"corr_fx"+"_x"+tileX+"_y"+tileY, sliceTitles);
     		} else {
+    			double [][] repacked_y = repackCluster(lma.dbgGetSamples(0),clust_width);
+    			double [][] repacked_fx = repackCluster(lma.dbgGetSamples(2),clust_width);
+    			double [][] y_minux_fx = new double [repacked_y.length][];
+    			for (int i = 0; i < repacked_y.length; i++) if ((repacked_y[i] != null) && (repacked_fx[i] != null)){
+    				y_minux_fx[i] = new double [repacked_y[i].length];
+    				for (int j = 0; j < y_minux_fx[i].length; j++) y_minux_fx[i][j] = repacked_y[i][j] - repacked_fx[i][j];
+    			}
     			(new ShowDoubleFloatArrays()).showArrays(
-    					repackCluster(lma.dbgGetSamples(0),clust_width),
+    					repacked_y,
     					dbg_out_width,
     					dbg_out_height,
     					true,
-    					"corr_values"+"_x"+tileX+"_y"+tileY, sliceTitles);
+    					"y"+"_x"+tileX+"_y"+tileY, sliceTitles);
     			(new ShowDoubleFloatArrays()).showArrays(
-    					repackCluster(lma.dbgGetSamples(1),clust_width),
+    					repacked_fx,
     					dbg_out_width,
     					dbg_out_height,
     					true,
-    					"corr_weights"+"_x"+tileX+"_y"+tileY, sliceTitles);
+    					"fx"+"_x"+tileX+"_y"+tileY, sliceTitles);
     			(new ShowDoubleFloatArrays()).showArrays(
-    					repackCluster(lma.dbgGetSamples(2),clust_width),
+    					y_minux_fx,
     					dbg_out_width,
     					dbg_out_height,
     					true,
-    					"corr_fx"+"_x"+tileX+"_y"+tileY, sliceTitles);
+    					"y-fx"+"_x"+tileX+"_y"+tileY, sliceTitles);
+    			(new ShowDoubleFloatArrays()).showArrays(
+    					repackCluster(lma.dbgGetSamples(1),clust_width),
+    					dbg_out_width,
+    					dbg_out_height,
+    					true,
+    					"weights"+"_x"+tileX+"_y"+tileY, sliceTitles);
     		}
     	}
 
@@ -2129,7 +2147,13 @@ public class Correlation2d {
     	if (imgdtt_params.lma_sigma > 0) gb = new DoubleGaussianBlur();
     	int center =       transform_size - 1;
     	int corr_size = 2 * transform_size - 1;
-    	Corr2dLMA lma = new Corr2dLMA(1,transform_size, corr_wnd);
+    	Corr2dLMA lma = new Corr2dLMA(
+    			1,
+    			transform_size,
+    			corr_wnd,
+    			imgdtt_params.lma_gaussian//boolean gaussian_mode
+    			);
+
 
     	double [][] dbg_corr =    debug_graphic ? new double [corrs.length][] : null;
     	double [][] dbg_weights = debug_graphic ? new double [corrs.length][] : null;

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

@@ -1746,7 +1746,7 @@ public class ImageDtt {
 					double centerX; // center of aberration-corrected (common model) tile, X
 					double centerY; //
 					double [][] fract_shiftsXY = new double[quad][];
-					double [][] corr_wnd = (new Corr2dLMA(1, transform_size, null)).getCorrWnd();
+					double [][] corr_wnd = (new Corr2dLMA(1, transform_size, null,imgdtt_params.lma_gaussian)).getCorrWnd();
 					double [] corr_wnd_inv_limited = null;
 					if (imgdtt_params.lma_min_wnd <= 1.0) {
 						corr_wnd_inv_limited = new double [corr_wnd.length * corr_wnd[0].length];
@@ -2431,7 +2431,7 @@ public class ImageDtt {
 					double [][][]   tcorr_partial =  null; // [quad][numcol+1][15*15]
 					double [][][][] tcorr_tpartial = null; // [quad][numcol+1][4][8*8]
 					double [] ports_rgb = null;
-					double [][] corr_wnd = (new Corr2dLMA(1, transform_size, null)).getCorrWnd();
+					double [][] corr_wnd = (new Corr2dLMA(1, transform_size, null,imgdtt_params.lma_gaussian)).getCorrWnd();
 					double [] corr_wnd_inv_limited = null;
 					if (imgdtt_params.lma_min_wnd <= 1.0) {
 						corr_wnd_inv_limited = new double [corr_wnd.length * corr_wnd[0].length];

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

@@ -99,6 +99,7 @@ public class ImageDttParameters {
 	public double  corr_wndx_blur =         5.0;   // 100% to 0 % vertical transition range
 
 // LMA parameters
+	public boolean lma_gaussian =           true;  // model correlation maximum as a Gaussian (false - as a parabola)
 	public boolean lma_adjust_wm =          true;  // used in new for width
 	public boolean lma_adjust_wy =          true;  // false; // used in new for ellipse
 	public boolean lma_adjust_wxy =         true;  // used in new for lazy eye adjust parallel-to-disparity correction
@@ -267,6 +268,8 @@ public class ImageDttParameters {
 					"Transition range, shifted sine is used");
 
 			gd.addTab("Corr LMA","Parameters for LMA fitting of the correlation maximum parameters");
+			gd.addCheckbox    ("Correlation maximum as gaussian",                                 this.lma_gaussian,
+					"Model correlation maximum as a Gaussian exp(-r^2)  (false - as a parabola - 1-r^2)");
 			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    ("Adjust ellipse parameters (was Fit extra vertical half-width)",   this.lma_adjust_wy,
@@ -412,6 +415,7 @@ public class ImageDttParameters {
 			this.corr_wndx_blur =        gd.getNextNumber();
 
 //LMA tab
+			this.lma_gaussian=           gd.getNextBoolean();
 			this.lma_adjust_wm=          gd.getNextBoolean();
 			this.lma_adjust_wy=          gd.getNextBoolean();
 			this.lma_adjust_wxy=         gd.getNextBoolean();
@@ -514,8 +518,7 @@ public class ImageDttParameters {
 		properties.setProperty(prefix+"corr_wndx_hwidth",     this.corr_wndx_hwidth +"");
 		properties.setProperty(prefix+"corr_wndx_blur",       this.corr_wndx_blur +"");
 
-
-
+		properties.setProperty(prefix+"lma_gaussian",         this.lma_gaussian +"");
 		properties.setProperty(prefix+"lma_adjust_wm",        this.lma_adjust_wm +"");
 		properties.setProperty(prefix+"lma_adjust_wy",        this.lma_adjust_wy +"");
 		properties.setProperty(prefix+"lma_adjust_wxy",       this.lma_adjust_wxy +"");
@@ -622,8 +625,7 @@ public class ImageDttParameters {
 		if (properties.getProperty(prefix+"corr_wndx_hwidth")!=null)     this.corr_wndx_hwidth=Double.parseDouble(properties.getProperty(prefix+"corr_wndx_hwidth"));
 		if (properties.getProperty(prefix+"corr_wndx_blur")!=null)       this.corr_wndx_blur=Double.parseDouble(properties.getProperty(prefix+"corr_wndx_blur"));
 
-
-
+		if (properties.getProperty(prefix+"lma_gaussian")!=null)        this.lma_gaussian=Boolean.parseBoolean(properties.getProperty(prefix+"lma_gaussian"));
 		if (properties.getProperty(prefix+"lma_adjust_wm")!=null)        this.lma_adjust_wm=Boolean.parseBoolean(properties.getProperty(prefix+"lma_adjust_wm"));
 		if (properties.getProperty(prefix+"lma_adjust_wy")!=null)        this.lma_adjust_wy=Boolean.parseBoolean(properties.getProperty(prefix+"lma_adjust_wy"));
 		if (properties.getProperty(prefix+"lma_adjust_wxy")!=null)       this.lma_adjust_wxy=Boolean.parseBoolean(properties.getProperty(prefix+"lma_adjust_wxy"));
@@ -736,6 +738,7 @@ public class ImageDttParameters {
 		idp.corr_wndx_hwidth =       this.corr_wndx_hwidth;
 		idp.corr_wndx_blur =         this.corr_wndx_blur;
 
+		idp.lma_gaussian =           this.lma_gaussian;
 		idp.lma_adjust_wm =          this.lma_adjust_wm;
 		idp.lma_adjust_wy =          this.lma_adjust_wy;
 		idp.lma_adjust_wxy =         this.lma_adjust_wxy;