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

@@ -199,6 +199,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) {
@@ -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];
@@ -1602,7 +1616,28 @@ public class TilePlanes {
 				divide_by_area,
 				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