improving initial plane detection

src/main/java/CLTPass3d.java

@@ -534,6 +534,10 @@ public class CLTPass3d{
 				double     strength_floor,
 				double     strength_pow,
 				double     stBlurSigma,
+				boolean    smplMode, //        = true;   // Use sample mode (false - regular tile mode)
+				int        smplSide, //        = 2;      // Sample size (side of a square)
+				int        smplNum, //         = 3;      // Number after removing worst
+				double     smplRms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
 				int        measSel)
 		{
 			this.superTiles = new SuperTiles(
@@ -546,15 +550,29 @@ public class CLTPass3d{
 					strength_floor,
 					strength_pow,
 					stBlurSigma,
+					smplMode, //        = true;   // Use sample mode (false - regular tile mode)
+					smplSide, //        = 2;      // Sample size (side of a square)
+					smplNum, //         = 3;      // Number after removing worst
+					smplRms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
 					measSel);
 			return this.superTiles;
 		}
-		public double [] showDisparityHistogram(int measSel)
+		public double [] showDisparityHistogram(
+				boolean    smplMode, //        = true;   // Use sample mode (false - regular tile mode)
+				int        smplSide, //        = 2;      // Sample size (side of a square)
+				int        smplNum,  //         = 3;      // Number after removing worst
+				double     smplRms,  //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
+				int        measSel)
 		{
 			if (this.superTiles == null){
 				return null;
 			}
-			return this.superTiles.showDisparityHistogram(measSel);
+			return this.superTiles.showDisparityHistogram(
+					smplMode, //        = true;   // Use sample mode (false - regular tile mode)
+					smplSide, //        = 2;      // Sample size (side of a square)
+					smplNum,  //         = 3;      // Number after removing worst
+					smplRms,  //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
+					measSel);
 		}
 
 		public double [] showDisparityHistogram()

src/main/java/EyesisCorrectionParameters.java

@@ -2122,9 +2122,17 @@ public class EyesisCorrectionParameters {
   		public double     stMinBgFract      = 0.1;   // Minimal fraction of the disparity histogram to use as background 
   		public double     stUseDisp         = 0.15;  // Use background disparity from supertiles if tile strength is less 
   		public double     stStrengthScale   = 50.0;  // Multiply st strength if used instead of regular strength
+
+  		public boolean    stSmplMode        = true;   // Use sample mode (false - regular tile mode)
+  		public int        stSmplSide        = 2;      // Sample size (side of a square)
+  		public int        stSmplNum         = 3;      // Number after removing worst
+  		public double     stSmplRms         = 0.1;    // Maximal RMS of the remaining tiles in a sample
   		
   		public int        stMeasSel         = 1;     // Select measurements for supertiles : +1 - combo, +2 - quad +4 - hor +8 - vert
   		
+  		public double     stSmallDiff       = 0.4;   // Consider merging initial planes if disparity difference below
+  		public double     stHighMix         = 0.4;   // Consider merging initial planes if jumps between ratio above
+  		
   		
   		public double     outlayerStrength  = 0.3;   // Outlayer tiles weaker than this may be replaced from neighbors
   		public double     outlayerDiff      = 0.4;   // Replace weak outlayer tiles that do not have neighbors within this disparity difference
@@ -2213,7 +2221,8 @@ public class EyesisCorrectionParameters {
   		public boolean    show_neighbors =         false; // show 'neighbors' 
   		public boolean    show_flaps_dirs =        false; // show 'flaps-dirs' 
   		public boolean    show_first_clusters =    false; // show 'first_N_clusters' 
-  		public boolean    show_planes =            false; // show planes 
+  		public boolean    show_planes =            false; // show planes
+  		public double []  vertical_xyz =           {0.0,1.0,0.0}; // real world up unit vector in camera CS (x - right, y - up, z - to camera};
   		
   		public CLTParameters(){}
   		public void setProperties(String prefix,Properties properties){
@@ -2414,7 +2423,16 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"stMinBgFract",     this.stMinBgFract +"");
 			properties.setProperty(prefix+"stUseDisp",        this.stUseDisp +"");
 			properties.setProperty(prefix+"stStrengthScale",  this.stStrengthScale +"");
-  			properties.setProperty(prefix+"stMeasSel",        this.stMeasSel+"");
+
+			properties.setProperty(prefix+"stSmplMode",       this.stSmplMode+"");
+  			properties.setProperty(prefix+"stSmplSide",       this.stSmplSide+"");
+  			properties.setProperty(prefix+"stSmplNum",        this.stSmplNum+"");
+			properties.setProperty(prefix+"stSmplRms",        this.stSmplRms +"");
+
+			properties.setProperty(prefix+"stMeasSel",        this.stMeasSel+"");
+			properties.setProperty(prefix+"stSmallDiff",      this.stSmallDiff +"");
+			properties.setProperty(prefix+"stHighMix",        this.stHighMix +"");
+  			
 			
 			properties.setProperty(prefix+"outlayerStrength", this.outlayerStrength +"");
 			properties.setProperty(prefix+"outlayerDiff",     this.outlayerDiff +"");
@@ -2494,7 +2512,11 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"show_flaps_dirs",        this.show_flaps_dirs+"");
 			properties.setProperty(prefix+"show_first_clusters",    this.show_first_clusters+"");
 			properties.setProperty(prefix+"show_planes",            this.show_planes+"");
-  		}
+
+			properties.setProperty(prefix+"vertical_xyz.x",         this.vertical_xyz[0]+"");
+			properties.setProperty(prefix+"vertical_xyz.y",         this.vertical_xyz[1]+"");
+			properties.setProperty(prefix+"vertical_xyz.z",         this.vertical_xyz[2]+"");
+}
   		public void getProperties(String prefix,Properties properties){
   			if (properties.getProperty(prefix+"transform_size")!=null) this.transform_size=Integer.parseInt(properties.getProperty(prefix+"transform_size"));
   			if (properties.getProperty(prefix+"clt_window")!=null)     this.clt_window=Integer.parseInt(properties.getProperty(prefix+"clt_window"));
@@ -2687,7 +2709,15 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"stMinBgFract")!=null)      this.stMinBgFract=Double.parseDouble(properties.getProperty(prefix+"stMinBgFract"));
   			if (properties.getProperty(prefix+"stUseDisp")!=null)         this.stUseDisp=Double.parseDouble(properties.getProperty(prefix+"stUseDisp"));
   			if (properties.getProperty(prefix+"stStrengthScale")!=null)   this.stStrengthScale=Double.parseDouble(properties.getProperty(prefix+"stStrengthScale"));
