plane detection

src/main/java/CLTPass3d.java

@@ -516,7 +516,7 @@ public class CLTPass3d{
 		}
 		
 		
-		public void setSuperTiles(
+		public SuperTiles setSuperTiles(
 				double     step_near,
 				double     step_far,
 				double     step_threshold,
@@ -536,6 +536,7 @@ public class CLTPass3d{
 					strength_floor,
 					strength_pow,
 					stBlurSigma);
+			return this.superTiles;
 		}
 		public double [] showDisparityHistogram()
 		{

src/main/java/DisparityProcessor.java

@@ -398,7 +398,7 @@ public class DisparityProcessor {
 			ImageDtt.startAndJoin(threads);
 			double [][] dbg_data = {measured_disparity,disp_data[0],disp_data[1],dbg_pull[0],dbg_pull[1],dbg_pull[2],strength};
 			String [] titles = {"measured","[0]","[1]","avg","meas","pull","strength"};
-			if ((debugLevel>0) && ((pass ==0) || (pass >= (num_passes-2)))){
+			if ((debugLevel> 2) && ((pass ==0) || (pass >= (num_passes-2)))){
 				sdfa_instance.showArrays(dbg_data,tilesX, tilesY, true, "disp_smoothed",titles);
 			}
 
@@ -978,7 +978,9 @@ public class DisparityProcessor {
 									int ty1 = ty + TRANSITIONS[tDir][dirChoice][1];
 									int tDir1 =    TRANSITIONS[tDir][dirChoice][2];
 									// Was already here (in the same direction?
-									if (field[ty1][tx1][tDir1] == FLD_WALKED) {
+									if ((tx1 < 0) || (ty1 < 0) || (tx1 > tilesX) || (ty1 > tilesY)){
+										prohib = true;	
+									} else if (field[ty1][tx1][tDir1] == FLD_WALKED) { // 325
 										break walking;
 									} else if (field[ty1][tx1][tDir1] == FLD_EMPTY) {
 										tx = tx1;
@@ -1103,7 +1105,7 @@ public class DisparityProcessor {
 	
 						geom[nTile] = new int [8];
 						int [] dbg_geom = geom[nTile];
-						int [] dbg_neib = {neighbors[nTile - tilesX],neighbors[nTile + 1] , neighbors[nTile + tilesX],neighbors[nTile - 1]};
+//						int [] dbg_neib = {neighbors[nTile - tilesX],neighbors[nTile + 1] , neighbors[nTile + tilesX],neighbors[nTile - 1]}; // out of bounds
 						int neib = neighbors[nTile];
 						if (neib != 0b11111111){ // do nothing for internal tiles
 							int tileY = nTile/tilesX; 

src/main/java/EyesisCorrectionParameters.java

@@ -2028,6 +2028,7 @@ public class EyesisCorrectionParameters {
   		public double     min_clstr_max    = 0.25;  // Minimal maximal strength of the cluster 
   		
   		public int        fill_gaps        = 4;     // same as in grow - 1:  4 directions by 1 step, 2: 8 directions by 1 step. +2*n - alternating hor/vert
+  		public int        fill_final       = 50;    // same as fill_gaps, on the final pass
   		public int        min_clstr_block  = 3;     // number of tiles in a cluster to block (just non-background?)
   		public int        bgnd_grow        = 2;     // number of tiles to grow (1 - hor/vert, 2 - hor/vert/diagonal)
   		
@@ -2113,7 +2114,7 @@ public class EyesisCorrectionParameters {
   		public double     stStepNear        = 0.5;   // Disaprity histogram step for near objects
   		public double     stStepThreshold   = 1.0;   // Disaprity threshold to switch cfrom linear to logarithmic steps
   		public double     stMinDisparity    = 0.0;   // Minimal disparity (center of a bin)
-  		public double     stMaxDisparity    = 50.0;  // Maximal disparity (center of a bin)
+  		public double     stMaxDisparity    = 15.0;  // Maximal disparity (center of a bin)
   		public double     stFloor           = 0.15;  // Subtract from strength, discard negative  
   		public double     stPow             = 1.0;   // raise strength to this power 
   		public double     stSigma           = 1.5;   // Blur disparity histogram (sigma in bins) 
@@ -2141,6 +2142,13 @@ public class EyesisCorrectionParameters {
   		public double     grow_disp_step =   6.0; // Increase disparity (from maximal tried) if nothing found in that tile // TODO: handle enclosed dips?  
   		public double     grow_min_diff =    0.5; // Grow more only if at least one channel has higher variance from others for the tile  
   		
+  		public double     plDispNorm           =   3.0;  // Normalize disparities to the average if above (now only for eigenvalue comparison)
+  		public int        plMinPoints          =     5;  // Minimal number of points for plane detection
+  		public double     plTargetEigen        =   0.1;  // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
+  		public double     plFractOutliers      =   0.3;  // Maximal fraction of outliers to remove
+  		public int        plMaxOutliers        =    20;  // Maximal number of outliers to remove
+  		
+  		
   		// other debug images
   		public boolean    show_ortho_combine =     false; // Show 'ortho_combine' 
   		public boolean    show_refine_supertiles = false; // show 'refine_disparity_supertiles' 
@@ -2266,6 +2274,7 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"min_clstr_max",    this.min_clstr_max +"");
   			
   			properties.setProperty(prefix+"fill_gaps",        this.fill_gaps+"");
+  			properties.setProperty(prefix+"fill_final",       this.fill_final+"");
   			properties.setProperty(prefix+"min_clstr_block",  this.min_clstr_block+"");
   			properties.setProperty(prefix+"bgnd_grow",        this.bgnd_grow+"");
 
@@ -2372,6 +2381,12 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"grow_disp_step",   this.grow_disp_step +"");
 			properties.setProperty(prefix+"grow_min_diff",    this.grow_min_diff +"");
 
+			properties.setProperty(prefix+"plDispNorm",       this.plDispNorm +"");
+  			properties.setProperty(prefix+"plMinPoints",      this.plMinPoints+"");
+			properties.setProperty(prefix+"plTargetEigen",    this.plTargetEigen +"");
+			properties.setProperty(prefix+"plFractOutliers",  this.plFractOutliers +"");
+  			properties.setProperty(prefix+"plMaxOutliers",    this.plMaxOutliers+"");
+
 			properties.setProperty(prefix+"show_ortho_combine",     this.show_ortho_combine+"");
 			properties.setProperty(prefix+"show_refine_supertiles", this.show_refine_supertiles+"");
 			properties.setProperty(prefix+"show_bgnd_nonbgnd",      this.show_bgnd_nonbgnd+"");
@@ -2490,6 +2505,7 @@ public class EyesisCorrectionParameters {
 
   			
   			if (properties.getProperty(prefix+"fill_gaps")!=null)         this.fill_gaps=Integer.parseInt(properties.getProperty(prefix+"fill_gaps"));
+  			if (properties.getProperty(prefix+"fill_final")!=null)        this.fill_final=Integer.parseInt(properties.getProperty(prefix+"fill_final"));
   			if (properties.getProperty(prefix+"min_clstr_block")!=null)   this.min_clstr_block=Integer.parseInt(properties.getProperty(prefix+"min_clstr_block"));
   			if (properties.getProperty(prefix+"bgnd_grow")!=null)         this.bgnd_grow=Integer.parseInt(properties.getProperty(prefix+"bgnd_grow"));
 
@@ -2593,6 +2609,12 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"grow_disp_step")!=null)    this.grow_disp_step=Double.parseDouble(properties.getProperty(prefix+"grow_disp_step"));
   			if (properties.getProperty(prefix+"grow_min_diff")!=null)     this.grow_min_diff=Double.parseDouble(properties.getProperty(prefix+"grow_min_diff"));
 
+  			if (properties.getProperty(prefix+"plDispNorm")!=null)        this.plDispNorm=Double.parseDouble(properties.getProperty(prefix+"plDispNorm"));
+  			if (properties.getProperty(prefix+"plMinPoints")!=null)       this.plMinPoints=Integer.parseInt(properties.getProperty(prefix+"plMinPoints"));
+  			if (properties.getProperty(prefix+"plTargetEigen")!=null)     this.plTargetEigen=Double.parseDouble(properties.getProperty(prefix+"plTargetEigen"));
+  			if (properties.getProperty(prefix+"plFractOutliers")!=null)   this.plFractOutliers=Double.parseDouble(properties.getProperty(prefix+"plFractOutliers"));
+  			if (properties.getProperty(prefix+"plMaxOutliers")!=null)     this.plMaxOutliers=Integer.parseInt(properties.getProperty(prefix+"plMaxOutliers"));
+
   			if (properties.getProperty(prefix+"show_ortho_combine")!=null)     this.show_ortho_combine=Boolean.parseBoolean(properties.getProperty(prefix+"show_ortho_combine"));
   			if (properties.getProperty(prefix+"show_refine_supertiles")!=null) this.show_refine_supertiles=Boolean.parseBoolean(properties.getProperty(prefix+"show_refine_supertiles"));
   			if (properties.getProperty(prefix+"show_bgnd_nonbgnd")!=null)      this.show_bgnd_nonbgnd=Boolean.parseBoolean(properties.getProperty(prefix+"show_bgnd_nonbgnd"));
@@ -2725,6 +2747,7 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("Minimal maximal strength of the cluster",                                      this.min_clstr_max,  3);
   			
   			gd.addNumericField("Fill gaps betsween clusters, see comments for 'grow'",                         this.fill_gaps,   0);
+  			gd.addNumericField("Same as fill_gaps above, on the final pass",                                   this.fill_final,   0);
   			gd.addNumericField("Number of tiles in a cluster to block (just non-background?)",                 this.min_clstr_block,   0);
   			gd.addNumericField("Number of tiles to grow tile selection (1 - hor/vert, 2 - hor/vert/diagonal)", this.bgnd_grow,   0);
 
@@ -2835,6 +2858,13 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("Trust measured disparity within +/- this value",                               this.grow_disp_trust,  6);
   			gd.addNumericField("Increase disparity (from maximal tried) if nothing found in that tile",        this.grow_disp_step,  6);
   			gd.addNumericField("Grow more only if at least one channel has higher variance from others for the tile", this.grow_min_diff,  6);
+  			gd.addMessage     ("--- Planes detection ---");
+  			gd.addNumericField("Normalize disparities to the average if above",                                this.plDispNorm,  6);
+  			gd.addNumericField("Minimal number of points for plane detection",                                  this.plMinPoints,  0);
+  			gd.addNumericField("Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below", this.plTargetEigen,  6);
+  			gd.addNumericField("Maximal fraction of outliers to remove",                                       this.plFractOutliers,  6);
+  			gd.addNumericField("Maximal number of outliers to remove",                                         this.plMaxOutliers,  0);
+
   			gd.addMessage     ("--- Other debug images ---");
   			gd.addCheckbox    ("Show 'ortho_combine'",                                                         this.show_ortho_combine);
   			gd.addCheckbox    ("Show 'refine_disparity_supertiles'",                                           this.show_refine_supertiles);
@@ -2960,6 +2990,7 @@ public class EyesisCorrectionParameters {
   			this.min_clstr_max=         gd.getNextNumber();
   			
   			this.fill_gaps=       (int) gd.getNextNumber();
+  			this.fill_final=      (int) gd.getNextNumber();
   			this.min_clstr_block= (int) gd.getNextNumber();
   			this.bgnd_grow=       (int) gd.getNextNumber();
 
@@ -3065,6 +3096,12 @@ public class EyesisCorrectionParameters {
   			this.grow_disp_step=        gd.getNextNumber();
   			this.grow_min_diff=         gd.getNextNumber();
 
+  			this.plDispNorm=            gd.getNextNumber();
+  			this.plMinPoints=     (int) gd.getNextNumber();
+  			this.plTargetEigen=         gd.getNextNumber();
+  			this.plFractOutliers=       gd.getNextNumber();
+  			this.plMaxOutliers=   (int) gd.getNextNumber();
+
   			this.show_ortho_combine=     gd.getNextBoolean();
   			this.show_refine_supertiles= gd.getNextBoolean();
   			this.show_bgnd_nonbgnd=      gd.getNextBoolean(); // first on second pass

src/main/java/GeometryCorrection.java

@@ -329,8 +329,13 @@ public class GeometryCorrection {
 	}
 	
 	
-	/*
-	 * Get real world coordinates from pixel coordinates and nominal disparity 
+	/**
+	 * Get real world coordinates from pixel coordinates and nominal disparity
+	 * @param px horizontal pixel coordinate (right)
+	 * @param py vertical pixel coordinate (down)
+	 * @param disparity nominal disparity (pixels)
+	 * @param correctDistortions true: correct lens distortions, false - no lens distortions
+	 * @return {x, y, z} in meters
 	 */
 	public double [] getWorldCoordinates(
 			double px,
@@ -350,9 +355,224 @@ public class GeometryCorrection {
 		double [] xyz = {x,y,z};
 		return xyz;
 	}
+	
+	/* Just for testing using delta instead of d */
+	public double [][] getWorldJacobian(
+			double px,
+			double py,
+			double disparity,
+			boolean correctDistortions, // correct distortion (will need corrected background too !)
+			double delta)
+	{
+		double [] dpxpy0 = {disparity,px,py};
+		double [][] jacobian = new double [3][3];
+		double [] xyz0 = getWorldCoordinates(dpxpy0[1],dpxpy0[2],dpxpy0[0],correctDistortions);
+		for (int i = 0; i< 3; i++){
+			double [] dpxpy = dpxpy0.clone();
+			dpxpy[i] += delta;
+			double [] xyz = getWorldCoordinates(dpxpy[1],dpxpy[2],dpxpy[0],correctDistortions);
+			for (int j = 0; j<3; j++){
+				jacobian[j][i] = (xyz[j] - xyz0[j])/delta; 
+			}
+		}
+		return jacobian;
+	}
+	
+	
+	/**
+	 * Get jacobian matrix - derivatives of real world [x,y,z] (in meters, right, up, to camera) by [disparity, px,py] (in pixels disparity, right, down) 
+	 * @param px horizontal pixel coordinate (right)
+	 * @param py vertical pixel coordinate (down)
+	 * @param disparity nominal disparity (pixels)
+	 * @param correctDistortions true: correct lens distortions, false - no lens distortions
+	 * @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}}
+	 */
+	public double [][] getWorldJacobian(
+			double px,
+			double py,
+			double disparity,
+			boolean correctDistortions, // correct distortion (will need corrected background too !)
+			boolean debug)
+	{
+		double pXcd = px - 0.5 * this.pixelCorrectionWidth;
+		double pYcd = py - 0.5 * this.pixelCorrectionHeight;
+		
+		double d_pXcd_d_px = 1.0; 
+		double d_pYcd_d_py = 1.0; 
+		
+		double pXcd2_pYcd2 = pXcd*pXcd + pYcd*pYcd;
+		double rD = Math.sqrt(pXcd2_pYcd2)*0.001*this.pixelSize; // distorted radius in a virtual center camera
+		
+		double rrD = rD / this.distortionRadius;
+		double d_rRD_d_px = pXcd * rD / pXcd2_pYcd2  / this.distortionRadius;   
+		double d_rRD_d_py = pYcd * rD / pXcd2_pYcd2  / this.distortionRadius;
+		
+
+		
+		double rND2R = correctDistortions?(getRByRDist(rrD, false)): 1.0;
+	
+		
+		
+		double rrND = rND2R * rrD; // relative to distortion radius non-distorted radius
+		
+		
+		// k = rD/r
+		double d_k_d_rrND = correctDistortions?getDerivRDistFromR(rrND):0.0;
+		double d_rND2R_d_rrD = - rND2R * rND2R * d_k_d_rrND / ( d_k_d_rrND * rrND + 1.0/ rND2R); // rrND);
+/*		
+
+		double d_rND2R_d_rrD0 = correctDistortions?(getDerivRByRDist(rrD, false)): 0.0;
+		
+		double d_rND2R_d_rrD1 = correctDistortions?(getDerivRByRDist(rrD, false, 0.00001)): 0.0;
+		if (debug) {
+			System.out.println("getWorldJacobian(): d_rND2R_d_rrD="+d_rND2R_d_rrD+", d_rND2R_d_rrD0="+d_rND2R_d_rrD0+", d_rND2R_d_rrD1="+d_rND2R_d_rrD1 ); 
+		}
+*/		
+		
+		double d_rND2R_d_px = d_rND2R_d_rrD * d_rRD_d_px;   
+		double d_rND2R_d_py = d_rND2R_d_rrD * d_rRD_d_py;   
+		
+		double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
+		double pYc = pYcd * rND2R; // in pixels
+		
+		double d_pXc_d_px = pXcd * d_rND2R_d_px + d_pXcd_d_px * rND2R; // incorrect
+		double d_pYc_d_py = pYcd * d_rND2R_d_py + d_pYcd_d_py * rND2R; // incorrect
+		
+		double d_pXc_d_py = pXcd * d_rND2R_d_py; // incorrect (too small)? 
+		double d_pYc_d_px = pYcd * d_rND2R_d_px; // incorrect (too small)?
+		
+		double z =  -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (disparity * 0.001*this.pixelSize); // "+" - near, "-" far
+		double kx =  SCENE_UNITS_SCALE * this.disparityRadius / disparity;
+		double ky = -SCENE_UNITS_SCALE * this.disparityRadius / disparity;
+		double x =  kx * pXc;
+		double y =  ky * pYc;
+		
+		double d_z_d_disparity = -z / disparity;
+		double d_x_d_disparity = -x / disparity;
+		double d_y_d_disparity = -y / disparity;
+		// d_z_d_px == d_z_d_py ==0
+		double [][] jacobian = 
+			{		{d_x_d_disparity, kx * d_pXc_d_px, kx * d_pXc_d_py},
+					{d_y_d_disparity, ky * d_pYc_d_px, ky * d_pYc_d_py},
+					{d_z_d_disparity, 0.0,             0.0}};
+		return jacobian; // xyz;
+	}
+	
+	/**
+	 * Get pixel disparity and coordinates from the real world coordinates (in meters)
+	 * @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
+	 * @param correctDistortions true: correct lens distortions, false - no lens distortions
+	 * @return {disparity, px, py} (right, down)
+	 */
+	public double [] getImageCoordinates(
+			double [] xyz,
+			boolean correctDistortions) // correct distortion (will need corrected background too !)
+	{
+		double x = xyz[0];
+		double y = xyz[1];
+		double z = xyz[2];
+		double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
+		// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm	
+		double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels 
+		double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels
+		double rND = Math.sqrt(pXc*pXc + pYc*pYc)*0.001*this.pixelSize; // mm
+		double rD2RND = correctDistortions?getRDistByR(rND/this.distortionRadius):1.0;
+		double px = pXc * rD2RND + 0.5 * this.pixelCorrectionWidth;  // distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
+		double py = pYc * rD2RND + 0.5 * this.pixelCorrectionHeight; // in pixels
+		double [] dxy = {disparity, px, py};
+		return dxy;
+	}
+	
+	/* Just for testing using delta instead of d */
+	public double [][] getImageJacobian(
+			double [] xyz0,
+			boolean correctDistortions, // correct distortion (will need corrected background too !)
+			double delta)
+	{
+		double [][] jacobian = new double [3][3];
+		double [] dpxpy0 = getImageCoordinates(xyz0,correctDistortions);
+		for (int i = 0; i< 3; i++){
+			double [] xyz = xyz0.clone();
+			xyz[i] += delta;
+			double [] dpxpy = getImageCoordinates(xyz,correctDistortions);
+			for (int j = 0; j<3; j++){
+				jacobian[j][i] = (dpxpy[j] - dpxpy0[j])/delta; 
+			}
+		}
+		return jacobian;
+	}
+	
+
+	/**
+	 * Get jacobian matrix - derivatives of [disparity, px,py] (in pixels disparity, right, down) by real world [x,y,z] (in meters, right, up, to camera) 
+	 * @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
+	 * @param correctDistortions true: correct lens distortions, false - no lens distortions
+	 * @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}}
+	 */
+	public double [][] getImageJacobian(
+			double [] xyz,
+			boolean correctDistortions,
+			int debugLevel)
+	{
+		if (debugLevel > 0){
+			System.out.println("getImageJacobian():");
+		}
+		double x = xyz[0];
+		double y = xyz[1];
+		double z = xyz[2];
+		double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
+
+		double d_disparity_d_z = -disparity / z; // no dependence on x,y 
+		// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm	
+		double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); 
+		double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
+
+		double d_pXc_d_z = -pXc / z; // pXc/disparity * d_disparity_d_z = pXc/disparity * (-disparity / z);
+		double d_pYc_d_z = -pYc / z; // pYc/disparity * d_disparity_d_z = pYc/disparity * (-disparity / z);
+
+		double d_pXc_d_x =  disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
+		double d_pYc_d_y = -disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
+		
+
+		double rND2 = pXc*pXc + pYc*pYc;
+		double rrND = Math.sqrt(rND2)*0.001*this.pixelSize/this.distortionRadius; // relative to distortion radius;
+
+
+		double d_rrND_d_pXc =  pXc*rrND/rND2;
+		double d_rrND_d_pYc =  pYc*rrND/rND2;
+		
+		
+		double rD2RND = correctDistortions?getRDistByR(rrND):1.0;
+		double d_rD2RND_d_rrND = correctDistortions?getDerivRDistFromR(rrND) : 0.0;
+/*		
+		double d_rD2RND_d_rrND0 = correctDistortions?getDerivRDistFromR(rrND,0.000001) : 0.0;		
+		if (debugLevel > 0){
+			System.out.println("getImageJacobian():, d_rD2RND_d_rrND0 = "+d_rD2RND_d_rrND0+" ("+0.000001+"), d_rD2RND_d_rrND="+d_rD2RND_d_rrND);
+		}
+*/
+		double p_px_d_x = d_pXc_d_x * (pXc * (d_rD2RND_d_rrND * d_rrND_d_pXc) + rD2RND); // / (0.001 * this.pixelSize);
+		double p_py_d_y = d_pYc_d_y * (pYc * (d_rD2RND_d_rrND * d_rrND_d_pYc) + rD2RND); // / (0.001 * this.pixelSize);
+		
+		
+		double p_px_d_y = pXc * (d_rD2RND_d_rrND * d_rrND_d_pYc * d_pYc_d_y); // / (0.001 * this.pixelSize);
+		double p_py_d_x = pYc * (d_rD2RND_d_rrND * d_rrND_d_pXc * d_pXc_d_x); // / (0.001 * this.pixelSize);
+
+		double d_rrND_d_z = d_rrND_d_pXc * d_pXc_d_z  + d_rrND_d_pYc * d_pYc_d_z;
+
+		double p_px_d_z = (pXc * (d_rD2RND_d_rrND * d_rrND_d_z) + (d_pXc_d_z * rD2RND)); // / (0.001 * this.pixelSize);
+		double p_py_d_z = (pYc * (d_rD2RND_d_rrND * d_rrND_d_z) + (d_pYc_d_z * rD2RND)); // / (0.001 * this.pixelSize);
+
+		double [][] jacobian = {
+				{     0.0,     0.0, d_disparity_d_z},
+				{p_px_d_x,p_px_d_y, p_px_d_z},
+				{p_py_d_x,p_py_d_y, p_py_d_z}};
+
+		return jacobian;		
+	}
+	
 	/*
 	 * Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image
-	 * and generic diparity, measured in pixels 
+	 * and generic disparity, measured in pixels 
 	 */
 
 	public double [][] getPortsCoordinates(
@@ -451,6 +671,51 @@ public class GeometryCorrection {
 		}
 		return true;
 	}
+	
+	/**
+	 * Get relative (to distortion radius) distorted radius ratio to non-distorted from non-distorted
+	 * @param r non-distorted radius (1.0 is 2.8512mm)
+	 * @return ratio of distorted to non-distorted radius
+	 */
+	public double getRDistByR(double r) // relative to distortion radius
+	{
+		boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
+		double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
+		
+		double k;
+		if (use8){
+			k=(((((((this.distortionA8)*r+this.distortionA7)*r+this.distortionA6)*r+this.distortionA5)*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d;
+		} else {
+			k=(((this.distortionA5*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d;
+		}
+		return k;
+	}
+
+	/**
+	 * Get derivative of relative (to distortion radius) d_Rdist/d_R from relative (to distortion radius) non-distorted radius  
+	 * @param r non-distorted relative radius
+	 * @return derivative d_Rdist/d_R from (relative to relative)
+	 */
+	public double getDerivRDistFromR(double r) // relative to distortion radius
+	{
+		boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
+//		double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
+		double drDistDr;
+		if (use8){
+			drDistDr=(((((((7*this.distortionA8)*r + 6*this.distortionA7)*r + 5*this.distortionA6)*r + 4*this.distortionA5)*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC); // +d;
+		} else {
+//			drDistDr=(((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC)*r;
+			drDistDr=((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC;
+		}
+		return drDistDr;
+	}
+	
+	public double getDerivRDistFromR(double r, double delta) // relative to distortion radius
+	{
+		return (getRDistByR(r+delta) -getRDistByR(r))/delta;
+		
+	}	
+
 
 	public double getRByRDist(double rDist, boolean debug){
 		// add exceptions;
@@ -478,7 +743,51 @@ public class GeometryCorrection {
 		}
 		return result;
 	}
+	
+	/*
+	public double getDerivRByRDist(double rDist, boolean debug, double delta){
+		return (getRByRDist(rDist+delta, false) - getRByRDist(rDist, false))/delta;
+	}
+
+	public double getDerivRByRDist(double rDist, boolean debug){
+		// add exceptions;
+		if (this.rByRDist==null) {
+			calcReverseDistortionTable();
+			if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): this.rByRDist==null");
+			//		return Double.NaN;
+		}
+		if (rDist<0) {
+			if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): rDist<0");
+			return Double.NaN;
+		}
+		int index=(int) Math.floor(rDist/this.stepR);
+		if (index>=(this.rByRDist.length-1)) {
+			if (debug) System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): index="+index+">="+(this.rByRDist.length-1));
+			return Double.NaN;
+		}
+		double result=this.rByRDist[index+1]-this.rByRDist[index];
+		if ((index > 0) || (index < (this.rByRDist.length-2))){ // interpolate
+			double a = rDist/this.stepR-index; // fractional part, 0..1.0
+			if ( a <= 0.5){
+				result= 2  * (0.5 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
+					                a *  (this.rByRDist[index+1] - this.rByRDist[index-1]);
+			} else {
+				result= 2  * (1.0 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
+						     (a - 0.5) * (this.rByRDist[index+2] - this.rByRDist[index]);
+				
+			}
+			
+		}
+		if (Double.isNaN(result)){
+			if (debug) System.out.println("this.rByRDist["+index+"]="+this.rByRDist[index]);
+			if (debug) System.out.println("this.rByRDist["+(index+1)+"]="+this.rByRDist[index+1]);
+			if (debug) System.out.println("rDist="+rDist);
+			if (debug) System.out.println("(rDist/this.stepR="+(rDist/this.stepR));
 
+		}
+		return result/this.stepR;
+	}
+	*/
 
 
 }

src/main/java/QuadCLT.java

@@ -5059,7 +5059,7 @@ public class QuadCLT {
 				  geometryCorrection,
 				  threadsMax,  // maximal number of threads to launch                         
 				  updateStatus,
-				  debugLevel);
+				  2); // debugLevel);
 		  // get images for predefined regions and disparities. First - with just fixed scans 1 .. list.size()
 
           tp.showScan(

src/main/java/SuperTiles.java

@@ -25,15 +25,19 @@ import java.util.concurrent.atomic.AtomicInteger;
 
 public class SuperTiles{
 	        TileProcessor tileProcessor;
-			double      step_near;
 			double      step_far;
-			double      step_threshold;
+			double      step_near;
+			double      step_threshold_far;    // relative to min_disparity
+			double      step_threshold_near;   // relative to min_disparity
+			double      bin_far;               // bin value (before rounding) matching  (min_disparity + step_threshold_far)
+			double      bin_near;              // bin value (before rounding) matching  (min_disparity + step_threshold_near)
 			double      min_disparity;
 			double      max_disparity;
 			double      strength_floor;
 			double      strength_pow;
 			double      stBlurSigma;
 			int         numBins;
+			double []   bin_centers;
 			double [][] disparityHistograms = null;
 			double []   stStrength =          null; // per super-tile correlation strength
 			double [][][] maxMinMax = null;
@@ -42,7 +46,6 @@ public class SuperTiles{
 //			TileProcessor.CLTPass3d cltPass3d;
 			CLTPass3d cltPass3d;
 			public SuperTiles(
-//					TileProcessor.CLTPass3d cltPass3d,
 					CLTPass3d cltPass3d,
 					double                  step_near,
 					double                  step_far,
@@ -53,22 +56,69 @@ public class SuperTiles{
 					double                  strength_pow,
 					double                  stBlurSigma)
 			{
-				this.cltPass3d =      cltPass3d;
-				this.tileProcessor =  cltPass3d.getTileProcessor();
-				this.step_near =      step_near;
-				this.step_far =       step_far;
-				this.step_threshold = step_threshold;
+				this.cltPass3d =           cltPass3d;
+				this.tileProcessor =       cltPass3d.getTileProcessor();
+				this.step_near =           step_near;
+				this.step_far =            step_far;
+				this.step_threshold_far = step_threshold;
 				this.min_disparity =  min_disparity;
 				this.max_disparity =  max_disparity;
 				this.strength_floor = strength_floor;
 				this.strength_pow =   strength_pow;
 				this.stBlurSigma =    stBlurSigma;
-//				this.numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1;
-				this.numBins = (int) ((max_disparity - min_disparity)/step_far) + 1;
-//				getDisparityHistograms(); // calculate and blur supertileas
-				getDisparityHistograms(null); // calculate and blur supertileas (for all, not just selected?)
+//				this.step_threshold_near = this.step_threshold_far * this.step_far / step_near;
+				this.step_threshold_near = this.step_threshold_far * step_near / this.step_far ;
+				this.bin_far =             this.step_threshold_far / this.step_far; 
+				this.bin_near =            this.step_threshold_far / this.step_far * (Math.log(this.step_near/this.step_far) + 1);
+				int bin_max = disparityToBin(max_disparity);
+				numBins = bin_max + 1;
+				bin_centers = new double [numBins];
+				int bin = 0;
+				for (bin = 0; bin < numBins; bin ++){
+					bin_centers[bin] = binToDisparity(bin);  
+				}
 				
+				getDisparityHistograms(null); // calculate and blur supertiles (for all, not just selected?)
+				if (tileProcessor.globalDebugLevel > -1){
+					System.out.println("SuperTiles(): min_disparity = "+min_disparity+", max_disparity="+max_disparity);
+					System.out.println("SuperTiles(): step_far = "+step_far+", step_near="+step_near);
+					System.out.println("SuperTiles(): numBins = "+numBins+", bin_far="+bin_far+", bin_near = "+bin_near);
+					System.out.println("SuperTiles(): step_threshold_far = "+step_threshold_far+", step_threshold_near="+step_threshold_near);
+					for (int i = 0; i<numBins; i++){
+						System.out.println(i+": "+bin_centers[i]+", "+disparityToBin(bin_centers[i]));
+					}
+					//
+				}
+			}
+			
+			public int disparityToBin(double disparity){ // not filtered
+				double x = disparity - min_disparity;
+				int bin;
+				if (x < step_threshold_far){
+					bin = (int) Math.round(x/step_far);
+				} else if (x < step_threshold_near){
+					bin = (int) Math.round(step_threshold_far / step_far * (Math.log(x / step_threshold_far) + 1));
+				} else  {
+					bin = (int) Math.round(bin_near + (x - step_threshold_near) / step_near);
+				}
+//				if (bin < 0) bin = 0;
+//				else if (bin >= numBins) bin = numBins - 1;
+				return bin;
 			}
+			
+			public double binToDisparity(double bin){
+				double d;
+				if (bin < bin_far ) {
+					d = bin * step_far;
+				} else if (bin < bin_near){
+					d =  step_threshold_far * Math.exp(bin * step_far / step_threshold_far - 1.0);
+				} else {
+					d = step_threshold_near + (bin - bin_near) * step_near; 
+				}
+				return min_disparity + d;  
+			}
+			
+			
 			private double [][] getLapWeights(){
 				final int superTileSize = tileProcessor.superTileSize;
 				final double [][] lapWeight = new double [2 * superTileSize][2 * superTileSize];
@@ -123,10 +173,8 @@ public class SuperTiles{
 				final int stilesX = (tilesX + superTileSize -1)/superTileSize;  
 				final int stilesY = (tilesY + superTileSize -1)/superTileSize;
 				final int nStiles = stilesX * stilesY; 
-//				final int numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1;
-				final double dMin = min_disparity - step_disparity/2; 
-				final double dMax = dMin + numBins * step_disparity;
-//				final double [][] dispHist =     new double [nStiles][numBins];
+///				final double dMin = min_disparity - step_disparity/2; 
+///				final double dMax = dMin + numBins * step_disparity;
 				final double [][] dispHist =     new double [nStiles][]; // now it will be sparse 
 				final double []   strengthHist = new double [nStiles];
 				final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
@@ -160,7 +208,9 @@ public class SuperTiles{
 													if (w > 0.0){
 														if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
 														w *= lapWeight[tY][tX];
-														int bin = (int) ((d-dMin)/step_disparity);
+//														int bin = (int) ((d-dMin)/step_disparity);
+														// ignore too near/ too far
+														int bin = disparityToBin(d);
 														if ((bin >= 0) && (bin < numBins)){ // maybe collect below min and above max somewhere?
 															hist[bin] += w; // +1]
 															sw +=w;
@@ -218,6 +268,7 @@ public class SuperTiles{
 				ImageDtt.startAndJoin(threads);
 			}
 			// returns odd-length array of max/min (x, strength) pairs
+			
 			public double [][][] getMaxMinMax(){
 				// first find all integer maximums, and if the top is flat - use the middle. If not flat - use 2-nd degree polynomial
 				if (disparityHistograms == null) getDisparityHistograms();
@@ -312,10 +363,10 @@ public class SuperTiles{
 									if (numMax > 0) {
 									maxMinMax[nsTile] = new double[numMax * 2 - 1][2];
 									for (int i = 0; i < maxMinMax[nsTile].length; i++){
-										maxMinMax[nsTile][i][0] = mmm[i][0];
+										maxMinMax[nsTile][i][0] = binToDisparity(mmm[i][0]); // convert to actual disparity !
 										maxMinMax[nsTile][i][1] = mmm[i][1] * stStrength[nsTile];
 									}
-									if (globalDebugLevel > -1 ) {
+									if (globalDebugLevel > 0 ) { //************
 										System.out.println(nsTile+": "+dh[0]+" "+dh[1]+" "+dh[2]+" "+dh[3]+" "+dh[4]+" "+
 												dh[5]+" "+dh[6]+" "+dh[7]+" "+dh[8]+" "+dh[9]+ " "+dh[10]+" "+dh[11]+" "+dh[12]+" "+dh[13]+" "+dh[14]+" "+
 												dh[15]+" "+dh[16]+" "+dh[17]+" "+dh[18]+" "+dh[19]+ " "+dh[20]);
@@ -549,7 +600,8 @@ public class SuperTiles{
 											}
 										}
 										//maxIndex is what we need. Which strength to use - individual or accumulated? Use individual.
-										bgDisparity[nsTile] = min_disparity + mmm[2 * maxIndex][0] * step_disparity;
+///										bgDisparity[nsTile] = min_disparity + mmm[2 * maxIndex][0] * step_disparity;
+										bgDisparity[nsTile] = binToDisparity (mmm[2 * maxIndex][0]);
 										bgStrength[nsTile] =  mmm[2 * maxIndex][1];
 									}
 								}
@@ -578,11 +630,8 @@ public class SuperTiles{
 				final int stilesX = (tilesX + superTileSize -1)/superTileSize;  
 				final int stilesY = (tilesY + superTileSize -1)/superTileSize;
 				final int nStiles = stilesX * stilesY; 
-				//				final int numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1;
-				//				final double [][] dispHist =     new double [nStiles][numBins];
 				final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
 				final AtomicInteger ai = new AtomicInteger(0);
-				//				final int st_end = st_start + superTileSize;
 				final int superTileSize2 = 2 * superTileSize;
 				final double [][] lapWeight = getLapWeights();
 				final double [] tileDisparity = new double [tilesY * tilesX]; // assuming all 0.0
@@ -652,7 +701,595 @@ public class SuperTiles{
 				return bgTileDispStrength;
 			}
 
+			public void processPlanes(
+
+					final boolean [] selected, // or null
+					final double     min_disp,
+					final boolean    invert_disp, // use 1/disparity
+					final double     plDispNorm,
+					final int        debugLevel)
+			{
+				
+				final int tilesX =        tileProcessor.getTilesX();
+				final int tilesY =        tileProcessor.getTilesY();
+				final int superTileSize = tileProcessor.getSuperTileSize();
+				final int tileSize =      tileProcessor.getTileSize();
+				final double  [] disparity = cltPass3d.getDisparity();
+				final double  [] strength =  cltPass3d.getStrength();
+				final int stilesX = (tilesX + superTileSize -1)/superTileSize;  
+				final int stilesY = (tilesY + superTileSize -1)/superTileSize;
+				final int nStiles = stilesX * stilesY; 
+				final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
+				final AtomicInteger ai = new AtomicInteger(0);
+				final int st_start = -superTileSize/2;
+				final int superTileSize2 = 2 * superTileSize;
+				final double [][] lapWeight = getLapWeights();
+				final int len2 = superTileSize2*superTileSize2;
+				final double []  double_zero =  new double  [len2];
+				final boolean [] boolean_zero = new boolean [len2];
+
+				final int debug_stile = 18 * stilesX + 25; 
+				
+				
+				for (int ithread = 0; ithread < threads.length; ithread++) {
+					threads[ithread] = new Thread() {
+						public void run() {
+							TilePlanes tpl = new TilePlanes(tileSize,superTileSize);
+							double [] stDisparity = new double [superTileSize2*superTileSize2];
+							double [] stStrength =  new double [superTileSize2*superTileSize2];
+							boolean [] stSel = new boolean [superTileSize2*superTileSize2];
+							for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
+								int stileY = nsTile / stilesX;  
+								int stileX = nsTile % stilesX;
+								double sw = 0.0; // sum weights
+								double [] hist = new double [numBins];
+								int tY0 = stileY * superTileSize + st_start;
+								int tX0 = stileX * superTileSize + st_start;
+								System.arraycopy(double_zero,  0, stDisparity, 0, len2);
+								System.arraycopy(double_zero,  0, stStrength,  0, len2);
+								System.arraycopy(boolean_zero, 0, stSel,       0, len2);
+								for (int tY = 0; tY < superTileSize2; tY++){
+									int tileY = tY0 +tY;
+									if ((tileY >= 0) && (tileY < tilesY)) {
+										for (int tX = 0; tX < superTileSize2; tX++){
+											int tileX = tX0 +tX;
+											if ((tileX >= 0) && (tileX < tilesX)) {
+												int indx = tileY*tilesX + tileX;
+												double d = disparity[indx];
+												if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
+													if (invert_disp){
+														d = 1.0/d;
+													}
+													double w = strength[indx] - strength_floor;
+													if (w > 0.0){
+														if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
+														w *= lapWeight[tY][tX];
+														int indx_out =  tY * superTileSize2 + tX;
+														stDisparity[indx_out] = d;
+														stStrength[indx_out] =  w;
+														stSel[indx_out] =       true;
+														sw +=w;
+													}
+												}
+											}
+										}
+									}
+								}
+								if (sw >0){
+									//									int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
+									//									int dl = (stileY == 17) ? 1 : 0;
+									int dl = (stileY >= 0) ? 1 : 0;
+									double [][][] rslt = tpl.getCovar(
+											stDisparity,
+											stStrength,
+											stSel,
+											plDispNorm,
+											0); // dl); // debugLevel);
+									if (dl > 0) {
+										int       numPoints =  (int) rslt[2][0][2];
+										double    kz =   rslt[2][0][1];
+										double    swc =  rslt[2][0][0];
+										double [] szxy = rslt[2][1];
+										double [][] eig_val =  rslt[0];
+										double [][] eig_vect = rslt[1];
+										if (swc > 1.0) {
+											System.out.println("Processing planes, nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
+													", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
+													", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
+													", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
+										}
+									}
+
+/*
+		double [][][] rslt = {
+				eig.getD().getArray(),
+				eig.getV().getArray(),
+				{
+					{sw,kz},
+					{swz, swx, swy}}};
+		return rslt;
+									
+ */
+									
+									
+									if (dl > 1) {
+										System.out.println("Processing planes with average disparity");
+										double ss = 0.0, sd = 0.0;
+										for (int i = 0; i < stDisparity.length; i++){
+											if (!Double.isNaN(stDisparity[i]) && stSel[i]){
+												ss += stStrength[i];
+												sd += stStrength[i] * stDisparity[i];
+											}
+										}
+										if (ss > 0) {
+											sd /= ss; 
+										}
+										for (int i = 0; i < stDisparity.length; i++){
+											stDisparity[i] = sd;
+										}
+										tpl.getCovar(
+												stDisparity,
+												stStrength,
+												stSel,
+												plDispNorm,
+												dl); // debugLevel);
+									}
+								}
+							}
+						}
+					};
+				}		      
+				ImageDtt.startAndJoin(threads);
+			}
+
+			public void processPlanes1(
+
+					final boolean [] selected, // or null
+					final double     min_disp,
+					final boolean    invert_disp, // use 1/disparity
+					final double     plDispNorm,
+					final int        debugLevel)
+			{
+				if (maxMinMax == null) getMaxMinMax();
+				final int np_min = 5; // minimal number of points to consider
+				final int tilesX =        tileProcessor.getTilesX();
+				final int tilesY =        tileProcessor.getTilesY();
+				final int superTileSize = tileProcessor.getSuperTileSize();
+				final int tileSize =      tileProcessor.getTileSize();
+				final double  [] disparity = cltPass3d.getDisparity();
+				final double  [] strength =  cltPass3d.getStrength();
+				final int stilesX = (tilesX + superTileSize -1)/superTileSize;  
+				final int stilesY = (tilesY + superTileSize -1)/superTileSize;
+				final int nStiles = stilesX * stilesY; 
+				final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
+				final AtomicInteger ai = new AtomicInteger(0);
+				final int st_start = -superTileSize/2;
+				final int superTileSize2 = 2 * superTileSize;
+				final double [][] lapWeight = getLapWeights();
+				final int len2 = superTileSize2*superTileSize2;
+				final double []  double_zero =  new double  [len2];
+				final boolean [] boolean_zero = new boolean [len2];
 
+				final int debug_stile = 18 * stilesX + 25; 
+				
+				
+				for (int ithread = 0; ithread < threads.length; ithread++) {
+					threads[ithread] = new Thread() {
+						public void run() {
+							TilePlanes tpl = new TilePlanes(tileSize,superTileSize);
+							double [] stDisparity = new double [superTileSize2*superTileSize2];
+							double [] stStrength =  new double [superTileSize2*superTileSize2];
+							boolean [] stSel = new boolean [superTileSize2*superTileSize2];
+							for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
+								int stileY = nsTile / stilesX;  
+								int stileX = nsTile % stilesX;
+								double sw = 0.0; // sum weights
+								double [] hist = new double [numBins];
+								int tY0 = stileY * superTileSize + st_start;
+								int tX0 = stileX * superTileSize + st_start;
+								System.arraycopy(double_zero,  0, stDisparity, 0, len2);
+								System.arraycopy(double_zero,  0, stStrength,  0, len2);
+								System.arraycopy(boolean_zero, 0, stSel,       0, len2);
+								for (int tY = 0; tY < superTileSize2; tY++){
+									int tileY = tY0 +tY;
+									if ((tileY >= 0) && (tileY < tilesY)) {
+										for (int tX = 0; tX < superTileSize2; tX++){
+											int tileX = tX0 +tX;
+											if ((tileX >= 0) && (tileX < tilesX)) {
+												int indx = tileY*tilesX + tileX;
+												double d = disparity[indx];
+												if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
+													if (invert_disp){
+														d = 1.0/d;
+													}
+													double w = strength[indx] - strength_floor;
+													if (w > 0.0){
+														if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
+														w *= lapWeight[tY][tX];
+														int indx_out =  tY * superTileSize2 + tX;
+														stDisparity[indx_out] = d;
+														stStrength[indx_out] =  w;
+														stSel[indx_out] =       true;
+														sw +=w;
+													}
+												}
+											}
+										}
+									}
+								}
+								if (sw >0){
+									
+									//									int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
+									//									int dl = (stileY == 17) ? 1 : 0;
+									int dl = (stileY >= 0) ? 1 : 0;
+									double [][][] rslt = tpl.getCovar(
+											stDisparity,
+											stStrength,
+											stSel,
+											plDispNorm,
+											0); // dl); // debugLevel);
+									double swc_common = 0.0;
+									if (dl > 0) {
+										int       numPoints =  (int) rslt[2][0][2];
+										double    kz =   rslt[2][0][1];
+										double    swc =  rslt[2][0][0];
+										double [] szxy = rslt[2][1];
+										double [][] eig_val =  rslt[0];
+										double [][] eig_vect = rslt[1];
+										swc_common = swc;
+										if (swc > 1.0) {
+											System.out.println("Processing planes, nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
+													", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
+													", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
+													", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
+										}
+									}
+									double [][] mm = maxMinMax[nsTile];
+									
+									if (mm.length > 1) { // multiple maximums - separate into multiple selections
+										boolean [][] stSels = new boolean[(mm.length +1)/2][stSel.length];
+										for (int i = 0; i< stSel.length; i++) if (stSel[i]){
+											double d = stDisparity[i];
+											loop:{
+												for (int m = 0; m < (stSels.length-1); m++){
+													if (d < mm[2 * m + 1][0]){
+														stSels[m][i] = true;
+														break loop;
+													}
+												}
+												stSels[stSels.length-1][i] = true;
+											}
+										}
+										double [][][][] rslts = new double [stSels.length][][][];
+										int [] np = new int[stSels.length];
+										String dbg_str = "";
+										for (int m = 0; m < stSels.length; m++){
+											for (int i =0; i < stSels[m].length; i++){
+												if (stSels[m][i]) np[m]++; 
+											}
+											dbg_str +="  "+np[m];
+											if (m < stSels.length -1){
+												dbg_str +="("+mm[2*m+1][0]+")";
+											}
+										}
+										if (swc_common > 1.0) {
+											System.out.println("Processing subplanes:"+dbg_str+", nsTile="+nsTile);
+										}
+										for (int m = 0; m < stSels.length; m++) if (np[m] > np_min) {
+
+											rslts[m] = tpl.getCovar(
+													stDisparity,
+													stStrength,
+													stSels[m],
+													plDispNorm,
+													0); // dl); // debugLevel);
+											if (dl > 0) {
+												if (rslts[m] != null) {
+													int       numPoints =  (int) rslts[m][2][0][2];
+													double    kz =   rslts[m][2][0][1];
+													double    swc =  rslts[m][2][0][0];
+													double [] szxy = rslts[m][2][1];
+													double [][] eig_val =  rslts[m][0];
+													double [][] eig_vect = rslts[m][1];
+													if (swc_common > 1.0) { // reduce output
+														System.out.println("Processing subplane["+m+"], nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
+																", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
+																", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
+																", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
+													}
+												} else {
+													System.out.println("Processing subplane["+m+"], nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+" RETURNED NULL");
+												}
+											}
+
+
+										}
+
+									}
+
+
+
+								}
+							}
+						}
+					};
+				}		      
+				ImageDtt.startAndJoin(threads);
+			}
+
+			public void processPlanes2(
+
+					final boolean [] selected, // or null
+					final double     min_disp,
+					final boolean    invert_disp, // use 1/disparity
+					final double     plDispNorm,
+					final int        plMinPoints, //           =     5;  // Minimal number of points for plane detection
+					final double     plTargetEigen, //         =   0.1;  // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
+					final double     plFractOutliers, //      =   0.3;  // Maximal fraction of outliers to remove
+					final int        plMaxOutliers, //        =    20;  // Maximal number of outliers to remove
+					final GeometryCorrection geometryCorrection,
+					final boolean    correct_distortions, 
+					final int        debugLevel)
+			{
+				if (maxMinMax == null) getMaxMinMax();
+//				final int np_min = 5; // minimal number of points to consider
+				final int tilesX =        tileProcessor.getTilesX();
+				final int tilesY =        tileProcessor.getTilesY();
+				final int superTileSize = tileProcessor.getSuperTileSize();
+				final int tileSize =      tileProcessor.getTileSize();
+				
+				final double  [] disparity = cltPass3d.getDisparity();
+				final double  [] strength =  cltPass3d.getStrength();
+				final int stilesX = (tilesX + superTileSize -1)/superTileSize;  
+				final int stilesY = (tilesY + superTileSize -1)/superTileSize;
+				final int nStiles = stilesX * stilesY; 
+				final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
+				final AtomicInteger ai = new AtomicInteger(0);
+				final int st_start = -superTileSize/2;
+				final int superTileSize2 = 2 * superTileSize;
+				final double [][] lapWeight = getLapWeights();
+				final int len2 = superTileSize2*superTileSize2;
+				final double []  double_zero =  new double  [len2];
+				final boolean [] boolean_zero = new boolean [len2];
+
+//				final int debug_stile = 18 * stilesX + 25; 
+//				final int debug_stile = 20 * stilesX + 24; 
+				final int debug_stile = 17 * stilesX + 10; 
+				
+				
+				for (int ithread = 0; ithread < threads.length; ithread++) {
+					threads[ithread] = new Thread() {
+						public void run() {
+							TilePlanes tpl = new TilePlanes(tileSize,superTileSize, geometryCorrection);
+							double [] stDisparity = new double [superTileSize2*superTileSize2];
+							double [] stStrength =  new double [superTileSize2*superTileSize2];
+							boolean [] stSel = new boolean [superTileSize2*superTileSize2];
+							for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
+								int stileY = nsTile / stilesX;  
+								int stileX = nsTile % stilesX;
+								int [] sTiles = {stileX, stileY};
+								double sw = 0.0; // sum weights
+								double [] hist = new double [numBins];
+								int tY0 = stileY * superTileSize + st_start;
+								int tX0 = stileX * superTileSize + st_start;
+								System.arraycopy(double_zero,  0, stDisparity, 0, len2);
+								System.arraycopy(double_zero,  0, stStrength,  0, len2);
+								System.arraycopy(boolean_zero, 0, stSel,       0, len2);
+								for (int tY = 0; tY < superTileSize2; tY++){
+									int tileY = tY0 +tY;
+									if ((tileY >= 0) && (tileY < tilesY)) {
+										for (int tX = 0; tX < superTileSize2; tX++){
+											int tileX = tX0 +tX;
+											if ((tileX >= 0) && (tileX < tilesX)) {
+												int indx = tileY*tilesX + tileX;
+												double d = disparity[indx];
+												if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
+													if (invert_disp){
+														d = 1.0/d;
+													}
+													double w = strength[indx] - strength_floor;
+													if (w > 0.0){
+														if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
+														w *= lapWeight[tY][tX];
+														int indx_out =  tY * superTileSize2 + tX;
+														stDisparity[indx_out] = d;
+														stStrength[indx_out] =  w;
+														stSel[indx_out] =       true;
+														sw +=w;
+													}
+												}
+											}
+										}
+									}
+								}
+								if (sw >0){
+
+//									int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
+//									int dl = ((stileY == 17) && (stileX > 4)) ? 1 : 0;
+//									int dl = (stileY >= 0) ? 1 : 0;
+									int dl1 =  (nsTile == debug_stile-1) ? 3 : 0;
+//									int dl =  (nsTile == debug_stile) ? 3 : 0;
+									int dl = ((stileY >= 15) && (stileY <= 18) && (stileX >= 5) && (stileX <= 31)) ? 1 : 0;
+//									int dl = ((stileY == 17) && (stileX == 27) ) ? 3 : 0;
+									
+									
+									boolean [] sel_all = stSel.clone();
+									TilePlanes.PlaneData pd = tpl.getPlane(
+											sTiles,
+											stDisparity,
+											stStrength,
+											sel_all,
+											0); // debugLevel);
+									if (pd != null) {
+										//correct_distortions
+										double swc_common = pd.getWeight();
+										if (dl > 0) {
+											if (swc_common > 1.0) {
+												System.out.println("Processing planes["+nsTile+"]"+
+														", stileX="+stileX+
+														", stileY="+stileY+
+														", numPoints="+ pd.getNumPoints()+
+														", sw = "+swc_common+
+														", swc = "+pd.getWeight()+
+														", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
+														", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
+														", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
+											}
+										}
+										// now try to remove outliers
+										int max_outliers = (int) Math.round(pd.getNumPoints() * plFractOutliers);
+										if (max_outliers > plMaxOutliers) max_outliers = plMaxOutliers;
+										double targetV = plTargetEigen;
+										double z0 = pd.getZxy()[0];
+										if ((plDispNorm > 0.0) && (z0 > plDispNorm)) {
+											double dd = (plDispNorm + z0)/ plDispNorm; // > 1
+											targetV *= dd * dd; // > original
+										}
+										if (pd.getValues()[0] > targetV) {
+											pd = tpl.removePlaneOutliers(
+													pd,
+													sTiles,
+													stDisparity,
+													stStrength,
+													sel_all,
+													targetV, // double     targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
+													max_outliers, // int        maxRemoved,  // maximal number of tiles to remove (not a constant)
+													plMinPoints,  // int        minLeft,     // minimal number of tiles to keep
+													dl1); // 0); // debugLevel);
+											if (dl > 0) {
+												if (swc_common > 1.0) {
+													System.out.println("Removed outliers["+nsTile+"]"+
+															", stileX="+stileX+
+															", stileY="+stileY+
+															", numPoints="+ pd.getNumPoints()+
+															", sw = "+swc_common+
+															", swc = "+pd.getWeight()+
+															", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
+															", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
+															", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
+												}
+											}
+										} // nothing to do if already OK
+										if (dl > 0) {
+											System.out.println("Calculating World normal["+nsTile+"]");
+										}
+										
+										double [] norm_xyz = pd.getWorldXYZ(
+												correct_distortions,
+												dl);
+										if (dl > 0) {
+											System.out.println("World normal["+nsTile+"] = {"+
+													norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
+
+										}
+
+										// now try for each of the disparity-separated clusters
+										
+										double [][] mm = maxMinMax[nsTile];
+
+										if (mm.length > 1) { // multiple maximums - separate into multiple selections
+											boolean [][] stSels = new boolean[(mm.length +1)/2][stSel.length];
+											for (int i = 0; i< stSel.length; i++) if (stSel[i]){
+												double d = stDisparity[i];
+												loop:{
+													for (int m = 0; m < (stSels.length-1); m++){
+														if (d < mm[2 * m + 1][0]){
+															stSels[m][i] = true;
+															break loop;
+														}
+													}
+													stSels[stSels.length-1][i] = true;
+												}
+											}
+											double [][][][] rslts = new double [stSels.length][][][];
+											int [] np = new int[stSels.length];
+											String dbg_str = "";
+											for (int m = 0; m < stSels.length; m++){
+												for (int i =0; i < stSels[m].length; i++){
+													if (stSels[m][i]) np[m]++; 
+												}
+												dbg_str +="  "+np[m];
+												if (m < stSels.length -1){
+													dbg_str +="("+mm[2*m+1][0]+")";
+												}
+											}
+											if ((dl > 0) && (swc_common > 1.0)) {
+												System.out.println("Processing subplanes["+nsTile+"]:"+dbg_str);
+											}
+											for (int m = 0; m < stSels.length; m++) if (np[m] > plMinPoints) {
+												
+												pd = tpl.getPlane(
+														sTiles,
+														stDisparity,
+														stStrength,
+														stSels[m],
+														0); // debugLevel);
+												if (pd != null) {
+													if (dl > 0) {
+														if (swc_common > 1.0) {
+															System.out.println("Processing subplane["+nsTile+"]["+m+"]"+
+																	", stileX="+stileX+
+																	", stileY="+stileY+
+																	", numPoints="+ pd.getNumPoints()+
+																	", sw = "+swc_common+
+																	", swc = "+pd.getWeight()+
+																	", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
+																	", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
+																	", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
+														}
+													}
+													// now try to remove outliers
+													max_outliers = (int) Math.round(pd.getNumPoints() * plFractOutliers);
+													if (max_outliers > plMaxOutliers) max_outliers = plMaxOutliers;
+													targetV = plTargetEigen;
+													z0 = pd.getZxy()[0];
+													if ((plDispNorm > 0.0) && (z0 > plDispNorm)) {
+														double dd = (plDispNorm + z0)/ plDispNorm; // > 1
+														targetV *= dd * dd; // > original
+													}
+													if (pd.getValues()[0] > targetV) {
+														pd = tpl.removePlaneOutliers(
+																pd,
+																sTiles,
+																stDisparity,
+																stStrength,
+																stSels[m],
+																targetV, // double     targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
+																max_outliers, // int        maxRemoved,  // maximal number of tiles to remove (not a constant)
+																plMinPoints,  // int        minLeft,     // minimal number of tiles to keep
+																dl1); // 0); // debugLevel);
+														if (dl > 0) {
+															if (swc_common > 1.0) {
+																System.out.println("Removed outliers["+nsTile+"]["+m+"]"+
+																		", stileX="+stileX+
+																		", stileY="+stileY+
+																		", numPoints="+ pd.getNumPoints()+
+																		", sw = "+swc_common+
+																		", swc = "+pd.getWeight()+
+																		", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
+																		", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
+																		", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
+															}
+														}
+													}
+													norm_xyz = pd.getWorldXYZ(
+															correct_distortions);
+													if (dl > 0) {
+														System.out.println("World normal["+nsTile+"]["+m+"] = {"+
+																norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
+
+													}
+												}
+											}
+										}
+									}
+								}
+							}
+						}
+					};
+				}		      
+				ImageDtt.startAndJoin(threads);
+			}
 
 
 		} // end of class SuperTiles

src/main/java/TilePlanes.java

+import Jama.EigenvalueDecomposition;
+import Jama.Matrix;
+
+/**
+ **
+ ** TilePlanes - detect planes in tile clusters
+ **
+ ** Copyright (C) 2017 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **  
+ **  TilePlanes.java is free software: you can redistribute it and/or modify
+ **  it under the terms of the GNU General Public License as published by
+ **  the Free Software Foundation, either version 3 of the License, or
+ **  (at your option) any later version.
+ **
+ **  This program is distributed in the hope that it will be useful,
+ **  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ **  GNU General Public License for more details.
+ **
+ **  You should have received a copy of the GNU General Public License
+ **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ ** -----------------------------------------------------------------------------**
+ **
+ */
+
+public class TilePlanes {
+	private int tileSize = 0; // 8;
+	private int stSize =   0; // 8;
+	GeometryCorrection   geometryCorrection = null;
+	public TilePlanes(
+			int tileSize,
+			int stSize,
+			GeometryCorrection geometryCorrection){
+		this.tileSize = tileSize;
+		this.stSize = stSize;
+		this.geometryCorrection =geometryCorrection;
+	}
+	public TilePlanes(
+			int tileSize,
+			int stSize){
+		this.tileSize = tileSize;
+		this.geometryCorrection = null;
+	}
+	public class PlaneData{
+		GeometryCorrection   geometryCorrection = null;
+		boolean     correctDistortions = false;
+		double []   zxy;               // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
+		double [][] vectors =    null; // [3][3] - re-ordered/re-directed eigenvectors(transposed): [0] - plane normal, most Z-like, towards camera, [1] - X-like, [2] - Y-like
+		double []   values =     null; // [3] -eigenvalues
+		int         num_points = 0;
+		double      weight =     0.0;
+		double []   world_xyz =  null; // world coordinates of the nearest point of the plane, in meters
+		double []   daxy      =  null; // disparity and 2 relative angles (ax and ay) corresponding to fisheye view, near (0,0) scale is pixel size
+		int         tileSize;
+		int         superTileSize;
+		int []      sTileXY =    null; // X and Y indices of this superTile in the image 
+		
+		public PlaneData (
+				int [] sTileXY, 
+				int tileSize,
+				int superTileSize,
+				GeometryCorrection   geometryCorrection)
+		{
+			this.geometryCorrection = geometryCorrection;
+			this.tileSize = tileSize;
+			this.superTileSize = superTileSize;
+			this.sTileXY = sTileXY.clone();
+		}
+		public void setCorrectDistortions(
+				boolean correctDistortions)
+		{
+			this.correctDistortions = correctDistortions;
+		}
+		public boolean getCorrectDistortions() {
+			return correctDistortions;
+		}
+		public int[] getSTileXY() {
+			return sTileXY;
+		}
+		public double[] getZxy() {
+			return zxy;
+		}
+		public void setZxy(double[] zxy) {
+			this.zxy = zxy;
+		}
+		public double[][] getVectors() {
+			return vectors;
+		}
+		public double[] getVector() {
+			return vectors[0];
+		}
+		public void setVectors(double[][] vectors) {
+			this.vectors = vectors;
+		}
+		public double[] getValues() {
+			return values;
+		}
+		public double getValue() {
+			return values[0];
+		}
+		public void setValues(double[] values) {
+			this.values = values;
+		}
+		public void setValues(double v1, double v2, double v3) {
+			this.values = new double[3];
+			this.values[0] = v1;
+			this.values[1] = v2;
+			this.values[2] = v3;
+		}
+		public int getNumPoints() {
+			return num_points;
+		}
+		public void setNumPoints(int num_points) {
+			this.num_points = num_points;
+		}
+		public double getWeight() {
+			return weight;
+		}
+		public void setWeight(double weight) {
+			this.weight = weight;
+		}
+		public double [] getWorldXYZ(boolean correct_distortions)
+		{
+			return getWorldXYZ(correct_distortions,0);
+		}
+		/**
+		 * Cross product of 2 3-d vectors as column matrices
+		 * @param v1
+		 * @param v2
+		 * @return v1 x v2 as a column (3x1) matrix
+		 */
+		public Matrix cross3d (Matrix v1, Matrix v2)
+		{
+			double [][]av1 = v1.getArray();
+			double [][]av2 = v2.getArray();
+			double [][] ar = {
+					{av1[1][0]*av2[2][0] - av1[2][0]*av2[1][0]},
+					{av1[2][0]*av2[0][0] - av1[0][0]*av2[2][0]},
+					{av1[0][0]*av2[1][0] - av1[1][0]*av2[0][0]}};
+			return new Matrix (ar);
+		}
+		
+		public double [] getWorldXYZ(
+				boolean correct_distortions,
+				int debugLevel)
+		{
+			double delta = 0.0001;
+			if (world_xyz != null) return world_xyz;
+			setCorrectDistortions(correct_distortions);
+			// get pixel coordinates of the plane origin point
+			double px = tileSize*(superTileSize * sTileXY[0] + superTileSize/2) + zxy[1];  // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
+			double py = tileSize*(superTileSize * sTileXY[1] + superTileSize/2) + zxy[2];
+			double disp =  zxy[0];
+			Matrix xyz = new Matrix(geometryCorrection.getWorldCoordinates(
+					px,
+					py,
+					disp,
+					this.correctDistortions),3); // column matrix
+			Matrix dpxpy = new Matrix(vectors[0],3); // 3 rows, 1 column
+			if (debugLevel > 0){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"), correctDistortions="+correctDistortions+", xyz= {"+
+						xyz.get(0, 0)+","+xyz.get(1, 0)+","+xyz.get(2, 0)+"}");
+//				xyz.print(10, 6); // w,d
+				
+//				double [] dpxpy = geometryCorrection.getImageCoordinates(xyz.getColumnPackedCopy(),this.correctDistortions);
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): disp, px, py="+disp+","+px+","+py); // + ", reversed:"+dpxpy[0]+","+dpxpy[1]+","+dpxpy[2]);
+				
+				Matrix xyz1 = new Matrix(geometryCorrection.getWorldCoordinates(
+						px + delta * dpxpy.get(1, 0),
+						py + delta * dpxpy.get(2, 0),
+						disp + delta * dpxpy.get(0, 0),
+						this.correctDistortions),3); // column matrix
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+") delta diff: {"+
+						((xyz1.get(0, 0)- xyz.get(0, 0))/delta)+","+
+						((xyz1.get(1, 0)- xyz.get(1, 0))/delta)+","+
+						((xyz1.get(2, 0)- xyz.get(2, 0))/delta)+"}");
+			}
+			Matrix jacobian =  new Matrix(geometryCorrection.getWorldJacobian(
+					px,
+					py,
+					disp,
+					this.correctDistortions,
+					(debugLevel > 2)
+					));
+			
+			// convert both orthogonal axes, normalize their cross product
+			Matrix v1 = jacobian.times(new Matrix(vectors[1],3)); // 3 rows, 1 column
+			Matrix v2 = jacobian.times(new Matrix(vectors[2],3)); // 3 rows, 1 column
+			Matrix norm_xyz =  cross3d(v1,v2);
+//			norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
+//			norm_xyz = jacobian.times(dpxpy); // plane normal vector in world xyz
+			if (debugLevel > 0){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz={"+
+						norm_xyz.get(0, 0)+", "+norm_xyz.get(1, 0)+", "+norm_xyz.get(2, 0)+"}, (dpxpy={"+
+						dpxpy.get(0, 0)+   ", "+dpxpy.get(1, 0)+   ", "+dpxpy.get(2, 0)+"})");
+			}
+			if (debugLevel > 2){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): dpxpy=");
+				dpxpy.print(10, 6); // w,d
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian=");
+				jacobian.print(10, 6); // w,d
+/*				
+				Matrix jacobian1 =  new Matrix(geometryCorrection.getWorldJacobian(
+						px,
+						py,
+						disp,
+						this.correctDistortions,
+						0.00001));
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1=");
+				jacobian1.print(10, 6); // w,d
+*/				
+			}
+			if (debugLevel > 2){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz=");
+				norm_xyz.print(10, 6); // w,d
+			}
+			if (debugLevel > 2){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image jacobian=");
+				Matrix img_jacobian =  new Matrix(geometryCorrection.getImageJacobian(
+						xyz.getColumnPackedCopy(),
+						this.correctDistortions,
+						1));
+				img_jacobian.print(10, 6); // w,d
+/*
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image jacobian1 (0.000001)=");
+				Matrix img_jacobian1 =  new Matrix(geometryCorrection.getImageJacobian(
+						xyz.getColumnPackedCopy(),
+						this.correctDistortions,
+						0.000001));
+				img_jacobian1.print(10, 6); // w,d
+*/				
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian)=");
+				jacobian.times(img_jacobian).print(10, 6); // w,d
+/*				
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian1)=");
+				jacobian.times(img_jacobian1).print(10, 6); // w,d
+
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian)=");
+				jacobian1.times(img_jacobian).print(10, 6); // w,d
+
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian1)=");
+				jacobian1.times(img_jacobian1).print(10, 6); // w,d
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image_jacobian.inverse()=");
+				img_jacobian.inverse().print(10, 6); // w,d
+*/
+				
+			}
+			
+			norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
+			if (debugLevel > 0){
+				System.out.println("+getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): unit plane normal={"+
+						norm_xyz.get(0, 0)+", "+norm_xyz.get(1, 0)+", "+norm_xyz.get(2, 0)+"})");
+			}
+			
+			
+			if (debugLevel > 2){
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz (normalized) =");
+				norm_xyz.print(10, 6); // w,d
+				System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): xyz.times(norm_xyz.transpose()).get(0,0) ="+xyz.times(norm_xyz.transpose()).get(0,0));
+			}
+			
+			// convert plane normal vector to world coordinates
+			//world_xyz
+			world_xyz = norm_xyz.times(-(xyz.times(norm_xyz.transpose()).get(0,0))).getColumnPackedCopy();
+			return world_xyz;
+		}
+	}
+	
+	/**
+	 * Calculate covariance matrix for a subset of tile data (disparities)
+	 * Subtract weight floor from weight
+	 * @param data    data array - square (2*stSize) * (2*stSize)  
+	 * @param weight  per sample weight (should have floor already subtracted)
+	 * @param select  sample selection
+	 * @return covariance (diagonal) matrix: [0]: disparity, [1]: d<disparity>/dx, [2]: d<disparity>/dy,
+	 */
+	public double [][][] getCovar(
+			double []  data,
+			double []  weight,
+			boolean [] select,
+			double     plDispNorm, //  Normalize disparities to the average if above
+			int        debugLevel){
+		int stSize2 = 2 * stSize;
+//		Matrix covar = new Matrix(3,3);
+		double [][] acovar = new double [3][3];
+		int numPoints = 0;
+		double sw =0.0, swz = 0.0, swx = 0.0, swy = 0.0;
+
+		for (int indx = 0; indx < data.length; indx++){
+			if (select[indx] && (weight[indx] > 0)){
+				numPoints++;
+				double w = weight[indx];
+				double d = data[indx];
+				int x = ((indx % stSize2) - stSize) * tileSize; // in pixels, not in tiles
+				int y = ((indx / stSize2) - stSize) * tileSize;
+				sw  += w;
+				swz += w * d;
+				swx += w * x;
+				swy += w * y;
+			}
+		}
+		if (sw == 0.0) {
+			return null;
+		}
+		swz /= sw;
+		swx /= sw;
+		swy /= sw;
+		
+		if (debugLevel > 0){
+			System.out.println("getCovar(): sw = "+sw +", swz = "+swz +", swx = "+swx +", swy = "+swy);
+		}
+		
+		// TODO: scale disparity to make same scale for 3 axes?
+		
+		double kz = ((plDispNorm > 0.0) && (swz > plDispNorm)) ? (plDispNorm / swz) : 1.0; 
+		for (int indx = 0; indx < data.length; indx++){
+			if (select[indx] && (weight[indx] > 0)){
+				double w = weight[indx] / sw;
+				double d = kz * (data[indx] - swz);
+				double wd = w*d;
+				double x = ((indx % stSize2) - stSize) * tileSize - swx;
+				double y = ((indx / stSize2) - stSize) * tileSize - swy;
+				acovar [0][0] += wd * d;
+				acovar [0][1] += wd * x;
+				acovar [0][2] += wd * y;
+				acovar [1][1] += w * x * x;
+				acovar [1][2] += w * x * y;
+				acovar [2][2] += w * y * y;
+			}
+		}
+		acovar [1][0] = acovar [0][1]; 
+		acovar [2][0] = acovar [0][2]; 
+		acovar [2][1] = acovar [1][2];
+		Matrix covar = new Matrix(acovar);
+		EigenvalueDecomposition eig = covar.eig();
+		if (debugLevel > 0){
+			System.out.println("getCovar(): covarianvce matrix, number of used points:"+numPoints);
+			covar.print(10, 6); // w,d
+			System.out.println("getCovar(): eigenvalues");
+			eig.getD().print(10, 6); // w,d
+			System.out.println("getCovar(): eigenvectors");
+			eig.getV().print(10, 6); // w,d
+		}
+		double [][][] rslt = {
+				eig.getD().getArray(),
+				eig.getV().getArray(),
+				{
+					{sw,kz,numPoints},
+					{swz, swx, swy}}};
+		return rslt;
+	}
+	
+	public PlaneData getPlane(
+			int [] sTileXY,
+			double []  data,
+			double []  weight,
+			boolean [] select,
+			int        debugLevel){
+		double [][][] rslt = getCovar(
+				data,
+				weight,
+				select,
+				0.0,
+				0); // debugLevel);
+		if (rslt == null) return null;
+		int       numPoints =  (int) rslt[2][0][2];
+//		double    kz =   rslt[2][0][1]; // == 1.0
+		double    swc =  rslt[2][0][0];
+		double [] szxy = rslt[2][1];
+		double [][] eig_val =  rslt[0];
+		double [][] eig_vect = rslt[1];
+		// find vector most orthogonal to view // (anyway it all works with that assumption), make it first
+		// TODO?
+		int oindx = 0;
+		for (int i = 1; i <3; i++){
+			if (Math.abs(eig_vect[0][i]) > Math.abs(eig_vect[0][oindx])){
+				oindx = i;
+			}
+		}
+		// Find two other axis - "mostly X" (horizontal) and "mostly Y" (vertical) 
+		int vindx = (oindx == 0)? 1 : 0;
+		int hindx = (oindx == 0)? 2 : ((oindx == 1) ? 2 : 1);
+		if (Math.abs(eig_vect[2][vindx]) < Math.abs(Math.abs(eig_vect[2][hindx]))){
+			int tmp = vindx;
+			vindx = hindx;
+			hindx = tmp;
+		}
+		PlaneData pd = new PlaneData(
+				sTileXY,
+				this.tileSize,
+				this.stSize,
+				this.geometryCorrection);
+		pd.setZxy(szxy);
+		
+		pd.setValues(eig_val[oindx][oindx],eig_val[hindx][hindx],eig_val[vindx][vindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
+
+		double [][] plane = {
+				{eig_vect[0][oindx],eig_vect[1][oindx],eig_vect[2][oindx]},  // plane normal to camera
+				{eig_vect[0][vindx],eig_vect[1][vindx],eig_vect[2][vindx]},  // "horizontal" axis // to detect skinny planes and poles
+				{eig_vect[0][hindx],eig_vect[1][hindx],eig_vect[2][hindx]}}; //  "vertical"   axis  // to detect skinny planes and poles
+		// Make normal be towards camera (positive disparity), next vector - positive in X direction (right), last one - in positive Y (down)
+		for (int v = 0; v <3; v++) {
+			if (plane[v][v] < 0.0) for (int i = 0; i < 3; i ++) plane[v][i] = -plane[v][i];
+		}
+		pd.setVectors   (plane);
+		pd.setNumPoints (numPoints);
+		pd.setWeight    (swc);
+		return pd;
+	}
+	
+	public PlaneData removePlaneOutliers(
+			PlaneData  pd,     // already found or null 
+			int [] sTileXY,    // may be null if pd is not null 
+			double []  data,
+			double []  weight,
+			boolean [] select, // will be modified
+			double     targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
+			int        maxRemoved,  // maximal number of tiles to remove (not a constant)
+			int        minLeft,     // minimal number of tiles to keep
+			int        debugLevel){
+		int stSize2 = 2 * stSize;
+		if (pd == null) {
+			pd = getPlane(
+					sTileXY, 
+					data,
+					weight,
+					select,
+					debugLevel);
+		}
+		if (maxRemoved > (pd.getNumPoints() - minLeft)) maxRemoved = pd.getNumPoints() - minLeft;
+		int numRemoved = 0;
+		for (; (pd.getValue() > targetEigen) && (numRemoved < maxRemoved); numRemoved++){
+			if (debugLevel > 2){
+				System.out.println("removePlaneOutliers(): numRemoved = "+numRemoved+" eigenValue = "+pd.getValue()+" target = "+targetEigen);
+			}
+			// make a plane and find the worst (largest disparity difference) tile
+			// z = -(x*Vx + y*Vy)/Vz
+			double worst_d2 = 0.0;
+			int worst_index =  -1;
+			double [] v = pd.getVector();
+			double [] zxy0 = pd.getZxy();
+			for (int indx = 0; indx < data.length; indx++){
+				if (select[indx] && (weight[indx] > 0)){
+//					double w = weight[indx];
+					double x = ((indx % stSize2) - stSize) - zxy0[1];
+					double y = ((indx / stSize2) - stSize) - zxy0[2];
+					double d = data[indx];
+					d -= zxy0[0];
+					d += (x * v[1]+y*v[2])/v[0];
+					double d2 = d*d;
+					if (d2 > worst_d2){
+						worst_d2 = d2;
+						worst_index = indx;
+					}
+				}
+			}
+			if (worst_index < 0) {
+				System.out.println("This is a BUG in removePlaneOutliers()");
+				break;
+			}
+			select[worst_index] = false;
+			pd = getPlane(    // re-calculate with point removed
+					pd.getSTileXY(),
+					data,
+					weight,
+					select,
+					debugLevel);
+		}
+		return pd;
+	}
+	
+}

src/main/java/TileProcessor.java

@@ -34,7 +34,7 @@ public class TileProcessor {
 	private double   corr_magic_scale =    0.85;  // reported correlation offset vs. actual one (not yet understood)
 	private double   trustedCorrelation =  4.0;   // trusted measured disparity difference (before scaling)    
 	private int      tileSize =           8; // number of linear pixels in a tile (tile is  square tileSize*tileSize)   
-	int      superTileSize =      8; // number of linear tiles in a super-tile (supertile is  square superTileSize*superTileSize tiles
+	int               superTileSize =      8; // number of linear tiles in a super-tile (supertile is  square superTileSize*superTileSize tiles
 	                                        // or (superTileSize*tileSize) * (superTileSize*tileSize) pixels, currently 64x64 pixels)
 	public int       threadsMax =       100; // maximal number of frames to run  
 	public int       globalDebugLevel = 0;
@@ -60,6 +60,8 @@ public class TileProcessor {
 	}
 	public int getTilesX() {return tilesX;};
 	public int getTilesY() {return tilesY;};
+	public int getTileSize() {return tileSize;};
+	public int getSuperTileSize() {return superTileSize;};
 	public void setTrustedCorrelation(double trustedCorrelation)
 	{
 		this.trustedCorrelation = trustedCorrelation;
@@ -1530,35 +1532,60 @@ public class TileProcessor {
 
 	public void fillGaps( // grows, then shrinks
 			int depth, // same as grow - odd - 4 directions, even - 8
+			boolean    poison, // do not fill gaps that even touch prohibited
 			boolean [] tiles,
 			boolean [] prohibit){
 		boolean [] orig_tiles = tiles.clone();
-		growTiles(
-				depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
-				tiles,
-				prohibit);
-		// invert selection, so "grow" will be "shrink"
-		for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
-		growTiles(
-				depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
-				tiles,
-				prohibit);
-		// invert selection again (restore)
-		for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
-		//		  for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] || orig_tiles[i]; // and or with original (why result does not have some of original - only with prohibit?
-		if (prohibit != null) { // second pass w/o prohoibit, then end results
+		if (poison && (prohibit != null)) {
+			
 			growTiles(
 					depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
-					orig_tiles,
+					tiles,
 					null);
-			// invert selection, so "grow" will be "shrink"
-			for (int i =0; i< tiles.length; i++) orig_tiles[i] = !orig_tiles[i];
+			boolean [] poisoned = prohibit.clone();
+			for (int i =0; i< tiles.length; i++) poisoned[i] &= tiles[i] && !orig_tiles[i];
 			growTiles(
 					depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
-					orig_tiles,
+					poisoned,
+					orig_tiles);
+			
+			// invert selection, so "grow" will be "shrink"
+			for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
+			growTiles(
+					depth,
+					tiles,
 					null);
 			// invert selection again (restore)
-			for (int i =0; i< tiles.length; i++) tiles[i] &= !orig_tiles[i];
+			for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] && !poisoned[i];
+
+		} else {
+			growTiles(
+					depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
+					tiles,
+					prohibit);
+			// invert selection, so "grow" will be "shrink"
+			for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
+			growTiles(
+					depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
+					tiles,
+					prohibit);
+			// invert selection again (restore)
+			for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
+			//		  for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] || orig_tiles[i]; // and or with original (why result does not have some of original - only with prohibit?
+			if (prohibit != null) { // second pass w/o prohibit, then and results
+				growTiles(
+						depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
+						orig_tiles,
+						null);
+				// invert selection, so "grow" will be "shrink"
+				for (int i =0; i< tiles.length; i++) orig_tiles[i] = !orig_tiles[i];
+				growTiles(
+						depth,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
+						orig_tiles,
+						null);
+				// invert selection again (restore)
+				for (int i =0; i< tiles.length; i++) tiles[i] &= !orig_tiles[i];
+			}
 		}
 	}
 	public void growTiles(
@@ -1854,16 +1881,33 @@ public class TileProcessor {
 			final double      this_sure,        // minimal strength to be considered definitely background
 			final double      this_maybe,       // maximal strength to ignore as non-background
 			final double      sure_smth,        // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
-			final EyesisCorrectionParameters.CLTParameters clt_parameters,
-//			final int         threadsMax,  // maximal number of threads to launch                         
-//			final boolean     updateStatus,
+//			final EyesisCorrectionParameters.CLTParameters clt_parameters,
+//			final int         threadsMax,  // maximal number of threads to launch
+			
+			final boolean     show_filter_scan,
+			final int         min_clstr_seed, // clt_parameters.min_clstr_seed
+			final double      min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+			final double      min_clstr_max,     // double     min_max_weight // minimal value of the maximal strengh in the cluster
+			final int         min_clstr_lone,    // int        min_area,  // minimal number of pixels
+			final int         fill_gaps,         // int depth, // same as grow - odd - 4 directions, even - 8
+			final int         poison_gaps,       // second (usually larger than fill_gaps) when any touch of prohibited cell invalidates patching
+			final boolean     zero_gap_strength, // set strength to zero when covering gaps
 			final int         debugLevel
 	)
-	{	
+	{
+/*
+ 					clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+					clt_parameters.min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
+					clt_parameters.min_clstr_lone,  // int        min_area,  // minimal number of pixels
+						clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
+		
+ */
+		
+		
 		final int dbg_tile = 49468; // x = 220, y = 152 (pavement line)
-		final boolean show_filter_scan = clt_parameters.show_filter_scan || (debugLevel > 1);
+		final boolean show_scan = show_filter_scan || (debugLevel > 1);
 		showDoubleFloatArrays sdfa_instance = null;
-		if ((debugLevel > -1) || show_filter_scan) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
+		if ((debugLevel > -1) || show_scan) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
 		if (debugLevel > 0){
 			System.out.println("FilterScan(,,"+disparity_far+", " +disparity_near+", "+ sure_smth);
 		}
@@ -1916,7 +1960,7 @@ public class TileProcessor {
 		boolean[] dbg_before_small = null;
 		boolean[] dbg_before_lone =  null;
 		boolean[] dbg_before_gaps =  null;
-		if (clt_parameters.min_clstr_seed > 1){
+		if (min_clstr_seed > 1){
 			
 			// TODO: check - now no limit on the strength of the offending selections, only on these onses
 			
@@ -1924,44 +1968,64 @@ public class TileProcessor {
 					false, //true,            // boolean    diag_en,   // enable diagonal directions, false only up, dowm, right,left
 					far_tiles,      // boolean [] tiles_src,    // selected tiles, will modified
 					null,            // double []   weights_src,   // or null
-					clt_parameters.min_clstr_seed,  // int        min_area,  // minimal number of pixels
+					min_clstr_seed,  // int        min_area,  // minimal number of pixels
 					0.0,  // clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
 					0.0); // clt_parameters.min_clstr_max);    // double     min_max_weight // minimal value of the maximal strengh in the cluster
 			removeSmallClusters(
 					false, // true,            // boolean    diag_en,   // enable diagonal directions, false only up, dowm, right,left
 					near_tiles,      // boolean [] tiles_src,    // selected tiles, will modified
 					null,            // double []   weights_src,   // or null
-					clt_parameters.min_clstr_seed,  // int        min_area,  // minimal number of pixels
+					min_clstr_seed,  // int        min_area,  // minimal number of pixels
 					0.0, // clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
 					0.0); // clt_parameters.min_clstr_max);    // double     min_max_weight // minimal value of the maximal strengh in the cluster
-			if ((sdfa_instance!=null) && show_filter_scan) dbg_before_small = these_tiles.clone();
+			if ((sdfa_instance!=null) && show_scan) dbg_before_small = these_tiles.clone();
 			// only remove far outstanding clusters
 			removeSmallClusters( // remove single-tile clusters - anywhere
 					false, // true,            // boolean    diag_en,   // enable diagonal directions, false only up, dowm, right,left
 					these_tiles,     // boolean [] tiles_src,    // selected tiles, will modified
 					this_strength, // orig_strength, // null,            // double []   weights_src,   // or null
-					clt_parameters.min_clstr_seed, // 2,               // int        min_area,  // minimal number of pixels
-					clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
-					clt_parameters.min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
+					min_clstr_seed, // 2,               // int        min_area,  // minimal number of pixels
+					min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+					min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
 					debugLevel);
-			if ((sdfa_instance!=null) && show_filter_scan) dbg_before_lone = these_tiles.clone();
+			if ((sdfa_instance!=null) && show_scan) dbg_before_lone = these_tiles.clone();
 			removeLoneClusters(
 					false, // true,            // boolean    diag_en,   // enable diagonal directions, false only up, dowm, right,left
 					these_tiles,      // boolean [] tiles_src,    // selected tiles, will modified
 					this_strength, // orig_strength, // null,            // double []   weights_src,   // or null
-					clt_parameters.min_clstr_lone,  // int        min_area,  // minimal number of pixels
-					clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
-					clt_parameters.min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
+					min_clstr_lone,  // int        min_area,  // minimal number of pixels
+					min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+					min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
 					debugLevel);
 
-			if ((sdfa_instance!=null) && show_filter_scan) dbg_before_gaps = these_tiles.clone();
-			prohibit = far_tiles; // do not fill gaps over known background/far tiles
-			if (clt_parameters.fill_gaps > 0) {
-				fillGaps( // grows, then shrinks
-						clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
-						these_tiles,               // boolean [] tiles,
-						prohibit);
+			if ((sdfa_instance!=null) && show_scan) dbg_before_gaps = these_tiles.clone();
+			boolean [] pre_gap = null;
+			if ((fill_gaps > 0) || (poison_gaps > 0)) {
+				if (zero_gap_strength) pre_gap = these_tiles.clone();
+				prohibit = far_tiles.clone(); // do not fill gaps over known background/far tiles
+				if (fill_gaps > 0) {
+					fillGaps( // grows, then shrinks
+							fill_gaps,    // int depth, // same as grow - odd - 4 directions, even - 8
+							false,         // boolean    poison, // do not fill gaps that even touch prohibited
+							these_tiles,               // boolean [] tiles,
+							prohibit);
+				}
+
+				if (poison_gaps > 0) { // only block for when poison_gaps is enabled (last filtering with high fill_gap value)
+					for (int i = 0; i< prohibit.length; i++) prohibit[i] |=  block_propagate[i];
+					fillGaps( // grows, then shrinks
+							poison_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
+							true, // boolean    poison, // do not fill gaps that even touch prohibited
+							these_tiles,               // boolean [] tiles,
+							prohibit);
+				}
+				if (zero_gap_strength) {
+					for (int i = 0; i < these_tiles.length; i++) if (these_tiles[i] && !pre_gap[i]){
+						this_strength[i] = 0.0;
+					}
+				}
 			}
+			//zero_gap_strength
 		}		  
 		
 		double [] this_disparity_masked = this_disparity.clone();
@@ -1970,7 +2034,7 @@ public class TileProcessor {
 		}
 		
 		
-		if ((sdfa_instance!=null) && show_filter_scan){
+		if ((sdfa_instance!=null) && show_scan){
 			
 			int [] enum_clusters = enumerateClusters(
 					true, // boolean diag_en,
@@ -1979,7 +2043,7 @@ public class TileProcessor {
 			String [] titles = {"masked","map","orig_map","hor_map","vert_map","bg_sel","far",
 					"before_small","before_lone","before_gaps","these","near","block",
 					"strength","hor-strength","vert-strength",
-					"diff0","diff1","diff2","diff3", "enum_clusters", "disp_cm", "disp_poly", "disp_hor", "disp_vert"};
+					"diff0","diff1","diff2","diff3", "enum_clusters", "disp_cm", "disp_poly", "disp_hor", "disp_vert", "poison"};
 			double [][] dbg_img = new double[titles.length][tilesY * tilesX];
 			for (int i = 0; i<dbg_img[0].length;i++){
 				dbg_img[ 0][i] =   this_disparity_masked[i];
@@ -2006,6 +2070,7 @@ public class TileProcessor {
 				dbg_img[18][i] =   these_diffs[2][i];
 				dbg_img[19][i] =   these_diffs[3][i];
 				dbg_img[20][i] =   enum_clusters[i];
+				dbg_img[25][i] =   prohibit[i] ? 1 : -1;
 			}
 			dbg_img[ 2] =   scan.getDisparity(1);
 			dbg_img[ 3] =   scan.getDisparity(2);
@@ -2019,6 +2084,7 @@ public class TileProcessor {
 			dbg_img[24] =  scan. disparity_map[ImageDtt.DISPARITY_INDEX_VERT];
 			
 			sdfa_instance.showArrays(dbg_img,  tilesX, tilesY, true, "FilterScan"+clt_3d_passes.size(),titles);
+			System.out.println("FilterScan"+clt_3d_passes.size());
 		}
 		
 		return these_tiles;
@@ -2303,10 +2369,13 @@ public class TileProcessor {
 					clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
 					clt_parameters.min_clstr_max);    // double     min_max_weight // minimal value of the maximal strengh in the cluster
 			dbg_before_gaps = these_tiles.clone();
-			prohibit = far_tiles; // do not fill gaps over known background/far tiles
+			prohibit = far_tiles.clone(); // do not fill gaps over known background/far tiles
+			for (int i = 0; i< prohibit.length; i++) prohibit[i] |=  block_propagate[i];
+			
 			if (clt_parameters.fill_gaps > 0) {
 				fillGaps( // grows, then shrinks
 						clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
+						false, // boolean    poison, // do not fill gaps that even touch prohibited
 						these_tiles,               // boolean [] tiles,
 						prohibit);
 			}
@@ -2504,9 +2573,14 @@ public class TileProcessor {
 				this_sure,        // final double      this_sure,        // minimal strength to be considered definitely background
 				this_maybe,       // final double      this_maybe,       // maximal strength to ignore as non-background
 				sure_smth,        // final double      sure_smth,        // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
-				clt_parameters,
-				//					final int         threadsMax,  // maximal number of threads to launch                         
-				//					final boolean     updateStatus,
+				clt_parameters.show_filter_scan,
+				clt_parameters.min_clstr_seed, // clt_parameters.min_clstr_seed
+				clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+				clt_parameters.min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
+				clt_parameters.min_clstr_lone,  // int        min_area,  // minimal number of pixels
+				clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
+				0, // final int     poison_gaps,      // Do not fill gaps that have even single "poisoned" tile 
+				false,             // final boolean     zero_gap_strength, // set strength to zero when covering gaps
 				debugLevel);
 
 
@@ -2602,7 +2676,6 @@ public class TileProcessor {
 				0.5 * disparity_far,
 				2.0 * disparity_near,
 				debugLevel);
-
 		dbg_outlayers = new double[outlayers.length];
 
 		for (int i = 0; i < outlayers.length; i++){
@@ -2759,6 +2832,7 @@ public class TileProcessor {
 		showDoubleFloatArrays sdfa_instance = null;
 		if (debugLevel > -1) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
 		//TODO: for next passes - combine all selected for previous passes (all passes with smaller disparity)
+		boolean show_st =    clt_parameters.stShow || (debugLevel > 1);
 		
 		boolean [] these_tiles;
 		int [] replaced =  null; // +1 - hor, +2 - vert
@@ -2813,12 +2887,17 @@ public class TileProcessor {
 					this_sure,        // final double      this_sure,        // minimal strength to be considered definitely background
 					this_maybe,       // final double      this_maybe,       // maximal strength to ignore as non-background
 					sure_smth,        // final double      sure_smth,        // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
-					clt_parameters,
+					true, // clt_parameters.show_filter_scan,
+					clt_parameters.min_clstr_seed, // clt_parameters.min_clstr_seed
+					clt_parameters.min_clstr_weight,  // double     min_weight // minimal total weight of the cluster
+					clt_parameters.min_clstr_max,    // double     min_max_weight // minimal value of the maximal strengh in the cluster
+					clt_parameters.min_clstr_lone,  // int        min_area,  // minimal number of pixels
+					clt_parameters.fill_gaps, // fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
+					clt_parameters.fill_final,             // final boolean     poison_gaps,      // Do not fill gaps that have even single "poisoned" tile
+					true,             // final boolean     zero_gap_strength, // set strength to zero when covering gaps
 					//					final int         threadsMax,  // maximal number of threads to launch                         
 					//					final boolean     updateStatus,
 					2); //debugLevel);
-			
-			
 		} else {
 			these_tiles= combineHorVertDisparity(
 				scan_prev,                     // final CLTPass3d   scan,
@@ -2847,7 +2926,7 @@ public class TileProcessor {
 		double [] dbg_outlayers =       null;
 //		boolean [] grown = these_tiles.clone();
 
-		if (use_supertiles) {	
+		if (use_supertiles || show_st) {	
 			String [] dbg_st_titles = {"raw", "blurred"+clt_parameters.stSigma,"max-min-max"}; 
 			double [][] dbg_hist = new double[dbg_st_titles.length][];
 
@@ -2862,7 +2941,7 @@ public class TileProcessor {
 					0.0); // NO BLUR double     stBlurSigma)
 			dbg_hist[0] = scan_prev.showDisparityHistogram();
 
-			scan_prev.setSuperTiles(
+			SuperTiles st = scan_prev.setSuperTiles(
 					clt_parameters.stStepNear,       // double     step_disparity,
 					clt_parameters.stStepFar,        // double     step_near,
 					clt_parameters.stStepThreshold,  // double     step_threshold,
@@ -2878,23 +2957,57 @@ public class TileProcessor {
 			int hist_width0 =  scan_prev.showDisparityHistogramWidth();
 			int hist_height0 = dbg_hist[0].length/hist_width0;
 
-			scan_prev.getBgDispStrength( // calculate (check non-null)?
-					clt_parameters.stMinBgDisparity, // final double minBgDisparity, 
-					clt_parameters.stMinBgFract); // final double minBgFract);
-
-
-			dbg_orig_disparity = scan_prev.getDisparity().clone();
+			if (show_st){
+				sdfa_instance.showArrays(dbg_hist, hist_width0, hist_height0, true, "disparity_supertiles_histograms",dbg_st_titles);
+			}
 			
-			// combine weak with supertiles
-			dbg_with_super_disp = scan_prev.combineSuper(
-					true, //boolean updateStrength, // use ST strength if true, keep original (update disparity only) if false
-					clt_parameters.stStrengthScale, // Multiply st strength if used instead of regular strength (only if updateStrength)
-					clt_parameters.stUseDisp); //.15;  // Use background disparity from supertiles if tile strength is less 
+			st.processPlanes2(
+					null, // final boolean [] selected, // or null
+					0.3, // final double     min_disp,
+					false, // final boolean    invert_disp, // use 1/disparity
+					clt_parameters.plDispNorm, //            =   2.0;  // Normalize disparities to the average if above
+					clt_parameters.plMinPoints, //           =     5;  // Minimal number of points for plane detection
+					clt_parameters.plTargetEigen, //         =   0.1;  // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
+					clt_parameters.plFractOutliers, //      =   0.3;  // Maximal fraction of outliers to remove
+					clt_parameters.plMaxOutliers, //        =    20;  // Maximal number of outliers to remove
+					geometryCorrection,
+					clt_parameters.correct_distortions,
+					debugLevel); // final int        debugLevel)
+			/*			
+			st.processPlanes1(
+					null, // final boolean [] selected, // or null
+					0.3, // final double     min_disp,
+					false, // final boolean    invert_disp, // use 1/disparity
+					clt_parameters.plDispNorm, //            =   2.0;  // Normalize disparities to the average if above
+					debugLevel); // final int        debugLevel)
+			st.processPlanes(
+					null, // final boolean [] selected, // or null
+					0.3, // final double     min_disp,
+					true, // final boolean    invert_disp, // use 1/disparity
+					clt_parameters.plDispNorm, //            =   2.0;  // Normalize disparities to the average if above - here wrong
+					debugLevel); // final int        debugLevel)
+*/
 			
 			
-			if (dbg_with_super_disp != null) dbg_with_super_disp = dbg_with_super_disp.clone(); // else no super disparity available
+			if (use_supertiles) {
+				scan_prev.getBgDispStrength( // calculate (check non-null)?
+						clt_parameters.stMinBgDisparity, // final double minBgDisparity, 
+						clt_parameters.stMinBgFract); // final double minBgFract);
+
+
+				dbg_orig_disparity = scan_prev.getDisparity().clone();
+
+				// combine weak with supertiles
+				dbg_with_super_disp = scan_prev.combineSuper(
+						true, //boolean updateStrength, // use ST strength if true, keep original (update disparity only) if false
+						clt_parameters.stStrengthScale, // Multiply st strength if used instead of regular strength (only if updateStrength)
+						clt_parameters.stUseDisp); //.15;  // Use background disparity from supertiles if tile strength is less 
+
+
+				if (dbg_with_super_disp != null) dbg_with_super_disp = dbg_with_super_disp.clone(); // else no super disparity available
+			}
 		}
-		// replace weak outlaye tiles with weighted averages (modifies disparity)
+		// replace weak outlier tiles with weighted averages (modifies disparity)
 		boolean[] outlayers = scan_prev.replaceWeakOutlayers(
 				null, // final boolean [] selection,
 				clt_parameters.outlayerStrength , //final double weakStrength,    // strength to be considered weak, subject to this replacement
@@ -3124,7 +3237,7 @@ public class TileProcessor {
 			}
 			if (numCycle >= (clt_parameters.tiNumCycles -2)) { // only heal during last 2 passes?
 				if (clt_parameters.tiHealSame > 0){
-					int numHealed = dp. healSame( // returns number of new ortho connections
+					int numHealed = dp.healSame( // returns number of new ortho connections // ty = 325
 							neighbors,
 							clt_parameters.tiHealSame , // int maxlen,
 							// just to fill in diagonals
@@ -3363,14 +3476,14 @@ public class TileProcessor {
 		if (debugLevel > -1){
 			System.out.println("secondPassSetup(): created "+ numScans+ " FPGA passes.");
 		}
-		if (debugLevel > 0){
+		if (debugLevel > 10){ // null pointer
 			String [] titles = new String [clt_3d_passes.size()];
 			double [][] disparities = new double [titles.length][tilesX*tilesY];
 			for (int i = 0; i < titles.length; i++) {
 				titles[i] = i+"_scan";
 				double [][] disparityTiles =  clt_3d_passes.get(i).disparity;
 				for (int ty = 0; ty < tilesY; ty++) for (int tx = 0; tx < tilesX; tx++){
-					disparities[i][ty*tilesX+tx] = disparityTiles[ty][tx];
+					disparities[i][ty*tilesX+tx] = disparityTiles[ty][tx]; // null pointer
 				}
 			}
 			sdfa_instance.showArrays(disparities,  tilesX, tilesY, true, "disparities_scans",titles);