Skip to content

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

Found and fixed bug in ExtrinsicAdjustment introduced while converting 

from quad
parent 2a11eb19
Hide whitespace changes
Inline Side-by-side
Showing with 77 additions and 18 deletions
src/main/java/com/elphel/imagej/tileprocessor/ExtrinsicAdjustment.java
View file @ f801b9b5
...@@ -589,6 +589,12 @@ public class ExtrinsicAdjustment { ...@@ -589,6 +589,12 @@ public class ExtrinsicAdjustment {
"Initial_y-fX_after_moving_objects"); "Initial_y-fX_after_moving_objects");
} }
if (debugLevel > 0) {
printYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
}
double lambda = 0.1; double lambda = 0.1;
double lambda_scale_good = 0.5; double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0; double lambda_scale_bad = 8.0;
...@@ -705,13 +711,11 @@ public class ExtrinsicAdjustment { ...@@ -705,13 +711,11 @@ public class ExtrinsicAdjustment {
fx[indx0] = this.weights[indx0] * d; fx[indx0] = this.weights[indx0] * d;
rms += fx[indx0]*d; // sum of weights rms += fx[indx0]*d; // sum of weights
for (int cam = 0; cam < num_sensors; cam++) { for (int cam = 0; cam < num_sensors; cam++) {
int indx = indx0 + cam + 1; int indx = indx0 + cam + 1; // index of dd component
// d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + cam] - fx[indx]);
d = (-measured_dsxy[cluster][indx_dd0 + cam] - fx[indx]); d = (-measured_dsxy[cluster][indx_dd0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d; fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights rms += fx[indx] * d; // sum of weights
indx = indx0 + cam + 5; // nd indx = indx0 + cam + num_sensors + 1; // // index of nd component
// d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + cam] - fx[indx]);
d = (-measured_dsxy[cluster][indx_nd0 + cam] - fx[indx]); d = (-measured_dsxy[cluster][indx_nd0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d; fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights rms += fx[indx] * d; // sum of weights
...@@ -2107,15 +2111,15 @@ public class ExtrinsicAdjustment { ...@@ -2107,15 +2111,15 @@ public class ExtrinsicAdjustment {
dbg_img[5][pix] = Double.NaN; dbg_img[5][pix] = Double.NaN;
} }
} else { } else {
// dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1]; // dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1];
dbg_img[0][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1]; dbg_img[0][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1];
dbg_img[1][pix] = -fx[indx]; dbg_img[1][pix] = -fx[indx];
// dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]; // dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]; dbg_img[2][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
if (weights[indx] > 0.0) { if (weights[indx] > 0.0) {
dbg_img[3][pix] = weights[indx]*clusters; dbg_img[3][pix] = weights[indx]*clusters;
// dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]); // dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]);
// dbg_img[5][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]; // dbg_img[5][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]); dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]);
dbg_img[5][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]; dbg_img[5][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
} else { } else {
...@@ -2149,6 +2153,55 @@ public class ExtrinsicAdjustment { ...@@ -2149,6 +2153,55 @@ public class ExtrinsicAdjustment {
} }
public void printYminusFxWeight(
double [] fx,
double [] weights,
String title) {
if (fx == null) {
fx = new double [weights.length];
}
int clusters = clustersX * clustersY;
// String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"};
System.out.println("===== "+title+" =====");
// text output
for (int mode = 0; mode < points_sample; mode++) {
if (mode == 0) {
System.out.println("Differential Disparity");
} else if (mode <= num_sensors) {
System.out.println("DD-"+(mode - 1));
} else {
System.out.println("ND-"+(mode - num_sensors - 1));
}
System.out.println( "clusterX clusterY Y -fX Y + fX Weight W*(Y+fX) Masked (Y + fX)");
for (int cluster = 0; cluster < clusters; cluster++) {
int clusterX = cluster % clustersX;
int clusterY = cluster / clustersX;
int indx = cluster * points_sample + mode;
if ((measured_dsxy[cluster] != null) && (weights[indx] > 0.0)){
double [] dbg_data = new double[6];
if (mode ==0) {
dbg_data[0] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
dbg_data[1] = -fx[indx];
dbg_data[2] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
dbg_data[3] = weights[indx]*clusters;
dbg_data[4] = weights[indx]*clusters*(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx]);
dbg_data[5] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
} else {
dbg_data[0] = measured_dsxy[cluster][indx_dd0 + mode - 1];
dbg_data[1] = -fx[indx];
dbg_data[2] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
dbg_data[3] = weights[indx]*clusters;
dbg_data[4] = weights[indx]*clusters*(measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]);
dbg_data[5] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
}
System.out.println(String.format(" %3d %3d %8.5f %8.5f %8.5f %8.5f %8.5f %8.5f",
clusterX, clusterY, dbg_data[0],dbg_data[1],dbg_data[2],dbg_data[3],dbg_data[4],dbg_data[5]));
}
}
}
}
private void dbgXY( private void dbgXY(
CorrVector corr_vector, CorrVector corr_vector,
String title) { String title) {
...@@ -2366,12 +2419,18 @@ public class ExtrinsicAdjustment { ...@@ -2366,12 +2419,18 @@ public class ExtrinsicAdjustment {
if (this.last_rms == null) { //first time, need to calculate all (vector is valid) if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points]; this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector); this.last_ymfx = getFx(corr_vector);
if (debug_level > -2) { // temporary if (debug_level > -1) { // temporary
dbgYminusFxWeight( dbgYminusFxWeight(
this.last_ymfx, this.last_ymfx,
this.weights, this.weights,
"Initial_y-fX_after_moving_objects"); "Initial_y-fX_after_moving_objects");
} }
if (debug_level > -1) { // temporary
printYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
}
if (last_ymfx == null) { if (last_ymfx == null) {
return null; // need to re-init/restart LMA return null; // need to re-init/restart LMA
...@@ -2400,7 +2459,7 @@ public class ExtrinsicAdjustment { ...@@ -2400,7 +2459,7 @@ public class ExtrinsicAdjustment {
lambda, // *10, // temporary lambda, // *10, // temporary
this.last_jt)); // double [][] jt) this.last_jt)); // double [][] jt)
if (debug_level>2) { if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ"); System.out.println("JtJ + lambda*diag(JtJ), lambda = "+lambda);
wjtjlambda.print(18, 6); wjtjlambda.print(18, 6);
double [] diag_sqrt = getWJtDiagSqrt(this.last_jt); double [] diag_sqrt = getWJtDiagSqrt(this.last_jt);
System.out.print("diag_sqrt={"); System.out.print("diag_sqrt={");
...@@ -2428,20 +2487,20 @@ public class ExtrinsicAdjustment { ...@@ -2428,20 +2487,20 @@ public class ExtrinsicAdjustment {
return rslt; return rslt;
} }
if (debug_level>2) { if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()"); System.out.println("(JtJ + lambda*diag(JtJ).inv(), lambda = "+lambda);
jtjl_inv.print(18, 6); jtjl_inv.print(18, 10);
} }
//last_jt has NaNs //last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted); Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) { if (debug_level>2) {
System.out.println("Jt * (y-fx)"); System.out.println("Jt * (y-fx)");
jty.print(18, 6); jty.print(18, 10);
} }
Matrix mdelta = jtjl_inv.times(jty); Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) { if (debug_level>2) {
System.out.println("mdelta"); System.out.println("mdelta");
mdelta.print(18, 6); mdelta.print(18, 10);
} }
double [] delta = mdelta.getColumnPackedCopy(); double [] delta = mdelta.getColumnPackedCopy();
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDttCPU.java
View file @ f801b9b5
...@@ -3222,10 +3222,10 @@ public class ImageDttCPU { ...@@ -3222,10 +3222,10 @@ public class ImageDttCPU {
if (disp_dist_cons != null) { if (disp_dist_cons != null) {
System.out.println("disp_dist_cons[0]="+disp_dist_cons[0]+", disp_dist_cons[0]="+disp_dist_cons[0]+" (this tile - weighted average will be discarded)"); System.out.println("disp_dist_cons[0]="+disp_dist_cons[0]+", disp_dist_cons[0]="+disp_dist_cons[0]+" (this tile - weighted average will be discarded)");
} }
(new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles()); // (new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());
// mcorr_comb_width, // combined correlation tile width // mcorr_comb_width, // combined correlation tile width
//mcorr_comb_height, // combined correlation tile full height //mcorr_comb_height, // combined correlation tile full height
(new ShowDoubleFloatArrays()).showArrays(corr_combo_all, mcorr_comb_width, mcorr_comb_height, "corr_combo_all_CX"+clustX+"-CY"+clustY); // (new ShowDoubleFloatArrays()).showArrays(corr_combo_all, mcorr_comb_width, mcorr_comb_height, "corr_combo_all_CX"+clustX+"-CY"+clustY);
} }
if (ixy != null) { //TODO - for CM use magic! if (ixy != null) { //TODO - for CM use magic!
...@@ -3246,7 +3246,7 @@ public class ImageDttCPU { ...@@ -3246,7 +3246,7 @@ public class ImageDttCPU {
if (disp_str_combo != null) { if (disp_str_combo != null) {
System.out.println("disp_str_combo[0]="+disp_str_combo[1]+", disp_str_combo[0]="+disp_str_combo[1]); System.out.println("disp_str_combo[0]="+disp_str_combo[1]+", disp_str_combo[0]="+disp_str_combo[1]);
} }
(new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles()); // (new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());
} }
lma2 = correlation2d.corrLMA2Single( lma2 = correlation2d.corrLMA2Single(
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ f801b9b5
...@@ -9537,7 +9537,7 @@ if (debugLevel > -100) return true; // temporarily ! ...@@ -9537,7 +9537,7 @@ if (debugLevel > -100) return true; // temporarily !
inf_max, // double inf_max, inf_max, // double inf_max,
threadsMax, //final int threadsMax, // maximal number of threads to launch threadsMax, //final int threadsMax, // maximal number of threads to launch
updateStatus,// final boolean updateStatus, updateStatus,// final boolean updateStatus,
1); // debugLevelInner); // final int debugLevel) debugLevelInner); // 1); // final int debugLevel)
if (!ok) break; if (!ok) break;
} else { } else {
boolean ok = quadCLT_aux.extrinsicsCLTfromGT( boolean ok = quadCLT_aux.extrinsicsCLTfromGT(
......
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