Skip to content

I
imagej-elphel
Commit 8f04633f authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

debugging

parent 4c3e69f5
Hide whitespace changes
Inline Side-by-side
Showing with 31 additions and 61 deletions
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 8f04633f
......@@ -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});
......@@ -1305,7 +1303,7 @@ public class ErsCorrection extends GeometryCorrection {
Rotation cam_orient_now_local = new Rotation(qpix.getQ0(), qpix.getQ1(), qpix.getQ2(),qpix.getQ3(), true); // boolean needsNormalization)
Vector3D dw_dpX = cam_orient_now_local.applyInverseTo(dWdP[0]);
Vector3D dw_dpY = cam_orient_now_local.applyInverseTo(dWdP[1]);
Vector3D dw_dd = cam_orient_now_local.applyInverseTo(dWdP[2]);
Vector3D dw_dd = cam_orient_now_local.applyInverseTo(dWdP[2]);
Vector3D v3= new Vector3D(xyz);
Vector3D cam_center_now_local = cam_orient_now_local.applyInverseTo(v3); // undone rotation relative to center
......@@ -1327,35 +1325,19 @@ public class ErsCorrection extends GeometryCorrection {
Vector3D cam_center_local = (is_infinity) ? cam_center_now_local : cam_center_now_local.add(cam_now_local); // skip translation for infinity
// 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);
Rotation cam_orient_center= new Rotation(RotationOrder.YXZ, ROT_CONV, camera_atr[0],camera_atr[1],camera_atr[2]);
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,33 +1370,20 @@ 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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment