fixing ground planes

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

@@ -546,27 +546,6 @@ public class GroundPlane {
      	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
      	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
      	
-     	//??? seems opposite rotation
-     	virtual_camera_from_camera[1] = to_ground_xyzatr[1]; 
-     	
-     	/*
-     	double [][] to_ground_xyz = {to_ground_xyzatr[0],new double[3]};
-     	double [][] to_ground_atr = {new double[3], to_ground_xyzatr[1]};
-     	double [][] from_ground_xyz = ErsCorrection.invertXYZATR(to_ground_xyz);
-     	// calculate rotated around ground relative to the reference frame (see if z-sign is correct, or swap to/from
-		//combineXYZATR
-     	double [][] to_ground_xyzatr1 = ErsCorrection.combineXYZATR(to_ground_xyz,  ErsCorrection.invertXYZATR(to_ground_atr)); 
-///     	double [][] to_ground_xyzatr1 = ErsCorrection.combineXYZATR(to_ground_atr,to_ground_xyz); 
-     	double [][] to_ground_xyzatr_centered = ErsCorrection.combineXYZATR(to_ground_xyzatr1, from_ground_xyz);
-///     	double [][] to_ground_xyzatr_centered = ErsCorrection.combineXYZATR(from_ground_xyz, to_ground_xyzatr1);
-     	double [][] to_ground_xyzatr_centered1 = ErsCorrection.combineXYZATR(from_ground_xyz,
-     			ErsCorrection.combineXYZATR(to_ground_atr, to_ground_xyz));
-     	
-     	to_ground_xyzatr_centered = new double [][] {
-     		{-to_ground_xyzatr_centered[0][0],-to_ground_xyzatr_centered[0][1],-to_ground_xyzatr_centered[0][2]},
-     		to_ground_xyzatr_centered[1]
-     	};
-     	*/
      	if ((dbg_title != null) || (gnd_disp != null)) {
      		// expand macrotiles results to match tiles
      		// rotate ref_disparity and show
@@ -588,7 +567,6 @@ public class GroundPlane {
          		int tilesY = wxyz.length/width;
         		int mtile_half = mtile_size/2;
         		int mtilesX = (tilesX - mtile_size)/mtile_half + 1;
-//        		int mtilesY = (tilesY - mtile_size)/mtile_half + 1;
         		double [][] macro_exp =  new double [tilesX * tilesY][];
         		double [][] macro_wexp = new double [tilesX * tilesY][];
         		for (int mtile = 0; mtile < macroplanes.length; mtile++){
@@ -691,16 +669,10 @@ public class GroundPlane {
      	if (good_tiles != null) {
      		System.arraycopy(macro_mask, 0, good_tiles, 0, macro_mask.length);
      	}
-     	
-     	
 		ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered);
-///		ref_Clt.getErsCorrection().printVectors    (to_ground_xyzatr_centered[0], to_ground_xyzatr_centered[1]); 		
 		ref_Clt.getErsCorrection().printVectors    (virtual_camera_from_camera[0], virtual_camera_from_camera[1]); 		
      	//num_good2
-///		return new double[][] {to_ground_xyzatr_centered[0],to_ground_xyzatr_centered[1], {1.0*num_good2/ref_disparity.length}};
-//		return new double[][] {virtual_camera_from_camera[0],virtual_camera_from_camera[1], {1.0*num_good2/ref_disparity.length}};
 		return new double[][] {to_ground_xyzatr[0], to_ground_xyzatr[1], {1.0*num_good2/ref_disparity.length}};
-//		return to_ground_xyzatr_centered;
 	}
 	
 	public static boolean [] getMaskFromMacro(
@@ -1141,7 +1113,7 @@ public class GroundPlane {
 			int [] xy0 = {tilesX/2, tilesY/2};
 			double inf_disparity = ref_Clt.getDispInfinityRef();
 			double [] disparity_tilts = gp.getDisparityTilts();
-			if ((test_bottom >0) && (test_bottom < 1)) {
+			if ((test_bottom > 0) && (test_bottom < 1)) {
 				int y1 = (int) ((terrain_plane.length/tilesX) * test_bottom);
 				if (disparity_tilts != null) {
 					for (int i = 0; i < terrain_plane.length; i++) {
@@ -1174,13 +1146,7 @@ public class GroundPlane {
 				to_ground_xyzatr);  // double [][] to_ground_xyzatr
 		
 		double [][] stereo_xyzatr = ErsCorrection.invertXYZATR(virt_camera);
-//		double [][] center_rot_xyzatr = {new double [3],stereo_xyzatr[1]};
-		/*
-		double [] terrain_plane = rotateRefDisparity(
-				ref_Clt,            // final QuadCLT     ref_Clt,
-				terrain_plane,      // double []         terrain,
-				stereo_xyzatr);     // virt_camera);       // final double [][] scene_xyzatr)
-		*/
+
 		ref_Clt.setTerrain(
 				clt_parameters,  // CLTParameters clt_parameters,
 				terrain_plane,   // double []     terrain,
@@ -1194,20 +1160,9 @@ public class GroundPlane {
 	    double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
      	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
      	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
-///    	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(virtual_camera_from_ground_xyz, ground_xyzatr);
-//     	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
-      	virtual_camera_from_camera[1] = to_ground_xyzatr[1];
+      	virtual_camera_from_camera[1] = to_ground_xyzatr[1]; // *******????????
      	return virtual_camera_from_camera;
 	}
-	/*
-	    double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
-     	double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
-     	double [][] virtual_camera_from_camera =     ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
-     	
-     	//??? seems opposite rotation
-     	virtual_camera_from_camera[1] = to_ground_xyzatr[1]; 
- 
-	 */
 
 	public static double [] getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good
 			final CLTParameters clt_parameters,
@@ -1669,7 +1624,24 @@ public class GroundPlane {
 		if (debugLevel > -3) {
 			System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]);
 		}
-     	double [][] ground_xyzatr =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
+///     	double [][] ground_xyzatr =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
+///     	double [][] ground_xyzatr00 =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{-Math.asin(z_tilts[1]), Math.asin(z_tilts[2]), 0.0}};
+     	// or opposite order?
+     	boolean invert_order = false;
+		if (debugLevel > -3) {
+			System.out.println("Using "+(invert_order?"inverted":"direct")+" azimuth/tilt order when convertingtiltX/tiltY to angles.");
+		}
+     	
+     	double tl = 0, az=0;
+     	if (invert_order) {
+     		tl = -Math.atan(-z_tilts[2]/Math.sqrt(1+z_tilts[1]*z_tilts[1]));
+     		az = -Math.atan(z_tilts[1]);
+     	} else {
+     		az = Math.atan(-z_tilts[1]/Math.sqrt(1+z_tilts[2]*z_tilts[2]));
+     		tl = Math.atan(z_tilts[2]);
+     	}
+ 		double [][] ground_xyzatr =  new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{az, tl, 0.0}};
+     	
      	// It is approximate for small angles. OK for now    		
      	double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(ground_xyzatr);
      	return to_ground_xyzatr; // from the camera coordinates to in-plane coordinates