merging planes in the same supertile, new connection costs calculation

src/main/java/EyesisCorrectionParameters.java

@@ -2178,6 +2178,14 @@ public class EyesisCorrectionParameters {
   		public double     plOKMergeEigen       =   0.03; // If result of the merged planes is below, OK to use thin planes (higher) threshold 
   		public double     plMaxWorldSin2       =   0.1;  // Maximal sine squared of the world angle between planes to merge. Set to >= 1.0 to disable
   		public double     plWeakWorsening      =   1.0;  // Relax merge requirements for weaker planes
+  		public double     plMaxOverlap         =   0.1;  // Maximal overlap between the same supertile planes to merge
+  		
+  		// comparing merge quality for plane pairs
+  		public double     plCostKrq            =   0.8;  // cost of merge quality weighted
+  		public double     plCostKrqEq          =   0.2;  // cost of merge quality equal weight
+  		public double     plCostSin2           =  10.0;  // cost of sin squared between normals
+  		public double     plCostRdist2         =1000.0;  // cost of squared relative distances       
+  		
   		
   		public boolean    plConflDualTri       =   false; // Resolve dual triangles conflict (odoodo)
   		public boolean    plConflMulti         =   false; // Resolve multiple odo triangles conflicts
@@ -2546,6 +2554,12 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"plOKMergeEigen",   this.plOKMergeEigen +"");
 			properties.setProperty(prefix+"plMaxWorldSin2",   this.plMaxWorldSin2 +"");
 			properties.setProperty(prefix+"plWeakWorsening",  this.plWeakWorsening +"");
+			properties.setProperty(prefix+"plMaxOverlap",     this.plMaxOverlap +"");
+
+			properties.setProperty(prefix+"plCostKrq",        this.plCostKrq +"");
+			properties.setProperty(prefix+"plCostKrqEq",      this.plCostKrqEq +"");
+			properties.setProperty(prefix+"plCostSin2",       this.plCostSin2 +"");
+			properties.setProperty(prefix+"plCostRdist2",     this.plCostRdist2 +"");
 
 			properties.setProperty(prefix+"plConflDualTri",   this.plConflDualTri+"");
 			properties.setProperty(prefix+"plConflMulti",     this.plConflMulti+"");
@@ -2893,6 +2907,12 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"plOKMergeEigen")!=null)    this.plOKMergeEigen=Double.parseDouble(properties.getProperty(prefix+"plOKMergeEigen"));
   			if (properties.getProperty(prefix+"plMaxWorldSin2")!=null)    this.plMaxWorldSin2=Double.parseDouble(properties.getProperty(prefix+"plMaxWorldSin2"));
   			if (properties.getProperty(prefix+"plWeakWorsening")!=null)   this.plWeakWorsening=Double.parseDouble(properties.getProperty(prefix+"plWeakWorsening"));
+  			if (properties.getProperty(prefix+"plMaxOverlap")!=null)      this.plMaxOverlap=Double.parseDouble(properties.getProperty(prefix+"plMaxOverlap"));
+
+  			if (properties.getProperty(prefix+"plCostKrq")!=null)         this.plCostKrq=Double.parseDouble(properties.getProperty(prefix+"plCostKrq"));
+  			if (properties.getProperty(prefix+"plCostKrqEq")!=null)       this.plCostKrqEq=Double.parseDouble(properties.getProperty(prefix+"plCostKrqEq"));
+  			if (properties.getProperty(prefix+"plCostSin2")!=null)        this.plCostSin2=Double.parseDouble(properties.getProperty(prefix+"plCostSin2"));
+  			if (properties.getProperty(prefix+"plCostRdist2")!=null)      this.plCostRdist2=Double.parseDouble(properties.getProperty(prefix+"plCostRdist2"));
 
   			if (properties.getProperty(prefix+"plConflDualTri")!=null)    this.plConflDualTri=Boolean.parseBoolean(properties.getProperty(prefix+"plConflDualTri"));
   			if (properties.getProperty(prefix+"plConflMulti")!=null)      this.plConflMulti=Boolean.parseBoolean(properties.getProperty(prefix+"plConflMulti"));
@@ -3268,6 +3288,13 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("If result of the merged planes is below, OK to use thin planes (higher) threshold ",this.plOKMergeEigen,  6);
   			gd.addNumericField("Maximal sine squared of the world angle between planes to merge. Set to >= 1.0 to disable", this.plMaxWorldSin2,  6);
   			gd.addNumericField("Relax merge requirements for weaker planes",                                   this.plWeakWorsening,  6);
