Implemented configurable parameters for ground plane rendering. Added

disabling ERS in the inter-scene LMA if there is not enough data (not in
all 4 quadrants)

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

@@ -181,8 +181,12 @@ public class ErsCorrection extends GeometryCorrection {
 	static final int DP_DSX =  24; // dw_dx,   (m)
 	static final int DP_DSY =  25; // dw_dy,   (m)
 	static final int DP_DSZ =  26; // dw_dz};  (m)
-	static final int DP_NUM_PARS = DP_DSZ+1; 
-	
+	static final int DP_NUM_PARS = DP_DSZ+1;
+	static final int [] DP_ERS_INDICES=
+		{       DP_DVAZ,  DP_DVTL,  DP_DVRL,
+				DP_DVX,   DP_DVY,   DP_DVZ,
+				DP_DSVAZ, DP_DSVTL, DP_DSVRL,
+				DP_DSVX,  DP_DSVY,  DP_DSVZ};
 	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
 	static final double THRESHOLD = 1E-10;
 	static final double LINE_ERR = 0.001; // line accuracy for ERS when converting from world to pixels.

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

@@ -33,6 +33,13 @@ public class IntersceneLmaParameters {
 	public double     ilma_disparity_weight =     2.0;    // disparity component weight compared to dX and dY
 	public boolean    ilma_3d_lma =               false;  // Use LMA for disparity in 3D pose matching mode
 	public boolean    ilma_3d_tilt_only =         true;   // remove disparity average, use tilts only
+	public double     ilma_per_scene =            1.0;    // (Not yet used) disable ers terms if per-scene pixel offset is smaller
+	
+	public boolean    ilma_ers_quads =            true;   // Disable ERS fitting if not enough data 
+	public double     ilma_gap_frac =             0.25;   // remove this center fraction of width/height when evaluating quads 
+	public int        ilma_min_quad_tiles =      10;      // minimal number of defined tiles in each quad for using ERS
+	public double     ilma_min_quad_weight =      0.01;   // minimal fraction of total tiles weight in each quad for using ERS
+	
 	public boolean    ilma_thread_invariant =     true; // Do not use DoubleAdder, provide results not dependent on threads
 	public boolean [] ilma_lma_select =             new boolean [ErsCorrection.DP_NUM_PARS]; // first three will not be used
 	public double  [] ilma_regularization_weights = new double  [ErsCorrection.DP_NUM_PARS]; // first three will not be used
@@ -116,13 +123,27 @@ public class IntersceneLmaParameters {
 		gd.addMessage("Interframe LMA parameters selection");
 	    gd.addCheckbox    ("3D mode (use disparity)",                     this.ilma_3d,
 	    		"Use disparity for interscene matching (as for UAS imagery)" );
-		gd.addNumericField("Disparity weight",                           this.ilma_disparity_weight, 6,8,"",
+		gd.addNumericField("Disparity weight",                            this.ilma_disparity_weight, 6,8,"",
 				"Disparity component weight compared to dX and dY");
 	    gd.addCheckbox    ("LMA in 3D mode",                              this.ilma_3d_lma,
 	    		"Use LMA disparity  for interscene matching (as for UAS imagery)" );
 	    gd.addCheckbox    ("Remove average disparity, use tilts only",    this.ilma_3d_tilt_only,
 	    		"Calculate disparity for tilts only." );
-	    
+		gd.addNumericField("Per-scene offest or ERS",                     this.ilma_per_scene, 6,8,"pix",
+				"Disable ERS if per-scene pixel difference is lower  (unused).");
+		
+		
+		gd.addMessage("Disable ERS fitting with LMA when data is insufficient");
+	    gd.addCheckbox    ("Enable ERS LMA filtering",                    this.ilma_ers_quads,
+	    		"Disable ERS fitting if not enough data." );
+		gd.addNumericField("Center inactive cross relative size",         this.ilma_gap_frac, 6,8,"",
+				"Remove this center fraction of width/height when evaluating quads.");
+		gd.addNumericField("Minimal tiles in each quad",                  this.ilma_min_quad_tiles, 0,3,"",
+				"Minimal number of defined tiles in each quad for using ERS.");
+		gd.addNumericField("Minimal relative weight of each quad",        this.ilma_min_quad_weight, 6,8,"",
+				"Minimal fraction of total tiles weight in each quad for using ERS.");
+		gd.addMessage("---");
+		
 	    gd.addCheckbox    ("Thread-invariant execution",                  this.ilma_thread_invariant,
 	    		"Do not use DoubleAdder and provide results not dependent on threads" );
 		for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
@@ -192,6 +213,13 @@ public class IntersceneLmaParameters {
 		this.ilma_disparity_weight =              gd.getNextNumber();
 		this.ilma_3d_lma =                        gd.getNextBoolean();
 		this.ilma_3d_tilt_only =                  gd.getNextBoolean();
+		this.ilma_per_scene =                     gd.getNextNumber();
+		
+		this.ilma_ers_quads =                     gd.getNextBoolean();
+		this.ilma_gap_frac =                      gd.getNextNumber();
+		this.ilma_min_quad_tiles =          (int) gd.getNextNumber();
+		this.ilma_min_quad_weight =               gd.getNextNumber();
+	
 		this.ilma_thread_invariant =              gd.getNextBoolean();
 		for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
 		    this.ilma_lma_select[i] =             gd.getNextBoolean();
@@ -227,7 +255,14 @@ public class IntersceneLmaParameters {
 		properties.setProperty(prefix+"ilma_3d",                this.ilma_3d+"");	
 		properties.setProperty(prefix+"ilma_disparity_weight",  this.ilma_disparity_weight+"");	
 		properties.setProperty(prefix+"ilma_3d_lma",            this.ilma_3d_lma+"");	
-		properties.setProperty(prefix+"ilma_3d_tilt_only",      this.ilma_3d_tilt_only+"");	
+		properties.setProperty(prefix+"ilma_3d_tilt_only",      this.ilma_3d_tilt_only+"");
+		properties.setProperty(prefix+"ilma_per_scene",         this.ilma_per_scene+"");
+
+		properties.setProperty(prefix+"ilma_ers_quads",         this.ilma_ers_quads+ "");         // boolean		
+		properties.setProperty(prefix+"ilma_gap_frac",          this.ilma_gap_frac+"");           // double		
+		properties.setProperty(prefix+"ilma_min_quad_tiles",    this.ilma_min_quad_tiles+"");     // int		
+		properties.setProperty(prefix+"ilma_min_quad_weight",   this.ilma_min_quad_weight+"");    // double		
+		
 		properties.setProperty(prefix+"ilma_thread_invariant",  this.ilma_thread_invariant+"");	
 		for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
 			properties.setProperty(prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_sel",           this.ilma_lma_select[i]+"");	
@@ -259,6 +294,13 @@ public class IntersceneLmaParameters {
 		if (properties.getProperty(prefix+"ilma_disparity_weight")!=null)   this.ilma_disparity_weight=Double.parseDouble(properties.getProperty(prefix+"ilma_disparity_weight"));
 		if (properties.getProperty(prefix+"ilma_3d_lma")!=null)             this.ilma_3d_lma=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_3d_lma"));
 		if (properties.getProperty(prefix+"ilma_3d_tilt_only")!=null)       this.ilma_3d_tilt_only=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_3d_tilt_only"));
+		if (properties.getProperty(prefix+"ilma_per_scene")!=null)          this.ilma_per_scene=Double.parseDouble(properties.getProperty(prefix+"ilma_per_scene"));
+		
+		if (properties.getProperty(prefix+"ilma_ers_quads")!=null)          this.ilma_ers_quads=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_ers_quads"));	
+		if (properties.getProperty(prefix+"ilma_gap_frac")!=null)           this.ilma_gap_frac=Double.parseDouble(properties.getProperty(prefix+"ilma_gap_frac"));
+		if (properties.getProperty(prefix+"ilma_min_quad_tiles")!=null)     this.ilma_min_quad_tiles=Integer.parseInt(properties.getProperty(prefix+"ilma_min_quad_tiles"));
+		if (properties.getProperty(prefix+"ilma_min_quad_weight")!=null)    this.ilma_min_quad_weight=Double.parseDouble(properties.getProperty(prefix+"ilma_min_quad_weight"));
+		
 		if (properties.getProperty(prefix+"ilma_thread_invariant")!=null)   this.ilma_thread_invariant=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_thread_invariant"));
 		for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
 			String pn_sel = prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_sel";
@@ -306,6 +348,11 @@ public class IntersceneLmaParameters {
 		ilp.ilma_disparity_weight =    this.ilma_disparity_weight;
 		ilp.ilma_3d_lma =              this.ilma_3d_lma;
 		ilp.ilma_3d_tilt_only =        this.ilma_3d_tilt_only;
+		ilp.ilma_per_scene =           this.ilma_per_scene;
+		ilp.ilma_ers_quads =           this.ilma_ers_quads;
+		ilp.ilma_gap_frac =            this.ilma_gap_frac;
+		ilp.ilma_min_quad_tiles =      this.ilma_min_quad_tiles;
+		ilp.ilma_min_quad_weight =     this.ilma_min_quad_weight;		
 		ilp.ilma_thread_invariant =    this.ilma_thread_invariant;
 		System.arraycopy(this.ilma_lma_select,             0, ilp.ilma_lma_select,              0, ilma_lma_select.length);
 		System.arraycopy(this.ilma_regularization_weights, 0, ilp.ilma_regularization_weights,  0, ilma_regularization_weights.length);

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

@@ -5875,36 +5875,6 @@ public class OpticalFlow {
 		int [] whs = new int[3];
 		double [] x0y0 = new double[2];		
 		if (export3d) { //combo_dsn_final had strength 1.0e-4 where it should not? Reset it?
-			/*
-    		boolean   use_lma =         true;  // ;
-    		double    discard_low =     0.01;  // fraction of all pixels
-    		double    discard_high =    0.5;   // fraction of all pixels
-    		double    discard_adisp =   0.2;   // discard above/below this fraction of average height 
-    		double    discard_rdisp =   0.02;  // discard above/below this fraction of average height
-    		double    pix_size =        0.005; // hdr_x0y0,       // in meters
-    		int       max_image_width = 3200;  // increase pixel size as a power of 2 until image fits
-//    		boolean   crop_empty =      true;
-//    		int       crop_extra =      20;
-//    		int [] tex_pals = {0,1,2};
-
-//    		int tex_palette_start = 0; //
-//    		int tex_palette_end =  12; 
-			int []    hdr_whs = new int[3];
-			double [] hdr_x0y0 = new double[2];		
-			double [][] to_ground_xyxatr = quadCLTs[ref_index].getGround(
-					use_lma,         // boolean use_lma,
-					clt_parameters.imp.range_disparity_offset,// double  range_disparity_offset
-					discard_low,     // double discard_low,    // fraction of all pixels
-					discard_high,    // double discard_high,   // fraction of all pixels
-					discard_adisp,   // double discard_adisp,  // discard above/below this fraction of average height 
-					discard_rdisp,   // double discard_rdisp   // discard above/below this fraction of average height
-					pix_size,        // double pix_size,       // in meters
-					max_image_width, // int    max_image_width // increase pixel size as a power of 2 until image fits
-		    		hdr_x0y0,        // double [] x0y0,        // initialize to double[2] to return width, height
-		    		hdr_whs,         // int [] whs,            // initialize to int[3] to return {width, height, scale reduction}
-		    		debugLevel);     // int debug_level
-			
-		    */	
 			
 			boolean ok_3d = TexturedModel.output3d( // quadCLTs have same image name, and everything else
 					clt_parameters,      // CLTParameters                            clt_parameters,
@@ -5923,10 +5893,6 @@ public class OpticalFlow {
 		
 		if (export_images) {
 			if (combo_dsn_final == null) {
-///				combo_dsn_final = quadCLTs[ref_index].readDoubleArrayFromModelDirectory(
-///						"-INTER-INTRA-LMA", // String      suffix,
-///						0, // int         num_slices, // (0 - all)
-///						null); // int []      wh);
 				combo_dsn_final =quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
 				
 			}
@@ -13416,21 +13382,72 @@ public class OpticalFlow {
 					mvTitles);
 		}
 		if (debug_level > 0){
-			int num_defined = 0;
-			double sum_strength = 0.0;
-			for (int i = 0; i < coord_motion[1].length; i++) if (coord_motion[1][i] != null){
-				sum_strength += coord_motion[1][i][2];
-				num_defined++;
-			}
-			System.out.println ("interCorrPair(): num_defined = "+num_defined+
-						", sum_strength = "+sum_strength+", avg_strength = "+(sum_strength/num_defined));
+			double  gap_frac = 0.25; 
+			double [][] quad_strengths = getQuadStrengths(
+					coord_motion,    // double [][][] coord_motion,
+					gap_frac,        //double           gap_frac, // 0.25 
+					tilesX);         // int           tilesX);
+			
+//			int num_defined = 0;
+//			double sum_strength = 0.0;
+			System.out.println ("interCorrPair(): quad_defined = ["+quad_strengths[0][0]+","
+					+quad_strengths[0][1]+","+quad_strengths[0][2]+","+quad_strengths[0][3]+"]"
+					+"), rel_strengths = ["
+					+quad_strengths[1][0]+","+quad_strengths[1][1]+","
+					+quad_strengths[1][2]+","+quad_strengths[1][3]+"] ");
 		}
 		if (fail_reason != null) {
 			fail_reason[0]= 0;
 		}
 		return coord_motion; // here non-null
 	}
-
+    /**
+     * Calculates data availability in 4 corners (excluding center gap)
+     * To disable ERS calculation in LMA 
+     * @param coord_motion interCorrPair() output - here only defined (non-null)
+     *                     and strength are used
+     * @param gap_frac     fraction of width and height to remove in the center
+     *                     0.25 - remove from 3/8 to 5/8 "cross" in the center
+     * @param tilesX       number of tiles in a row
+     * @return             [2][4] - [0][]: number of non-null tiles in each quadrant,
+     *                     [1][] - fraction of weight of total weight (total does not exclude center)  
+     */
+	public static double [][] getQuadStrengths(
+			double [][][] coord_motion,
+			double        gap_frac, // 4 
+			int           tilesX) {
+		int tilesY = coord_motion[1].length/ tilesX;
+		int num_defined = 0;
+		double sum_strength = 0.0;
+		double [][] quad_strengths = new double[2][4];
+		int igapX = (int) (gap_frac * tilesX), igapY = (int) (gap_frac * tilesY);
+		int igapX0 = (tilesX - igapX)/2;
+		int igapX1 = igapX0 + igapX; 
+		int igapY0 = (tilesY - igapY)/2;
+		int igapY1 = igapY0 + igapY; 
+		
+		for (int i = 0; i < coord_motion[1].length; i++) if (coord_motion[1][i] != null){
+			sum_strength += coord_motion[1][i][2];
+			num_defined++;
+			int ty = i / tilesX;
+			int tx = i % tilesX;
+			if (((ty > igapY0) && (ty < igapY1)) || ((tx > igapX0) && (tx < igapX1))) {
+				continue;
+			}
+			int iy = ty/(tilesY/2);
+			int ix = tx/(tilesX/2);
+			int ixy = 2*iy+ix;
+			quad_strengths[0][ixy] += 1.0;
+			quad_strengths[1][ixy] += coord_motion[1][i][2];
+		}
+		if (sum_strength > 0) {
+			for (int i = 0; i< quad_strengths[1].length; i++) {
+//				quad_strengths[0][i]/=num_defined;
+				quad_strengths[1][i]/=sum_strength;
+			}
+		}
+		return quad_strengths;
+	}
 	
 	/**
 	 * Equalize weights of the motion vectors to boost that of important buy weak one.
@@ -15059,6 +15076,11 @@ public class OpticalFlow {
 			int            debug_level)
 	{
 		boolean use3D = clt_parameters.ilp.ilma_3d;
+		boolean filter_by_ers =   clt_parameters.ilp.ilma_ers_quads;
+		double  gap_frac =        clt_parameters.ilp.ilma_gap_frac ; // 0.25;
+		int     min_quad_tiles =  clt_parameters.ilp.ilma_min_quad_tiles ; // 10;
+		double  min_quad_weight = clt_parameters.ilp.ilma_min_quad_weight ; // 0.01;
+		
 //		boolean use3D_lma = clt_parameters.ilp.ilma_3d_lma;
 		double disparity_weight = use3D? clt_parameters.ilp.ilma_disparity_weight : 0.0;
 		int        margin = clt_parameters.imp.margin;
@@ -15107,6 +15129,45 @@ public class OpticalFlow {
 	        	System.out.println("adjustPairsLMAInterscene() returned null");
 	        	return null;
 	        }
+	        boolean ers_lma = false;
+	        for (int i:ErsCorrection.DP_ERS_INDICES) {
+	        	ers_lma |= param_select[i];
+	        }
+			boolean disable_ers=false;
+			boolean [] param_select_mod = param_select; 
+			if (ers_lma && filter_by_ers) {
+				double [][] quad_strengths = getQuadStrengths(
+						coord_motion,                                      // double [][][] coord_motion,
+						gap_frac,                                          //double           gap_frac, // 0.25 
+						reference_QuadClt.getTileProcessor().getTilesX()); // int           tilesX);
+				for (int i = 0; i < quad_strengths[0].length; i++) {
+					if ((quad_strengths[0][i] < min_quad_tiles) || (quad_strengths[1][i] < min_quad_weight)) {
+						disable_ers = true;
+						break;
+					}
+				}
+				if (disable_ers) {
+					param_select_mod = param_select.clone();
+					for (int i:ErsCorrection.DP_ERS_INDICES) {
+						param_select_mod[i] = false;
+					}
+					// now just copy ers from the reference 
+					ErsCorrection ers_ref =   reference_QuadClt.getErsCorrection();
+					ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
+					ers_scene.ers_watr_center_dt = ers_ref.ers_watr_center_dt.clone();
+					ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
+					if (clt_parameters.imp.debug_level > -2) {
+						System.out.print("adjustPairsLMAInterscene(): insufficeinet data for ERS, skipping. ");
+						System.out.println ("quad_defined = ["+((int)quad_strengths[0][0])+","
+								+((int)quad_strengths[0][1])+","+((int)quad_strengths[0][2])+","+((int)quad_strengths[0][3])+"] (needed = "+
+								min_quad_tiles +"), rel_strengths = ["
+								+quad_strengths[1][0]+","+quad_strengths[1][1]+","
+								+quad_strengths[1][2]+","+quad_strengths[1][3]+"] (needed "
+								+min_quad_weight+")");
+					}
+				}
+			}
+	        
 			intersceneLma.prepareLMA(
 					camera_xyz0,         // final double []   scene_xyz0,     // camera center in world coordinates (or null to use instance)
 					camera_atr0,         // final double []   scene_atr0,     // camera orientation relative to world frame (or null to use instance)
@@ -15115,7 +15176,7 @@ public class OpticalFlow {
 					// reference atr, xyz are considered 0.0
 					scene_QuadClt,       // final QuadCLT     scene_QuadClt,
 					reference_QuadClt,   // final QuadCLT     reference_QuadClt,
-					param_select,        // final boolean[]   param_select,
+					param_select_mod, // param_select,        // final boolean[]   param_select,
 					param_regweights,    // final double []   param_regweights,
 					coord_motion[1],     // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
 					coord_motion[0],     // final double [][] centers,    // macrotile centers (in pixels and average disparities

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

@@ -2451,7 +2451,20 @@ public class TexturedModel {
 			final boolean                            updateStatus,
 			final int                                debugLevel)
 	{
-		final boolean render_hdr =        true; //false; // true; // generate textures w/o normalization to generate undistorted
+		final boolean map_en =            clt_parameters.gmap_en;
+		final boolean render_hdr =        clt_parameters.gmap_render_hdr || map_en;// true; //false; // true; // generate textures w/o normalization to generate undistorted
+		
+		final boolean   use_lma =         clt_parameters.gmap_use_lma ; // true;  // ;
+		final double    discard_low =     clt_parameters.gmap_discard_low ; //0.01;  // fraction of all pixels
+		final double    discard_high =    clt_parameters.gmap_discard_high ; //0.5;   // fraction of all pixels
+		final double    discard_adisp =   clt_parameters.gmap_discard_adisp ; //0.2;   // discard above/below this fraction of average height 
+		final double    discard_rdisp =   clt_parameters.gmap_discard_rdisp ; //0.02;  // discard above/below this fraction of average height
+		final double    pix_size =        clt_parameters.gmap_pix_size ; //0.005; // hdr_x0y0,       // in meters
+		final int       max_image_width = clt_parameters.gmap_max_image_width ; //4000; // 3200;  // increase pixel size as a power of 2 until image fits
+		final boolean   crop_empty =      clt_parameters.gmap_crop_empty ; //true;
+		final int       crop_extra =      clt_parameters.gmap_crop_extra ; //20;
+		final int []    tex_pals =        clt_parameters.gmap_tex_pals ; //{0,1,2};
+		
 		final double  range_disparity_offset = clt_parameters.imp.range_disparity_offset;// double  range_disparity_offset
 		final boolean batch_mode =        clt_parameters.multiseq_run; // batch_run;
 		final boolean gltf_emissive =     clt_parameters.gltf_emissive;
@@ -2984,21 +2997,6 @@ public class TexturedModel {
 			// if (imp_textures[nslice] != null)
 		} // for (int nslice = 0; nslice < tileClusters.length; nslice++){
 		if (render_hdr) {
-    		boolean   use_lma =         true;  // ;
-    		double    discard_low =     0.01;  // fraction of all pixels
-    		double    discard_high =    0.5;   // fraction of all pixels
-    		double    discard_adisp =   0.2;   // discard above/below this fraction of average height 
-    		double    discard_rdisp =   0.02;  // discard above/below this fraction of average height
-    		double    pix_size =        0.005; // hdr_x0y0,       // in meters
-    		int       max_image_width = 4000; // 3200;  // increase pixel size as a power of 2 until image fits
-    		boolean   crop_empty =      true;
-    		int       crop_extra =      20;
-    		int [] tex_pals = {0,1,2};
-//    		boolean use_fixed_range =  true;
-//    		double  fixed_range =      250.0;
-
-    		int tex_palette_start = 0; 
-    		int tex_palette_end =  12; 
 			int []    hdr_whs = new int[3];
 			double [] hdr_x0y0 = new double[2];		
 			double [][] to_ground_xyxatr = scenes[ref_index].getGround(

src/main/java/com/elphel/imagej/x3d/export/Render3D.java

@@ -54,24 +54,18 @@ public class Render3D {
 
 	
 	public Render3D (
-//			String       x3d_dir,
-//			String       model_name,
 			QuadCLT      ref_scene,    // all coordinates relative to this scene
 			double [][]  toground, // projection plane center relative to reference scene
 			double       pixel_size,   // in meters
 			double []    x0_y0,        // usually negative - top-left point of the output render
 			int          out_width,    // output rendered image width in pixels
 			int          out_height){   // output rendered image height in pixels
-//		this.x3d_dir = x3d_dir;
-//		this.model_name = model_name;
 		this.ref_scene =  ref_scene;
 		this.pixel_per_m = 1.0 / pixel_size; 
 		this.out_width =  out_width; 
 		this.out_height = out_height; 
 		this.toground = toground;
 		this.tocam = ErsCorrection.invertXYZATR(this.toground);  // null
-//		double [][] test1 = ErsCorrection.invertXYZATR(this.tocam); //OK
-//		double [][] test2 = ErsCorrection.invertXYZATR(test1); // OK
 		// ground plane x0, y0 in camera coordinates
 		ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0],      x0_y0[1],     0.0}));
 		Vector3D v3x1 = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0] + 1.0,x0_y0[1],     0.0}));
@@ -84,7 +78,6 @@ public class Render3D {
 		ground_normal_unit = ground_normal.normalize(); // unitary vector normal and away from the ground plane
 		above_ground = ground_normal.getNorm();
 		xy_offs = new double[] {ground_x.dotProduct(ground_origin), ground_y.dotProduct(ground_origin)};
-//		double [] test_gp=projectToPlanePixels(ground_origin);
 	}
 	
 	public double [] projectToPlaneLinear(Vector3D v3) { // get ground place pixel coordinate from camera x,y,z