Skip to content

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

debugging

parent 4c3e69f5
Show 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 { ...@@ -858,7 +858,7 @@ public class ErsCorrection extends GeometryCorrection {
int DP_DX = 12; // dw_dx, (m) int DP_DX = 12; // dw_dx, (m)
int DP_DY = 13; // dw_dy, (m) int DP_DY = 13; // dw_dy, (m)
int DP_DZ = 14; // dw_dz}; (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(); double [] deltas = deltas0.clone();
for (int i = 0; i < deltas.length; i++) deltas[i] *= scale_delta; for (int i = 0; i < deltas.length; i++) deltas[i] *= scale_delta;
int dbg_tile = 16629; int dbg_tile = 16629;
...@@ -919,13 +919,17 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -919,13 +919,17 @@ public class ErsCorrection extends GeometryCorrection {
pXpYD_original[nTile] = new double[] {centerX, centerY,disparity}; pXpYD_original[nTile] = new double[] {centerX, centerY,disparity};
// find derivatives by pX, pY, pZ // find derivatives by pX, pY, pZ
if (nTile == dbg_tile) {
System.out.println("compareDSItoWorldDerivatives()_0: nTile = "+nTile+" ("+tileX+"/"+tileY+")");
}
double [][][] xyz_pm = { double [][][] xyz_pm = {
{ getWorldCoordinatesERS(centerX + deltas[DP_DPX], centerY, disparity, true), { getWorldCoordinatesERS(centerX + deltas[DP_DPX], centerY, disparity, true, camera_xyz, camera_atr),
getWorldCoordinatesERS(centerX - deltas[DP_DPX], centerY, disparity, true) getWorldCoordinatesERS(centerX - deltas[DP_DPX], centerY, disparity, true, camera_xyz, camera_atr)
},{ getWorldCoordinatesERS(centerX, centerY + deltas[DP_DPY], disparity, true), },{ getWorldCoordinatesERS(centerX, centerY + deltas[DP_DPY], disparity, true, camera_xyz, camera_atr),
getWorldCoordinatesERS(centerX, centerY - deltas[DP_DPY], disparity, true) getWorldCoordinatesERS(centerX, centerY - deltas[DP_DPY], disparity, true, camera_xyz, camera_atr)
},{ getWorldCoordinatesERS(centerX, centerY, disparity + deltas[DP_DD], true), },{ getWorldCoordinatesERS(centerX, centerY, disparity + deltas[DP_DD], true, camera_xyz, camera_atr),
getWorldCoordinatesERS(centerX, centerY, disparity - deltas[DP_DD], true)}}; getWorldCoordinatesERS(centerX, centerY, disparity - deltas[DP_DD], true, camera_xyz, camera_atr)}};
for (int n = 0; n < 3; n++) { for (int n = 0; n < 3; n++) {
int par_index =DP_DPX + n; int par_index =DP_DPX + n;
if ((xyz_pm[n][0] != null) && (xyz_pm[n][1] != null)) { if ((xyz_pm[n][0] != null) && (xyz_pm[n][1] != null)) {
...@@ -989,7 +993,7 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -989,7 +993,7 @@ public class ErsCorrection extends GeometryCorrection {
if (disparity < max_inf_disparity) { // or only for abs() ? if (disparity < max_inf_disparity) { // or only for abs() ?
disparity = 0.0; 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 { ...@@ -1229,7 +1233,6 @@ public class ErsCorrection extends GeometryCorrection {
if (Double.isNaN(pXpYD[2])) { if (Double.isNaN(pXpYD[2])) {
return null; return null;
} }
// boolean is_infinity = Math.abs(pXpYD[2]) < THRESHOLD; // Maybe all negative - too?
int line = (int) Math.round(pXpYD[1]); int line = (int) Math.round(pXpYD[1]);
if (line < 0) { if (line < 0) {
line = 0; line = 0;
...@@ -1262,12 +1265,9 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -1262,12 +1265,9 @@ public class ErsCorrection extends GeometryCorrection {
double d_rDn_d_pX = pXcd * (r_scale * r_scale) / rDn; 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_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_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 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_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; double d_pYc_d_pY = rND2RD + pYcd * d_rND2rD_d_rDn * d_rDn_d_pY;
...@@ -1281,9 +1281,7 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -1281,9 +1281,7 @@ public class ErsCorrection extends GeometryCorrection {
double d_yh_d_py = -SCENE_UNITS_SCALE * d_pYc_d_pY * this.disparityRadius; double d_yh_d_py = -SCENE_UNITS_SCALE * d_pYc_d_pY * this.disparityRadius;
double [] xyz = {xh, yh, zh}; double [] xyz = {xh, yh, zh};
// Matrix dWdP;
Vector3D [] dWdP= new Vector3D[3]; Vector3D [] dWdP= new Vector3D[3];
if (is_infinity) { if (is_infinity) {
dWdP[0] = new Vector3D(new double [] {d_xh_d_px, d_yh_d_px, 0.0}); 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}); 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 { ...@@ -1328,34 +1326,18 @@ public class ErsCorrection extends GeometryCorrection {
// undo camera rotation during acquisition of the center line. // 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]); 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); 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 // 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_dpX = cam_orient_center.applyInverseTo(dw_dpX);
dw_dpY = cam_orient_center.applyInverseTo(dw_dpY); dw_dpY = cam_orient_center.applyInverseTo(dw_dpY);
dw_dd = cam_orient_center.applyInverseTo(dw_dd); dw_dd = cam_orient_center.applyInverseTo(dw_dd);
/* */
// convert coordinates to the real world coordinates // convert coordinates to the real world coordinates
Vector3D world_xyz = (is_infinity) ? cam_center_world : cam_center_world.add(new Vector3D(camera_xyz)); Vector3D world_xyz = (is_infinity) ? cam_center_world : cam_center_world.add(new Vector3D(camera_xyz));
double [] wxyz = world_xyz.toArray(); double [] wxyz = world_xyz.toArray();
...@@ -1380,6 +1362,7 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -1380,6 +1362,7 @@ public class ErsCorrection extends GeometryCorrection {
Vector3D d_vy = new Vector3D(0.0, delta_t_xyz, 0.0); 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 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_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_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)); Vector3D dw_dvrl = cam_orient_center.applyInverseTo(Vector3D.crossProduct(d_vrl, cam_center_now_local));
...@@ -1387,34 +1370,21 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -1387,34 +1370,21 @@ public class ErsCorrection extends GeometryCorrection {
Vector3D dw_dvx = cam_orient_center.applyInverseTo(d_vx); Vector3D dw_dvx = cam_orient_center.applyInverseTo(d_vx);
Vector3D dw_dvy = cam_orient_center.applyInverseTo(d_vy); Vector3D dw_dvy = cam_orient_center.applyInverseTo(d_vy);
Vector3D dw_dvz = cam_orient_center.applyInverseTo(d_vz); 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 // 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_daz = matrixTimesVector(rot_deriv_matrices[1], cam_center_local);
Vector3D dw_dtl = matrixTimesVector(rot_deriv_matrices[2], 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); 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_dx = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_I;
Vector3D dw_dy = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_J; Vector3D dw_dy = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_J;
Vector3D dw_dz = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_K; Vector3D dw_dz = is_infinity ? Vector3D.ZERO : Vector3D.PLUS_K;
......
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