Commit 2cb7df5a authored by Andrey Filippov's avatar Andrey Filippov

Debugging, next snapshot

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