+
+  			if (properties.getProperty(prefix+"stSmplMode")!=null)        this.stSmplMode=Boolean.parseBoolean(properties.getProperty(prefix+"stSmplMode"));
+  			if (properties.getProperty(prefix+"stSmplSide")!=null)        this.stSmplSide=Integer.parseInt(properties.getProperty(prefix+"stSmplSide"));
+  			if (properties.getProperty(prefix+"stSmplNum")!=null)         this.stSmplNum=Integer.parseInt(properties.getProperty(prefix+"stSmplNum"));
+  			if (properties.getProperty(prefix+"stSmplRms")!=null)         this.stSmplRms=Double.parseDouble(properties.getProperty(prefix+"stSmplRms"));
+
   			if (properties.getProperty(prefix+"stMeasSel")!=null)         this.stMeasSel=Integer.parseInt(properties.getProperty(prefix+"stMeasSel"));
+  			if (properties.getProperty(prefix+"stSmallDiff")!=null)       this.stSmallDiff=Double.parseDouble(properties.getProperty(prefix+"stSmallDiff"));
+  			if (properties.getProperty(prefix+"stHighMix")!=null)         this.stHighMix=Double.parseDouble(properties.getProperty(prefix+"stHighMix"));
 
   			if (properties.getProperty(prefix+"outlayerStrength")!=null)  this.outlayerStrength=Double.parseDouble(properties.getProperty(prefix+"outlayerStrength"));
   			if (properties.getProperty(prefix+"outlayerDiff")!=null)      this.outlayerDiff=Double.parseDouble(properties.getProperty(prefix+"outlayerDiff"));
