refactoring

src/main/java/CLTPass3d.java

@@ -36,7 +36,7 @@ public class CLTPass3d{
 		double []               calc_disparity_hor =   null; // composite disparity, calculated from "disparity", and "disparity_map" fields
 		double []               calc_disparity_vert =  null; // composite disparity, calculated from "disparity", and "disparity_map" fields
 		double []               calc_disparity_combo = null; // composite disparity, calculated from "disparity", and "disparity_map" fields
-		double []               strength =             null; // composite strength, initially uses a copy of raw 4-sensor correleation strength
+		double []               strength =             null; // composite strength, initially uses a copy of raw 4-sensor correlation strength
 		double []               strength_hor =         null; // updated hor strength, initially uses a copy of raw measured
 		double []               strength_vert =        null; // updated hor strength, initially uses a copy of raw measured
 		// Bg disparity & strength is calculated from the supertiles and used instead of the tile disparity if it is too weak. Assuming, that
@@ -333,7 +333,7 @@ public class CLTPass3d{
 		}
 		/**
 		 * Get one of the line-scan per-tile correlation data.
-		 * @param mode 0 - final data (initially copy FPGA generated 4-pair correation)
+		 * @param mode 0 - final data (initially copy FPGA generated 4-pair correlation)
 		 *             1 - original FPGA generated 4-sensor correlation
 		 *             2 - 2 - horizontal pairs correlation, detecting vertical features
 		 *             3 - 2 - vertical pairs correlation, detecting horizontal features

src/main/java/EyesisCorrectionParameters.java

@@ -2182,6 +2182,7 @@ public class EyesisCorrectionParameters {
   		
   		public boolean    replaceWeakOutlayers =   true; // false; 
   		
+  		public boolean    dbg_migrate =            true; 
   		
   		// other debug images
   		public boolean    show_ortho_combine =     false; // Show 'ortho_combine' 
@@ -2451,7 +2452,9 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"plSnapDispMax",    this.plSnapDispMax +"");
 			properties.setProperty(prefix+"plSnapDispWeight", this.plSnapDispWeight +"");
   			properties.setProperty(prefix+"plSnapZeroMode",   this.plSnapZeroMode+"");
-			
+
+			properties.setProperty(prefix+"dbg_migrate",            this.dbg_migrate+"");
+  			
 			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+"");
@@ -2711,6 +2714,8 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"plSnapDispMax")!=null)     this.plSnapDispMax=Double.parseDouble(properties.getProperty(prefix+"plSnapDispMax"));
   			if (properties.getProperty(prefix+"plSnapDispWeight")!=null)  this.plSnapDispWeight=Double.parseDouble(properties.getProperty(prefix+"plSnapDispWeight"));
   			if (properties.getProperty(prefix+"plSnapZeroMode")!=null)    this.plPrecision=Integer.parseInt(properties.getProperty(prefix+"plSnapZeroMode"));
+ 
+  			if (properties.getProperty(prefix+"dbg_migrate")!=null)       this.dbg_migrate=Boolean.parseBoolean(properties.getProperty(prefix+"dbg_migrate"));
 
   			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"));
@@ -2995,6 +3000,8 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("Maximal disparity diff. by weight product to snap to plane",                   this.plSnapDispWeight,  6);
   			gd.addNumericField("Zero strength snap mode: 0: no special treatment, 1 - strongest, 2 - farthest",this.plSnapZeroMode,  0);
   			
+  			gd.addCheckbox    ("Test new mode after migration",                                                this.dbg_migrate);
+
   			gd.addMessage     ("--- Other debug images ---");
   			gd.addCheckbox    ("Show 'ortho_combine'",                                                         this.show_ortho_combine);
   			gd.addCheckbox    ("Show 'refine_disparity_supertiles'",                                           this.show_refine_supertiles);
@@ -3264,6 +3271,8 @@ public class EyesisCorrectionParameters {
   			this.plSnapDispWeight=      gd.getNextNumber();
   			this.plSnapZeroMode=  (int) gd.getNextNumber();
 
+  			this.dbg_migrate=           gd.getNextBoolean();
+
   			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/MeasuredLayers.java

+/**
+ ** MeasuredLayer - per-tile measured disparity/strength pairs,
+ ** multiple layers can be used for 4-disparity and 2 (hor/vert) pairs
+ ** separately 
+ **
+ ** Copyright (C) 2017 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **  
+ **  MeasuredLayer.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 MeasuredLayers {
+	private DispStrength [][] layers;
+	private int tilesX;
+	private int tilesY;
+	private int superTileSize;
+	private double [][] lapWeight = null;
+	public class DispStrength {
+		public double disparity = Double.NaN;
+		public double strength =  0.0;
+		public DispStrength (
+				double disparity,
+				double strength)
+		{
+			this.disparity = disparity;
+			this.strength = strength;
+			if (Double.isNaN(disparity)){
+				this.strength = 0.0;
+			}
+		}
+		public double [] getDispStrength()
+		{
+			double [] ds = {disparity, strength};
+			return ds;
+		}
+		public void setDisparity(double disparity)
+		{
+			this.disparity = disparity;
+			if (Double.isNaN(disparity)){
+				this.strength = 0.0;
+			}
+		}
+		public void setStrength(double strength)
+		{
+			this.strength = strength;
+		}
+		public double getDisparity()
+		{
+			return disparity;
+		}
+		public double getStrength()
+		{
+			return strength;
+		}
+	}
+	/**
+	 * Create a set of measured layers. Several layers can be used to represent alternative measurements
+	 * (with different preset disparity) or for different modes (4 pair correlation, vertical, horizontal)
+	 * @param num_layers number of alternative layers used (3 for quad, hor, vert)
+	 * @param tilesX number of tiles horizontally in the image
+	 * @param tilesY number of tiles vertically in the image
+	 * @param superTileSize size of the supertile square
+	 */
+	public MeasuredLayers(
+			int num_layers,
+			int tilesX,
+			int tilesY,
+			int superTileSize)
+	{
+		layers = new DispStrength [num_layers][];
+		this.tilesX = tilesX;
+		this.tilesY = tilesY;
+		this.superTileSize = superTileSize;
+		this.lapWeight =  getLapWeights();
+	}
+	
+	/**
+	 * Get number of tiles in the image horizontally
+	 * @return tilesX
+	 */
+	public int getTilesX()
+	{
+			return tilesX;
+	}
+
+	/**
+	 * Get number of tiles in the image vertically
+	 * @return tilesY
+	 */
+	public int getTilesY()
+	{
+			return tilesY;
+	}
+	/**
+	 * Get number of tiles in each supertile in eac direction
+	 * @return superTileSize
+	 */
+
+	public int getSuperTileSize()
+	{
+			return superTileSize;
+	}
+	
+	/**
+	 * Set measured layer 
+	 * @param num_layer number of the layer to set
+	 * @param disparity array of per-tile disparity values (linescan order),
+	 *  NaN deletes the tile 
+	 * @param strength array of per-tile disparity values (linescan order),
+	 *  0.0 strength is OK - does not delete the tile
+	 * @param selection optional tile selection (or null). If unselected, tile is deleted 
+	 */
+	public void setLayer (
+			int       num_layer,
+			double [] disparity,
+			double [] strength,
+			boolean [] selection) // may be null
+	{	
+		if (layers[num_layer] == null) {
+			int llen = disparity.length;
+			if (layers[num_layer] == null) {
+				layers[num_layer] = new DispStrength [llen];
+			}
+		}
+		for (int i = 0; i < disparity.length; i++){
+			if ((selection != null) && !selection[i]){
+				layers[num_layer][i] = null;
+			} else {
+				if (Double.isNaN(disparity[i])){
+					layers[num_layer][i] = null;
+				} else {
+					if (layers[num_layer][i] != null){
+						layers[num_layer][i].setDisparity(disparity[i]);
+						layers[num_layer][i].setStrength(strength[i]);
+					} else {
+						layers[num_layer][i] = new DispStrength(disparity[i], strength[i]);
+					}
+				}
+			}
+		}
+	}
+	
+	/**
+	 * Set disparity values for selected measurement layer 
+	 * @param num_layer number of the layer to set
+	 * @param disparity array of per-tile disparity values (linescan order),
+	 *  NaN deletes the tile. New tile set strength = 0.0 
+	 */
+	public void setDisparity (
+			int       num_layer,
+			double [] disparity)
+	{	
+		if (layers[num_layer] == null) {
+			int llen = disparity.length;
+			if (layers[num_layer] == null) {
+				layers[num_layer] = new DispStrength [llen];
+			}
+		}
+		for (int i = 0; i < disparity.length; i++){
+			if (Double.isNaN(disparity[i])){
+				layers[num_layer][i] = null;
+			} else {
+				if (layers[num_layer][i] != null){
+					layers[num_layer][i].setDisparity(disparity[i]);
+				} else {
+					layers[num_layer][i] = new DispStrength(disparity[i], 0.0);
+				}
+			}
+		}
+	}
+
+	/**
+	 * Set strength values for selected measurement layer. Does not allocate new tiles
+	 * if that element was null  
+	 * @param num_layer number of the layer to set
+	 * @param strength array of per-tile strength values (linescan order)
+	 */
+	public void setStrength (
+			int       num_layer,
+			double [] strength)
+	{	
+		if (layers[num_layer] != null) {
+			for (int i = 0; i < strength.length; i++){
+				if (layers[num_layer][i] != null){
+					layers[num_layer][i].setStrength(strength[i]);
+				}
+			}
+		}
+	}
+	
+	/**
+	 * Change tile selection for the layer. Can only unselect existing tiles, not add new ones
+	 * @param num_layer number of the layer to set
+	 * @param selection array of per-tile boolean selection values (linescan order),
+	 */
+
+	public void setSelection (
+			int       num_layer,
+			boolean [] selection)
+	{	
+		for (int i = 0; i < selection.length; i++){
+			if (!selection[i]){
+				layers[num_layer][i] = null;
+			}
+		}
+	}
+	
+	/**
+	 * Get total number of the measurement layers in this object
+	 * @return number of layers (some may be uninitialized
+	 */
+	
+	public int getnumLayers()
+	{
+		if (layers == null){
+			return 0;
+		} else {
+			return layers.length;
+		}
+	}
+
+	/**
+	 * Get array of disparity values for the selected layer
+	 * @param num_layer number of the layer to read
+	 * @return array of per-tile disparity values (in linescan order),
+	 *  Double.NaN for missing tiles 
+	 */
+	public double [] getDisparity(
+			int num_layer)
+	{
+		double [] disparity = new double [layers[num_layer].length];
+		for (int i = 0; i < disparity.length; i++){
+			if (layers[num_layer][i] == null){
+				disparity[i] = Double.NaN;
+			} else {
+				disparity[i] = layers[num_layer][i].getDisparity();
+			}
+		}
+		return disparity;
+	}
+	
+	/**
+	 * Get array of correlation strength values for the selected layer
+	 * @param num_layer number of the layer to read
+	 * @return array of per-tile strength values (in linescan order),
+	 * 0.0 for missing tiles 
+	 */
+
+	public double [] getStrength(
+			int num_layer)
+	{
+		double [] strength = new double [layers[num_layer].length];
+		for (int i = 0; i < strength.length; i++){
+			if (layers[num_layer][i] == null){
+				strength[i] = 0.0;
+			} else {
+				strength[i] = layers[num_layer][i].getStrength();
+			}
+		}
+		return strength;
+	}
+	
+	/**
+	 * Get array of existing tiles for the selected layer
+	 * @param num_layer number of the layer to read
+	 * @return boolean (per tile) array of existing tiles
+	 */
+	public boolean [] getSelection(
+			int num_layer)
+	{
+		boolean [] selection = new boolean [layers[num_layer].length];
+		for (int i = 0; i < selection.length; i++){
+			selection[i] = layers[num_layer][i] != null; 
+		}
+		return selection;
+	}
+	
+	/**
+	 * Calculate weights for overlapping supertiles to multiply correlation strengths
+	 * @return square 2-d array of weights 0.0 ... 1.0, each dimension twice the
+	 * supertile size
+	 */
+	private double [][] getLapWeights(){
+		final double [][] lapWeight = new double [2 * superTileSize][2 * superTileSize];
+		final double [] lapWeight1d = new double [superTileSize];
+		final int superTileSize2 = 2 * superTileSize;
+		for (int i = 0; i < superTileSize; i++){
+			lapWeight1d[i] = 0.5*(1.0 - Math.cos((i + 0.5)* Math.PI/superTileSize));
+		}
+		for (int i = 0; i < superTileSize; i++){
+			for (int j = 0; j < superTileSize; j++){
+				lapWeight[i]                     [                     j] = lapWeight1d[i]*lapWeight1d[j]; 
+				lapWeight[superTileSize2 - 1 - i][                     j] = lapWeight[i][j];
+				lapWeight[i]                     [superTileSize2 - 1 - j] = lapWeight[i][j]; 
+				lapWeight[superTileSize2 - 1 - i][superTileSize2 - 1 - j] = lapWeight[i][j]; 
+			}
+		}
+		double s = 0.0;
+		for (int i = 0; i < superTileSize2; i++){
+			for (int j = 0; j < superTileSize2; j++){
+				s+=lapWeight[i][j];
+			}
+		}
+		System.out.println("getLapWeights: sum = "+s);
+		return lapWeight;
+	}
+	
+	/**
+	 * Get selection for the specific measurement layer and supertile X,Y coordinates
+	 * in the image. Combined with input selection
+	 * @param num_layer number of the measurement layer to process
+	 * @param stX supertile horizontal position in the image
+	 * @param stY supertile vertical position in the image
+	 * @param sel_in optional selection for this supertile (linescan, 4 * supetile size)
+	 * @param null_if_none return null if there are no selected tiles in teh result selection
+	 * @return boolean array [4*superTileSize] of the selected tiles on the specified
+	 * measurement layer
+	 */
+	public boolean [] getSupertileSelection(
+			int num_layer,
+			int stX,
+			int stY,
+			boolean [] sel_in,
+			double     strength_floor,
+			boolean null_if_none)
+	{
+		if ((layers[num_layer] == null) && null_if_none){
+			return null;
+		}
+		int st2 = 2 * superTileSize;
+		int st_half = superTileSize/2;
+		boolean [] selection = new boolean [st2 * st2];
+		int num_selected = 0;
+		if (layers[num_layer] != null) {
+			for (int dy = 0; dy < st2; dy ++){
+				int y = superTileSize * stY -st_half + dy;
+				if ((y >= 0) && (y < tilesY)) {
+					for (int dx = 0; dx < st2; dx ++){
+						int x = superTileSize * stX -st_half + dx;
+						if ((x >= 0) && (x < tilesX)) {
+							int indx = y * tilesX + x;
+							int indx_st = dy * st2 + dx;
+							if (((sel_in == null) || sel_in[indx_st]) && 
+									(layers[num_layer][indx] != null) &&
+									(layers[num_layer][indx].getStrength() >= strength_floor)){
+								selection[indx_st] = true;
+								num_selected ++;
+							}
+						}
+					}
+				}
+			}
+		}
+		if (null_if_none && (num_selected == 0)) return null;
+		return selection;
+	}
+
+	/**
+	 * Get selection for the specific measurement layer and supertile X,Y coordinates
+	 * in the image and combine with disparity far/near limits. Alse combined with
+	 * input selection
+	 * @param num_layer number of the measurement layer to process
+	 * @param stX supertile horizontal position in the image
+	 * @param stY supertile vertical position in the image
+	 * @param sel_in optional selection for this supertile (linescan, 4 * supetile size)
+	 * @param disp_far lowest acceptable disparity value. Double.NaN - do not check.
+	 * @param disp_near highest acceptable disparity value. Double.NaN - do not check.
+	 * @param null_if_none return null if there are no selected tiles in teh result selection
+	 * @return boolean array [4*superTileSize] of the selected tiles on the specified
+	 * measurement layer
+	 */
+	public boolean [] getSupertileSelection(
+			int        num_layer,
+			int        stX,
+			int        stY,
+			boolean [] sel_in,
+			double     disp_far,
+			double     disp_near,
+			double     strength_floor,
+			boolean    null_if_none)
+	{
+		if ((layers[num_layer] == null) && null_if_none){
+			return null;
+		}
+		int st2 = 2 * superTileSize;
+		int st_half = superTileSize/2;
+		boolean [] selection = new boolean [st2 * st2];
+		int num_selected = 0;
+		if (layers[num_layer] != null) {
+			for (int dy = 0; dy < st2; dy ++){
+				int y = superTileSize * stY -st_half + dy;
+				if ((y >= 0) && (y < tilesY)) {
+					for (int dx = 0; dx < st2; dx ++){
+						int x = superTileSize * stX -st_half + dx;
+						if ((x >= 0) && (x < tilesX)) {
+							int indx = y * tilesX + x;
+							int indx_st = dy * st2 + dx;
+							if (((sel_in == null) || sel_in[indx_st]) && (layers[num_layer][indx] != null)){
+								if (    (Double.isNaN(disp_far)  || (layers[num_layer][indx].getDisparity() >= disp_far)) &&
+										(Double.isNaN(disp_near) || (layers[num_layer][indx].getDisparity() <= disp_near)) &&
+										(layers[num_layer][indx].getStrength() >= strength_floor)){
+									selection[indx_st] = 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
+	 * @param selected boolean array to count set elements in
+	 * @return number of selected elements
+	 */
+	public static int getNumSelected(
+			boolean [] selected)
+	{
+		if (selected == null) return 0;
+		int num_selected = 0;
+		for (int i = 0; i < selected.length; i++){
+			if (selected[i]) num_selected ++;
+		}
+		return num_selected;
+	}
+
+	public static double getSumStrength(
+			double [][] disp_strength)
+	{
+		if (disp_strength == null) return 0.0;
+		double sw = 0.0;
+		for (int i = 0; i < disp_strength[1].length; i++){
+			sw += disp_strength[1][i];
+		}
+		return sw;
+	}
+	
+	/**
+	 * Get disparity and correlation strength for the specific measurement layer and
+	 * supertile X,Y coordinates in the image. Combined with input selection
+	 * @param num_layer number of the measurement layer to process
+	 * @param stX supertile horizontal position in the image
+	 * @param stY supertile vertical position in the image
+	 * @param sel_in optional selection for this supertile (linescan, 4 * supetile size)
+	 * @param null_if_none return null if there are no selected tiles in the result selection
+	 * @param strength_pow Non-linear treatment of correlation strength. Raise data to the strength_pow power
+	 * @return double [2][4*superTileSize] {disparity[4*superTileSize], strength [4*superTileSize]}
+	 */
+
+	public double[][] getDisparityStrength (
+			int num_layer,
+			int stX,
+			int stY,
+			boolean [] sel_in,
+			double strength_floor,
+			double strength_pow,
+			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;
+		if (layers[num_layer] != null) {
+			for (int dy = 0; dy < st2; dy ++){
+				int y = superTileSize * stY -st_half + dy;
+				if ((y >= 0) && (y < tilesY)) {
+					for (int dx = 0; dx < st2; dx ++){
+						int x = superTileSize * stX -st_half + dx;
+						if ((x >= 0) && (x < tilesX)) {
+							int indx = y * tilesX + x;
+							int indx_st = dy * st2 + dx;
+							if (((sel_in == null) || sel_in[indx_st]) && (layers[num_layer][indx] != null)){
+								ds[0][indx_st] = 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];
+									ds[0][indx_st] = layers[num_layer][indx].getDisparity();
+									ds[1][indx_st] = w;
+									num_selected ++;
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+		if (null_if_none && (num_selected == 0)) return null;
+		return ds;
+	}
+}

src/main/java/SuperTiles.java

@@ -52,11 +52,11 @@ public class SuperTiles{
 	double [][][] maxMinMax = null;
 	double []   bgDisparity = null;
 	double []   bgStrength = null;
-	//			TileProcessor.CLTPass3d cltPass3d;
+	
+	MeasuredLayers measuredLayers = null;
+	
 	CLTPass3d cltPass3d;
-	//			ArrayList<TilePlanes.PlaneData>[] planes = null;
 	TilePlanes.PlaneData [][] planes =  null;
-	// 
 	TilePlanes.PlaneData [][] planes_mod = null;
 	double [][][] fuse_coeff = null; // coefficients to fuse planes in neighbor supertiles
 
@@ -109,6 +109,45 @@ public class SuperTiles{
 			//
 		}
 		initFuseCoeff(0.5, false); // true);
+		
+// Set up MeasuredLayers		
+		measuredLayers = new MeasuredLayers(
+				4, // combo, quad, hor, vert
+				tileProcessor.getTilesX(),
+				tileProcessor.getTilesY(),
+				tileProcessor.getSuperTileSize());
+// MeasuredLayer.setLayer(int, double[], double[], boolean[])
+		measuredLayers.setLayer( // combo disparity
+				0,                                // int       num_layer,
+				cltPass3d.getDisparity(0),        // double [] disparity, 
+				cltPass3d.getStrength(),          // double [] strength,
+				cltPass3d.getSelected());         // boolean [] selection) // may be null
+		measuredLayers.setLayer(                  // quad disparity
+				1,                                // int       num_layer,
+				cltPass3d.getDisparity(1),        // double [] disparity, 
+				cltPass3d.getOriginalStrength(),  // double [] strength,
+				cltPass3d.getSelected());         // boolean [] selection) // may be null
+		measuredLayers.setLayer(                  // hor disparity
+				2,                                // int       num_layer,
+				cltPass3d.getDisparity(2),        // double [] disparity, 
+				cltPass3d.getHorStrength(),       // double [] strength,
+				cltPass3d.getSelected());         // boolean [] selection) // may be null
+		measuredLayers.setLayer(                  // vert disparity
+				3,                                // int       num_layer,
+				cltPass3d.getDisparity(3),        // double [] disparity, 
+				cltPass3d.getVertStrength(),      // double [] strength,
+				cltPass3d.getSelected());         // boolean [] selection) // may be null
+		
+		String [] titles = {"d0","s0","d1","s1","d2","s2","d3","s3"};
+		double [][] dbg_img = new double [titles.length][];
+		for (int i = 0; i < measuredLayers.getnumLayers(); i++){
+			dbg_img[2 * i] =     measuredLayers.getDisparity(i);
+			dbg_img[2 * i + 1] = measuredLayers.getStrength(i);
+		}
+	  showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
+	  sdfa_instance.showArrays(dbg_img,  tileProcessor.getTilesX(), tileProcessor.getTilesY(), true, "measuredLayers",titles);
+	  
+
 	}
 
 	public void initFuseCoeff(
@@ -1192,6 +1231,7 @@ public class SuperTiles{
 
 		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; 
@@ -1207,6 +1247,19 @@ public class SuperTiles{
 		//				this.planes = (ArrayList<TilePlanes.PlaneData>[]) new ArrayList[nStiles];
 		this.planes = new TilePlanes.PlaneData[nStiles][];
 
+		if (debugLevel > -1){
+			String [] titles = {"disp","strength","selection"};
+			double [][] dbg_img = new double [titles.length][];
+			dbg_img[0] = disparity;
+			dbg_img[1] = strength;
+			dbg_img[2] = strength.clone();
+			for (int i = 0; i < strength.length; i++) if (strength[i] != 0) dbg_img[2][i] = 1.0;
+			showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
+			sdfa_instance.showArrays(dbg_img,  tileProcessor.getTilesX(), tileProcessor.getTilesY(), true, "disp_strength_sel",titles);
+
+		}
+		
+		
 		//				final int debug_stile = 18 * stilesX + 25; 
 		//				final int debug_stile = 20 * stilesX + 24; 
 		//				final int debug_stile = 16 * stilesX + 27; // 10; 
@@ -1228,7 +1281,7 @@ public class SuperTiles{
 						int stileX = nsTile % stilesX;
 						int [] sTiles = {stileX, stileY};
 						double sw = 0.0; // sum weights
-						double [] hist = new double [numBins];
+///						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);
@@ -1265,7 +1318,7 @@ public class SuperTiles{
 						if (sw >0){ // there are some non-zero tiles, process them (all points, not clustered by disparity value)
 							ArrayList<TilePlanes.PlaneData> st_planes = new ArrayList<TilePlanes.PlaneData>();
 							int dl1 =  (nsTile == debug_stile) ? 3 : 0;
-							int dl =  (nsTile == debug_stile) ? 3 : 0;
+							int dl =   (nsTile == debug_stile) ? 3 : 0;
 							boolean [] sel_all = stSel.clone();
 							TilePlanes.PlaneData pd = tpl.getPlane(
 									sTiles,
@@ -1273,7 +1326,7 @@ public class SuperTiles{
 									stStrength,
 									sel_all,
 									plPreferDisparity,									
-									0); // debugLevel);
+									dl); // 0); // debugLevel);
 							if (pd != null) { // not too few points, probably
 								//correct_distortions
 								double swc_common = pd.getWeight();
@@ -1289,6 +1342,19 @@ public class SuperTiles{
 												", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
 												", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
 									}
+									
+									if (debugLevel > -1){
+										  showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
+										  double [][] dbg_img = new double [3][];
+										  dbg_img[0] = stDisparity;
+										  dbg_img[1] =stStrength;
+										  dbg_img[2] = new double [superTileSize2*superTileSize2];
+										  for (int i = 0; i < dbg_img[2].length; i++){
+											  dbg_img[2][i] = sel_all[i]?1.0:0.0;
+										  }
+										  sdfa_instance.showArrays(dbg_img,  superTileSize2, superTileSize2, true, "disp_str_sel");
+									}
+									
 								}
 								// now try to remove outliers
 								int max_outliers = (int) Math.round(pd.getNumPoints() * plFractOutliers);
@@ -1459,6 +1525,239 @@ public class SuperTiles{
 		ImageDtt.startAndJoin(threads);
 	}
 
+	public void processPlanes3(
+			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 boolean    plPreferDisparity, // Always start with disparity-most axis (false - lowest eigenvalue)
+			final GeometryCorrection geometryCorrection,
+			final boolean    correct_distortions,
+			final int        debugLevel,
+			final int        dbg_X,
+			final int        dbg_Y)
+	{
+		if (maxMinMax == null) getMaxMinMax();
+		final int tilesX =        tileProcessor.getTilesX();
+		final int tilesY =        tileProcessor.getTilesY();
+		final int superTileSize = tileProcessor.getSuperTileSize();
+		final int tileSize =      tileProcessor.getTileSize();
+
+		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);
+		this.planes = new TilePlanes.PlaneData[nStiles][];
+		final int debug_stile = (debugLevel > -1)? (dbg_Y * stilesX + dbg_X):-1;
+		final boolean [][] dflt_select = {{}, null, null, null, null}; // use layer 0 (combo) only
+		// TODO: Remove when promoting PlaneData
+		final TilePlanes tpl = new TilePlanes(tileSize,superTileSize, geometryCorrection);
+		
+//		final double  [] disparity = cltPass3d.getDisparity();
+//		final double  [] strength =  cltPass3d.getStrength();
+		
+		measuredLayers.setLayer (
+				0, // int       num_layer,
+				cltPass3d.getDisparity(), // double [] disparity,
+				cltPass3d.getStrength(), // double [] strength,
+				null); // boolean [] selection) // may be null
+		if (debugLevel > -1) {
+			String [] titles = {"d0","s0","d1","s1","d2","s2","d3","s3","s","d"};
+			double [][] dbg_img = new double [titles.length][];
+			for (int i = 0; i < measuredLayers.getnumLayers(); i++){
+				dbg_img[2 * i] =     measuredLayers.getDisparity(i);
+				dbg_img[2 * i + 1] = measuredLayers.getStrength(i);
+			}
+			dbg_img[8] = cltPass3d.getDisparity();
+			dbg_img[9] = cltPass3d.getStrength();
+			showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
+			sdfa_instance.showArrays(dbg_img,  tileProcessor.getTilesX(), tileProcessor.getTilesY(), true, "measuredLayers",titles);
+		}
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
+						if (nsTile == debug_stile){
+							System.out.println("processPlanes3(): nsTile="+nsTile);
+						}
+						int stileY = nsTile / stilesX;  
+						int stileX = nsTile % stilesX;
+						int [] sTiles = {stileX, stileY};
+						planes[nsTile] = null;
+						// first make a plane from all tiles
+						TilePlanes.PlaneData pd0 = tpl.new  PlaneData (
+								sTiles, // int [] sTileXY, 
+								tileSize, // int tileSize,
+								geometryCorrection, // GeometryCorrection   geometryCorrection,
+								measuredLayers,     // MeasuredLayers measuredLayers,
+								plPreferDisparity);   // boolean preferDisparity)
+						boolean [][] tile_sel = dflt_select.clone();
+						for (int i = 0; i < dflt_select.length; i++){
+							if (dflt_select[i] != null) tile_sel[i] = dflt_select[i].clone();
+						}
+						int dl1 =  (nsTile == debug_stile) ? 3 : 0;
+						int dl =  (nsTile == debug_stile) ? 3 : 0;
+
+						boolean OK = pd0.getPlaneFromMeas(
+								tile_sel,     // boolean [][] tile_sel, // null - do not use, {} use all (will be modified)
+								min_disp,     // double       disp_far, // minimal disparity to select (or NaN)
+								Double.NaN,   // double       disp_near, // maximal disparity to select (or NaN)
+								0.0,          // plDispNorm,  // double       dispNorm,   //  Normalize disparities to the average if above
+								0.0,          // double       min_weight,
+								plMinPoints,  // int          min_tiles,
+								strength_floor, // 
+								strength_pow, // double       strength_pow,
+								dl);          // int          debugLevel)
+
+
+						if (OK){ // there are some non-zero tiles, process them (all points, not clustered by disparity value)
+							TilePlanes.PlaneData pd0_full = pd0.clone();  //
+							ArrayList<TilePlanes.PlaneData> st_planes = new ArrayList<TilePlanes.PlaneData>();
+							// now try to remove outliers
+							int max_outliers = (int) Math.round(pd0.getNumPoints() * plFractOutliers);
+							if (max_outliers > plMaxOutliers) max_outliers = plMaxOutliers;
+							double targetV = corrMaxEigen(
+									plTargetEigen,
+									plDispNorm,
+									pd0);
+							if (pd0.getValue() > targetV) {
+								OK = pd0.removeOutliers( // getPlaneFromMeas should already have run
+										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)
+										dl); // int        debugLevel)
+								if (!OK) continue;
+								if (dl > 0) {
+									if (pd0.getWeight() > 0.3) { // 1.0) {
+										System.out.println("Removed outliers["+nsTile+"]"+
+												", stileX="+stileX+
+												", stileY="+stileY+
+												", numPoints="+ pd0.getNumPoints()+
+												", swc = "+pd0.getWeight()+
+												", center=["+pd0.getZxy()[0]+","+pd0.getZxy()[1]+","+pd0.getZxy()[2]+"]"+
+												", eig_val = {"+pd0.getValues()[0]+","+pd0.getValues()[1]+","+pd0.getValues()[2]+"}"+
+												", eig_vect[0] = {"+pd0.getVector()[0]+","+pd0.getVector()[1]+","+pd0.getVector()[2]+"}");
+									}
+								}
+							} // nothing to do if already OK
+							if (dl > 0) {
+								System.out.println("Calculating World normal["+nsTile+"]");
+							}
+							double swc_common = pd0.getWeight();
+							double [] norm_xyz = pd0.getWorldXYZ(
+									correct_distortions,
+									dl);
+							if (dl > 0) {
+								System.out.println("World normal["+nsTile+"] = {"+
+										norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
+
+							}
+							st_planes.add(pd0);  // adding [0] - all supertile tiles, not clustered by disparity value
+
+							// now try for each of the disparity-separated clusters (only for multi-peak histograms)
+
+							double [][] mm = maxMinMax[nsTile];
+
+							if (mm.length > 1) { // multiple maximums - separate into multiple selections
+								for (int m = 0; m < (mm.length +1)/2; m++){
+									double [] far_near = new double [2];
+									if (m == 0) {
+										far_near[0] = Double.NaN; // pd0 already filtered by min_disp
+									} else {
+										far_near[0] = mm[2 * m - 1][0];
+									}
+									if (m == (mm.length -1)/2) {
+										far_near[1] = Double.NaN; // pd0 already filtered by min_disp
+									} else {
+										far_near[1] = mm[2 * m + 1][0];
+									}
+
+
+									TilePlanes.PlaneData pd = pd0_full.clone(); 
+									OK = pd.getPlaneFromMeas(
+											tile_sel,       // boolean [][] tile_sel, // null - do not use, {} use all (will be modified)
+											far_near[0],    // double       disp_far, // minimal disparity to select (or NaN)
+											far_near[1],    // double       disp_near, // maximal disparity to select (or NaN)
+											0.0,            // plDispNorm,  // double       dispNorm,   //  Normalize disparities to the average if above
+											0.0,            // double       min_weight,
+											plMinPoints,    // int          min_tiles,
+											strength_floor, // 
+											strength_pow,   // double       strength_pow,
+											dl);            // int          debugLevel)
+									if (OK) {
+										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;
+										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) {
+											OK = pd.removeOutliers( // getPlaneFromMeas should already have run
+													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)
+													dl); // int        debugLevel)
+											if (!OK) {
+												continue;
+											}
+											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);
+										st_planes.add(pd);
+										if (dl > 0) {
+											System.out.println("World normal["+nsTile+"]["+m+"] = {"+
+													norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
+
+										}
+									}
+								}
+							}
+							if (st_planes.size() > 0){
+								planes[nsTile] = st_planes.toArray(new TilePlanes.PlaneData[0] );
+								if (dl >0){
+									System.out.println("processPlanes3(): nsTile="+nsTile);
+								}
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+	}
+
 	public int [] getShowPlanesWidthHeight()
 	{
 		final int tilesX =        tileProcessor.getTilesX();

src/main/java/TilePlanes.java

@@ -46,6 +46,7 @@ public class TilePlanes {
 	public class PlaneData{
 		GeometryCorrection   geometryCorrection = null;
 		boolean     correctDistortions = false;
+		// just for visualization - no there can be several measured layers and same tile can be used multiple times. Will just logical or 
 		boolean []  plane_sel =  null; // tile selection - has twice supertile size in each direction
 		double []   zxy =        null; // [3] - plane center point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile (in pixels, relative to this supertile center)
 		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
@@ -68,7 +69,17 @@ public class TilePlanes {
 		
 		int         tileSize;
 		int         superTileSize;
-		int []      sTileXY =    null; // X and Y indices of this superTile in the image 
+		int []      sTileXY =    null; // X and Y indices of this superTile in the image
+		
+		MeasuredLayers measuredLayers = null;
+		boolean [][] measuredSelection = null; // [number of layers in measuredLayers][2*superTileSize * 2*superTileSize]
+		double       measured_strength_pow = 1.0;
+		double       strength_floor =        0.0;
+		double       min_weight =            0.0;  // minimal weight of the ellipsoid
+		int          min_tiles =             10;
+		double       dispNorm =              5.0;  //  Normalize disparities to the average if above
+		
+		boolean      preferDisparity = false;
 		
 		public PlaneData clone(){
 			PlaneData pd = new PlaneData(
@@ -92,18 +103,25 @@ public class TilePlanes {
 				pd.vectors[1] = this.vectors[1].clone();
 				pd.vectors[2] = this.vectors[2].clone();
 			}
-			copyNeib(this,pd);
-			/*
-			if (this.merged_eig_val != null){
-				pd.merged_eig_val = this.merged_eig_val.clone();
-				for (int i = 0; i<this.merged_eig_val.length; i++){
-					if (this.merged_eig_val[i] != null){
-						pd.merged_eig_val[i] = this.merged_eig_val[i].clone();
+			if (this.measuredLayers != null) pd.measuredLayers = this.measuredLayers;
+			if (this.measuredSelection != null) {
+				pd.measuredSelection = this.measuredSelection.clone();
+				for (int i = 0; i < this.measuredSelection.length; i++){
+					if (this.measuredSelection[i] != null) {
+						pd.measuredSelection[i] = this.measuredSelection[i].clone();
 					}
 				}
 			}
-			if (this.neib_best != null) pd.neib_best = this.neib_best.clone();
-			*/
+			pd.measured_strength_pow = this.measured_strength_pow;
+			pd.strength_floor =        this.strength_floor;
+
+			pd.min_weight =            this.min_weight;
+			pd.min_tiles =             this.min_tiles;
+			pd.dispNorm =              this.dispNorm;
+			
+			pd.preferDisparity = this.preferDisparity;
+			
+			copyNeib(this,pd);
 			return pd;
 		}
 		
@@ -147,6 +165,391 @@ public class TilePlanes {
 			this.superTileSize = superTileSize;
 			this.sTileXY = sTileXY.clone();
 		}
+
+		public PlaneData (
+				int [] sTileXY, 
+				int tileSize,
+				GeometryCorrection   geometryCorrection,
+				MeasuredLayers measuredLayers,
+				boolean preferDisparity)
+		{
+			this.geometryCorrection = geometryCorrection;
+			this.tileSize = tileSize;
+			this.superTileSize = measuredLayers.getSuperTileSize();
+			this.sTileXY = sTileXY.clone();
+			this.measuredLayers = measuredLayers;
+			this.preferDisparity = preferDisparity;
+		}
+		/**
+		 * Remove outliers from the set of tiles contributing to a single plane ellipsoid
+		 * Should run after getPlaneFromMeas as some parameter4s will be copied from that run
+		 * @param targetEigen Target value for the lowest eigenvalue (thickness of the ellipsoid)
+		 * @param maxRemoved maximal number of tiles to be removed
+		 * @param debugLevel debug level
+		 * @return true if OK (currently false is when a program bug happens), TODO: change to "goal reached" 
+		 */
+		
+		public boolean removeOutliers( // getPlaneFromMeas should already have run
+				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        debugLevel)
+		{
+			int stSize2 = 2 * stSize;
+			if (maxRemoved > (getNumPoints() - this.min_tiles)) maxRemoved = getNumPoints() - this.min_tiles;
+			int numRemoved = 0;
+			boolean no_bugs = true;
+			for (; (getValue() > targetEigen) && (numRemoved < maxRemoved); numRemoved++){
+				if (debugLevel > 2){
+					System.out.println("removePlaneOutliers("+sTileXY[0]+":"+sTileXY[1]+"): numRemoved = "+numRemoved+" eigenValue = " + getValue()+" target = "+targetEigen);
+				}
+				// make a plane and find the worst (largest disparity difference) tile
+				// z = -(x*Vx + y*Vy)/Vz
+				double [][][] disp_str = new double [measuredSelection.length][][];
+
+				double worst_d2 = 0.0;
+				int [] worst_layer_index = {-1,-1};
+				double [] v = getVector();
+				double [] zxy0 = getZxy();
+				for (int nl = 0; nl < measuredSelection.length; nl++){
+					if (measuredSelection[nl] != null){
+						disp_str[nl] = measuredLayers.getDisparityStrength(
+								nl, // int num_layer,
+								sTileXY[0], // int stX,
+								sTileXY[1], // int stY,
+								measuredSelection[nl], // boolean [] sel_in,
+								strength_floor, // 
+								measured_strength_pow, //
+								true); // boolean null_if_none)
+						if (disp_str[nl] != null) {
+							for (int indx = 0; indx < disp_str[nl][0].length; indx++){
+								double w = disp_str[nl][1][indx];
+								if (w > 0.0){
+									double x = ((indx % stSize2) - stSize) - zxy0[1];
+									double y = ((indx / stSize2) - stSize) - zxy0[2];
+									double d = disp_str[nl][0][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_layer_index[0] = nl;
+										worst_layer_index[1] = indx;
+									}
+								}
+							}
+						}
+					}
+				}
+				if (worst_layer_index[0] < 0) {
+					System.out.println("This is a BUG in removePlaneOutliers()");
+					no_bugs = false;
+					break;
+				}
+				measuredSelection[worst_layer_index[0]][worst_layer_index[1]] = false;
+				if (debugLevel > 2){
+					System.out.println("removePlaneOutliers() worst_layer="+worst_layer_index[0]+", worst_index="+worst_layer_index[1]);
+				}
+				
+				boolean OK = getPlaneFromMeas(
+						null,            // boolean [][] tile_sel, // null - do not use, {} use all (will be modified)
+						Double.NaN,      // double       disp_far, // minimal disparity to select (or NaN)
+						Double.NaN,      // double       disp_near, // maximal disparity to select (or NaN)
+						this.dispNorm,   // double       dispNorm,   //  Normalize disparities to the average if above
+						this.min_weight, // double       min_weight,
+						this.min_tiles, // int          min_tiles,
+						strength_floor,
+						measured_strength_pow, // double       strength_pow,
+						debugLevel);
+				
+				if (!OK){ // restore last selection, re-run getPlaneFromMeas
+					measuredSelection[worst_layer_index[0]][worst_layer_index[1]] = true;
+					OK = getPlaneFromMeas(
+							null,            // boolean [][] tile_sel, // null - do not use, {} use all (will be modified)
+							Double.NaN,      // double       disp_far, // minimal disparity to select (or NaN)
+							Double.NaN,      // double       disp_near, // maximal disparity to select (or NaN)
+							this.dispNorm,   // double       dispNorm,   //  Normalize disparities to the average if above
+							this.min_weight, // double       min_weight,
+							this.min_tiles, // int          min_tiles,
+							strength_floor,
+							measured_strength_pow, // double       strength_pow,
+							debugLevel);
+					if (!OK) {
+						System.out.println("This is a BUG in removePlaneOutliers() - run with previous selection and failed");
+						no_bugs = false;
+					}
+					break;
+				}
+			}			
+			return no_bugs;
+		}
+		
+		/**
+		 * Get "plane" - ellipsoid from covariance matrix of measured data
+		 * @param tile_sel multi-layer selection of the tiles to use (first dimension should
+		 *                 match number of measurement layers. each element can be either:
+		 *                 a) boolean array 4* superTileSize * superTileSize length
+		 *                 b) zero-length array - it will be calculated from all measurement
+		 *                    data on that layer matching optional diaparity limits
+		 *                 c) null - this measuremen5t layer will not be used   
+		 * @param disp_far optional low limit for tile disparity (NaN - do not check)
+		 * @param disp_near optional high limit for tile disparity (NaN - do not check)
+		 * @param dispNorm reduce scale of the disparity differences for ellipsoids with 
+		 *                 disparity center above that value
+		 * @param min_weight minimal total weight of the ellipsoid do process
+		 * @param min_tiles minimal number of tiles used for calculation
+		 * @param strength_pow raise correlation strength to this power
+		 * @param debugLevel debug level
+		 * @return true if OK, false if failed
+		 */
+		public boolean getPlaneFromMeas(
+				boolean [][] tile_sel, // null - do not use, {} use all (will be modified)
+				double       disp_far, // minimal disparity to select (or NaN)
+				double       disp_near, // maximal disparity to select (or NaN)
+				double       dispNorm,   //  Normalize disparities to the average if above
+				double       min_weight,
+				int          min_tiles,
+				double       strength_floor,
+				double       strength_pow,
+				int          debugLevel)
+		{
+			double mindet = 1E-15;
+			int stSize2 = 2 * stSize;
+			int num_tiles = 0;
+			double sw = 0.0;
+			if (tile_sel != null) {
+				this.measuredSelection =      tile_sel;
+			} else {
+				tile_sel = this.measuredSelection;
+			}
+			double [][][] disp_str =      new double [tile_sel.length][][];
+			this.strength_floor =         strength_floor;
+			this.measured_strength_pow =  strength_pow;
+			this.min_weight =             min_weight;
+			this.min_tiles =              min_tiles;
+			this.dispNorm =               dispNorm;
+			if (debugLevel > 1){
+				System.out.println("getPlaneFromMeas()");
+			}
+
+			for (int nl = 0; nl < tile_sel.length; nl++){
+				if (tile_sel[nl] != null){
+					if (Double.isNaN(disp_far) && Double.isNaN(disp_near)){
+						tile_sel[nl] =  measuredLayers.getSupertileSelection(
+								nl, // int num_layer,
+								sTileXY[0], // int stX,
+								sTileXY[1], // int stY,
+								((tile_sel[nl].length == 0)? null:tile_sel[nl]), // boolean [] sel_in,
+								strength_floor,
+								true); // boolean null_if_none)
+					} else {
+						tile_sel[nl] =  measuredLayers.getSupertileSelection(
+								nl,            // int num_layer,
+								sTileXY[0],    // int stX,
+								sTileXY[1],    // int stY,
+								((tile_sel[nl].length == 0)? null:tile_sel[nl]), // boolean [] sel_in,
+								disp_far,      // 		double     disp_far,
+								disp_near,     // double     disp_near,
+								strength_floor,
+								true);         // boolean null_if_none)
+
+					}
+					num_tiles += MeasuredLayers.getNumSelected(tile_sel[nl]);
+					if (tile_sel[nl] != null){
+						disp_str[nl] = measuredLayers.getDisparityStrength(
+								nl, // int num_layer,
+								sTileXY[0], // int stX,
+								sTileXY[1], // int stY,
+								tile_sel[nl], // boolean [] sel_in,
+								strength_floor,
+								strength_pow, //
+								true); // boolean null_if_none)
+						sw += MeasuredLayers.getSumStrength(disp_str[nl]);
+					}
+					if ((debugLevel > 3) && (disp_str[nl] != null)){
+						  showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
+						  double [][] dbg_img = new double [3][];
+						  dbg_img[0] = disp_str[nl][0];
+						  dbg_img[1] = disp_str[nl][1];
+						  dbg_img[2] = new double [stSize2*stSize2];
+						  for (int i = 0; i < dbg_img[2].length; i++){
+							  dbg_img[2][i] = tile_sel[nl][i]?1.0:0.0;
+						  }
+						  sdfa_instance.showArrays(dbg_img,  stSize2, stSize2, true, "disp_str_"+nl);
+						  
+					}
+				}
+			}
+			if ((sw < min_weight) || (num_tiles < min_tiles)) {
+				if (debugLevel > 1){
+					System.out.println("getPlaneFromMeas():return false");
+				}
+				return false; // too weak plane or too few tiles selected
+			}
+			
+			
+			double [][] acovar = new double [3][3];
+			double swz = 0.0, swx = 0.0, swy = 0.0;
+			sw =0.0;
+			for (int nl = 0; nl < tile_sel.length; nl++){
+				if (disp_str[nl] != null) {
+					for (int indx = 0; indx < disp_str[nl][0].length; indx++){
+						double w = disp_str[nl][1][indx];
+						if (w > 0.0){
+							double d = disp_str[nl][0][indx];
+							// referencing samples to centers of pixels
+							double x = ((indx % stSize2) - stSize + 0.5) * tileSize + 0.5; // in pixels, not in tiles
+							double y = ((indx / stSize2) - stSize + 0.5) * tileSize + 0.5;
+							sw  += w;
+							swz += w * d;
+							swx += w * x;
+							swy += w * y;
+						}
+					}
+				}
+			}
+			if (sw == 0.0) {
+				return false; //
+			}
+			swz /= sw;
+			swx /= sw;
+			swy /= sw;
+
+			double kz = ((dispNorm > 0.0) && (swz > dispNorm)) ? (dispNorm / swz) : 1.0; 
+			
+			if (debugLevel > 0){
+				System.out.println("getPlaneFromMeas(): num_tiles="+num_tiles+", sw = "+sw +", swz = "+swz +", swx = "+swx +", swy = "+swy+", kz="+kz);
+			}
+
+			// TODO: scale disparity to make same scale for 3 axes?
+			
+			for (int nl = 0; nl < tile_sel.length; nl++){
+				if (disp_str[nl] != null) {
+					for (int indx = 0; indx < disp_str[nl][0].length; indx++){
+						double w = disp_str[nl][1][indx] / sw;
+						if (w > 0.0){
+							double d =  kz * (disp_str[nl][0][indx] - swz);
+							double wd = w*d;
+							double x = ((indx % stSize2) - stSize + 0.5) * tileSize + 0.5 - swx;
+							double y = ((indx / stSize2) - stSize + 0.5) * tileSize + 0.5 - 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 (Double.isNaN(eig.getV().get(0, 0))){
+				System.out.println("getCovar(): Double.isNaN(eig.getV().get(0, 0))");
+				debugLevel = 20;
+			}
+			
+//			if (eig.getD().get(0, 0) == 0.0){
+//				debugLevel = 10;
+//			}
+			if (debugLevel > 0){
+				System.out.println("getCovar(): sw = "+sw +", swz = "+swz +", swx = "+swx +", swy = "+swy +", covar.det() = "+covar.det());
+				System.out.println("getCovar(): covarianvce matrix, number of used points:"+num_tiles);
+				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
+			}
+			if ((eig.getD().get(0, 0) == 0.0) || (Math.abs(covar.det()) < mindet)) {
+				return false; // testing with zero eigenvalue
+				// Problem with zero eigenvalue is with derivatives and coordinate conversion
+			}
+/*			
+			double [][][] rslt = {
+					eig.getD().getArray(),
+					eig.getV().getArray(),
+					{
+						{sw,kz,numPoints},
+						{swz, swx, swy}}};
+			return rslt;
+*/
+
+			double [][] eig_val =  eig.getD().getArray(); // rslt[0];
+			double [][] eig_vect = eig.getV().getArray(); // rslt[1];
+			// find vector most orthogonal to view // (anyway it all works with that assumption), make it first
+			// TODO normalize to local linear scales
+			int oindx = 0;
+			if (preferDisparity) {
+				for (int i = 1; i <3; i++){
+					if (Math.abs(eig_vect[0][i]) > Math.abs(eig_vect[0][oindx])){
+						oindx = i;
+					}
+				}
+			} else {
+				for (int i = 1; i < 3 ; i++){
+					if (eig_val[i][i] < eig_val[oindx][oindx]){
+						oindx = i;
+					}
+				}
+			}
+			if (eig_val[oindx][oindx] == 0.0){
+				System.out.println("getPlane(): zero eigenvalue!!");
+			}
+			
+			// select 2 other axes for increasing eigenvalues (so v is short axis, h  is the long one)
+			int vindx = (oindx == 0)? 1 : 0;
+			int hindx = (oindx == 0)? 2 : ((oindx == 1) ? 2 : 1);
+			if (eig_val[vindx][vindx] > eig_val[hindx][hindx]){
+				int tmp = vindx;
+				vindx = hindx;
+				hindx = tmp;
+			}
+
+			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)
+			for (int v = 0; v < 2; v++) {
+				if (plane[v][v] < 0.0) for (int i = 0; i < 3; i ++) plane[v][i] = -plane[v][i];
+			}
+			
+			// make  direction last vector so px (x) py (.) disp < 0 (left-hand coordinate system) 
+			if (new Matrix(plane).det() > 0){
+				for (int i = 0; i < 3; i ++) plane[2][i] = -plane[2][i];
+			}
+			
+			setZxy(swz, swx, swy);
+			setWeight(sw);
+			setValues(eig_val[oindx][oindx],eig_val[vindx][vindx],eig_val[hindx][hindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
+			setVectors   (plane);
+			setNumPoints (num_tiles);
+			boolean [] plane_sel = null;
+			boolean need_clone = true;
+			for (int nl = 0; nl < tile_sel.length; nl++){
+				if (tile_sel[nl] != null) {
+					if (plane_sel == null) {
+						plane_sel = tile_sel[nl];
+					} else {
+						if (need_clone) {
+							plane_sel = plane_sel.clone();
+							need_clone = false;
+						}
+						for (int i = 0; i < plane_sel.length; i++){
+							plane_sel[i] |=  tile_sel[nl][i];
+						}
+					} 
+				}
+			}
+			
+			setPlaneSelection(plane_sel);
+			return true;
+		}
 		
 		public double [][] initMergedValue()
 		{
@@ -244,6 +647,16 @@ public class TilePlanes {
 		public void setZxy(double[] zxy) {
 			this.zxy = zxy;
 		}
+		public void setZxy(
+				double z,
+				double x,
+				double y) {
+			this.zxy = new double [3];
+			this.zxy[0] = z;
+			this.zxy[1] = x;
+			this.zxy[2] = y;
+			
+		}
 		
 		public double[][] getVectors() {
 			return vectors;
@@ -910,6 +1323,8 @@ public class TilePlanes {
 		}
 	}
 	
+	//TODO: Remove below methods and promote PlaneData (no TilePlanes) after tested
+	
 	/**
 	 * Calculate covariance matrix for a subset of tile data (disparities)
 	 * Subtract weight floor from weight
@@ -952,13 +1367,13 @@ public class TilePlanes {
 		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; 
+		if (debugLevel > 0){
+			System.out.println("getCovar(): sw = "+sw +", swz = "+swz +", swx = "+swx +", swy = "+swy+" kz = "+kz);
+		}
 		for (int indx = 0; indx < data.length; indx++){
 			if (select[indx] && (weight[indx] > 0)){
 				double w = weight[indx] / sw;
@@ -1024,15 +1439,15 @@ public class TilePlanes {
 			select = new boolean [4*stSize];
 			for (int i = 0; i < select.length; i++) select[i] = true;
 		}
-//		int debugLevel1 = ((sTileXY[0] == 27) && (sTileXY[1] == 16))? 1: 0; // check why v[0][0] <0  
-		
-		
+		if (debugLevel > 0){
+			System.out.println("getPlane()");
+		}
 		double [][][] rslt = getCovar(
 				data,
 				weight,
 				select,
 				0.0,
-				0); // debugLevel1); //0); // debugLevel);
+				debugLevel); // debugLevel1); //0); // debugLevel);
 		if (rslt == null) return null;
 		int       numPoints =  (int) rslt[2][0][2];
 		double    swc =  rslt[2][0][0];
@@ -1173,6 +1588,10 @@ public class TilePlanes {
 				break;
 			}
 			select[worst_index] = false;
+			if (debugLevel > 2){
+				System.out.println("removePlaneOutliers() worst_index = " + worst_index);
+			}
+			
 			pd = getPlane(    // re-calculate with point removed
 					pd.getSTileXY(),
 					data,

src/main/java/TileProcessor.java

@@ -2945,7 +2945,8 @@ public class TileProcessor {
 		SuperTiles st = scan_prev.getSuperTiles();
 		
 // moved here
-		st.processPlanes2(
+		if (clt_parameters.dbg_migrate) {
+		st.processPlanes3(
 				null, // final boolean [] selected, // or null
 				0.3, // final double     min_disp,
 				false, // final boolean    invert_disp, // use 1/disparity
@@ -2960,7 +2961,23 @@ public class TileProcessor {
 				0, // -1, // debugLevel,                  // final int        debugLevel)
 				clt_parameters.tileX,
 				clt_parameters.tileY);
-		
+		} else {
+			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\
+					clt_parameters.plPreferDisparity,
+					geometryCorrection,
+					clt_parameters.correct_distortions,
+					0, // -1, // debugLevel,                  // final int        debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
+		}
 		
 		showDoubleFloatArrays sdfa_instance = null;
 		if (debugLevel > -1) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?