Commit 8f04633f authored by Andrey Filippov's avatar Andrey Filippov
Browse files

debugging

parent 4c3e69f5

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

+31 −61
Original line number Diff line number Diff line
@@ -858,7 +858,7 @@ public class ErsCorrection extends GeometryCorrection { 
	    int DP_DX =  12; // dw_dx,   (m)

	    int DP_DY =  13; // dw_dy,   (m)

	    int DP_DZ =  14; // dw_dz};  (m)

		double scale_delta = 0.1; // 0.5;

		double scale_delta = 1.0; // 0.1; // 0.5;

		double [] deltas = deltas0.clone();

		for (int i = 0; i < deltas.length; i++) deltas[i] *= scale_delta;

	    int dbg_tile = 16629;
@@ -919,13 +919,17 @@ public class ErsCorrection extends GeometryCorrection { 
				

				pXpYD_original[nTile] = new double[] {centerX, centerY,disparity};

				// find derivatives by pX, pY, pZ

				if (nTile == dbg_tile) {

					System.out.println("compareDSItoWorldDerivatives()_0: nTile = "+nTile+" ("+tileX+"/"+tileY+")");

				}



				double [][][] xyz_pm = {

						{	getWorldCoordinatesERS(centerX + deltas[DP_DPX], centerY, disparity, true),

							getWorldCoordinatesERS(centerX - deltas[DP_DPX], centerY, disparity, true)

						},{ getWorldCoordinatesERS(centerX, centerY + deltas[DP_DPY], disparity, true),

							getWorldCoordinatesERS(centerX,	centerY - deltas[DP_DPY], disparity, true)

						},{ getWorldCoordinatesERS(centerX, centerY, disparity + deltas[DP_DD],	 true),

							getWorldCoordinatesERS(centerX, centerY, disparity - deltas[DP_DD],	 true)}};

						{	getWorldCoordinatesERS(centerX + deltas[DP_DPX], centerY, disparity, true, camera_xyz, camera_atr),

							getWorldCoordinatesERS(centerX - deltas[DP_DPX], centerY, disparity, true, camera_xyz, camera_atr)

						},{ getWorldCoordinatesERS(centerX, centerY + deltas[DP_DPY], disparity, true, camera_xyz, camera_atr),

							getWorldCoordinatesERS(centerX,	centerY - deltas[DP_DPY], disparity, true, camera_xyz, camera_atr)

						},{ getWorldCoordinatesERS(centerX, centerY, disparity + deltas[DP_DD],	 true, camera_xyz, camera_atr),

							getWorldCoordinatesERS(centerX, centerY, disparity - deltas[DP_DD],	 true, camera_xyz, camera_atr)}};

				for (int n = 0; n < 3; n++) {

					int par_index =DP_DPX + n;

					if ((xyz_pm[n][0] != null) && (xyz_pm[n][1] != null)) {
@@ -989,7 +993,7 @@ public class ErsCorrection extends GeometryCorrection { 
					if (disparity < max_inf_disparity) { // or only for abs() ?

						disparity = 0.0;

					}

					ers_pm[n][sign01][nTile]=  getWorldCoordinatesERS(centerX, centerY, disparity, true);

					ers_pm[n][sign01][nTile]=  getWorldCoordinatesERS(centerX, centerY, disparity, true, camera_xyz, camera_atr);

				}

			}

		}
@@ -1229,7 +1233,6 @@ public class ErsCorrection extends GeometryCorrection { 
		if (Double.isNaN(pXpYD[2])) {

			return null;

		}

//		boolean is_infinity = Math.abs(pXpYD[2]) < THRESHOLD; // Maybe all negative - too?

		int line = (int) Math.round(pXpYD[1]);

		if (line < 0) {

			line = 0;
@@ -1262,12 +1265,9 @@ public class ErsCorrection extends GeometryCorrection { 
	    

	    double d_rDn_d_pX = pXcd * (r_scale * r_scale) / rDn;

		double d_rDn_d_pY = pYcd * (r_scale * r_scale) / rDn;

// 		double d_rDn_d_disparity= 0.0;

		

//		double pXc = pXcd * rND2RD;

 		double d_pXc_d_pX = rND2RD + pXcd * d_rND2rD_d_rDn * d_rDn_d_pX; 	    

 		double d_pXc_d_pY =          pXcd * d_rND2rD_d_rDn * d_rDn_d_pY; 	    

//		double pYc = pYcd * rND2RD;

 		double d_pYc_d_pX =          pYcd * d_rND2rD_d_rDn * d_rDn_d_pX; 	    

 		double d_pYc_d_pY = rND2RD + pYcd * d_rND2rD_d_rDn * d_rDn_d_pY;
@@ -1281,9 +1281,7 @@ public class ErsCorrection extends GeometryCorrection { 
		double d_yh_d_py = -SCENE_UNITS_SCALE * d_pYc_d_pY * this.disparityRadius;

		double [] xyz = {xh, yh, zh};

		

//		Matrix dWdP;

		Vector3D [] dWdP= new Vector3D[3];

		

		if (is_infinity) {

			dWdP[0] = new Vector3D(new double [] {d_xh_d_px, d_yh_d_px, 0.0});

			dWdP[1] = new Vector3D(new double [] {d_xh_d_py, d_yh_d_py, 0.0});
@@ -1328,34 +1326,18 @@ public class ErsCorrection extends GeometryCorrection { 


		// undo camera rotation during acquisition of the center line. 

		Rotation cam_orient_center= new Rotation(RotationOrder.YXZ, ROT_CONV, camera_atr[0],camera_atr[1],camera_atr[2]);

		Vector3D cam_center_world0 = cam_orient_center.applyInverseTo(cam_center_local);

		Vector3D cam_center_world = matrixTimesVector(rot_deriv_matrices[0], cam_center_local);

		if (debug_level > 1) {

			System.out.println("\nrot_deriv_matrices[0]:"); rot_deriv_matrices[0].print(8,6);

			System.out.println("\nrot_deriv_matrices[1]:"); rot_deriv_matrices[1].print(8,6);

			System.out.println("\nrot_deriv_matrices[2]:"); rot_deriv_matrices[2].print(8,6);

			System.out.println("\nrot_deriv_matrices[3]:"); rot_deriv_matrices[3].print(8,6);



			Matrix rotation_matrix = new Matrix(cam_orient_center.getMatrix());



			System.out.println("\nrotation_matrix:"); rotation_matrix.print(8,6);

			System.out.println("\nrotation_matrix.inverse():"); rotation_matrix.inverse().print(8,6);

//			Vector3D v1 = matrixTimesVector(rotation_matrix, cam_center_local);

//			Vector3D v1i = matrixTimesVector(rotation_matrix.inverse(), cam_center_local);

			System.out.println("cam_center_world0="+cam_center_world0.toString());

//			System.out.println("v1="+v1.toString());

//			System.out.println("v1i="+v1i.toString());

//			Vector3D v2 = matrixTimesVector(rot_deriv_matrices[0], cam_center_local);

			System.out.println("cam_center_world="+cam_center_world.toString());

		}

		

//		double [] dbg_angles = cam_orient_center.getAngles(RotationOrder.YXZ, ROT_CONV);



// Apply same rotation to the d_d* vectors

/**/		

		Vector3D dw_dpX0 = matrixTimesVector(rot_deriv_matrices[0], dw_dpX);

		Vector3D dw_dpY0 = matrixTimesVector(rot_deriv_matrices[0], dw_dpY);

		Vector3D dw_dd0 =  matrixTimesVector(rot_deriv_matrices[0], dw_dd);

/* */		

/* */		

		dw_dpX = cam_orient_center.applyInverseTo(dw_dpX);

		dw_dpY = cam_orient_center.applyInverseTo(dw_dpY);

		dw_dd =  cam_orient_center.applyInverseTo(dw_dd);

		

/* */		

		// convert coordinates to the real world coordinates

		Vector3D world_xyz = (is_infinity) ? cam_center_world : cam_center_world.add(new Vector3D(camera_xyz));

		double [] wxyz = world_xyz.toArray();
@@ -1380,6 +1362,7 @@ public class ErsCorrection extends GeometryCorrection { 
		Vector3D d_vy = new Vector3D(0.0,         delta_t_xyz, 0.0);

		Vector3D d_vz = new Vector3D(0.0,         0.0,         delta_t_xyz);

		

		/* */

		Vector3D dw_dvaz = cam_orient_center.applyInverseTo(Vector3D.crossProduct(d_vaz, cam_center_now_local));

		Vector3D dw_dvtl = cam_orient_center.applyInverseTo(Vector3D.crossProduct(d_vtl, cam_center_now_local));

		Vector3D dw_dvrl = cam_orient_center.applyInverseTo(Vector3D.crossProduct(d_vrl, cam_center_now_local));
@@ -1387,34 +1370,21 @@ public class ErsCorrection extends GeometryCorrection { 
		Vector3D dw_dvx =  cam_orient_center.applyInverseTo(d_vx);

		Vector3D dw_dvy =  cam_orient_center.applyInverseTo(d_vy);

		Vector3D dw_dvz =  cam_orient_center.applyInverseTo(d_vz);

		/* */

		Vector3D dw_dvaz0 = matrixTimesVector(rot_deriv_matrices[0], Vector3D.crossProduct(d_vaz, cam_center_now_local));

		Vector3D dw_dvtl0 = matrixTimesVector(rot_deriv_matrices[0], Vector3D.crossProduct(d_vtl, cam_center_now_local));

		Vector3D dw_dvrl0 = matrixTimesVector(rot_deriv_matrices[0], Vector3D.crossProduct(d_vrl, cam_center_now_local));

		

		Vector3D dw_dvx0 =  matrixTimesVector(rot_deriv_matrices[0], d_vx);

		Vector3D dw_dvy0 =  matrixTimesVector(rot_deriv_matrices[0], d_vy);

		Vector3D dw_dvz0 =  matrixTimesVector(rot_deriv_matrices[0], d_vz);

		

		// overall rotations and offsets		

		Vector3D dw_daz0 = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(0.0, 1.0, 0.0)), cam_center_world));

		Vector3D dw_dtl0 = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(1.0, 0.0, 0.0)), cam_center_world));

		Vector3D dw_drl0 = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(0.0, 0.0, 1.0)), cam_center_world));

		/*

		Vector3D dw_daz = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(1.0, 0.0, 0.0)), cam_center_world));

		Vector3D dw_dtl = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(0.0, 1.0, 0.0)), cam_center_world));

		Vector3D dw_drl = cam_orient_center.applyInverseTo(Vector3D.crossProduct((new Vector3D(0.0, 0.0, 1.0)), cam_center_world));

		*/

		

		Vector3D dw_daz = matrixTimesVector(rot_deriv_matrices[1], cam_center_local);

		Vector3D dw_dtl = matrixTimesVector(rot_deriv_matrices[2], cam_center_local);

		Vector3D dw_drl = matrixTimesVector(rot_deriv_matrices[3], cam_center_local);

		

		if (debug_level > 1) {

			System.out.println("dw_daz0="+dw_daz0.toString());

			System.out.println("dw_dtl0="+dw_dtl0.toString());

			System.out.println("dw_drl0="+dw_drl0.toString());

			



			System.out.println("dw_daz="+dw_daz.toString());

			System.out.println("dw_dtl="+dw_dtl.toString());

			System.out.println("dw_drl="+dw_drl.toString());

		}

		

		

		

		Vector3D dw_dx = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_I;

		Vector3D dw_dy = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_J;

		Vector3D dw_dz = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_K;