@@ -2767,6 +2797,12 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"show_flaps_dirs")!=null)        this.show_flaps_dirs=Boolean.parseBoolean(properties.getProperty(prefix+"show_flaps_dirs"));
   			if (properties.getProperty(prefix+"show_first_clusters")!=null)    this.show_first_clusters=Boolean.parseBoolean(properties.getProperty(prefix+"show_first_clusters"));
   			if (properties.getProperty(prefix+"show_planes")!=null)            this.show_planes=Boolean.parseBoolean(properties.getProperty(prefix+"show_planes"));
+  			
+  			
+  			if (properties.getProperty(prefix+"vertical_xyz.x")!=null)     this.vertical_xyz[0]=Double.parseDouble(properties.getProperty(prefix+"vertical_xyz.x"));
+  			if (properties.getProperty(prefix+"vertical_xyz.y")!=null)     this.vertical_xyz[1]=Double.parseDouble(properties.getProperty(prefix+"vertical_xyz.y"));
+  			if (properties.getProperty(prefix+"vertical_xyz.z")!=null)     this.vertical_xyz[2]=Double.parseDouble(properties.getProperty(prefix+"vertical_xyz.z"));
+  			
   		}
   		
   		public boolean showDialog() {
@@ -2981,7 +3017,15 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("Minimal fraction of the disparity histogram to use as background",             this.stMinBgFract,  6);
   			gd.addNumericField("Use background disparity from supertiles if tile strength is less",            this.stUseDisp,  6);
   			gd.addNumericField("Multiply st strength if used instead of regular strength ",                    this.stStrengthScale,  6);
+
+  			gd.addCheckbox    ("Use sample mode (false - regular tile mode)",                                  this.stSmplMode);
+  			gd.addNumericField("Sample size (side of a square)",                                               this.stSmplSide,  0);
+  			gd.addNumericField("Number after removing worst",                                                  this.stSmplNum,  0);
+  			gd.addNumericField("Maximal RMS of the remaining tiles in a sample",                               this.stSmplRms,  6);
+  			
   			gd.addNumericField("Select measurements for supertiles : +1 - combo, +2 - quad +4 - hor +8 - vert",this.stMeasSel,  0);
+  			gd.addNumericField("Consider merging initial planes if disparity difference below",                this.stSmallDiff,  6);
+  			gd.addNumericField("Consider merging initial planes if jumps between ratio above",                 this.stHighMix,  6);
 
   			gd.addNumericField("Outlayer tiles weaker than this may be replaced from neighbors",               this.outlayerStrength,  6);
   			gd.addNumericField("Replace weak outlayer tiles that do not have neighbors within this disparity difference", this.outlayerDiff,  6);
@@ -3064,6 +3108,8 @@ public class EyesisCorrectionParameters {
   			gd.addCheckbox    ("Show 'flaps-dirs'",                                                            this.show_flaps_dirs);
   			gd.addCheckbox    ("Show 'first_N_clusters'",                                                      this.show_first_clusters);
   			gd.addCheckbox    ("Show planes",                                                                  this.show_planes);
+  			gd.addMessage     ("Unity up vector in camera coordinate system (x - right, y - up, z - to camera): {"+
+  			this.vertical_xyz[0]+","+this.vertical_xyz[1]+","+this.vertical_xyz[2]+"}");
   			WindowTools.addScrollBars(gd);
   			gd.showDialog();
   			
@@ -3264,7 +3310,16 @@ public class EyesisCorrectionParameters {
   			this.stMinBgFract=          gd.getNextNumber();
   			this.stUseDisp=             gd.getNextNumber();
   			this.stStrengthScale=       gd.getNextNumber();
+
+  			this.stSmplMode=            gd.getNextBoolean();
+  			this.stSmplSide=      (int) gd.getNextNumber();
+  			this.stSmplNum=       (int) gd.getNextNumber();
+  			this.stSmplRms=             gd.getNextNumber();
+  			
   			this.stMeasSel=       (int) gd.getNextNumber();
+  			this.stSmallDiff=           gd.getNextNumber();
+  			this.stHighMix=             gd.getNextNumber();
+
   			this.outlayerStrength=      gd.getNextNumber();
   			this.outlayerDiff=          gd.getNextNumber();
   			this.outlayerDiffPos=       gd.getNextNumber();

src/main/java/GeometryCorrection.java

@@ -311,6 +311,11 @@ public class GeometryCorrection {
 	public double getZFromDisparity(double disparity){
 		return SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (disparity * 0.001*this.pixelSize);
 	}
+
+	public double getDisparityFromZ(double z){
+		return (1000.0 * SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / this.pixelSize) / z;
+	}
+	
 	
 	public double getFOVPix(){ // get ratio of 1 pixel X/Y to Z (distance to object)
 		return 0.001 * this.pixelSize / this.focalLength;

src/main/java/MeasuredLayers.java

@@ -368,6 +368,7 @@ public class MeasuredLayers {
 		if (null_if_none && (num_selected == 0)) return null;
 		return selection;
 	}
+	
 
 	/**
 	 * Get selection for the specific measurement layer and supertile X,Y coordinates
@@ -425,7 +426,80 @@ public class MeasuredLayers {
 		if (null_if_none && (num_selected == 0)) return null;
 		return selection;
 	}
+	
+	/**
+	 * Get selection when disparity/strength is already calculated. Useful when
+	 * disparity/strength are not just measured values, by sampled over an area
+	 * @param disparityStrength array of {disparity, strength} where each is
+	 *  a linescan data of the (2 * superTileSize) * (2 * superTileSize)   
+	 * @param sel_in optional input selection array or null - use all
+	 * @param null_if_none return null if selection has no tiles enabled
+	 * @return boolean array of per-tile enabled/disabled values in linescan
+	 *  order, (2 * superTileSize) * (2 * superTileSize)
+	 */
 
+	public boolean [] getSupertileSelection(
+			double [][] disparityStrength,
+			boolean [] sel_in,
+			boolean null_if_none)
+	{
+		int st2 = 2 * superTileSize;
+		boolean [] selection = new boolean [st2 * st2];
+		int num_selected = 0;
+		for (int i = 0; i < disparityStrength[1].length; i++){
+			if (	(disparityStrength[1][i] > 0.0) &&
+					((sel_in == null) || (sel_in[i]))){
+				selection[i] = true;
+				num_selected ++;
+			}
+		}
+		
+		if (null_if_none && (num_selected == 0)) return null;
+		return selection;
+	}
+	
+	/**
+	 * Get selection when disparity/strength is already calculated. Useful when
+	 * disparity/strength are not just measured values, by sampled over an area
+	 * Includes low/high limits for disparity values
+	 * @param disparityStrength array of {disparity, strength} where each is
+	 *  a linescan data of the (2 * superTileSize) * (2 * superTileSize)   
+	 * @param sel_in optional input selection array or null - use all
+	 * @param disp_far low limit for disparity, Double.NaN - do not check
+	 * @param disp_near high limit for disparity, Double.NaN - do not check
+	 * @param null_if_none return null if selection has no tiles enabled
+	 * @return boolean array of per-tile enabled/disabled values in linescan
+	 *  order, (2 * superTileSize) * (2 * superTileSize)
+	 * @return
+	 */
+	public boolean [] getSupertileSelection(
+			double [][] disparityStrength,
+			boolean [] sel_in,
+			double     disp_far,
+			double     disp_near,
+			boolean null_if_none)
+	{
+		int st2 = 2 * superTileSize;
+		boolean [] selection = new boolean [st2 * st2];
+		int num_selected = 0;
+		for (int i = 0; i < disparityStrength[1].length; i++){
+			if (	(disparityStrength[1][i] > 0.0) &&
+					((sel_in == null) || (sel_in[i]))){
+				if (    (Double.isNaN(disp_far)  || (disparityStrength[0][i] >= disp_far)) &&
+						(Double.isNaN(disp_near) || (disparityStrength[0][i] <= disp_near))) {
+					
+				}
+				selection[i] = true;
+				num_selected ++;
+			}
+		}
+		
+		if (null_if_none && (num_selected == 0)) return null;
+		return selection;
+	}
+	
+	
+	
 	
 	/**
 	 * Get number of "true" elements in a boolean array. Null is OK, it results in 0
@@ -453,6 +527,19 @@ public class MeasuredLayers {
 		}
 		return sw;
 	}
+	public static double getSumStrength(
+			double [][] disp_strength,
+			boolean [] selected)
+	{
+		if (disp_strength == null) return 0.0;
+		double sw = 0.0;
+		for (int i = 0; i < disp_strength[1].length; i++){
+			if ((selected == null) || selected[i]) {
+				sw += disp_strength[1][i];
+			}
+		}
+		return sw;
+	}
 	
 	/**
 	 * Get disparity and correlation strength for the specific measurement layer and
@@ -510,4 +597,153 @@ public class MeasuredLayers {
 		if (null_if_none && (num_selected == 0)) return null;
 		return ds;
 	}
+
+	/**
+	 * Get double-size  (for overlapping) array of disparities and strengths for the supertile
+	 * Using best (producing lowest disparity variance) subset of neighbor tiles
+	 * @param num_layer number of measurement layer (currently 0 - composite, 1 - quad, 2 - horizontal
+	 * and 3 - vertical piars correlation
+	 * @param stX supertile horizontal index
+	 * @param stY supertile vertical index
+	 * @param sel_in input selection of the output data samples (or null)
+	 * @param strength_floor subtract from the correlation strength (limit by 0) before using as a
+	 *  sample weight
+	 * @param strength_pow raise correlation strength (after subtracting strength_floor) to this power
+	 *  to use as a sample weight
+	 * @param smplSide size of the square sample side 
+	 * @param smplNum number of averaged samples (should be <= smplSide * smplSide and > 1) 
+	 * @param smplRms maximal square root of variance (in disparity pixels) to accept the result
+	 * @param null_if_none return null if there are no usable tiles in the result
+	 * @return a pair of arrays (disparity and strengths) in line-scan order each
+	 */
+	public double[][] getDisparityStrength (
+			int num_layer,
+			int stX,
+			int stY,
+			boolean [] sel_in,
+			double     strength_floor,
+			double     strength_pow,
+			int        smplSide, //        = 2;      // Sample size (side of a square)
+			int        smplNum, //         = 3;      // Number after removing worst (should be >1)
+			double     smplRms, //         = 0.1;    // Maximal RMS of the remaining tiles in a sample
+			boolean null_if_none)
+	{
+		if ((layers[num_layer] == null) && null_if_none){
+			return null;
+		}
+		int st2 = 2 * superTileSize;
+		int st_half = superTileSize/2;
+		double [][] ds = new double [2][st2*st2];
+		int num_selected = 0;
+		int smpl_center = smplSide /2;
+		int st2e = st2 + smplSide;
+		int smplLen = smplSide*smplSide;
+		double [] disp =     new double [st2e * st2e]; 
+		double [] weight = new double [st2e * st2e]; 
+		int st_halfe = st_half + smpl_center;
+		double smlVar = smplRms * smplRms; // maximal variance (weighted average of the squared difference from the mean)
+		
+		if (layers[num_layer] != null) {
+			for (int dy = 0; dy < st2e; dy ++){
+				int y = superTileSize * stY -st_halfe + dy;
+				if ((y >= 0) && (y < tilesY)) {
+					for (int dx = 0; dx < st2e; dx ++){
+						int x = superTileSize * stX -st_halfe + dx;
+						if ((x >= 0) && (x < tilesX)) {
+							int indx = y * tilesX + x;
+							int indx_ste = dy * st2e + dx;
+							if (layers[num_layer][indx] != null){ // apply sel_in later
+								disp[indx_ste] = layers[num_layer][indx].getDisparity();
+								double w = layers[num_layer][indx].getStrength() - strength_floor;
+								if (w > 0) {
+									if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
+//									w *= lapWeight[dy][dx];
+									disp[indx_ste] = layers[num_layer][indx].getDisparity();
+									weight[indx_ste] = w;
+									num_selected ++;
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+		if (null_if_none && (num_selected == 0)) return null;
+		// now work with disp, strength [st2e*st2de] and filter results to ds[2][st2*st2], applying sel_in
+		num_selected = 0;
+		for (int dy = 0; dy < st2; dy ++){
+			for (int dx = 0; dx < st2; dx ++){
+				int indx = dy * st2 + dx;
+				if (((sel_in == null) || sel_in[indx])){
+					int num_in_sample = 0;
+					boolean [] smpl_sel = new boolean [smplLen];
+					double [] smpl_d =  new double [smplLen];
+					double [] smpl_w =  new double [smplLen];
+					for (int sy = 0; sy < smplSide; sy++){
+						int y = dy + sy; //  - smpl_center;
+						for (int sx = 0; sx < smplSide; sx++){
+							int x = dx + sx; // - smpl_center;
+							int indxe = y * st2e + x;
+							if (weight[indxe] > 0.0){
+								int indxs = sy * smplSide + sx;
+								smpl_sel[indxs] = true;
+								smpl_d[indxs] = disp[indxe]; 
+								smpl_w[indxs] = weight[indxe]; 
+								num_in_sample ++;
+							}
+						}
+					}
+					if (num_in_sample >= smplNum){ // try, remove worst
+						// calculate 
+						double sd=0.0, sd2 = 0.0, sw = 0.0;
+						for (int i = 0; i < smplLen; i++) if (smpl_sel[i]) {
+							double dw = smpl_d[i] * smpl_w[i];
+							sd += dw;
+							sd2 += dw * smpl_d[i];
+							sw +=       smpl_w[i];
+						}
+						// remove worst, update sd2, sd and sw
+						while ((num_in_sample > smplNum) && (sw > 0)){ // try, remove worst
+							double d_mean = sd/sw;
+							int iworst = -1;
+							double dworst2 = 0.0;
+							for (int i = 0; i < smplLen; i++) if (smpl_sel[i]) {
+								double d2 = (smpl_d[i] - d_mean);
+								d2 *=d2;
+								if (d2 > dworst2) {
+									iworst = i;
+									dworst2 = d2;
+								}
+							}
+							if (iworst < 0){
+								System.out.println("**** this is a BUG in getDisparityStrength() ****");
+								break;
+							}
+							// remove worst sample
+							smpl_sel[iworst] = false;
+							double dw = smpl_d[iworst] * smpl_w[iworst];
+							sd -= dw; 
+							sd2 -= dw * smpl_d[iworst];
+							sw -=       smpl_w[iworst];
+							num_in_sample --;
+						}
+						// calculate variance of the remaining set
+						if (sw > 0.0) {
+							sd /= sw;
+							sd2 /= sw;
+							double var = sd2 - sd * sd;
+							if (var < smlVar) { // good, save in the result array
+								ds[0][indx] = sd;
+								ds[1][indx] = sw * lapWeight[dy][dx] /num_in_sample; // average weights, multiply by window
+							}
+						} else {
+							num_in_sample = 0;
+							System.out.println("**** this is a BUG in getDisparityStrength(), shoud not happen ? ****");
+						}
+					}
+				}				
+			}
+		}
+		return ds;
+	}
 }