Working on smooth ground surface

src/main/java/com/elphel/imagej/cameras/CLTParameters.java

@@ -468,6 +468,17 @@ public class CLTParameters {
 	public boolean    gltf_alpha_blend =  false; // true;   // Use alpha blend (false - opaque)
 	
 	// parameters for ground maps generation
+	// discard vegetation (tall objects), LPF ground surface. Especially needed for low quality depth map (no SfM)
+	public boolean    gsmth_use_lma =       true; // use only LMA-generated depth tiles 
+	public double     gsmth_discard_low =   0.2;  // discard % of the lowest  tiles (holes?)
+	public double     gsmth_discard_high =  0.3;  // discard % of the tallest tiles (high objects)
+	public int        gsmth_stile_hstep =   4;    // use supertiles twice larger
+	public double     gsmth_stile_radius =  8.0;  // fit planes to the data within this radius of the supertile center 
+	public double     gsmth_sigma =         4.0;  // Additionally LPF after fitting planes
+	public int        gsmth_radius_levels = 4;    // increase radius twice multiple time if there are no enough tiles 
+	public int        gsmth_min_non_empty = 10;   // minimal number of non-empty tiles for fitting planes
+	public int        gsmth_border_level =  1;    // 1 - start with twice larger radius for forder supertiles 
+	
 	public boolean    gmap_render_hdr =      true; //  generate textures w/o normalization to generate undistorted
 	public boolean    gmap_en =              true; //  generate ground map from a drone (enables gmap_render_hdr)
 	public boolean    gmap_parallel_proj =   true; // Use parallel projection (map)
@@ -1625,6 +1636,16 @@ public class CLTParameters {
 
 		properties.setProperty(prefix+"gltf_emissive",              this.gltf_emissive+"");
 		properties.setProperty(prefix+"gltf_alpha_blend",           this.gltf_alpha_blend+"");
+		
+		properties.setProperty(prefix+"gsmth_use_lma",              this.gsmth_use_lma+      "");        // boolean
+		properties.setProperty(prefix+"gsmth_discard_low",          this.gsmth_discard_low+  "");        // double
+		properties.setProperty(prefix+"gsmth_discard_high",         this.gsmth_discard_high+ "");        // double
+		properties.setProperty(prefix+"gsmth_stile_hstep",          this.gsmth_stile_hstep+  "");        // int
+		properties.setProperty(prefix+"gsmth_stile_radius",         this.gsmth_stile_radius+ "");        // double 
+		properties.setProperty(prefix+"gsmth_sigma",                this.gsmth_sigma+        "");        // double
+		properties.setProperty(prefix+"gsmth_radius_levels",        this.gsmth_radius_levels+"");        // int
+		properties.setProperty(prefix+"gsmth_min_non_empty",        this.gsmth_min_non_empty+"");        // int
+		properties.setProperty(prefix+"gsmth_border_level",         this.gsmth_border_level+ "");        // int 		
 
 		properties.setProperty(prefix+"gmap_render_hdr",            this.gmap_render_hdr+"");            // boolean 
 		properties.setProperty(prefix+"gmap_en",                    this.gmap_en+"");                    // boolean
@@ -2659,6 +2680,16 @@ public class CLTParameters {
 		if (properties.getProperty(prefix+"gltf_emissive")!=null)                 this.gltf_emissive=Boolean.parseBoolean(properties.getProperty(prefix+"gltf_emissive"));
 		if (properties.getProperty(prefix+"gltf_alpha_blend")!=null)              this.gltf_alpha_blend=Boolean.parseBoolean(properties.getProperty(prefix+"gltf_alpha_blend"));
 
+		if (properties.getProperty(prefix+"gsmth_use_lma")!=null)        this.gsmth_use_lma      =Boolean.parseBoolean(properties.getProperty(prefix+ "gsmth_use_lma"      ));
+		if (properties.getProperty(prefix+"gsmth_discard_low")!=null)    this.gsmth_discard_low  =Double.parseDouble(properties.getProperty(prefix+   "gsmth_discard_low"  ));
+		if (properties.getProperty(prefix+"gsmth_discard_high")!=null)   this.gsmth_discard_high =Double.parseDouble(properties.getProperty(prefix+   "gsmth_discard_high" ));
+		if (properties.getProperty(prefix+"gsmth_stile_hstep")!=null)    this.gsmth_stile_hstep  =Integer.parseInt(properties.getProperty(prefix+     "gsmth_stile_hstep"  ));
+		if (properties.getProperty(prefix+"gsmth_stile_radius")!=null)   this.gsmth_stile_radius =Double.parseDouble(properties.getProperty(prefix+   "gsmth_stile_radius" ));
+		if (properties.getProperty(prefix+"gsmth_sigma")!=null)          this.gsmth_sigma        =Double.parseDouble(properties.getProperty(prefix+   "gsmth_sigma"        ));
+		if (properties.getProperty(prefix+"gsmth_radius_levels")!=null)  this.gsmth_radius_levels=Integer.parseInt(properties.getProperty(prefix+     "gsmth_radius_levels"));
+		if (properties.getProperty(prefix+"gsmth_min_non_empty")!=null)  this.gsmth_min_non_empty=Integer.parseInt(properties.getProperty(prefix+     "gsmth_min_non_empty"));
+		if (properties.getProperty(prefix+"gsmth_border_level")!=null)   this.gsmth_border_level =Integer.parseInt(properties.getProperty(prefix+     "gsmth_border_level" ));
+
 		if (properties.getProperty(prefix+"gmap_render_hdr")!=null)      this.gmap_render_hdr=Boolean.parseBoolean(properties.getProperty(prefix+   "gmap_render_hdr"));
 		if (properties.getProperty(prefix+"gmap_en")!=null)              this.gmap_en=Boolean.parseBoolean(properties.getProperty(prefix+           "gmap_en"));
 		if (properties.getProperty(prefix+"gmap_parallel_proj")!=null)   this.gmap_parallel_proj=Boolean.parseBoolean(properties.getProperty(prefix+"gmap_parallel_proj"));
@@ -3919,8 +3950,28 @@ public class CLTParameters {
 				"Minimal number of background tiles to generate background.");
 		
 		gd.addMessage     ("glTF export");
-		gd.addCheckbox    ("glTF use emissive textures",                                                             this.gltf_emissive);
-		gd.addCheckbox    ("glTF use alpha blend",                                                                   this.gltf_alpha_blend);
+		gd.addCheckbox    ("glTF use emissive textures",           this.gltf_emissive);
+		gd.addCheckbox    ("glTF use alpha blend",                 this.gltf_alpha_blend);
+		gd.addMessage     ("Extract smooth ground surface");
+
+		gd.addCheckbox ("Use only LMA disparity for ground",       this.gsmth_use_lma,
+				"Use only LMA-generated disparity to find the ground plane to project the map.");
+		gd.addNumericField("Discard low fraction",                 this.gmap_discard_low,  4,6,"",
+				"Discard lowest tiles (ditches) up to this fraction of all available tiles when finding the ground plane for the map projection.");
+		gd.addNumericField("Discard high fraction",                this.gsmth_discard_high,  4,6,"",
+				"Discard highest tiles (vegetation, structure) up to this fraction of all available tiles when finding the ground plane for the map projection.");
+		gd.addNumericField("Half-pitch of the supertiles",         this.gsmth_stile_hstep, 0,3,"tiles",
+				"Supertiles centers will be at 1x, 3x, 5x, ... this value in each of the ortho directions.");
+		gd.addNumericField("Plane-fitting radius",                 this.gsmth_stile_radius,  4,6,"tiles",
+				"Fit a plane to each supertile using tiles within this radius around the center.");
+		gd.addNumericField("LPF sigma",                            this.gsmth_sigma,  4,6,"tiles",
+				"Use Gaussian blur to additionally blur the surface of the fitted planes.");
+		gd.addNumericField("Number of radius levels",              this.gsmth_radius_levels, 0,3,"pix",
+				"Number of times to double fitting radius when there are insufficient tiles for plane fitting.");
+		gd.addNumericField("Minimal tiles for fitting",            this.gsmth_min_non_empty, 0,3,"pix",
+				"Minimal number of tiles needed for plane fitting.");
+		gd.addNumericField("Border radius level",                  this.gsmth_border_level, 0,3,"pix",
+				"Start with higher radius level for border supertiles (1 - use twice larger radius.");
 
 		gd.addMessage     ("Ground map export");
 		gd.addMessage     ("Use of IMS/GNSS data is "+(imp.ims_use? "ENABLED":"DISABLED")+" (see 'IMS' configuration tab)");
@@ -5184,6 +5235,16 @@ public class CLTParameters {
 		this.gltf_emissive=         gd.getNextBoolean();
 		this.gltf_alpha_blend=      gd.getNextBoolean();
 		
+		this.gsmth_use_lma=             gd.getNextBoolean();
+		this.gsmth_discard_low=         gd.getNextNumber();
+		this.gsmth_discard_high=        gd.getNextNumber();
+		this.gsmth_stile_hstep=   (int) gd.getNextNumber();
+		this.gsmth_stile_radius=        gd.getNextNumber();
+		this.gsmth_sigma=               gd.getNextNumber();
+		this.gsmth_radius_levels= (int) gd.getNextNumber();
+		this.gsmth_min_non_empty= (int) gd.getNextNumber();
+		this.gsmth_border_level=  (int) gd.getNextNumber();
+		
 		this.gmap_render_hdr=       gd.getNextBoolean();
 		this.gmap_en=               gd.getNextBoolean();
 		this.gmap_parallel_proj=    gd.getNextBoolean();

src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java

@@ -5429,7 +5429,7 @@ public class OpticalFlow {
 			}
 		}
 		
-//		quadCLTs[ref_index].getSmoothGround(clt_parameters);
+		quadCLTs[ref_index].getSmoothGround(clt_parameters, debugLevel);
 		
 		// later move to the right place
 		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {

src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java

@@ -1682,46 +1682,49 @@ public class QuadCLTCPU {
     				debug_level);
     	}
     }
-    public double [] getSmoothGround(final CLTParameters clt_parameters) {
-		final boolean       use_lma =       true; // false, or there will be too many gaps?
-		final double        discard_low =   0.1;   // fraction of all pixels
-		final double        discard_high =  0.5;   // fraction of all pixels
-//		final double        discard_adisp,  // discard above/below this fraction of average height 
-//		final double        discard_rdisp,  // discard above/below this fraction of average height
-		final int           stile_hstep =   4;
-		final double        stile_radius =  8.0; // 
-		final double        sigma =         2.0;
-		final int           radius_levels = 4;
-		final int           min_non_empty = 10;  // minimal number of non-empty tiles for fitting planes
-		final int           debugLevel = 3;
-	return getSmoothGround(
-			clt_parameters, // final CLTParameters clt_parameters,
-			use_lma,        // final boolean       use_lma, // false, or there will be too many gaps?
-			discard_low,    // final double        discard_low,    // fraction of all pixels
-			discard_high,   // final double        discard_high,   // fraction of all pixels
-			stile_hstep,    // final int           stile_hstep,
-			stile_radius,   // final double        stile_radius,
-			sigma,          // final double        sigma,
-			radius_levels,  // final int           radius_levels,
-			min_non_empty,  // final int           min_non_empty,  // minimal number of non-empty tiles for fitting planes
-			debugLevel);    // final int           debugLevel);
+    public double [] getSmoothGround(
+    		CLTParameters clt_parameters,
+    		int           debugLevel) {
+    	boolean       use_lma =         clt_parameters.gsmth_use_lma;    
+    	double        discard_low =     clt_parameters.gsmth_discard_low;
+    	double        discard_high =    clt_parameters.gsmth_discard_high;
+    	int           stile_hstep =     clt_parameters.gsmth_stile_hstep;
+    	double        stile_radius =    clt_parameters.gsmth_stile_radius; 
+    	double        sigma =           clt_parameters.gsmth_sigma;
+    	int           radius_levels =   clt_parameters.gsmth_radius_levels;
+    	int           min_non_empty =   clt_parameters.gsmth_min_non_empty;
+    	int           border_start_lev= clt_parameters.gsmth_border_level;
+    	debugLevel = 3;
+    	return getSmoothGround(
+    			clt_parameters, // final CLTParameters clt_parameters,
+    			use_lma,        // final boolean       use_lma, // false, or there will be too many gaps?
+    			discard_low,    // final double        discard_low,    // fraction of all pixels
+    			discard_high,   // final double        discard_high,   // fraction of all pixels
+    			stile_hstep,    // final int           stile_hstep,
+    			stile_radius,   // final double        stile_radius,
+    			sigma,          // final double        sigma,
+    			radius_levels,  // final int           radius_levels,
+    			min_non_empty,  // final int           min_non_empty,  // minimal number of non-empty tiles for fitting planes
+    			border_start_lev, // final int           border_start_lev0,
+    			debugLevel);    // final int           debugLevel);
     }
+    
     public double [] getSmoothGround(
     		final CLTParameters clt_parameters,
     		final boolean       use_lma, // false, or there will be too many gaps?
     		final double        discard_low,    // fraction of all pixels
     		final double        discard_high,   // fraction of all pixels
-//    		final double        discard_adisp,  // discard above/below this fraction of average height 
-//    		final double        discard_rdisp,  // discard above/below this fraction of average height
     		final int           stile_hstep,
     		final double        stile_radius,
     		final double        sigma,
     		final int           radius_levels,
     		final int           min_non_empty,  // minimal number of non-empty tiles for fitting planes
+    		final int           border_start_lev0,
     		final int           debugLevel) {
+    	final int border_start_lev = Math.min(border_start_lev0,radius_levels-1); 
     	final int stile_step = 2 * stile_hstep;
     	final int num_bins = 1000;
-    	final double normal_damping = 0.001; // pull to horizontal if not enough data 
+    	final double normal_damping = 0.000001; // 0.001; // pull to horizontal if not enough data 
     	final double hist_rlow =    0.5;
     	final double hist_rhigh =   2.0;
     	// allow strong/high, but not near the edges
@@ -1734,6 +1737,8 @@ public class QuadCLTCPU {
     	final double [][][] wnd= new double[radius_levels][][]; // ][irad][irad];
     	final double [] rad = new double [radius_levels];
     	final int [] irada = new int [radius_levels];
+    	
+    	final double [][] dbg2_img= (debugLevel > 0) ? new double [2][tilesX*tilesY]: null;
     	for (int nlev = 0; nlev < radius_levels; nlev++ ) {
     		if (nlev==0) {
     			rad[nlev] = stile_radius;
@@ -1779,16 +1784,23 @@ public class QuadCLTCPU {
 						int stileX = nstile % stilesX;
 						int stileY = nstile / stilesX;
 						int nlev = 0;
+						if ((stileX == 0) || (stileY==0) || (stileX == (stilesX-1)) || (stileY == (stilesY-1))) {
+							nlev = border_start_lev; // start from the second level
+						}
 						int num_non_empty;							// fill data arrays and the histogram
 						int tileXC = stile_hstep + stile_step * stileX; // center tiles
 						int tileYC = stile_hstep + stile_step * stileY;
 						int ntileC = tileXC + tilesX * tileYC; 
-						
+				    	int numgood = 0;
+						int irad=0;
+						int idia=0;
+						double [] stile_d=null;
+						double [] stile_w=null;
 						for (; nlev < radius_levels; nlev++) {
-							int irad = irada[nlev];
-							int idia = 2 * irad - 1;
-							double [] stile_d = new double [idia*idia];
-							double [] stile_w = new double [idia*idia];
+							irad = irada[nlev];
+							idia = 2 * irad - 1;
+							stile_d = new double [idia*idia];
+							stile_w = new double [idia*idia];
 //							Arrays.fill(stile_w,  0.0); // not all elements may be filled
 							Arrays.fill(hist,     0.0);
 							double sw=0; // , swd=0;
@@ -1842,7 +1854,7 @@ public class QuadCLTCPU {
 					    	}
 					    	shp = sh- hist[indx];
 					    	double thr_high = hist_low+ (indx + (sh - dh)/(sh-shp))/num_bins *(hist_high-hist_low);
-					    	int numgood = 0;
+					    	numgood = 0;
 //					    	boolean [] good = new boolean[ds[0].length];
 					    	sw = 0.0;
 //					    	swd = 0.0;
@@ -1860,40 +1872,45 @@ public class QuadCLTCPU {
 								}
 							}
 					    	
-					    	if (numgood < min_non_empty) {
-								continue; // go to the next radius level
+					    	if (numgood >= min_non_empty) {
+								break; // go to the next radius level
 					    	}
-					    	// fit plane
-					    	double [][][] mdata = new double [numgood][][];
-					    	int mindx = 0;
-							for (int i = 0; i < idia; i++) {
-								int dy = i-irad+1;
-								for (int j = 0; j < idia; j++) {
-									int dx = j-irad+1;
-									int ltile = i * idia + j;
-									if (stile_w[ltile] > 0) {
-							    		mdata[mindx] = new double[3][];
-							    		mdata[mindx][0] = new double [2];
-							    		mdata[mindx][0][0] = dx;
-							    		mdata[mindx][0][1] = dy;
-							    		mdata[mindx][1] = new double [1];
-							    		mdata[mindx][1][0] =  stile_d[ltile]; 
-							    		mdata[mindx][2] = new double [1];
-							    		mdata[mindx][2][0] =  stile_w[ltile];
-										mindx++;
-									}
+						}
+						if (dbg2_img != null) {
+							dbg2_img[0][ntileC] = nlev;
+							dbg2_img[1][ntileC] = numgood;
+						}
+						// fit plane
+						double [][][] mdata = new double [numgood][][];
+						int mindx = 0;
+						for (int i = 0; i < idia; i++) {
+							int dy = i-irad+1;
+							for (int j = 0; j < idia; j++) {
+								int dx = j-irad+1;
+								int ltile = i * idia + j;
+								if (stile_w[ltile] > 0) {
+									mdata[mindx] = new double[3][];
+									mdata[mindx][0] = new double [2];
+									mdata[mindx][0][0] = dx;
+									mdata[mindx][0][1] = dy;
+									mdata[mindx][1] = new double [1];
+									mdata[mindx][1][0] =  stile_d[ltile]; 
+									mdata[mindx][2] = new double [1];
+									mdata[mindx][2][0] =  stile_w[ltile];
+									mindx++;
 								}
 							}
-							
-					    	double[][] approx2d = pa.quadraticApproximation(
-					    			mdata,
-					    			true,       // boolean forceLinear,  // use linear approximation
-					    			damping,    // double [] damping, null OK
-					    			-1);        // debug level
-					    	stiles_coeff[nstile][0] = approx2d[0][2]; //  offset
-					    	stiles_coeff[nstile][1] = approx2d[0][0]; //  tiltX
-					    	stiles_coeff[nstile][2] = approx2d[0][1]; //  tiltY
-						}						
+						}
+
+						double[][] approx2d = pa.quadraticApproximation(
+								mdata,
+								true,       // boolean forceLinear,  // use linear approximation
+								damping,    // double [] damping, null OK
+								-1);        // debug level
+						stiles_coeff[nstile][0] = approx2d[0][2]; //  offset
+						stiles_coeff[nstile][1] = approx2d[0][0]; //  tiltX
+						stiles_coeff[nstile][2] = approx2d[0][1]; //  tiltY
+
 					}
 				}
 			};
@@ -1929,12 +1946,14 @@ public class QuadCLTCPU {
 		ImageDtt.startAndJoin(threads);
 		// Maybe use Gaussian? to additionally smooth "seams"
 		double [][] dbg_img = null;
-    	if (debugLevel > 0 ) {
-    		dbg_img = new double[5][];
+    	if (dbg2_img !=null ) {
+    		dbg_img = new double[7][];
     		dbg_img[0] = dls[2]; // strt
     		dbg_img[1] = dls[0]; // disp
     		dbg_img[2] = dls[1]; // lma
     		dbg_img[3] = smooth_disparity.clone();
+    		dbg_img[5] = dbg2_img[0]; // nlev
+    		dbg_img[6] = dbg2_img[1]; // numgood
     	}
 		
     	if (sigma > 0) {
@@ -1942,13 +1961,13 @@ public class QuadCLTCPU {
     	}
     	if (dbg_img != null) {
     		dbg_img[4] = smooth_disparity;
-    		String [] dbg_titles = {"str", "disp", "lma", "smooth", "blur"};
+    		String [] dbg_titles = {"str", "disp", "lma", "smooth", "blur", "nlev","numgood"};
     		ShowDoubleFloatArrays.showArrays(
     				dbg_img,
     				tp.getTilesX(),
     				tp.getTilesY(),
     				true,
-    				getImageName()+"-smooth-ground.tiff ",
+    				getImageName()+"-smooth-ground.tiff",
     				dbg_titles); 
     	}