Skip to content

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

Debugging, next snapshot

parent df05481d
Expand all Show whitespace changes
Inline Side-by-side
Showing with 254 additions and 51 deletions
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 2cb7df5a
......@@ -220,18 +220,22 @@ public class ErsCorrection extends GeometryCorrection {
private double [][] ers_atr_dt; // angular velocities per scan line. It is now actually 2*omega!
public static boolean [] getParamSelect(
boolean use_Z,
boolean use_R,
boolean use_XY,
boolean use_AT,
boolean use_ERS,
boolean use_ERS_tilt) {
boolean use_ERS_tilt,
boolean use_ERS_roll) {
boolean [] param_select = new boolean[DP_NUM_PARS];
boolean use_ERS_roll = use_ERS && use_ERS_tilt;
for (int i:DP_ZR_INDICES) param_select[i] = true;
// for (int i:DP_ZR_INDICES) param_select[i] = true;
if (use_Z || use_XY) param_select[DP_DSZ] = true;
if (use_R || use_AT) param_select[DP_DSRL] = true;
if (use_XY) for (int i:DP_XY_INDICES) param_select[i] = true;
if (use_AT) for (int i:DP_AT_INDICES) param_select[i] = true;
if (use_ERS) param_select[DP_DSVAZ] = true;
if (use_ERS && use_ERS_tilt) param_select[DP_DSVTL] = true;
if (use_ERS_roll) param_select[DP_DSVRL] = true;
if (use_ERS && use_ERS_roll) param_select[DP_DSVRL] = true;
return param_select;
}
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 2cb7df5a
......@@ -322,7 +322,30 @@ public class Interscene {
start_ref_pointers[0] = start_ref_pointers1[0];
return earliest_scene2; // cent_index;
}
if (generate_egomotion) {
String ego_path = quadCLTs[cent_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego-preinvert-"+quadCLTs[cent_index].getNumOrient()+".csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
cent_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
// invert first half, reference to the cent_index, add to cent_index map, generate ref_index ponter and cent_index,
if (debugLevel > -3) {
System.out.println("pre invertInitialOrientation(): ref_index "+ref_index+", cent_index="+
cent_index+", last_index="+". readjust="+readjust+"\n");
}
invertInitialOrientation(
clt_parameters, // final CLTParameters clt_parameters,
......@@ -335,6 +358,24 @@ public class Interscene {
ref_index, // final int ref_old, // original ref_index (normally == last_index)
earliest_scene2, //final int earliest_index// first to process
debugLevel); // final int debugLevel
if (generate_egomotion) {
String ego_path = quadCLTs[cent_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego-afterinvert-"+quadCLTs[cent_index].getNumOrient()+".csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
cent_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
String cent_ts = quadCLTs[cent_index].getImageName();
// write config for both ref_index and cent_index scenes
// ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
......@@ -425,12 +466,18 @@ public class Interscene {
}
return 0;
} else { // if (readjust) {
boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always
lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R,
true, // boolean use_XY
false, // boolean use_AT,
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
false, // boolean use_ERS_tilt);
false );// boolean use_ERS_roll);
double maximal_series_rms = 0.0;
double [][][] scenes_xyzatr = new double [last_index+1][][];
double [][][] dxyzatr_dt = new double [last_index+1][][];
......@@ -761,12 +808,20 @@ public class Interscene {
double max_zoom_diff = clt_parameters.imp.max_zoom_diff;
boolean fpn_skip = clt_parameters.imp.fpn_skip; // if false - fail as before
boolean fpn_rematch = clt_parameters.imp.fpn_rematch; // if false - keep previous
boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
if (debugLevel > -3) {
System.out.println("setInitialOrientationsIms(): lma_use_Z="+lma_use_Z+", lma_use_R="+lma_use_R);
}
boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always
true, // boolean use_XY
false, // boolean use_AT,
lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R,
true, // boolean use_XY, May change with oorient preference
false, // boolean use_AT, May change with oorient preference
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
false, // boolean use_ERS_tilt);
false); // boolean use_ERS_roll);
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--) {
......@@ -1815,8 +1870,12 @@ public class Interscene {
public static int reAdjustPairsLMAInterscene( // after combo dgi is available and preliminary poses are known
CLTParameters clt_parameters,
double mb_max_gain,
boolean use_Z,
boolean use_R,
boolean disable_ers,
boolean disable_ers_y,
boolean disable_ers_r,
boolean lma_xyzatr,
boolean configured_lma,
boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen,
......@@ -1833,16 +1892,20 @@ public class Interscene {
int debugLevel)
{
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain+
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y);
boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = false; // true;
boolean restore_imu = false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+
", use_Z="+use_Z+", use_R="+use_R);
boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true;
boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect(
true, // !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
true, // readjust_xy_ims || lpf_xy, // boolean use_AT,
lma_xyzatr || use_Z, // boolean use_Z,
lma_xyzatr || use_R, // boolean use_R,
lma_xyzatr || ( !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0))), // false only in mode c): freeze X,Y// boolean use_XY
lma_xyzatr || ( readjust_xy_ims || lpf_xy), // boolean use_AT,
!disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
!disable_ers_y); // boolean use_ERS_tilt);
!disable_ers_y, // boolean use_ERS_tilt,
!disable_ers_r); // boolean use_ERS_roll);
final double [] param_regweights = configured_lma? clt_parameters.ilp.ilma_regularization_weights :
ErsCorrection.getParamRegWeights(
(freeze_xy_pull? 0.0: reg_weight_xy)); // double reg_weight,
......@@ -1996,10 +2059,13 @@ public class Interscene {
}
}
param_select = ErsCorrection.getParamSelect(
true, // boolean use_Z,
true, // boolean use_R,
true, // boolean use_XY
true, // boolean use_AT,
false, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false, // boolean use_ERS_tilt);
false); // boolean use_ERS_roll);
}
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
......@@ -5118,7 +5184,18 @@ public class Interscene {
sb.append(nscene+"\t"+timestamp);
if (use_processed) {
double [] nan3 = new double[] {Double.NaN, Double.NaN,Double.NaN};
double [][] nan23 = new double[][] {nan3,nan3};
if (scenes_xyzatr[nscene] != null) {
if (scenes_xyzatr[nscene][0] == null) scenes_xyzatr[nscene][0]= nan3;
if (scenes_xyzatr[nscene][1] == null) scenes_xyzatr[nscene][1]= nan3;
if (dxyzatr_dt[nscene]== null) dxyzatr_dt[nscene] = nan23;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene]== null) scenes_xyzatr_dt[nscene] = nan23;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
// x,y,z - scene position in reference scene frame (reference scene is all 0-s)
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
// a,tilt,roll - scene orientation in reference scene frame (reference scene is all 0-s)
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 2cb7df5a
......@@ -5032,6 +5032,7 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
/*
// reference to earliest
String ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
......@@ -5057,6 +5058,7 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
}
*/
}
} else if (ims_use) {
......@@ -5289,18 +5291,26 @@ public class OpticalFlow {
}
// TODO: move to config
// boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
boolean configured_lma = false;
boolean lpf_xy = false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean configured_lma = clt_parameters.imp.configured_lma; // false;
boolean lpf_xy = clt_parameters.imp.lpf_xy; // false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen = clt_parameters.imp.avg_len; // 3.0;
boolean readjust_xy_ims = true; // false;
double reg_weight_xy = 0; // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
boolean readjust_xy_ims = clt_parameters.imp.readjust_xy_ims; //true; // false;
double reg_weight_xy = clt_parameters.imp.reg_weight_xy; // 0; // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
int mb_ers_index = clt_parameters.imp.mb_ers_index;
int mb_ers_y_index = clt_parameters.imp.mb_ers_y_index;
int mb_ers_r_index = clt_parameters.imp.mb_ers_r_index;
int mb_all_index = clt_parameters.imp.mb_all_index;
int mb_gain_index_pose = clt_parameters.imp.mb_gain_index_pose; // pose readjust pass to switch to full mb_max_gain from mb_max_gain_inter
boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < mb_ers_index);
boolean disable_ers_y = (quadCLTs[ref_index].getNumOrient() < mb_ers_y_index);
boolean disable_ers_r = (quadCLTs[ref_index].getNumOrient() < mb_ers_r_index);
boolean lma_xyzatr = (quadCLTs[ref_index].getNumOrient() == mb_all_index);
boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean ers_from_ims = true; // false; // change later
int ers_mode = 0; // keep
......@@ -5324,8 +5334,12 @@ public class OpticalFlow {
earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R,
disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r,
lma_xyzatr, // boolean lma_xyzatr,
configured_lma, // boolean configured_lma,
lpf_xy, // boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
avg_rlen, // double avg_rlen,
......@@ -5372,7 +5386,7 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
/*
// reference to earliest
String ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
......@@ -5398,7 +5412,7 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
}
*/
}
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 2cb7df5a
......@@ -68,6 +68,13 @@ public class QuaternionLma {
parameters_vector = quat0.clone();
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < 3; j++) {
x_vector[3 * i + j] = 0.0;
y_vector[3 * i + j] = 0.0;
weights[3*i + j] = 0.0;
}
} else {
for (int j = 0; j < 3; j++) {
x_vector[3 * i + j] = vect_x[i][j];
y_vector[3 * i + j] = vect_y[i][j];
......@@ -76,6 +83,7 @@ public class QuaternionLma {
sw += w;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights[3 * N] = 1.0 - pure_weight;
......@@ -84,7 +92,7 @@ public class QuaternionLma {
}
// TODO: Consider adding differences between x and y for regularization (or it won't work)
// goal - to minimize "unneded" rotation along the commonn axis
// goal - to minimize "unneeded" rotation along the common axis
private double [] getFxDerivs(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
......
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