Skip to content

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

debugging longer series

parent 793b2588
Hide whitespace changes
Inline Side-by-side
Showing with 142 additions and 44 deletions
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ df05481d
......@@ -225,11 +225,13 @@ public class ErsCorrection extends GeometryCorrection {
boolean use_ERS,
boolean use_ERS_tilt) {
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;
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) 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;
return param_select;
}
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ df05481d
......@@ -328,10 +328,12 @@ public class Interscene {
clt_parameters, // final CLTParameters clt_parameters,
batch_mode, // final boolean batch_mode,
readjust, // final boolean readjust,
quadCLTs, //final QuadCLT[] quadCLTs, //
ref_index, // final int ref_index,
cent_index, // final int cent_index,
earliest_scene2, // int earliest_index//
false, // final int set_self, // set record for the new reference to new reference
quadCLTs, // final QuadCLT[] quadCLTs, //
ref_index, // final int last_index, // last to process (normally old reference)
cent_index, // final int ref_index, // new reference index (center)
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
String cent_ts = quadCLTs[cent_index].getImageName();
// write config for both ref_index and cent_index scenes
......@@ -383,37 +385,46 @@ public class Interscene {
public static int invertInitialOrientation(
final CLTParameters clt_parameters,
final boolean batch_mode,
final boolean readjust,
final boolean readjust,
final boolean set_self, // set record for the new reference to new reference
final QuadCLT[] quadCLTs, //
final int last_index, // original ref_index (last)
final int ref_index, // new reference index (center)
final int earliest_index,
final int ref_old, // original ref_index (normally == last_index)
final int earliest_index, // only used with readjust
final int debugLevel
) {
ErsCorrection ers_old_reference = quadCLTs[last_index].getErsCorrection();
if (debugLevel > -3) {
System.out.println("\ninvertInitialOrientation(): earliest_index "+earliest_index+", ref_index="+
ref_index+", last_index="+last_index+". readjust="+readjust+"\n");
}
ErsCorrection ers_old_reference = quadCLTs[ref_old].getErsCorrection();
int offs = set_self ? 0 : 1;
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
String cent_ts = quadCLTs[ref_index].getImageName();
double [][] center_xyzatr = new double [][] {ers_old_reference.getSceneXYZ(cent_ts), ers_old_reference.getSceneATR(cent_ts)};
double [][] inv_cent_xyzatr = ErsCorrection.invertXYZATR(center_xyzatr);
// Invert half-sequence to reference cent_index
for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
double [][] scene_xyzatr,dxyzatr_dt;
if (scene_index == last_index) {
scene_xyzatr = new double [2][3];
dxyzatr_dt = ers_old_reference.getErsXYZATR_dt();
} else {
String ts = quadCLTs[scene_index].getImageName();
scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
dxyzatr_dt = ers_old_reference.getSceneErsXYZATR_dt(ts);
if (!readjust) {
// Invert half-sequence to reference cent_index
for (int scene_index = ref_index + offs; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
double [][] scene_xyzatr,dxyzatr_dt;
if (scene_index == ref_old) {
scene_xyzatr = new double [2][3];
dxyzatr_dt = ers_old_reference.getErsXYZATR_dt();
} else {
String ts = quadCLTs[scene_index].getImageName();
scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
dxyzatr_dt = ers_old_reference.getSceneErsXYZATR_dt(ts);
}
double [][] scene_cent_xyzatr = ErsCorrection.combineXYZATR(scene_xyzatr, inv_cent_xyzatr);
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scene_cent_xyzatr,
dxyzatr_dt // ers_scene.getErsXYZATR_dt(),
);
}
double [][] scene_cent_xyzatr = ErsCorrection.combineXYZATR(scene_xyzatr, inv_cent_xyzatr);
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scene_cent_xyzatr,
dxyzatr_dt // ers_scene.getErsXYZATR_dt(),
);
}
if (readjust) {
return 0;
} else { // if (readjust) {
boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always
true, // boolean use_XY
......@@ -429,8 +440,12 @@ public class Interscene {
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
}
double [] min_max = {0, 10000, 0.0} ; // {min, max, actual rms)
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
double min_offset = clt_parameters.imp.min_offset; // 0.0; //
double max_offset = clt_parameters.imp.max_rel_offset * tilesX * tile_size;
double [] min_max = {min_offset, 2*max_offset, 0.0} ; // {min, max, actual rms)
// set reference
double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
......@@ -439,9 +454,6 @@ public class Interscene {
double sfm_minmax = clt_parameters.imp.sfm_minmax; //10.0; // minimal value of the SfM gain maximum to consider available
double sfm_fracmax = clt_parameters.imp.sfm_fracmax; // 0.75; // minimal fraction of the SfM maximal gain
double sfm_fracall = clt_parameters.imp.sfm_fracall; // 0.3; // minimal relative area of the SfM-e
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
boolean fmg_initial_en = clt_parameters.imp.fmg_initial_en; // enable IMS-based FPN mitigation for initial orientation
double fmg_distance = clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
double fmg_max_quad = clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
......@@ -492,9 +504,9 @@ public class Interscene {
ArrayList<Integer> fpn_list = new ArrayList<Integer>();
double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
boolean fpn_skip = true;
// boolean fpn_skip = true;
double [][] last_corr_xyzatr = {ZERO3,ZERO3};
for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
for (int scene_index = ref_index + offs; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
double [][] scene_xyzatr_old; // ,dxyzatr_dt;
if (scene_index == last_index) {
scene_xyzatr_old = new double [2][3];
......@@ -623,7 +635,7 @@ public class Interscene {
quadCLTs, // QuadCLT [] quadCLTs,
scenes_xyzatr, // double [][][] scenes_xyzatr,
avg_z, // double avg_z,
ref_index, // int ref_index, // >= earliest_scene
last_index, // ref_index, // int ref_index, // >= earliest_scene
earliest_index); // int earliest_scene)
for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) {
......@@ -1827,8 +1839,8 @@ public class Interscene {
boolean 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(
!freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
readjust_xy_ims || lpf_xy, // boolean use_AT,
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,
!disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
!disable_ers_y); // boolean use_ERS_tilt);
final double [] param_regweights = configured_lma? clt_parameters.ilp.ilma_regularization_weights :
......@@ -1992,6 +2004,8 @@ public class Interscene {
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
// double avg_rlen
readjust_xy_ims = false;
if (lpf_xy && (reg_weight_xy > 0.0)) {
scenes_xyzatr_pull = QuadCLT.refineFromLPF(
clt_parameters, // CLTParameters clt_parameters,
......@@ -5079,7 +5093,7 @@ public class Interscene {
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
0, // final int early_index,
earliest_scene, // final int early_index,
(quadCLTs.length -1) // int last_index,
);
int cent_index = earliest_scene + (quadCLTs.length - earliest_scene) / 2;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ df05481d
......@@ -5032,6 +5032,32 @@ 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()+
"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv";
Interscene.invertInitialOrientation(
clt_parameters, // final CLTParameters clt_parameters,
batch_mode, // final boolean batch_mode,
false, // final boolean readjust,
true, // final int set_self, // set record for the new reference to new reference
quadCLTs, // final QuadCLT[] quadCLTs, //
quadCLTs.length-1,// final int last_index, // last to process (normally old reference)
earliest_scene, // final int ref_index, // new reference index (center)
ref_index, // final int ref_old, // original ref_index (normally == last_index)
earliest_scene, // final int earliest_index// first to process
debugLevel); // final int debugLevel
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
earliest_scene,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path_early, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
}
}
} else if (ims_use) {
earliest_scene = Interscene.setInitialOrientationsIms(
......@@ -5267,7 +5293,7 @@ public class OpticalFlow {
boolean 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 = 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
double 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_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
......@@ -5346,6 +5372,33 @@ 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()+
"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv";
Interscene.invertInitialOrientation(
clt_parameters, // final CLTParameters clt_parameters,
batch_mode, // final boolean batch_mode,
false, // final boolean readjust,
true, // final int set_self, // set record for the new reference to new reference
quadCLTs, // final QuadCLT[] quadCLTs, //
quadCLTs.length-1,// final int last_index, // last to process (normally old reference)
earliest_scene, // final int ref_index, // new reference index (center)
ref_index, // final int ref_old, // original ref_index (normally == last_index)
earliest_scene, // final int earliest_index// first to process
debugLevel); // final int debugLevel
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
earliest_scene,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path_early, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
}
}
}
}
......@@ -8653,14 +8706,16 @@ public class OpticalFlow {
boolean generate_outlines = false; // true; // TODO: move to configs
System.out.println("intersceneExport(), scene timestamp="+ref_scene.getImageName());
// int num_scenes = scenes.length;
String [] combo_dsn_titles_full = COMBO_DSN_TITLES.clone();
String [] combo_dsn_titles_full = COMBO_DSN_TITLES.clone(); // keep all titles and slices
String [] combo_dsn_titles = new String [COMBO_DSN_INDX_DISP_BG];
/*
for (int i = 0; i < combo_dsn_titles.length; i++) {
combo_dsn_titles[i] = combo_dsn_titles_full[i];
}
if (clt_parameters.rig.mll_max_refines_bg <= 0) {
combo_dsn_titles_full = combo_dsn_titles;
}
*/
double min_disp_change = clt_parameters.rig.mll_min_disp_change_pre; // 0.001; // stop re-measure when difference is below
final int max_refines_presplit =
......@@ -8783,9 +8838,13 @@ public class OpticalFlow {
double [] target_disparity = combo_dsn_change[COMBO_DSN_INDX_DISP].clone();
double [] target_disparity_orig = target_disparity.clone(); // will just use NaN/not NaN to restore tasks before second pass with LMA
// double [][] combo_dsn_final = new double [combo_dsn_titles.length][combo_dsn[0].length];
// double [][] combo_dsn_final =
// new double [(clt_parameters.rig.mll_max_refines_bg > 0)?
// combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
double [][] combo_dsn_final =
new double [(clt_parameters.rig.mll_max_refines_bg > 0)?
combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
new double [COMBO_DSN_TITLES.length][combo_dsn[0].length];
//COMBO_DSN_INDX_SFM_GAIN
combo_dsn_final[COMBO_DSN_INDX_DISP]= combo_dsn[COMBO_DSN_INDX_DISP].clone();
for (int i = 1; i < combo_dsn_final.length; i++) {
if ( (i != COMBO_DSN_INDX_SFM_GAIN) &&
......@@ -9244,7 +9303,7 @@ public class OpticalFlow {
} //for (int nrefine = 0; nrefine < max_refines; nrefine++) {
// Add duplicate of FG disparity and FG+BG disparity (FG where no BG) for visual comparison
if (clt_parameters.rig.mll_max_refines_bg > 0) {
if (true) { // clt_parameters.rig.mll_max_refines_bg > 0) {
for (int nTile =0; nTile < combo_dsn_change[0].length; nTile++) {
combo_dsn_final[COMBO_DSN_INDX_DISP_FG][nTile] = combo_dsn_final[COMBO_DSN_INDX_DISP][nTile];
if (Double.isNaN(combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile])) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ df05481d
......@@ -256,8 +256,16 @@ public class QuadCLTCPU {
double t00= quadCLTs[early_index].getTimeStamp();
double t0 = t00;
double s0 = 0, sx = 0, sx2 = 0;
double [] sxy = new double[3], sy = new double[3];
double [] sxy = new double[3], sy = new double[3];
double [] vel_ref = new double[3], vel_ref_prev = null;
for (int nscene = early_index; nscene <= last_index; nscene++) {
// calculate inertial (reference frame) velocities
Rotation rot = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
rot.applyTo(dxyzatr[nscene][0], vel_ref);
if (vel_ref_prev == null) {
vel_ref_prev = vel_ref;
}
double t = quadCLTs[nscene].getTimeStamp();
double x = t - t00; // from early_index
tim[nscene] = x;
......@@ -270,12 +278,15 @@ public class QuadCLTCPU {
if (nscene == early_index) {
xyz_integ[nscene][i] = 0;
} else {
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene][0][i]);
// xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene][0][i]);
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (vel_ref_prev[i] + vel_ref[i]);
}
double y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
sy[i] += y;
sxy[i] += x*y;
}
vel_ref_prev = vel_ref;
}
double denom = sx2 * s0 - sx * sx;
......@@ -475,7 +486,19 @@ public class QuadCLTCPU {
}
return dxyzatr;
}
/**
* Integrate position and orientation of the camera relative to the
* (inertial) reference frame using IMU-derived local angular and
* linera velocities
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence
* @param ref_index reference scene index
* @param dxyzatr local linear and angular velocities
* @param early_index earliest (lowest) scene index in quadCLTs to use
* @param last_index last (highest) scene index to use
* @return per-scene {{X,Y,Z},{A,T,R}} relative to the reference frame
*/
public static double [][][] integratePIMU(
final CLTParameters clt_parameters,
final QuadCLT[] quadCLTs,
......
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