Fixed display of the ground planes

src/main/java/com/elphel/imagej/cuas/Cuas.java

@@ -1311,6 +1311,69 @@ public class Cuas {
         return ref_pXpYD;
     }
 
+    /**
+     * Re-sample disparity map to match uniform grid on the virtual view, such as
+     * the view orthogonal to the terrain tangent/average surface 
+     * @param clt_parameters configuration parameters
+     * @param disparity_ref known disparity for the camera reference frame
+     * @param scene_xyzatr virtual view center position and orientation relative to the reference frame
+     * @param reference_QuadClt scene instance for the reference frame
+     * @param around number of tiles to look around when interpolating using average planes
+     * @param sigma reduce weight of far samples e-times at this distance
+     * @param num_refines number of refine passes
+     * @param debugSuffix image name suffix to generate debug images
+     * @return array of per-tile disparity (NaN for undefined tiles) 
+     */
+    public static double [] getDisparityVirtual(
+    		CLTParameters     clt_parameters,
+		double []         disparity_ref,
+		final double [][] scene_xyzatr,      // camera center and orientation in world (reference frame) coordinates
+		final QuadCLT     reference_QuadClt,
+		final int         around,            // 2 search around for interpolation
+		final double      sigma,
+		final int         num_refines,
+		final String      debugSuffix){
+			double [][] reference_pXpYD = 
+					transformFromVirtual(
+							clt_parameters, // CLTParameters     clt_parameters,
+							disparity_ref,  // double []         disparity_ref,
+							scene_xyzatr[0],      // final double []   scene_xyz,         // camera center in world (reference) coordinates
+							scene_xyzatr[1],  // final double []   scene_atr,         // camera orientation relative to world (reference) frame
+							reference_QuadClt, // final QuadCLT     reference_QuadClt,
+							around,   // final int         around,            // 2 search around for interpolation
+							 sigma, // final double      sigma,
+							 num_refines,   // final int         num_refines,
+							 debugSuffix); // final String      debugSuffix)
+			// get scene pXpYD corresponding to reference_pXpYD
+			double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
+					reference_pXpYD,    // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
+					scene_xyzatr[0],    // final double []   scene_xyz,         // camera center in world (reference) coordinates
+					scene_xyzatr[1],    // final double []   scene_atr,         // camera orientation relative to world (reference) frame
+					reference_QuadClt,  // final QuadCLT     reference_QuadClt) //
+					null);              // final QuadCLT     scene_QuadClt) //
+			double [] scene_disparity = new double [scene_pXpYD.length];
+			Arrays.fill(scene_disparity, Double.NaN);
+			for (int ntile = 0; ntile < scene_disparity.length; ntile++) if (scene_pXpYD[ntile] != null) {
+				scene_disparity[ntile] = scene_pXpYD[ntile][2];
+			}
+			return scene_disparity; 
+	
+    }
+    
+    /**
+     * Re-sample disparity map to match uniform grid on the virtual view, producing a non-uniform
+     * coordinates+disparity in the reference scene that corresponds to the uniform grid in the virtual view.
+     * @param clt_parameters configuration parameters
+     * @param disparity_ref known disparity for the camera reference frame
+     * @param scene_xyzatr virtual view center position and orientation relative to the reference frame
+     * @param reference_QuadClt scene instance for the reference frame
+     * @param around number of tiles to look around when interpolating using average planes
+     * @param sigma reduce weight of far samples e-times at this distance
+     * @param num_refines number of refine passes
+     * @param debugSuffix image name suffix to generate debug images
+     * @return array of non-uniform grid triplets of pixel X, pixel Y and disparity for the reference scene
+     *         corresponding to the uniform grid in a virtual camera view 
+     */
 
 	public static double [][] transformFromVirtual(
 			CLTParameters     clt_parameters,
@@ -1322,7 +1385,7 @@ public class Cuas {
 			final double      sigma,
 			final int         num_refines,
 			final String      debugSuffix){
-		final boolean debug = (debugSuffix != null)  && !clt_parameters.batch_run;
+		final boolean debug = (debugSuffix != null); //   && !clt_parameters.batch_run;
 		final double sigma2 = 2*sigma*sigma;
     	final double normal_damping = 0.001; // pull to horizontal if not enough data 
     	final double [] damping = new double [] {normal_damping, normal_damping};
@@ -1363,9 +1426,6 @@ public class Cuas {
 			System.arraycopy(disparity_ref1, 0, disparity_ref, 0, disparity_ref.length);
 		}
 		
-		
-		
-		
 		final String debugTitle=debug?reference_QuadClt.getImageName()+"-"+debugSuffix : null;
 		String [] debug_frame_titles = {"X", "Y", "D"};
 		String [] debug_titles = new String[num_refines+1];
@@ -1385,10 +1445,16 @@ public class Cuas {
 		for (int nTile = 0; nTile < tiles_around; nTile++) {
 			interp_list.add(new ArrayList<Integer>(initial_capacity));
 		}
-		// create uniform grid for initial interpolations
+		/*
+		 *  create uniform grid for initial interpolations
+		 */  
 		final double [][] reference_pXpYD = new double [tiles][];
 		for (int nrefine = 0; nrefine <= num_refines; nrefine++) {
 			if (nrefine == 0) {
+				/*
+				 * The first iteration of the reference_pXpYD is just a uniform grid with
+				 * reference disparity
+				 */
 				ai.set(0);
 				for (int ithread = 0; ithread < threads.length; ithread++) {
 					threads[ithread] = new Thread() {
@@ -1408,18 +1474,26 @@ public class Cuas {
 					};
 				}		      
 				ImageDtt.startAndJoin(threads);
-				
 		} else {
-				// get scene pXpYD corresponding to reference_pXpYD
+			/*
+			 * Next iterations create uniform grid on the virtual scene  by the bi-linear
+			 * interpolation using small patches around the grid points
+			 * Start with creatring non-iniform grid scene_pXpYD, corresponding to the
+			 * current reference_pXpYD (also non-uniform for all but the very first iteration)
+			 * 
+			 * tiles_around - larger grid    
+			 */
 			final double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
 						reference_pXpYD,    // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
 						scene_xyz,          // final double []   scene_xyz,         // camera center in world (reference) coordinates
 						scene_atr,          // final double []   scene_atr,         // camera orientation relative to world (reference) frame
 						reference_QuadClt, // final QuadCLT     reference_QuadClt) //
 						null); // final QuadCLT     scene_QuadClt) //
-
-				// deep clone reference_pXpYD
 				final double [][] reference_pXpYD_next = new double [tiles][];
+				/*
+				 * Initialize lists of the scene tiles containing converted reference tiles in the vicinity
+				 * of scene uniform grid
+				 */
 				ai.set(0);
 				for (int ithread = 0; ithread < threads.length; ithread++) {
 					threads[ithread] = new Thread() {
@@ -1431,7 +1505,11 @@ public class Cuas {
 					};
 				}		      
 				ImageDtt.startAndJoin(threads);
-				// not multithreaded 
+				/*
+				 * Populate lists in a single thread. Each reference_pXpYD tile projection scene_pXpYD
+				 * gets to some uniform-grit tile. Some of the uniform grid tiles gets multiple reference
+				 * sources, others - none.
+				 */
 				for (int nTile = 0; nTile < tiles; nTile++) if (scene_pXpYD[nTile] != null) {
 					int tileX = (int) Math.floor (scene_pXpYD[nTile][0]/transform_size);
 					int tileY = (int) Math.floor (scene_pXpYD[nTile][1]/transform_size);
@@ -1442,7 +1520,9 @@ public class Cuas {
 						interp_list.get(tile_around).add(nTile);
 					}
 				}
-				// interpolate reference_pXpYD_next 
+				/*
+				 *  interpolate reference_pXpYD_next 
+				 */
 				ai.set(0);
 				for (int ithread = 0; ithread < threads.length; ithread++) {
 					threads[ithread] = new Thread() {
@@ -1464,7 +1544,9 @@ public class Cuas {
 								}
 								int mindx = 0;
 								if (num_samples >= min_samples) {
-									// inter/extrapolate with regularization (for few samples) and weights
+									/*
+									 * inter/extrapolate with regularization (for few samples) and weights
+									 */
 									double [][][] mdata = new double [num_samples][3][];
 									for (int dy = -around; dy <= around; dy++) {
 										for (int dx = -around; dx <= around; dx++) {
@@ -1552,13 +1634,14 @@ public class Cuas {
 			}
 		}
 		if (debug) {
-			ShowDoubleFloatArrays.showArraysHyperstack(
+			ImagePlus img_debug = ShowDoubleFloatArrays.showArraysHyperstack(
 					debug_data,         // double[][][] pixels, 
 					tilesX,             // int          width, 
 					debugTitle,         // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
 					debug_titles,       // String []    titles, // all slices*frames titles or just slice titles or null
 					debug_frame_titles, // String []    frame_titles, // frame titles or null
-					true);              // boolean      show)
+					false); // true);              // boolean      show)
+			reference_QuadClt.saveImagePlusInModelDirectory(img_debug);
 
 		}		
 		

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

@@ -1080,6 +1080,11 @@ min_str_neib_fpn	0.35
 	public  double  fgnd_ct_high =        1.0;   // highest CT level relative to the ground
 	public  double  fgnd_ct_step =        0.2;   // CT step
 	
+	public  int     fgnd_virt_around =    2;     // virtual views interpolate around
+	public  double  fgnd_virt_sigma =     4.0;   // virtual views sigma
+	public  int     fgnd_virt_refine =    4;     // virtual views number of refines
+    public  boolean fgnd_debug =          true;  // generate ground planes debug images
+    public  String  fgnd_debug_prefix =   "-ground"; // ground planes debug prefix (will add -tilts, -virtual
 	
 	
 	// TODO: move next parameters elsewhere - they are not the motion blur ones.
@@ -3180,6 +3185,19 @@ min_str_neib_fpn	0.35
 		gd.addNumericField("CT scan step",        this.fgnd_ct_step, 5,7,"m",
 				"CT scan step.");
 
+		gd.addMessage("Calculate disparty for the virtual views orthogonal to the ground plane surface");
+		gd.addNumericField("Virtual views interpolate around",        this.fgnd_virt_around, 0,3,"pix",
+				"Interpolate using this number of tiles around.");
+		gd.addNumericField("Virtual views sigma",                     this.fgnd_virt_sigma, 5,7,"pix",
+				"Exponentially reduce weight of the far tiles (radius).");
+		gd.addNumericField("Virtual views number of refines",         this.fgnd_virt_refine, 0,3,"",
+				"Iterate conversion from the known reference frame this number of times.");
+		gd.addMessage("Ground planes debug images");
+		gd.addCheckbox ("Generate ground planes debug images",        this.fgnd_debug,
+				"Always adjust Z (altitude) when running interscene LMA. False - same as X and Y.");
+		gd.addStringField("Ground planes debug images prefix",        this.fgnd_debug_prefix, 40,
+				"Ground planes debug imaghes prefix (Will add -tilts, -virtual).");
+		
 		gd.addTab("LMA sequence","Interscene LMA sequence control");
 		gd.addMessage("Parameters for control of the LMA pose adjustment sequence");
 		gd.addNumericField("Pose readjust number for full mb_gain",  this.mb_gain_index_pose, 0,3,"",
@@ -4507,6 +4525,12 @@ min_str_neib_fpn	0.35
 		this.fgnd_ct_high =             gd.getNextNumber();
 		this.fgnd_ct_step =             gd.getNextNumber();
 		
+		this.fgnd_virt_around =   (int) gd.getNextNumber();
+		this.fgnd_virt_sigma =          gd.getNextNumber();
+		this.fgnd_virt_refine =   (int) gd.getNextNumber();
+		this.fgnd_debug =               gd.getNextBoolean();
+		this.fgnd_debug_prefix =        gd.getNextString();
+		
 		this.mb_gain_index_pose = (int) gd.getNextNumber();
 		this.mb_gain_index_depth =(int) gd.getNextNumber();
 		
@@ -5723,6 +5747,12 @@ min_str_neib_fpn	0.35
 		properties.setProperty(prefix+"fgnd_ct_high",         this.fgnd_ct_high+"");        // double
 		properties.setProperty(prefix+"fgnd_ct_step",         this.fgnd_ct_step+"");        // double
 
+		properties.setProperty(prefix+"fgnd_virt_around",     this.fgnd_virt_around+"");    // int
+		properties.setProperty(prefix+"fgnd_virt_sigma",      this.fgnd_virt_sigma+"");     // double
+		properties.setProperty(prefix+"fgnd_virt_refine",     this.fgnd_virt_refine+"");    // int
+		properties.setProperty(prefix+"fgnd_debug",           this.fgnd_debug+"");          // boolean
+		properties.setProperty(prefix+"fgnd_debug_prefix",    this.fgnd_debug_prefix+"");   // String
+		
 		properties.setProperty(prefix+"mb_gain_index_pose",   this.mb_gain_index_pose+"");  // int
 		properties.setProperty(prefix+"mb_gain_index_depth",  this.mb_gain_index_depth+""); // int
 
@@ -6923,6 +6953,12 @@ min_str_neib_fpn	0.35
 		if (properties.getProperty(prefix+"fgnd_ct_high")!=null)         this.fgnd_ct_high=Double.parseDouble(properties.getProperty(prefix+"fgnd_ct_high"));
 		if (properties.getProperty(prefix+"fgnd_ct_step")!=null)         this.fgnd_ct_step=Double.parseDouble(properties.getProperty(prefix+"fgnd_ct_step"));
 		
+		if (properties.getProperty(prefix+"fgnd_virt_around")!=null)     this.fgnd_virt_around=Integer.parseInt(properties.getProperty(prefix+"fgnd_virt_around"));
+		if (properties.getProperty(prefix+"fgnd_virt_sigma")!=null)      this.fgnd_virt_sigma=Double.parseDouble(properties.getProperty(prefix+"fgnd_virt_sigma"));
+		if (properties.getProperty(prefix+"fgnd_virt_refine")!=null)     this.fgnd_virt_refine=Integer.parseInt(properties.getProperty(prefix+"fgnd_virt_refine"));
+		if (properties.getProperty(prefix+"fgnd_debug")!=null)           this.fgnd_debug=Boolean.parseBoolean(properties.getProperty(prefix+"fgnd_debug"));
+		if (properties.getProperty(prefix+"fgnd_debug_prefix")!=         null) fgnd_debug_prefix=(String) properties.getProperty(prefix+"fgnd_debug_prefix");
+		
 		if (properties.getProperty(prefix+"mb_gain_index_pose")!=null)   this.mb_gain_index_pose=Integer.parseInt(properties.getProperty(prefix+"mb_gain_index_pose"));
 		if (properties.getProperty(prefix+"mb_ers_index")!=null)         this.mb_ers_index=Integer.parseInt(properties.getProperty(prefix+"mb_ers_index"));
 		if (properties.getProperty(prefix+"mb_ers_y_index")!=null)       this.mb_ers_y_index=Integer.parseInt(properties.getProperty(prefix+"mb_ers_y_index"));
@@ -8102,6 +8138,12 @@ min_str_neib_fpn	0.35
 		imp.fgnd_ct_high =     		this.fgnd_ct_high;    
 		imp.fgnd_ct_step =     		this.fgnd_ct_step; 
 		
+		imp.fgnd_virt_around =      this.fgnd_virt_around;
+		imp.fgnd_virt_sigma =       this.fgnd_virt_sigma;
+		imp.fgnd_virt_refine =      this.fgnd_virt_refine;
+		imp.fgnd_debug =            this.fgnd_debug;
+		imp.fgnd_debug_prefix =     this.fgnd_debug_prefix;
+
 		imp.mb_gain_index_pose    = this.mb_gain_index_pose;
 		imp.mb_gain_index_depth   = this.mb_gain_index_depth;
 

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

@@ -3059,8 +3059,6 @@ public class OpticalFlow {
 		return pXpYD;
 	}
 
-
-	
 	//TODO: refine inter-scene pose to accommodate refined disparity map
 	/**
 	 * Removing BG tiles that are not visible because of the FG ones