re-assigning supertiles links

src/main/java/EyesisCorrectionParameters.java

@@ -2187,6 +2187,8 @@ public class EyesisCorrectionParameters {
   		public double     plWeakEigen2         =   0.05; // Maximal eigenvalue of the result of non-weighted merge  (second variant)
   		public double     plSumThick           =   1.2;  // Do not merge if any sqrt of merged eigenvalue exceeds scaled sum of components 
   		public double     plNeNeibCost         =   5.0;  // When calculating non-exclusive planes, do not use neighbors with high cost
+  	    public double     plExNeibCost         =   5.0;  // When calculating exclusive planes links, do not use neighbors with high cost
+  		
   		public double     plNeOwn              =   5.0;  // When calculating non-exclusive planes, use cenrter plane relative weight
   		public double     plMaxZRatio          =   2.0;  // Maximal ratio of Z to allow plane merging
   		public double     plMaxDisp            =   0.6;  // Maximal disparity of one of the planes to apply  maximal ratio
@@ -2598,6 +2600,7 @@ public class EyesisCorrectionParameters {
 			properties.setProperty(prefix+"plWeakEigen2",     this.plWeakEigen2 +"");
 			properties.setProperty(prefix+"plSumThick",       this.plSumThick +"");
 			properties.setProperty(prefix+"plNeNeibCost",     this.plNeNeibCost +"");
+			properties.setProperty(prefix+"plExNeibCost",     this.plExNeibCost +"");
 			properties.setProperty(prefix+"plNeOwn",          this.plNeOwn +"");
 			
 			properties.setProperty(prefix+"plMaxZRatio",      this.plMaxZRatio +"");
@@ -2985,6 +2988,7 @@ public class EyesisCorrectionParameters {
   			if (properties.getProperty(prefix+"plWeakEigen2")!=null)      this.plWeakEigen2=Double.parseDouble(properties.getProperty(prefix+"plWeakEigen2"));
   			if (properties.getProperty(prefix+"plSumThick")!=null)        this.plSumThick=Double.parseDouble(properties.getProperty(prefix+"plSumThick"));
   			if (properties.getProperty(prefix+"plNeNeibCost")!=null)      this.plNeNeibCost=Double.parseDouble(properties.getProperty(prefix+"plNeNeibCost"));
+  			if (properties.getProperty(prefix+"plExNeibCost")!=null)      this.plExNeibCost=Double.parseDouble(properties.getProperty(prefix+"plExNeibCost"));
   			if (properties.getProperty(prefix+"plNeOwn")!=null)           this.plNeOwn=Double.parseDouble(properties.getProperty(prefix+"plNeOwn"));
   			
   			if (properties.getProperty(prefix+"plMaxZRatio")!=null)       this.plMaxZRatio=Double.parseDouble(properties.getProperty(prefix+"plMaxZRatio"));
@@ -3401,6 +3405,7 @@ public class EyesisCorrectionParameters {
   			gd.addNumericField("Maximal eigenvalue of the result of non-weighted merge (second variant)",      this.plWeakEigen2,  6);
   			gd.addNumericField("Do not merge if any sqrt of merged eigenvalue exceeds scaled sum of components", this.plSumThick,  6);
   			gd.addNumericField("When calculating non-exclusive planes, do not use neighbors with high cost",   this.plNeNeibCost,  6);
+  			gd.addNumericField(" When calculating exclusive planes links, do not use neighbors with high cost",this.plExNeibCost,  6);
   			gd.addNumericField("When calculating non-exclusive planes, use cenrter plane relative weight",     this.plNeOwn,  6);
 
   			gd.addMessage     ("---  ---");
@@ -3804,6 +3809,7 @@ public class EyesisCorrectionParameters {
   			this.plWeakEigen2=          gd.getNextNumber();
   			this.plSumThick=            gd.getNextNumber();
   			this.plNeNeibCost=          gd.getNextNumber();
+  			this.plExNeibCost=          gd.getNextNumber();
   			this.plNeOwn=               gd.getNextNumber();
 
   			this.plMaxZRatio=           gd.getNextNumber();

src/main/java/LinkPlanes.java

@@ -54,7 +54,8 @@ public class LinkPlanes {
 	public double     plWeakEigen2; //         =   0.05; // Maximal eigenvalue of the result of non-weighted merge  (second variant)
     public double     plSumThick; //           =   1.2;  // Do not merge if any sqrt of merged eigenvalue exceeds scaled sum of components
     public double     plNeNeibCost; //         =   5.0;  // When calculating non-exclusive planes, do not use neighbors with high cost
-    public double     plNeOwn; //              =   5.0;  // When calculating non-exclusive planes, use cenrter plane relative weight
+    public double     plExNeibCost; //         =   5.0;  // When calculating non-exclusive planes, do not use neighbors with high cost
+    public double     plNeOwn; //              =   5.0;  // When calculating non-exclusive planes, use center plane relative weight
     
 
 		// comparing merge quality for plane pairs
@@ -101,6 +102,7 @@ public class LinkPlanes {
 		plWeakEigen2 =      clt_parameters.plWeakEigen2;
 		plSumThick =        clt_parameters.plSumThick;
 		plNeNeibCost =      clt_parameters.plNeNeibCost;
+		plExNeibCost =      clt_parameters.plExNeibCost;
 		plNeOwn =           clt_parameters.plNeOwn;
 		
 
@@ -467,7 +469,30 @@ public class LinkPlanes {
 	// 0 - this_rq,
 	// 1 - this_rq_eq
 	// 2 - composite
-	// 3..7 contribution of each of the factor to the overall cost 
+	// 3..7 contribution of each of the factor to the overall cost
+	public double getLinkCost(
+			TilePlanes.PlaneData plane1, // should belong to the same supertile (or be converted for one)
+			TilePlanes.PlaneData plane2,
+			double               merged_ev,    // if NaN will calculate assuming the same supertile
+			double               merged_ev_eq, // if NaN will calculate assuming the same supertile
+			double               merged_wev,    // if NaN will calculate assuming the same supertile - for world
+			double               merged_wev_eq, // if NaN will calculate assuming the same supertile - for world
+			String prefix,
+			int debugLevel){
+		int cost_index = 2;
+		
+		double [] costs = getFitQualities(
+				plane1, // should belong to the same supertile (or be converted for one)
+				plane2,
+				merged_ev,    // if NaN will calculate assuming the same supertile
+				merged_ev_eq, // if NaN will calculate assuming the same supertile
+				merged_wev,    // if NaN will calculate assuming the same supertile - for world
+				merged_wev_eq, // if NaN will calculate assuming the same supertile - for world
+				prefix,
+				debugLevel); // [cost_index];
+		if (costs == null) return Double.NaN;
+		else return costs[cost_index];
+	}
 	public double [] getFitQualities(
 			TilePlanes.PlaneData plane1, // should belong to the same supertile (or be converted for one)
 			TilePlanes.PlaneData plane2,
@@ -784,6 +809,230 @@ public class LinkPlanes {
 		}		      
 		ImageDtt.startAndJoin(threads);
 	}
+	
+	/**
+	 * Create matrix of connection costs between layer pairs for each direction. Even as currently costs seem to be symmetrical
+	 * this method calculates all 8 directions, from the PoV of the current supertile.
+	 * Intended to be used after first round of smoothing to re-evaluate which connections are valid and run more smoothing with remaining
+	 * connections. No-Link supertiles will be set to original measured data
+	 * @param planes
+	 * @param debugLevel
+	 * @param dbg_X
+	 * @param dbg_Y
+	 */
+	
+	public void interPlaneCosts(
+			final TilePlanes.PlaneData [][] planes,			
+			final int                       debugLevel,
+			final int                       dbg_X,
+			final int                       dbg_Y)
+	{
+		final int tilesX =        st.tileProcessor.getTilesX();
+		final int tilesY =        st.tileProcessor.getTilesY();
+		final int superTileSize = st.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 double [] nan_plane = new double [superTileSize*superTileSize];
+		for (int i = 0; i < nan_plane.length; i++) nan_plane[i] = Double.NaN;
+		//				final int debug_stile = 20 * stilesX + 27;
+		final int debug_stile = dbg_Y * stilesX + dbg_X;
+		final TileNeibs tnSurface = new TileNeibs(stilesX, stilesY);
+
+		final Thread[] threads = ImageDtt.newThreadArray(st.tileProcessor.threadsMax);
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nsTile0 = ai.getAndIncrement(); nsTile0 < nStiles; nsTile0 = ai.getAndIncrement()) {
+						int dl = ((debugLevel > -1) && (nsTile0 == debug_stile)) ? 1:0;
+						if ( planes[nsTile0] != null) {
+							if (dl > 0){
+								System.out.println("interPlaneCosts(): nsTile0 ="+nsTile0);
+							}
+							for (int np0 = 0; np0 < planes[nsTile0].length; np0++){ // nu
+								//										planes[nsTile0][np0].initNeibBest(); // 
+								TilePlanes.PlaneData this_plane = planes[nsTile0][np0];
+								if (this_plane != null) {
+									this_plane.initLinkCosts();
+									for (int dir = 0; dir < 8; dir++){ // All 8 directions if the costs are not symmetrical
+										int nsTile = tnSurface.getNeibIndex(nsTile0, dir);
+										if (nsTile >= 0){
+											TilePlanes.PlaneData [] other_planes = planes[nsTile];
+											if (other_planes != null) {
+												this_plane.initLinkCosts(dir,other_planes.length); // filled with NaN
+												for (int np = 0; np < other_planes.length; np ++){
+													if (other_planes[np] != null) {
+														TilePlanes.PlaneData other_plane = this_plane.getPlaneToThis(
+																other_planes[np],
+																dl-1); // debugLevel);
+														if (other_plane !=null) { // now always, but may add later
+															double link_cost = getLinkCost(
+																	this_plane,  // TilePlanes.PlaneData plane1, // should belong to the same supertile (or be converted for one)
+																	other_plane, // TilePlanes.PlaneData plane2,
+																	Double.NaN, // double               merged_ev,    // if NaN will calculate assuming the same supertile
+																	Double.NaN, // double               merged_ev_eq, // if NaN will calculate assuming the same supertile
+																	Double.NaN, // double               merged_wev,    // if NaN will calculate assuming the same supertile - for world
+																	Double.NaN, // double               merged_wev_eq, // if NaN will calculate assuming the same supertile - for world
+																	"interPlaneCosts(): "+nsTile0+":"+np0+"-("+dir+")-->"+nsTile+":"+np, // String prefix,
+																	dl - 1);
+															this_plane.setLinkCosts(dir, np, link_cost);
+														}
+													}
+												}
+											}
+										}
+									}
+								}
+							}
+							if (dl > 0){
+								System.out.println("interPlaneCosts(): nsTile0 ="+nsTile0+ " Done.");
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+	}
+
+	public void setExclusiveLinks(
+			final TilePlanes.PlaneData [][] planes,
+			final int                       debugLevel,
+			final int                       dbg_X,
+			final int                       dbg_Y)
+	{
+		setExclusiveLinks(
+				planes,
+				plExNeibCost,
+				debugLevel,
+				dbg_X,
+				dbg_Y);
+	}	
+	public void setExclusiveLinks(
+			final TilePlanes.PlaneData [][] planes,
+			final double                    max_cost,
+			final int                       debugLevel,
+			final int                       dbg_X,
+			final int                       dbg_Y)
+	{
+		final int tilesX =        st.tileProcessor.getTilesX();
+		final int tilesY =        st.tileProcessor.getTilesY();
+		final int superTileSize = st.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 double [] nan_plane = new double [superTileSize*superTileSize];
+		for (int i = 0; i < nan_plane.length; i++) nan_plane[i] = Double.NaN;
+		//				final int debug_stile = 20 * stilesX + 27;
+		final int debug_stile = dbg_Y * stilesX + dbg_X;
+		final TileNeibs tnSurface = new TileNeibs(stilesX, stilesY);
+
+		final Thread[] threads = ImageDtt.newThreadArray(st.tileProcessor.threadsMax);
+		final AtomicInteger ai = new AtomicInteger(0);
+		// Reset neighbors
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nsTile0 = ai.getAndIncrement(); nsTile0 < nStiles; nsTile0 = ai.getAndIncrement()) {
+						if ( planes[nsTile0] != null) {
+							for (int np0 = 0; np0 < planes[nsTile0].length; np0++){ // nu
+								TilePlanes.PlaneData this_plane = planes[nsTile0][np0];
+								if (this_plane != null) {
+									this_plane.initNeibBest();
+								}
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		ai.set(0);				
+		final class LinkPair{
+			int src;
+			int dst;
+			double cost;
+			LinkPair (int src, int dst, double cost){
+				this.src = src;
+				this.dst = dst;
+				this.cost = cost;
+			}
+			boolean hasConflict(LinkPair lp){
+				return (src == lp.src) || (dst == lp.dst);
+			}
+			
+		}
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nsTile0 = ai.getAndIncrement(); nsTile0 < nStiles; nsTile0 = ai.getAndIncrement()) {
+						int dl = ((debugLevel > -1) && (nsTile0 == debug_stile)) ? 1:0;
+						if ( planes[nsTile0] != null) {
+							int num_sp = planes[nsTile0].length; 
+							if (dl > 0){
+								System.out.println("setExclusiveLinks(): nsTile0 ="+nsTile0);
+							}
+							for (int dir = 0; dir < 4; dir++){//
+								int dir_back = (dir+4) % 8;
+								int nsTile = tnSurface.getNeibIndex(nsTile0, dir);
+								if ((nsTile >= 0) && (planes[nsTile] != null)){
+									int num_dp = planes[nsTile].length;
+									double [][] inter_costs = new double [num_sp][num_dp];
+									for (int np0 = 0; np0 < num_sp; np0++) if (planes[np0]!=null){
+										for (int np = 0; np < num_dp; np++)if (planes[np]!=null){
+											inter_costs[np0][np] = 0.5* (
+													planes[nsTile0][np0].getLinkCosts(dir, np)+
+													planes[nsTile][np].getLinkCosts(dir_back, np0));
+										}
+									}
+									HashSet<LinkPair> pairs_set = new HashSet<LinkPair>();
+									for (int np0 = 0; np0 < num_sp; np0++) if (planes[np0]!=null){
+										for (int np = 0; np < num_dp; np++)if (planes[np]!=null){
+											if (!Double.isNaN(inter_costs[np0][np]) && (inter_costs[np0][np] <= max_cost)){
+												pairs_set.add(new LinkPair(np0, np, inter_costs[np0][np]));
+											}
+										}										
+									}
+									HashSet<LinkPair> links_set = new HashSet<LinkPair>();
+									while (!pairs_set.isEmpty()){
+										LinkPair link =Collections.max(pairs_set, new Comparator<LinkPair>() {
+											@Override
+											public int compare(LinkPair lhs, LinkPair rhs) { // ascending
+												return (lhs.cost < rhs.cost) ? -1 : (lhs.cost > rhs.cost ) ? 1 : 0;
+											}
+										});
+										links_set.add(link);
+										pairs_set.remove(link);
+										HashSet<LinkPair> conflict_links = new HashSet<LinkPair>();
+										for (LinkPair lp:pairs_set){
+											if (link.hasConflict(lp)) {
+												conflict_links.add(lp);
+											}
+										}
+										pairs_set.removeAll(conflict_links);
+									}
+									for (LinkPair lp:links_set){
+										planes[nsTile0][lp.src].setNeibBest(dir, lp.dst);
+										planes[nsTile ][lp.dst].setNeibBest(dir_back, lp.src);
+									}
+								}
+							}							
+							if (dl > 0){
+								System.out.println("setExclusiveLinks(): nsTile0 ="+nsTile0+ " Done.");
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+	}
+	
+	
+	
 	/**
 	 * Mark which links between neighbor planes are valid
 	 * @param debugLevel debug level
@@ -2104,8 +2353,6 @@ public class LinkPlanes {
 			threads[ithread] = new Thread() {
 				public void run() {
 					for (int nsTile0 = ai.getAndIncrement(); nsTile0 < nStiles; nsTile0 = ai.getAndIncrement()) if ( merge_candidates[nsTile0] != null) {
-						int sty0 = nsTile0 / stilesX;  
-						int stx0 = nsTile0 % stilesX;
 						int dl = ((debugLevel > 0) && (nsTile0 == debug_stile)) ? 3: ((debugLevel > 1) ? 2:0);
 						if (dl > 2){
 							System.out.println("costSameTileConnections(): nsTile="+nsTile0);

src/main/java/TilePlanes.java

@@ -109,7 +109,8 @@ public class TilePlanes {
 		double [][]  wvectors =    null; // [3][3] - eigenvectors calculated in the real world
 		double []    wvalues =     null; // [3] -eigenvalues calculated in the real world
 		double [][]  merged_weig_val = null; // for each of the directions (N, NE, .. NW) quality match for each layer 
-		double [][]  merged_weig_eq =  null; // for each of the directions (N, NE, .. NW) quality match for each layer - ignoring weights 
+		double [][]  merged_weig_eq =  null; // for each of the directions (N, NE, .. NW) quality match for each layer - ignoring weights
+		double [][]  link_costs; // for each of the directions (N, NE, .. NW) composite cost of connection
 
 		int mark0 = -1; // just for temporary labeling the plane
 		
@@ -480,6 +481,16 @@ public class TilePlanes {
 				}
 			}
 
+			if (src.link_costs != null){
+				dst.link_costs = src.link_costs.clone();
+				for (int i = 0; i < src.link_costs.length; i++){
+					if (src.link_costs[i] != null){
+						dst.link_costs[i] = src.link_costs[i].clone();
+					}
+				}
+			}
+			
+			
 			if (src.neib_best != null) dst.neib_best = src.neib_best.clone();
 			
 			// also copy original plane parameters - tile selection and number of points
@@ -1728,11 +1739,11 @@ public class TilePlanes {
 		
 		public double [][] initMergedValue()
 		{
-			this.merged_eig_val = new double[8][];
-			this.merged_eig_eq = new double[8][];
-			this.merged_weig_val = new double[8][];
-			this.merged_weig_eq = new double[8][];
-			this.merged_valid = new boolean[8][];
+			this.merged_eig_val =      new double[8][];
+			this.merged_eig_eq =       new double[8][];
+			this.merged_weig_val =     new double[8][];
+			this.merged_weig_eq =      new double[8][];
+			this.merged_valid =        new boolean[8][];
 			this.merged_strong_valid = new boolean[8][];
 			return this.merged_eig_val;
 		}
@@ -1755,7 +1766,23 @@ public class TilePlanes {
 		{
 			return this.merged_weig_eq;
 		}
+		public double [][] getLinkCosts()
+		{
+			return this.link_costs;
+		}
+		public double [] getLinkCosts(int dir)
+		{
+			return this.link_costs[dir];
+		}
+		public double getLinkCosts(int dir, int np)
+		{
+			return this.link_costs[dir][np];
+		}
 		
+		public void setLinkCosts(int dir, int plane, double value)
+		{
+			this.link_costs[dir][plane] = value;
+		}
 		
 		public double [] initMergedValue(int dir, int leng)
 		{
@@ -1774,6 +1801,19 @@ public class TilePlanes {
 			return getMergedValue(dir);
 		}
 
+		public double [][] initLinkCosts(){
+			this.link_costs =          new double[8][];
+			return link_costs;
+		}
+		public double [] initLinkCosts(int dir, int leng)
+		{
+			this.link_costs[dir] =            new double[leng];
+			for (int i = 0; i < leng; i++) {
+				this.link_costs[dir][i] =      Double.NaN;
+			}
+			return getLinkCosts(dir);
+		}
+		
 		public double [] getMergedValue(int dir)
 		{
 			if (this.merged_eig_val == null) {

src/main/java/TileProcessor.java

@@ -3251,6 +3251,11 @@ public class TileProcessor {
 					2, // -1, // debugLevel,                  // final int        debugLevel)
 					clt_parameters.tileX,
 					clt_parameters.tileY);
+			lp.interPlaneCosts( // not used yet, just for testing
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
 
 			lp.filterNeighborPlanes(
 					st.planes, // final TilePlanes.PlaneData [][] planes,
@@ -3698,6 +3703,12 @@ public class TileProcessor {
 					clt_parameters.tileX,
 					clt_parameters.tileY);
 
+			lp.interPlaneCosts( // not used yet, just for testing
+					st.planes, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
+
 			lp.filterNeighborPlanes(
 					st.planes, // final TilePlanes.PlaneData [][] planes,
 					true, // final boolean merge_low_eigen,
@@ -3769,7 +3780,32 @@ public class TileProcessor {
 					clt_parameters.plPreferDisparity,
 					0, // 1,// 0, // final int debugLevel)
 					clt_parameters.tileX,
-					clt_parameters.tileY); 
+					clt_parameters.tileY);
+			// create costs for the modified planes
+			lp.interPlaneCosts(
+					st.planes_mod, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
+			lp.setExclusiveLinks(
+					st.planes_mod, // final TilePlanes.PlaneData [][] planes,			
+					2, // -1, // debugLevel,                  // final int        debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
+			// once more after updating exclusive links
+			planes_mod = st.planesSmooth(
+					lp,                                           // LinkPlanes       lp,			
+					clt_parameters.plPull,                        // final double      meas_pull,//  relative pull of the original (measured) plane with respect to the average of the neighbors
+					clt_parameters.plMaxEigen,                    // final double      maxValue, // do not combine with too bad planes
+					clt_parameters.plIterations,                  // final int         num_passes,
+					clt_parameters.plStopBad,                     // Do not update supertile if any of connected neighbors is not good (false: just skip that neighbor)
+					clt_parameters.plNormPow,                     // 0.0: 8 neighbors pull 8 times as 1, 1.0 - same as 1
+					Math.pow(10.0,  -clt_parameters.plPrecision), // final double      maxDiff, // maximal change in any of the disparity values
+					clt_parameters.plPreferDisparity,
+					0, // 1,// 0, // final int debugLevel)
+					clt_parameters.tileX,
+					clt_parameters.tileY);
+			
 		} else {
 			st.planes_mod = st.planes; // just use the measured ones
 		}