+  			gd.addNumericField("Maximal overlap between the same supertile planes to merge",                   this.plMaxOverlap,  6);
+
+  			gd.addMessage     ("--- Planes merge costs ---");
+  			gd.addNumericField("Cost of merge quality weighted",                                               this.plCostKrq,     6);
+  			gd.addNumericField("Cost of merge quality equal weight",                                           this.plCostKrqEq,   6);
+  			gd.addNumericField("Cost of sin squared between normals",                                          this.plCostSin2,    6);
+  			gd.addNumericField("Cost of squared relative plane-to-other-center distances",                     this.plCostRdist2,  6);
 
   			gd.addCheckbox    ("Resolve dual triangles conflict (odoodo)",                                     this.plConflDualTri);
   			gd.addCheckbox    ("Resolve multiple odo triangles conflicts",                                     this.plConflMulti);
@@ -3628,6 +3655,12 @@ public class EyesisCorrectionParameters {
   			this.plOKMergeEigen=        gd.getNextNumber();
   			this.plMaxWorldSin2=        gd.getNextNumber();
   			this.plWeakWorsening=       gd.getNextNumber();
+  			this.plMaxOverlap=          gd.getNextNumber();
+
+  			this.plCostKrq=             gd.getNextNumber();
+  			this.plCostKrqEq=           gd.getNextNumber();
+  			this.plCostSin2=            gd.getNextNumber();
+  			this.plCostRdist2=          gd.getNextNumber();
 
   			this.plConflDualTri=        gd.getNextBoolean();
   			this.plConflMulti=          gd.getNextBoolean();

src/main/java/TilePlanes.java

@@ -200,6 +200,30 @@ public class TilePlanes {
 			}
 		}
 
+		public void orMeasSelection(boolean [][] meas_sel)
+		{
+			if (meas_sel == null) 
+				this.measuredSelection = null;
+			else {
+				if (this.measuredSelection == null) {
+					this.measuredSelection = meas_sel.clone();
+				}
+				for (int i = 0; i < meas_sel.length; i++){
+					if (meas_sel[i] != null) {
+						if (this.measuredSelection[i] == null) {
+							this.measuredSelection[i] = meas_sel[i].clone();
+						} else {
+							for (int j = 0; j < meas_sel[i].length; j++){
+								this.measuredSelection[i][j] |= meas_sel[i][j];
+							}
+						}
+					} 
+				}
+			}
+		}
+		
+		
+		
 		public boolean [] getMeasSelection(int nl){
 			if (this.measuredSelection == null) {
 				return null;
@@ -570,16 +594,6 @@ public class TilePlanes {
 									measured_strength_pow,  // double strength_pow,
 									true);                  // boolean null_if_none);
 						}
-/*						
-						disp_strength[nl] = measuredLayers.getDisparityStrength(
-								nl,                     // int num_layer,
-								getSTileXY()[0],        // int stX,
-								getSTileXY()[1],        // int stY,
-								null,                   // boolean [] sel_in, null here - all measured data
-								strength_floor,         //  double strength_floor,
-								measured_strength_pow,  // double strength_pow,
-								true);                  // boolean null_if_none);
-*/								
 						//disp_strength[nl] = measuredLayers.getDisparityStrength(
 						for (int indx = 0; indx < disp_strength[nl][1].length; indx++){
 							double w = disp_strength[nl][1][indx];
@@ -1603,6 +1617,27 @@ public class TilePlanes {
 				scale_projection,
 				debugLevel);
 		}
+		
+		public EigenvalueDecomposition get2dDecomposition()
+		{
+			double [] vals3d =      getValues();
+			double [][] vectors3d = getVectors();
+			double [][] acovar = new double [2][2];
+			for (int i = 0; i < 2; i++){
+				for (int j = i; j < 2; j++){
+					acovar[i][j] = 0.0;
+					for (int k = 0; k < 3; k++){
+						acovar[i][j] += vals3d[k] * vectors3d[k][i+1] * vectors3d[k][j+1]; // 0 - z, disparity == 0
+					}
+					if (i != j) {
+						acovar[j][i] =acovar[i][j];
+					}
+				}
+			}
+			Matrix covar = new Matrix(acovar); // 2d, x y only
+			return covar.eig();
+		}
+		
 		/**
 		 * Get disparity values for the tiles of this overlapping supertile as [2*superTileSize * 2*superTileSize] array
 		 * and weights combined from provided window function, optional selection and using ellipsoid projection on the
@@ -1632,22 +1667,7 @@ public class TilePlanes {
 			double k_gauss = 0;
 			Matrix val2d = null, vect2d = null;
 			if (scale_projection > 0.0){
-				double [] vals3d =      getValues();
-				double [][] vectors3d = getVectors();
-				double [][] acovar = new double [2][2];
-				for (int i = 0; i < 2; i++){
-					for (int j = i; j < 2; j++){
-						acovar[i][j] = 0.0;
-						for (int k = 0; k < 3; k++){
-							acovar[i][j] += vals3d[k] * vectors3d[k][i+1] * vectors3d[k][j+1]; // 0 - z, disparity == 0
-						}
-						if (i != j) {
-							acovar[j][i] =acovar[i][j];
-						}
-					}
-				}
-				Matrix covar = new Matrix(acovar); // 2d, x y only
-				EigenvalueDecomposition eig = covar.eig();
+				EigenvalueDecomposition eig = get2dDecomposition();
 				val2d = eig.getD();
 				vect2d = eig.getV().transpose();
 				k_gauss = 0.5/(scale_projection*scale_projection);
@@ -1758,22 +1778,7 @@ public class TilePlanes {
 			double k_gauss = 0;
 			Matrix val2d = null, vect2d = null;
 			if (scale_projection > 0.0){
-				double [] vals3d =      getValues();
-				double [][] vectors3d = getVectors();
-				double [][] acovar = new double [2][2];
-				for (int i = 0; i < 2; i++){
-					for (int j = i; j < 2; j++){
-						acovar[i][j] = 0.0;
-						for (int k = 0; k < 3; k++){
-							acovar[i][j] += vals3d[k] * vectors3d[k][i+1] * vectors3d[k][j+1]; // 0 - z, disparity == 0
-						}
-						if (i != j) {
-							acovar[j][i] =acovar[i][j];
-						}
-					}
-				}
-				Matrix covar = new Matrix(acovar); // 2d, x y only
-				EigenvalueDecomposition eig = covar.eig();
+				EigenvalueDecomposition eig = get2dDecomposition();
 				val2d = eig.getD();
 				vect2d = eig.getV().transpose();
 				k_gauss = 0.5/(scale_projection*scale_projection);
@@ -1861,10 +1866,10 @@ public class TilePlanes {
 		 * and weights combined from provided window function, optional selection and using ellipsoid projection on the
 		 * px, py plane (constant disparity
 		 * Sharp weights - when selecting the best match - use exponent of (delta_disp) ^2 ?
-		 * Or divide weight by ellipse arae?
+		 * Or divide weight by ellipse area?
 		 * @param useWorld calculate disparity in the real world (false - just px, py, disparity plane)
 		 * @param window null or window function as [2*superTileSize * 2*superTileSize] array
-		 * @param dir - source tile shift from the targer: -1 center, 0 - N, 1 - NE
+		 * @param dir - source tile shift from the target: -1 center, 0 - N, 1 - NE
 		 * @param use_sel use plane selection (this.sel_mask) to select only some part of the plane
 		 * @param divide_by_area divide weights by ellipsoid area
 		 * @param scale_projection use plane ellipsoid projection for weight: 0 - do not use, > 0 linearly scale ellipsoid 
@@ -1888,22 +1893,7 @@ public class TilePlanes {
 			double k_gauss = 0;
 			Matrix val2d = null, vect2d = null;
 			if (scale_projection > 0.0){
-				double [] vals3d =      getValues();
-				double [][] vectors3d = getVectors();
-				double [][] acovar = new double [2][2];
-				for (int i = 0; i < 2; i++){
-					for (int j = i; j < 2; j++){
-						acovar[i][j] = 0.0;
-						for (int k = 0; k < 3; k++){
-							acovar[i][j] += vals3d[k] * vectors3d[k][i+1] * vectors3d[k][j+1]; // 0 - z, disparity == 0
-						}
-						if (i != j) {
-							acovar[j][i] =acovar[i][j];
-						}
-					}
-				}
-				Matrix covar = new Matrix(acovar); // 2d, x y only
-				EigenvalueDecomposition eig = covar.eig();
+				EigenvalueDecomposition eig = get2dDecomposition();
 				val2d = eig.getD();
 				vect2d = eig.getV().transpose();
 				k_gauss = 0.5/(scale_projection*scale_projection);
@@ -2038,6 +2028,20 @@ public class TilePlanes {
 			return getWorldPlaneDist2(otherPd, this.correctDistortions);
 		}
 		
+		/**
+		 * Get squared relative (to the z of the center) distance from the other plane to the center of the current "plane" (ellipsoid) 
+		 * @param otherPd other plane data
+		 * @return squared ratio of the distance the other plane to the (ellipsoid) center of this one over Z-distance
+		 */
+		public double getWorldPlaneRDist2(
+				PlaneData otherPd)
+		{
+			double dist2 = getWorldPlaneDist2(otherPd, this.correctDistortions);
+			double z =getCenterXYZ(this.correctDistortions, 0)[2];
+			return dist2/(z*z);
+		}
+
+		
 		
 		/**
 		 * Combine 2 Plane instances using centers, eigenvalues eihenvectors and total weights of this and other PlaneData objects

src/main/java/TileProcessor.java

@@ -3409,7 +3409,6 @@ public class TileProcessor {
 */
 		showDoubleFloatArrays sdfa_instance = null;
 		if (debugLevel > -1) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
-		if (clt_parameters.dbg_migrate) {
 		// Trying new class
 		LinkPlanes lp = new LinkPlanes (clt_parameters, st);
 		lp.matchPlanes(
@@ -3430,71 +3429,77 @@ public class TileProcessor {
 				clt_parameters.tileX,
 				clt_parameters.tileY);
 
+		boolean [][][] plane_nooverlaps = lp.overlapSameTileCandidates (
+				st.planes, // final TilePlanes.PlaneData [][] planes,			
+				merge_candidates,       // final int [][][] merge_candidates,
+				2, // -1, // debugLevel,                  // final int        debugLevel)
+				clt_parameters.tileX,
+				clt_parameters.tileY);
+/*		
+		merge_candidates =  lp.filterMergeSameTileCandidates(
+				st.planes, // final TilePlanes.PlaneData [][] planes,			
+				merge_candidates,       // final int [][][] merge_candidates,
+				2, // -1, // debugLevel,                  // final int        debugLevel)
+				clt_parameters.tileX,
+				clt_parameters.tileY);
+		
 		boolean [][] pairs_to_merge = lp.mergeSameTileEvaluate(
 				st.planes,              // final TilePlanes.PlaneData [][] planes,
 				merge_candidates,       // final int [][][] merge_candidates,
+				plane_nooverlaps, // boolean [][][] plane_overlaps,
 				2,                      // -1, // debugLevel,                  // final int        debugLevel)
 				clt_parameters.tileX,
 				clt_parameters.tileY);
 		System.out.println(pairs_to_merge.length);		
+*/
 		
-			lp.selectNeighborPlanesMutual(
+		int [][][] merge_groups = lp.extractMergeSameTileGroups(
 				st.planes,              // final TilePlanes.PlaneData [][] planes,
-					0, // final int debugLevel)
+				merge_candidates,       // final int [][][] merge_candidates,
+				plane_nooverlaps, // boolean [][][] plane_overlaps,
+				2,                      // -1, // debugLevel,                  // final int        debugLevel)
 				clt_parameters.tileX,
 				clt_parameters.tileY);
-		} else {		
-			st.matchPlanes(
-					clt_parameters.plPreferDisparity,
-					0, // debugLevel
+
+		int num_removed_by_merging = st.applyMergePlanes(
+				st.planes,    // final TilePlanes.PlaneData[][]   planes,
+				merge_groups, // final int [][][]                 merge_groups,			
+				// parameters to generate ellipsoids			
+				0.0, // 3,                       // final double                     disp_far, // minimal disparity to select (or NaN)
+				Double.NaN,                      // final double                     disp_near, // maximal disparity to select (or NaN)
+				clt_parameters.plDispNorm,       // final double                     dispNorm,   //  Normalize disparities to the average if above
+				0.0,                             // final double                     min_weight,
+				clt_parameters.plMinPoints,      // final int                        min_tiles,
+				// parameters to reduce outliers			
+				clt_parameters.plTargetEigen,    // final double                     targetEigen,   //     =   0.1;  // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
+				clt_parameters.plFractOutliers,  // final double                     fractOutliers, //     =   0.3;  // Maximal fraction of outliers to remove
+				clt_parameters.plMaxOutliers,    // final int                        maxOutliers,   //     =   20;  // Maximal number of outliers to remove
+				2,                      // -1, // debugLevel,                  // final int        debugLevel)
 				clt_parameters.tileX,
 				clt_parameters.tileY);
 
-			st.filterNeighborPlanes(
-					clt_parameters.plWorstWorsening, // final double worst_worsening,
-					clt_parameters.plWorstWorsening2,// final double worst_worsening2  Worst case worsening for thin planes,
-					clt_parameters.plWorstEq,        // final double worstEq,        // Worst case worsening after merge with equal weights
-					clt_parameters.plWorstEq2,       // final double worstEq2,       // Worst case worsening for thin planes with equal weights
-					clt_parameters.plWeakWorsening,  // final double worst_worsening,
-					clt_parameters.plOKMergeEigen,   // final double okMergeEigen, f result of the merged planes is below, OK to use thin planes (higher) threshold
-					clt_parameters.plMaxWorldSin2,   // final double maxWorldSin2,
-					clt_parameters.plDispNorm,
-					clt_parameters.plMaxEigen,
-					clt_parameters.plEigenFloor,    // final double eigenFloor, // Add to eigenvalues of each participating plane and result to validate connections 
-					clt_parameters.plMinStrength,
-					0, // final int debugLevel)
+		System.out.println("Removed "+num_removed_by_merging+" planes by merging, recalculating connections");
+		if (num_removed_by_merging > 0){ // re-calculate all links
+			lp.matchPlanes(
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);
 
-			int [][][] merge_candidates =  st.getMergeSameTileCandidates(
-					2, // final int debugLevel,
+			lp.filterNeighborPlanes(
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);
+		}
 		
-			boolean [][] pairs_to_merge = st.mergeSameTileEvaluate(
-					merge_candidates, // final int [][][] merge_candidates,
-					clt_parameters.plWorstWorsening, // final double worst_worsening,
-					clt_parameters.plWorstWorsening2,// final double worst_worsening2  Worst case worsening for thin planes,
-					clt_parameters.plWorstEq,        // final double worstEq,        // Worst case worsening after merge with equal weights
-					clt_parameters.plWorstEq2,       // final double worstEq2,       // Worst case worsening for thin planes with equal weights
-					clt_parameters.plWeakWorsening,  // final double worst_worsening,
-					clt_parameters.plOKMergeEigen,   // final double okMergeEigen, f result of the merged planes is below, OK to use thin planes (higher) threshold
-					clt_parameters.plMaxWorldSin2,   // final double maxWorldSin2,
-					clt_parameters.plDispNorm,
-					clt_parameters.plMaxEigen,
-					clt_parameters.plEigenFloor,    // final double eigenFloor, // Add to eigenvalues of each participating plane and result to validate connections 
-					0.0, // clt_parameters.plMinStrength,
-					clt_parameters.plPreferDisparity,
+		
+		double [][] quality_stats1 = lp.selectNeighborPlanesMutual(
+				st.planes,              // final TilePlanes.PlaneData [][] planes,
 				2, // final int debugLevel)
 				clt_parameters.tileX,
 				clt_parameters.tileY);
 
-			st.selectNeighborPlanesMutual(
-					clt_parameters.plEigenFloor,    // final double eigenFloor, // Add to eigenvalues of each participating plane and result to validate connections 
-					0, // final int debugLevel)
-					clt_parameters.tileX,
-					clt_parameters.tileY);
-		}
 		st.resolveConflicts(
 				clt_parameters.plMaxEigen,
 				clt_parameters.plConflDualTri, // boolean    conflDualTri,  // Resolve dual triangles conflict (odoodo)
@@ -3609,31 +3614,21 @@ public class TileProcessor {
 				System.out.println("replaceBrokenPlanes(): Replaced " + numSplitPlanes + " planes by pairs.");
 			}
 			// now re-create connections and best neighbors
-			st.matchPlanes(
-					clt_parameters.plPreferDisparity,
-					0, // debugLevel
+			lp = new LinkPlanes (clt_parameters, st);
+			lp.matchPlanes(
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);
 
-			st.filterNeighborPlanes(
-					clt_parameters.plWorstWorsening, // final double worst_worsening,
-					clt_parameters.plWorstWorsening2,// final double worst_worsening2  Worst case worsening for thin planes,
-					clt_parameters.plWorstEq,        // ffinal double worstEq,        // Worst case worsening after merge with equal weights
-					clt_parameters.plWorstEq2,       // ffinal double worstEq2,       // Worst case worsening for thin planes with equal weights
-					clt_parameters.plWeakWorsening,  // final double worst_worsening,
-					clt_parameters.plOKMergeEigen,   // final double okMergeEigen,
-					clt_parameters.plMaxWorldSin2,   // final double maxWorldSin2,
-
-					clt_parameters.plDispNorm,
-					clt_parameters.plMaxEigen,
-					clt_parameters.plEigenFloor,    // final double eigenFloor, // Add to eigenvalues of each participating plane and result to validate connections 
-					clt_parameters.plMinStrength,
-					0, // final int debugLevel)
+			lp.filterNeighborPlanes(
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);
 
-			st.selectNeighborPlanesMutual(
-					clt_parameters.plEigenFloor,    // final double eigenFloor, // Add to eigenvalues of each participating plane and result to validate connections 
+			double [][] quality_stats2 = lp.selectNeighborPlanesMutual(
+					st.planes,              // final TilePlanes.PlaneData [][] planes,
 					0, // final int debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);