Skip to content

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

Updated initial orientation, started refining position

parent defa789f
Show whitespace changes
Inline Side-by-side
Showing with 1418 additions and 376 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ e17a0659
...@@ -71,7 +71,7 @@ public class Interscene { ...@@ -71,7 +71,7 @@ public class Interscene {
} }
} }
public static double [] ZERO3 = {0.0,0.0,0.0}; public static final double [] ZERO3 = {0.0,0.0,0.0};
// public static double LINE_ERR = 0.1; // public static double LINE_ERR = 0.1;
public static int THREADS_MAX = 100; // maximal number of threads to launch public static int THREADS_MAX = 100; // maximal number of threads to launch
...@@ -143,6 +143,7 @@ public class Interscene { ...@@ -143,6 +143,7 @@ public class Interscene {
*/ */
public static int setInitialOrientationsCenterIms( public static int setInitialOrientationsCenterIms(
final CLTParameters clt_parameters, final CLTParameters clt_parameters,
final boolean compensate_ims_rotation,
int min_num_scenes, int min_num_scenes,
final ColorProcParameters colorProcParameters, final ColorProcParameters colorProcParameters,
EyesisCorrectionParameters.DebayerParameters debayerParameters, EyesisCorrectionParameters.DebayerParameters debayerParameters,
...@@ -181,7 +182,7 @@ public class Interscene { ...@@ -181,7 +182,7 @@ public class Interscene {
threadsMax, threadsMax,
debugLevel-2); debugLevel-2);
String ts = quadCLTs[adjusted_scene_index].getImageName(); String ts = quadCLTs[adjusted_scene_index].getImageName();
if (ts == null) { if ((ts == null) || (ers_reference.getSceneXYZATR(ts) == null)){
break; break;
} }
if (adjusted_scene_index < (min_num_scenes_half-1)) { if (adjusted_scene_index < (min_num_scenes_half-1)) {
...@@ -211,6 +212,7 @@ public class Interscene { ...@@ -211,6 +212,7 @@ public class Interscene {
// TODO: add ref scene itself to the list? // TODO: add ref scene itself to the list?
cent_index = setInitialOrientationsIms( cent_index = setInitialOrientationsIms(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes_half, // int min_num_scenes, min_num_scenes_half, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters, colorProcParameters, // final ColorProcParameters colorProcParameters,
quadCLTs, // final QuadCLT[] quadCLTs, // quadCLTs, // final QuadCLT[] quadCLTs, //
...@@ -264,6 +266,7 @@ public class Interscene { ...@@ -264,6 +266,7 @@ public class Interscene {
int [] start_ref_pointers2 = new int[2]; int [] start_ref_pointers2 = new int[2];
int earliest_scene2 = setInitialOrientationsIms( int earliest_scene2 = setInitialOrientationsIms(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes_half, // int min_num_scenes, min_num_scenes_half, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters, colorProcParameters, // final ColorProcParameters colorProcParameters,
quadCLTs, // final QuadCLT[] quadCLTs, // quadCLTs, // final QuadCLT[] quadCLTs, //
...@@ -327,9 +330,10 @@ public class Interscene { ...@@ -327,9 +330,10 @@ public class Interscene {
return earliest_scene2; return earliest_scene2;
} }
@Deprecated
public static int setInitialOrientationsIms( public static int setInitialOrientationsImsOld( // tested version, will remove
final CLTParameters clt_parameters, final CLTParameters clt_parameters,
final boolean compensate_ims_rotation, // not used here
int min_num_scenes, int min_num_scenes,
final ColorProcParameters colorProcParameters, final ColorProcParameters colorProcParameters,
final QuadCLT[] quadCLTs, // final QuadCLT[] quadCLTs, //
...@@ -341,21 +345,14 @@ public class Interscene { ...@@ -341,21 +345,14 @@ public class Interscene {
final int threadsMax, // int threadsMax, final int threadsMax, // int threadsMax,
final boolean updateStatus, final boolean updateStatus,
final int debugLevel) { final int debugLevel) {
// double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
double maximal_series_rms = 0.0; double maximal_series_rms = 0.0;
double min_ref_str = clt_parameters.imp.min_ref_str; double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma; boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
// boolean ref_need_lma_combo = clt_parameters.imp.ref_need_lma_combo;
double min_ref_frac= clt_parameters.imp.min_ref_frac; double min_ref_frac= clt_parameters.imp.min_ref_frac;
// int min_num_scenes = clt_parameters.imp.min_num_scenes; // abandon series if there are less than this number of scenes in it
int max_num_scenes = clt_parameters.imp.max_num_scenes; // cut longer series int max_num_scenes = clt_parameters.imp.max_num_scenes; // cut longer series
boolean ims_use = clt_parameters.imp.ims_use; boolean ims_use = clt_parameters.imp.ims_use;
double [] ims_ortho = clt_parameters.imp.ims_ortho; double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
// double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] quat_ims_cam = Imx5.quaternionImsToCam(
// ims_mount_atr, // new double[] {0, 0.13, 0},
// ims_ortho);
double boost_max_short = 2.0; // double boost_max_short = 2.0; //
double boost_zoom_short = 1.5; // double boost_zoom_short = 1.5; //
...@@ -430,13 +427,16 @@ public class Interscene { ...@@ -430,13 +427,16 @@ public class Interscene {
} }
} }
boolean scale_ims_velocities = true;
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
scale_ims_velocities); // boolean scale)
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
boolean after_spiral = false; boolean after_spiral = false;
boolean got_spiral = false; boolean got_spiral = false;
// int search_rad = clt_parameters.imp.search_rad; // 10;
boolean scale_ims_velocities = true;
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2; Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
// apply correction to orientation - better not to prevent extra loops. // apply correction to orientation - better not to prevent extra loops.
// And it only applies to rotations // And it only applies to rotations
...@@ -446,19 +446,17 @@ public class Interscene { ...@@ -446,19 +446,17 @@ public class Interscene {
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu); double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu}; double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][] last_corr_xyzatr = {ZERO3,ZERO3}; double [][] last_corr_xyzatr = {ZERO3,ZERO3};
double [][] cam_dxyzatr_ref = quadCLTs[ref_index].getDxyzatrIms(
clt_parameters,
scale_ims_velocities); // scale IMS velocities
ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
scenes_xyzatr[ref_index][0], scenes_xyzatr[ref_index][0],
scenes_xyzatr[ref_index][1], scenes_xyzatr[ref_index][1],
cam_dxyzatr_ref[0], // later may overwrite from overlap scenes_dxyzatr[ref_index][0], // cam_dxyzatr_ref[0], // later may overwrite from overlap
cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt() scenes_dxyzatr[ref_index][1]); //cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt()
// Will be used in prepareLMA() // Will be used in prepareLMA()
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!) quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs( QuadCLTCPU.scaleDtToErs(
clt_parameters, clt_parameters,
cam_dxyzatr_ref)); scenes_dxyzatr[ref_index])); // cam_dxyzatr_ref));
// Trying new version with motion blur and single-setting of the reference frame // Trying new version with motion blur and single-setting of the reference frame
boolean mb_en = clt_parameters.imp.mb_en; boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
...@@ -534,14 +532,11 @@ public class Interscene { ...@@ -534,14 +532,11 @@ public class Interscene {
" pixels"); " pixels");
} }
} }
double [][] cam_dxyzatr = quadCLTs[scene_index].getDxyzatrIms(
clt_parameters,
scale_ims_velocities); // scale IMS velocities
// Will be used in prepareLMA() // Will be used in prepareLMA()
quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!) quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs( QuadCLTCPU.scaleDtToErs(
clt_parameters, clt_parameters,
cam_dxyzatr)); scenes_dxyzatr[scene_index])); // cam_dxyzatr));
/* /*
// Does not use motion blur for reference scene here! // Does not use motion blur for reference scene here!
scenes_xyzatr[scene_index] = adjustPairsLMAInterscene( scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
...@@ -719,7 +714,6 @@ public class Interscene { ...@@ -719,7 +714,6 @@ public class Interscene {
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!) quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt, dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt,
dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt); dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
// for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
for (int scene_index = ref_index; scene_index >= earliest_scene ; scene_index--) { for (int scene_index = ref_index; scene_index >= earliest_scene ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName(), ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scenes_xyzatr[scene_index][0], scenes_xyzatr[scene_index][0],
...@@ -737,10 +731,9 @@ public class Interscene { ...@@ -737,10 +731,9 @@ public class Interscene {
return earliest_scene; return earliest_scene;
} }
public static int setInitialOrientationsIms(
public static int setInitialOrientations(
final CLTParameters clt_parameters, final CLTParameters clt_parameters,
final boolean compensate_ims_rotation,
int min_num_scenes, int min_num_scenes,
final ColorProcParameters colorProcParameters, final ColorProcParameters colorProcParameters,
final QuadCLT[] quadCLTs, // final QuadCLT[] quadCLTs, //
...@@ -752,28 +745,28 @@ public class Interscene { ...@@ -752,28 +745,28 @@ public class Interscene {
final int threadsMax, // int threadsMax, final int threadsMax, // int threadsMax,
final boolean updateStatus, final boolean updateStatus,
final int debugLevel) { final int debugLevel) {
int num_scene_retries = 10;
int retry_radius = 2;
double scale_extrap_atr = clt_parameters.imp.scale_extrap_atr;
double scale_extrap_xyz = clt_parameters.imp.scale_extrap_xyz;
int avg_len = clt_parameters.imp.avg_len;
double maximal_series_rms = 0.0; double maximal_series_rms = 0.0;
double min_ref_str = clt_parameters.imp.min_ref_str; double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma; boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
double min_ref_frac= clt_parameters.imp.min_ref_frac; double min_ref_frac= clt_parameters.imp.min_ref_frac;
// int min_num_scenes = clt_parameters.imp.min_num_scenes; // abandon series if there are less than this number of scenes in it
int max_num_scenes = clt_parameters.imp.max_num_scenes; // cut longer series int max_num_scenes = clt_parameters.imp.max_num_scenes; // cut longer series
boolean ims_use = clt_parameters.imp.ims_use;
double boost_max_short = 2.0; // double boost_max_short = 2.0; //
double boost_zoom_short = 1.5; // double boost_zoom_short = 1.5; //
if (!ims_use) {
System.out.println("setInitialOrientationsIms(): IMS use is disabled");
return -1;
}
if (clt_parameters.ilp.ilma_3d) {
System.out.println("*** setInitialOrientationsIms(): clt_parameters.ilp.ilma_3d is TRUE. You may want to disable it when using IMS");
}
double [] lma_rms = new double[2]; double [] lma_rms = new double[2];
double [] use_atr = null;
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX(); int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
// int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY(); int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize(); int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
double min_offset = 0.0; // clt_parameters.imp.min_offset; double min_offset = 0.0; // clt_parameters.imp.min_offset;
double max_offset = clt_parameters.imp.max_rel_offset * tilesX * tile_size; double max_offset = clt_parameters.imp.max_rel_offset * tilesX * tile_size;
...@@ -792,19 +785,22 @@ public class Interscene { ...@@ -792,19 +785,22 @@ public class Interscene {
threadsMax, threadsMax,
debugLevel-2); debugLevel-2);
} // split cycles to remove output clutter } // split cycles to remove output clutter
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection(); ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
int debug_scene = -15; int debug_scene = -15;
boolean debug2 = !batch_mode; // false; // true; boolean debug2 = !batch_mode; // false; // true;
boolean [] reliable_ref = null; boolean [] reliable_ref = null;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi; boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [] reduced_strength = new double[1]; double [] reduced_strength = new double[1];
// use combo if second pass?
if (min_ref_str > 0.0) { if (min_ref_str > 0.0) {
reliable_ref = quadCLTs[ref_index].getReliableTiles( // will be null if does not exist. reliable_ref = quadCLTs[ref_index].getReliableTiles( // will be null if does not exist.
false, // boolean use_combo, false, // boolean use_combo,
min_ref_str, // double min_strength, min_ref_str, // double min_strength,
min_ref_frac, // double min_ref_frac, min_ref_frac, // double min_ref_frac,
ref_need_lma, // boolean needs_lma); ref_need_lma, // boolean needs_lma);
true, // boolean needs_lma_combo); true, // ref_need_lma_combo, // boolean needs_lma_combo);
reduced_strength); // if not null will return >0 if had to reduce strength (no change if did not reduce) reduced_strength); // if not null will return >0 if had to reduce strength (no change if did not reduce)
if (reduced_strength[0] > 0) { if (reduced_strength[0] > 0) {
use_lma_dsi = false; // too few points use_lma_dsi = false; // too few points
...@@ -831,17 +827,43 @@ public class Interscene { ...@@ -831,17 +827,43 @@ public class Interscene {
} }
} }
boolean scale_ims_velocities = true;
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
scale_ims_velocities); // boolean scale)
double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; //
// quat_corr may still be null if does not exist
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
quat_corr, // double [] quat_corr, // only applies to rotations - verify!
debugLevel) ; // int debugLevel)
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
boolean after_spiral = false; boolean after_spiral = false;
boolean got_spiral = false; boolean got_spiral = false;
int search_rad = clt_parameters.imp.search_rad; // 10; double [][] last_corr_xyzatr = {ZERO3,ZERO3};
ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
scenes_xyzatr[ref_index][0], scenes_xyzatr[ref_index][0],
scenes_xyzatr[ref_index][1], scenes_xyzatr[ref_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(), scenes_dxyzatr[ref_index][0], // cam_dxyzatr_ref[0], // later may overwrite from overlap
ZERO3); // ers_scene.getErsATR_dt() scenes_dxyzatr[ref_index][1]); //cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt()
// Will be used in prepareLMA()
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
scenes_dxyzatr[ref_index])); // cam_dxyzatr_ref));
// Trying new version with motion blur and single-setting of the reference frame
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
double mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
TpTask[][] tp_tasks_ref = new TpTask[2][];
double [][] pXpYD_ref = new double [tilesX*tilesY][];
for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) { for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
if ((ref_index - scene_index) >= max_num_scenes){ if ((ref_index - scene_index) >= max_num_scenes){
earliest_scene = scene_index + 1; earliest_scene = scene_index + 1;
...@@ -859,74 +881,18 @@ public class Interscene { ...@@ -859,74 +881,18 @@ public class Interscene {
System.out.println("scene_index = "+scene_index); System.out.println("scene_index = "+scene_index);
} }
QuadCLT scene_QuadClt = quadCLTs[scene_index]; QuadCLT scene_QuadClt = quadCLTs[scene_index];
// get initial xyzatr: double [][] pose_ims = ims_xyzatr[scene_index];
// TODO:repeat spiralSearchATR if found scene was too close
// if (scene_index == ref_index - 1) { // search around for the best fit
if (!got_spiral) { // search around for the best fit
use_atr = spiralSearchATR(
clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // may be turned off by the caller if there are too few points
search_rad, // int search_rad
quadCLTs[ref_index], // QuadCLT reference_QuadClt,
scene_QuadClt, // QuadCLT scene_QuadClt,
reliable_ref, // ********* boolean [] reliable_ref,
debugLevel);
if (use_atr == null) {
if (num_scene_retries-- > 0) {
search_rad = retry_radius; // faster, no need/sense to look far
continue;
}
earliest_scene = scene_index + 1;
if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" spiralSearchATR() FAILED. Setting earliest_scene to "+earliest_scene);
}
// set this and all previous to null
for (; scene_index >= 0 ; scene_index--) {
if (quadCLTs[scene_index] == null) {
System.out.println("setInitialOrientations(): quadCLTs["+scene_index+"] is already null!");
} else {
ers_reference.addScene(quadCLTs[scene_index].getImageName()); // remove
}
}
break; // failed with even first before reference
}
after_spiral = true;
got_spiral = true;
scenes_xyzatr[scene_index] = new double [][] {new double[3], use_atr};
} else { // assume linear motion
after_spiral = false;
if (scene_index == debug_scene) {
System.out.println("adjusting orientation, scene_index="+scene_index);
System.out.println("adjusting orientation, scene_index="+scene_index);
}
int na = avg_len;
if ((scene_index + 1 + na) > ref_index) {
na = ref_index - (scene_index + 1);
}
double [][] last_diff = ErsCorrection.combineXYZATR( // Apply correction from the earlier processed scenes (closer to reference)
scenes_xyzatr[scene_index + 1], double [][] initial_pose = ErsCorrection.combineXYZATR(
ErsCorrection.invertXYZATR(scenes_xyzatr[scene_index+1 + na])); last_corr_xyzatr, // current correction
for (int i = 0; i < 3; i++) { pose_ims); // raw IMS prediction
last_diff[0][i] /= na; scenes_xyzatr[scene_index] = new double [][]{
last_diff[1][i] /= na; initial_pose[0].clone(),
} initial_pose[1].clone()
for (int i = 0; i < 3; i++) { };
last_diff[0][i] *= scale_extrap_xyz;
last_diff[1][i] *= scale_extrap_atr;
}
scenes_xyzatr[scene_index] = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index+1],
last_diff);
}
// Refine with LMA // Refine with LMA
double [][] initial_pose = new double[][]{ double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
scenes_xyzatr[scene_index][0].clone(),
scenes_xyzatr[scene_index][1].clone()};
boolean rot_to_transl = after_spiral && clt_parameters.ilp.ilma_translation_priority;
double [] reg_weights = rot_to_transl? clt_parameters.ilp.ilma_regularization_weights0 : clt_parameters.ilp.ilma_regularization_weights;
min_max[1] = max_offset; min_max[1] = max_offset;
//boost_zoom_short //boost_zoom_short
double max_z_change_scaled = max_z_change; double max_z_change_scaled = max_z_change;
...@@ -950,27 +916,39 @@ public class Interscene { ...@@ -950,27 +916,39 @@ public class Interscene {
" pixels"); " pixels");
} }
} }
// Does not use motion blur for reference scene here! // Will be used in prepareLMA()
scenes_xyzatr[scene_index] = adjustPairsLMAInterscene( quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
scenes_dxyzatr[scene_index])); // cam_dxyzatr));
// Trying new version with motion blur and single-setting of the reference frame
scenes_xyzatr[scene_index] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // clt_parameters.imp.use_lma_dsi, use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers, true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs[ref_index], // QuadCLT reference_QuadCLT, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
tp_tasks_ref, // TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
pXpYD_ref, // double [][] pXpYD_ref, // should be se or at least double [num_tiles][] if tp_tasks_ref[0] == null
ref_index, // int nscene0, // may be == ref_index
scene_index, // int nscene1, // compares to nscene0
null, // double [] ref_disparity, // null or alternative reference disparity null, // double [] ref_disparity, // null or alternative reference disparity
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles reliable_ref, //boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scene_QuadClt, // QuadCLT scene_QuadCLT, scenes_xyzatr[ref_index], // double [][] scene0_xyzatr,,
initial_pose[0], // xyz initial_pose, // double [][] scene1_xyzatr,
initial_pose[1], // atr initial_pose, // double [] scene1_xyzatr_pull, // if both are not null, specify target values to pull to
initial_pose[0], // double [] scene_xyz_pull, // if both are not null, specify target values to pull to clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
rot_to_transl? (new double[3]):initial_pose[1], // double [] scene_atr_pull, // reg_weights, // double [] param_regweights,
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select, lma_rms, // double [] rms_out, // null or double [2]
reg_weights, // clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
lma_rms, // double [] rms, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms, clt_parameters.imp.max_rms, // double max_rms,
clt_parameters.imp.debug_level); // 1); // -1); // int debug_level); mb_en, // boolean mb_en,
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
clt_parameters.imp.debug_level); // int debugLevel);
boolean adjust_OK = scenes_xyzatr[scene_index] != null; boolean adjust_OK = scenes_xyzatr[scene_index] != null;
if (adjust_OK) { // check only for initial orientation, do not check on readjustments if (adjust_OK) { // check only for initial orientation, do not check on readjustments
...@@ -994,7 +972,6 @@ public class Interscene { ...@@ -994,7 +972,6 @@ public class Interscene {
// FAIL_REASON_ROLL // FAIL_REASON_ROLL
handle_failure: { handle_failure: {
if (!adjust_OK) { if (!adjust_OK) {
// boolean OK = false;
System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+ System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")"); " ("+getFailReason(fail_reason[0])+")");
if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) { if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
...@@ -1023,6 +1000,14 @@ public class Interscene { ...@@ -1023,6 +1000,14 @@ public class Interscene {
break; break;
} }
} }
// update last_corr_xyzatr, // current correction
double [][] corr_diff = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index], // new
ErsCorrection.invertXYZATR(initial_pose)); //old
last_corr_xyzatr = ErsCorrection.combineXYZATR (
last_corr_xyzatr,
corr_diff);
// not used here with IMS: after_spiral == false
if (after_spiral && (scene_index < (ref_index-1))) { // need to interpolate skipped scenes if (after_spiral && (scene_index < (ref_index-1))) { // need to interpolate skipped scenes
// here we expect very small translations/angles, so linear scaling is OK // here we expect very small translations/angles, so linear scaling is OK
double s = 1.0 / (ref_index - scene_index); double s = 1.0 / (ref_index - scene_index);
...@@ -1043,158 +1028,1033 @@ public class Interscene { ...@@ -1043,158 +1028,1033 @@ public class Interscene {
} }
} }
/* old version double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
if (scenes_xyzatr[scene_index] == null) { clt_parameters,
earliest_scene = scene_index + 1; quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
));
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),
adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()
);
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
}
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+ System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+ quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" FAILED. Setting earliest_scene to "+earliest_scene); " Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
}
} // for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--)
if ((ref_index - earliest_scene + 1) < min_num_scenes) {
System.out.println("Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
" < "+min_num_scenes+". Scrapping this series.");
if (start_ref_pointers != null) {
start_ref_pointers[0] = earliest_scene;
}
return -1;
}
if (earliest_scene > 0) {
System.out.println("setInitialOrientationsIms(): Not all scenes matched, earliest useful scene = "+earliest_scene+
" (total number of scenes = "+(ref_index - earliest_scene + 1)+
").Maximal RMSE was "+maximal_series_rms);
} else if (debugLevel > -4) {
System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
}
// Set ERS data - now old way, change to IMS data (and apply to adjustPairsLMAInterscene())
double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
ref_index, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt,
dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
for (int scene_index = ref_index; scene_index >= earliest_scene ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
dxyzatr_dt[scene_index][0], // ers_scene.getErsXYZ_dt(),
dxyzatr_dt[scene_index][1] // ers_scene.getErsATR_dt()
);
}
quadCLTs[ref_index].set_orient(1); // first orientation
quadCLTs[ref_index].set_accum(0); // reset accumulations ("build_interscene") number
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
return earliest_scene;
}
public static int setInitialOrientations(
final CLTParameters clt_parameters,
int min_num_scenes,
final ColorProcParameters colorProcParameters,
final QuadCLT[] quadCLTs, //
final int ref_index,
final QuadCLT.SetChannels [] set_channels,
final boolean batch_mode,
int earliest_scene,
int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index
final int threadsMax, // int threadsMax,
final boolean updateStatus,
final int debugLevel) {
int num_scene_retries = 10;
int retry_radius = 2;
double scale_extrap_atr = clt_parameters.imp.scale_extrap_atr;
double scale_extrap_xyz = clt_parameters.imp.scale_extrap_xyz;
int avg_len = clt_parameters.imp.avg_len;
double maximal_series_rms = 0.0;
double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
double min_ref_frac= clt_parameters.imp.min_ref_frac;
// int min_num_scenes = clt_parameters.imp.min_num_scenes; // abandon series if there are less than this number of scenes in it
int max_num_scenes = clt_parameters.imp.max_num_scenes; // cut longer series
double boost_max_short = 2.0; //
double boost_zoom_short = 1.5; //
double [] lma_rms = new double[2];
double [] use_atr = null;
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 = 0.0; // clt_parameters.imp.min_offset;
double max_offset = clt_parameters.imp.max_rel_offset * tilesX * tile_size;
double max_roll = clt_parameters.imp.max_roll_deg*Math.PI/180.0;
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
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
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--) {
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT(
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
} // split cycles to remove output clutter
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
int debug_scene = -15;
boolean debug2 = !batch_mode; // false; // true;
boolean [] reliable_ref = null;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [] reduced_strength = new double[1];
if (min_ref_str > 0.0) {
reliable_ref = quadCLTs[ref_index].getReliableTiles( // will be null if does not exist.
false, // boolean use_combo,
min_ref_str, // double min_strength,
min_ref_frac, // double min_ref_frac,
ref_need_lma, // boolean needs_lma);
true, // boolean needs_lma_combo);
reduced_strength); // if not null will return >0 if had to reduce strength (no change if did not reduce)
if (reduced_strength[0] > 0) {
use_lma_dsi = false; // too few points
}
if (debug2) {
double [] dbg_img = new double [reliable_ref.length];
for (int i = 0; i < dbg_img.length; i++) {
dbg_img[i] = reliable_ref[i]?1:0;
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
quadCLTs[ref_index].getTileProcessor().getTilesX(),
quadCLTs[ref_index].getTileProcessor().getTilesY(),
"reliable_ref");
}
}
double max_z_change = Double.NaN; // only applicable for drone images
if (max_zoom_diff > 0) { // ignore if set to
double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
max_z_change = avg_z * max_zoom_diff;
if (debugLevel > -3) {
System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
}
}
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
boolean after_spiral = false;
boolean got_spiral = false;
int search_rad = clt_parameters.imp.search_rad; // 10;
ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
scenes_xyzatr[ref_index][0],
scenes_xyzatr[ref_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3); // ers_scene.getErsATR_dt()
for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
if ((ref_index - scene_index) >= max_num_scenes){
earliest_scene = scene_index + 1;
if (debugLevel > -3) {
System.out.println("Cutting too long series at scene "+scene_index+" (of "+ quadCLTs.length+". excluding) ");
}
// set this and all previous to null
for (; scene_index >= 0 ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName()); // remove
}
break;
}
if (scene_index == debug_scene) {
System.out.println("scene_index = "+scene_index);
System.out.println("scene_index = "+scene_index);
}
QuadCLT scene_QuadClt = quadCLTs[scene_index];
// get initial xyzatr:
// TODO:repeat spiralSearchATR if found scene was too close
// if (scene_index == ref_index - 1) { // search around for the best fit
if (!got_spiral) { // search around for the best fit
use_atr = spiralSearchATR(
clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // may be turned off by the caller if there are too few points
search_rad, // int search_rad
quadCLTs[ref_index], // QuadCLT reference_QuadClt,
scene_QuadClt, // QuadCLT scene_QuadClt,
reliable_ref, // ********* boolean [] reliable_ref,
debugLevel);
if (use_atr == null) {
if (num_scene_retries-- > 0) {
search_rad = retry_radius; // faster, no need/sense to look far
continue;
}
earliest_scene = scene_index + 1;
if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" spiralSearchATR() FAILED. Setting earliest_scene to "+earliest_scene);
}
// set this and all previous to null
for (; scene_index >= 0 ; scene_index--) {
if (quadCLTs[scene_index] == null) {
System.out.println("setInitialOrientations(): quadCLTs["+scene_index+"] is already null!");
} else {
ers_reference.addScene(quadCLTs[scene_index].getImageName()); // remove
}
}
break; // failed with even first before reference
}
after_spiral = true;
got_spiral = true;
scenes_xyzatr[scene_index] = new double [][] {new double[3], use_atr};
} else { // assume linear motion
after_spiral = false;
if (scene_index == debug_scene) {
System.out.println("adjusting orientation, scene_index="+scene_index);
System.out.println("adjusting orientation, scene_index="+scene_index);
}
int na = avg_len;
if ((scene_index + 1 + na) > ref_index) {
na = ref_index - (scene_index + 1);
}
double [][] last_diff = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index + 1],
ErsCorrection.invertXYZATR(scenes_xyzatr[scene_index+1 + na]));
for (int i = 0; i < 3; i++) {
last_diff[0][i] /= na;
last_diff[1][i] /= na;
}
for (int i = 0; i < 3; i++) {
last_diff[0][i] *= scale_extrap_xyz;
last_diff[1][i] *= scale_extrap_atr;
}
scenes_xyzatr[scene_index] = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index+1],
last_diff);
}
// Refine with LMA
double [][] initial_pose = new double[][]{
scenes_xyzatr[scene_index][0].clone(),
scenes_xyzatr[scene_index][1].clone()};
boolean rot_to_transl = after_spiral && clt_parameters.ilp.ilma_translation_priority;
double [] reg_weights = rot_to_transl? clt_parameters.ilp.ilma_regularization_weights0 : clt_parameters.ilp.ilma_regularization_weights;
min_max[1] = max_offset;
//boost_zoom_short
double max_z_change_scaled = max_z_change;
if ((ref_index - scene_index) < min_num_scenes) {
min_max[1] = boost_max_short * max_offset;
max_z_change_scaled = max_z_change * boost_zoom_short;
if (debugLevel > -3) {
System.out.println("As the current series ("+(ref_index - scene_index)+
" scenes) is shorter than minimal ("+min_num_scenes+"), the maximal shift is scaled by "+
boost_max_short + " to " + (boost_max_short * max_offset)+
" pixels");
}
}
if ((scene_index - earliest_scene) < min_num_scenes) {
min_max[1] = boost_max_short * max_offset;
max_z_change_scaled = max_z_change * boost_zoom_short;
if (debugLevel > -3) {
System.out.println("As the remaining number of scenes to process ("+(scene_index - earliest_scene + 1)+
") is less than minimal ("+min_num_scenes+"), the maximal shift is scaled by "+
boost_max_short + " to " + (boost_max_short * max_offset)+
" pixels");
}
}
// Does not use motion blur for reference scene here!
scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs[ref_index], // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scene_QuadClt, // QuadCLT scene_QuadCLT,
initial_pose[0], // xyz
initial_pose[1], // atr
initial_pose[0], // double [] scene_xyz_pull, // if both are not null, specify target values to pull to
rot_to_transl? (new double[3]):initial_pose[1], // double [] scene_atr_pull, //
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
reg_weights, // clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
lma_rms, // double [] rms, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms,
clt_parameters.imp.debug_level); // 1); // -1); // int debug_level);
boolean adjust_OK = scenes_xyzatr[scene_index] != null;
if (adjust_OK) { // check only for initial orientation, do not check on readjustments
if (Math.abs(scenes_xyzatr[scene_index][1][2]) > max_roll) {
fail_reason[0] = FAIL_REASON_ROLL;
adjust_OK = false;
}
if (max_zoom_diff > 0) { // ignore if set to
if (Math.abs(scenes_xyzatr[scene_index][0][2]) > max_z_change) {
if (Math.abs(scenes_xyzatr[scene_index][0][2]) > max_z_change_scaled) {
fail_reason[0] = FAIL_REASON_ZOOM;
adjust_OK = false;
} else {
System.out.println("Z-change "+Math.abs(scenes_xyzatr[scene_index][0][2])+
"m exceeds limit of "+max_z_change+"m, but as it is beginning/end of the series, limit is raised to "+
max_z_change_scaled+"m");
}
}
}
}
// FAIL_REASON_ROLL
handle_failure: {
if (!adjust_OK) {
// boolean OK = false;
System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
if (fpn_skip) {
System.out.println("fpn_skip is set, just using initial pose");
scenes_xyzatr[scene_index] = initial_pose;
break handle_failure; // to next scene
} else {
System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
// set this and all previous to null
}
}
// all other reasons lead to failure
earliest_scene = scene_index + 1;
if (debugLevel > -4) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
}
// set this and all previous to null
for (; scene_index >= 0 ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName()); // remove
// quadCLTs[scene_index] = null; // completely remove early scenes?
}
break;
}
}
if (after_spiral && (scene_index < (ref_index-1))) { // need to interpolate skipped scenes
// here we expect very small translations/angles, so linear scaling is OK
double s = 1.0 / (ref_index - scene_index);
for (int interp_index = ref_index-1; interp_index > scene_index; interp_index--) {
scenes_xyzatr[interp_index] = new double[2][3];
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
scenes_xyzatr[interp_index][i][j] =
scenes_xyzatr[scene_index][i][j] * s *(interp_index - scene_index);
}
}
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr[interp_index][0],
scenes_xyzatr[interp_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3 // ers_scene.getErsATR_dt()
);
}
}
/* old version
if (scenes_xyzatr[scene_index] == null) {
earliest_scene = scene_index + 1;
if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" FAILED. Setting earliest_scene to "+earliest_scene);
}
// set this and all previous to null
for (; scene_index >= 0 ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName(), null);
}
break;
}
*/
// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3 // ers_scene.getErsATR_dt()
);
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
}
if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
}
} // for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--)
if ((ref_index - earliest_scene + 1) < min_num_scenes) {
System.out.println("Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
" < "+min_num_scenes+". Scrapping this series.");
if (start_ref_pointers != null) {
start_ref_pointers[0] = earliest_scene;
}
return -1;
}
if (earliest_scene > 0) {
System.out.println("setInitialOrientations(): Not all scenes matched, earliest useful scene = "+earliest_scene+
" (total number of scenes = "+(ref_index - earliest_scene + 1)+
").Maximal RMSE was "+maximal_series_rms);
} else if (debugLevel > -4) {
System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
}
quadCLTs[ref_index].set_orient(1); // first orientation
quadCLTs[ref_index].set_accum(0); // reset accumulations ("build_interscene") number
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
return earliest_scene;
}
public static void readjustShowCorrected(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
double [][] dxyzatr_dt_nscene,
double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
double mb_max_gain,
double [] mb_ref_disparity,
int ref_index,
int nscene,
int debug_scene, // = 68
int debugLevel) {
boolean show_corrected = false;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
if (nscene == debug_scene) {
System.out.println("2.nscene = "+nscene);
System.out.println("3.nscene = "+nscene);
}
dbg_mb_img[nscene] = new double [tilesX*tilesY*2];
Arrays.fill(dbg_mb_img[nscene],Double.NaN);
String ts = quadCLTs[nscene].getImageName();
double [] mb_scene_xyz = (nscene != ref_index)? ers_reference.getSceneXYZ(ts):ZERO3;
double [] mb_scene_atr = (nscene != ref_index)? ers_reference.getSceneATR(ts):ZERO3;
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
mb_ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene_QuadClt,
quadCLTs[ref_index]); // final QuadCLT reference_QuadClt)
double [][] motion_blur = OpticalFlow.getMotionBlur( // dxyzatr_dt[][] should not be scaled here
quadCLTs[ref_index], // QuadCLT ref_scene,
quadCLTs[nscene], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
mb_scene_xyz, // double [] camera_xyz,
mb_scene_atr, // double [] camera_atr,
dxyzatr_dt_nscene[0], // double [] camera_xyz_dt,
dxyzatr_dt_nscene[1], // double [] camera_atr_dt,
-1, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
for (int nTile = 0; nTile < motion_blur[0].length; nTile++) {
int tx = nTile % tilesX;
int ty = nTile / tilesX;
dbg_mb_img[nscene][tx + tilesX * (ty*2 +0)] = mb_tau * motion_blur[0][nTile];
dbg_mb_img[nscene][tx + tilesX * (ty*2 +1)] = mb_tau * motion_blur[1][nTile];
}
while (show_corrected) {
ImagePlus imp_mbc = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
false, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc.show();
ImagePlus imp_mbc_merged = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
true, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED-MERGED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc_merged.show();
show_corrected = false; // manually set/reset
}
}
@Deprecated
// Old working version
public static int reAdjustPairsLMAIntersceneOLd( // after combo dgi is available and preliminary poses are known
CLTParameters clt_parameters,
double mb_max_gain,
boolean disable_ers,
boolean [] reliable_ref, // null or bitmask of reliable reference tiles
QuadCLT [] quadCLTs,
int ref_index,
int [] range,
int ers_mode, // 0 - keep, 1 - set from velocity, 2 - set from IMS
boolean test_motion_blur,
int debugLevel)
{
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
boolean fail_on_zoom_roll=false; // it should fail on initial adjustment
int avg_len = clt_parameters.imp.avg_len;
// boolean test_motion_blur = true;//false
// Set up velocities from known coordinates, use averaging
double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
boolean debug_ers = clt_parameters.ilp.ilma_debug_ers ; // true;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // for offset estimation
String dbg_ers_string =
((dbg_scale_dt[1][0] > 0)?"a":((dbg_scale_dt[1][0] < 0) ? "A":"0"))+
((dbg_scale_dt[1][1] > 0)?"t":((dbg_scale_dt[1][1] < 0) ? "T":"0"))+
((dbg_scale_dt[1][2] > 0)?"r":((dbg_scale_dt[1][2] < 0) ? "R":"0"))+
((dbg_scale_dt[0][0] > 0)?"x":((dbg_scale_dt[0][0] < 0) ? "X":"0"))+
((dbg_scale_dt[0][1] > 0)?"y":((dbg_scale_dt[0][1] < 0) ? "Y":"0"))+
((dbg_scale_dt[0][2] > 0)?"z":((dbg_scale_dt[0][2] < 0) ? "Z":"0"));
// boolean ers_readjust_enable = true; // debugging ers during readjust 11.02.2022
// boolean ers_use_xyz = false; // true; // false; // enable when ready 11.02.2022
// int [][] dbg_scale_dt = {{0,0,0},{0, 0, 0}}; // no_ers - causes singular matrices (not alwasy), but works
// int [][] dbg_scale_dt = {{0,0,0},{1,-1, 1}}; // aTr
// int [][] dbg_scale_dt = {{0,0,0},{1, 1,-1}}; // atR
// int [][] dbg_scale_dt = {{0,0,0},{1, 1, 1}}; // atr
// int [][] dbg_scale_dt = {{1,1,1},{1, 1, 1}}; // atrxyz
// int [][] dbg_scale_dt = {{1,1,1},{1, 1,-1}}; // atRxyz
// int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,1}}; // atrXYZ
// int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,-1}}; // atRXYZ
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
// double mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
int margin = clt_parameters.imp.margin;
double second_margin = 1.1; // allow slightly larger offset for readjsutments (to be limited by the first pass)
double boost_max_short = 5.0; // 2.0; // Some bug?
int earliest_scene = range[0];
int last_scene = range[1];
boolean use_combo_dsi = clt_parameters.imp.use_combo_dsi;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
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;
double max_offset = boost_max_short*second_margin*clt_parameters.imp.max_rel_offset * tilesX * tile_size;
double max_roll = second_margin*clt_parameters.imp.max_roll_deg*Math.PI/180.0;
double max_zoom_diff = second_margin*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
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
System.out.println("reAdjustPairsLMAInterscene(): min_offset="+min_max[0]+", max_offset="+min_max[1]);
double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity);
/// double [][] combo_dsn_final = quadCLTs[ref_index].readDoubleArrayFromModelDirectory(
/// "-INTER-INTRA-LMA", // String suffix,
/// 0, // int num_slices, // (0 - all)
/// null); // int [] wh);
double [][] combo_dsn_final = quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
quadCLTs[ref_index], // QuadCLT scene,
debugLevel);
// double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
}
double [][] ref_pXpYD = null;
double [][] dbg_mb_img = null;
double [] mb_ref_disparity =null;
// if (test_motion_blur) {
mb_ref_disparity = interscene_ref_disparity;
if (mb_ref_disparity == null) {
mb_ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
}
// ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
mb_ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene_QuadClt,
quadCLTs[ref_index]); // final QuadCLT reference_QuadClt)
if (test_motion_blur) {
dbg_mb_img = new double[quadCLTs.length][];
}
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
double [][][] dxyzatr_dt = new double [quadCLTs.length][][];
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
// should have at least next or previous non-null
int debug_scene = 69; // -68; // -8;
double maximal_series_rms = 0.0;
double [][] mb_vectors_ref = null;
TpTask[][] tp_tasks_ref = null;
if (debug_ers) {
System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
}
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
if (nscene == debug_scene) {
System.out.println("nscene = "+nscene);
System.out.println("nscene = "+nscene);
}
// just checking it is not isolated
if (quadCLTs[nscene] == null) {
earliest_scene = nscene + 1;
break;
}
if (nscene != ref_index) {
String ts = quadCLTs[nscene].getImageName();
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
earliest_scene = nscene + 1;
break;
}
scenes_xyzatr[nscene] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
}
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
if (debug_ers) {
// All 4 variants - just for testing. Normally should try center first and use
// quad if there is no significant movement
double est_center = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
false); // boolean rectilinear)
double est_quad = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
false); // boolean rectilinear)
double est_center_rectilinear = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
true); // boolean rectilinear)
double est_quad_rectilinear = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
true); // boolean rectilinear)
System.out.println(String.format("%3d xyz: %9.5f %9.5f %9.5f atr: %9.5f %9.5f %9.5f est_pix: %9.4f %9.4f %9.4f %9.4f",
nscene,
scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2],
// est_pix, est_pix1, est_pix2, est_pix3,
est_center, est_quad, est_center_rectilinear, est_quad_rectilinear));
}
}
switch (ers_mode) {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
break;
default: // do nothing - already read
}
if (debug_ers) {
System.out.println();
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
System.out.println(String.format("%3d xyz_dt: %9.5f %9.5f %9.5f atr_dt: %9.5f %9.5f %9.5f",
nscene,
dxyzatr_dt[nscene][0][0], dxyzatr_dt[nscene][0][1], dxyzatr_dt[nscene][0][2],
dxyzatr_dt[nscene][1][0], dxyzatr_dt[nscene][1][1], dxyzatr_dt[nscene][1][2]));
}
System.out.println();
}
double max_z_change = Double.NaN; // only applicable for drone images
if (max_zoom_diff > 0) { // ignore if set to
max_z_change = avg_z * max_zoom_diff;
if (fail_on_zoom_roll) {
if (debugLevel > -3) {
System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
}
}
}
boolean [] failed_scenes = new boolean[quadCLTs.length];
int num_failed =0;
int [] scene_seq = new int [last_scene-earliest_scene+1];
{
int indx=0;
for (int i = ref_index; i >= earliest_scene; i--) {
scene_seq[indx++] = i;
}
for (int i = ref_index+1; i <= last_scene; i++) {
scene_seq[indx++] = i;
}
}
boolean rectilinear = false;
double max_quad = 10; // use quad if center offset is less than this
for (int nscene:scene_seq) {
if (nscene == debug_scene) {
System.out.println("nscene = "+nscene);
System.out.println("nscene = "+nscene);
}
double est_shift = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
rectilinear); // boolean rectilinear)
if (est_shift < max_quad) {
est_shift = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
rectilinear); // boolean rectilinear)
}
if (debugLevel > -4) {
System.out.println("nscene="+nscene+": est offset "+
est_shift+"pix");
}
// just checking it is not isolated
if ((quadCLTs[nscene] == null) ||
((nscene != ref_index) &&
((ers_reference.getSceneXYZ(quadCLTs[nscene].getImageName())== null) ||
(ers_reference.getSceneATR(quadCLTs[nscene].getImageName())== null)))) {
earliest_scene = nscene + 1;
break;
}
String ts = quadCLTs[nscene].getImageName();
double [] scene_xyz_pre = ZERO3;
double [] scene_atr_pre = ZERO3;
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[nscene]));
if (dbg_mb_img != null) { // fill in 1 slice of dbg_mb_img, show all at once later
readjustShowCorrected(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
dxyzatr_dt[nscene], // double [][] dxyzatr_dt_nscene,
dbg_mb_img, //double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
mb_max_gain, // double mb_max_gain,
mb_ref_disparity, // double [] mb_ref_disparity,
ref_index, // int ref_index,
nscene, // int nscene,
debug_scene, // = 68 int debug_scene, // = 68
debugLevel); // int debugLevel);
}
if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes
mb_vectors_ref = null;
if (mb_en) {
mb_vectors_ref = OpticalFlow.getMotionBlur(
quadCLTs[ref_index], // QuadCLT ref_scene,
quadCLTs[ref_index], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
ZERO3, // double [] camera_xyz,
ZERO3, // double [] camera_atr,
dxyzatr_dt[nscene][0], // double [] camera_xyz_dt,
dxyzatr_dt[nscene][1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
tp_tasks_ref = Interscene.setReferenceGPU (
clt_parameters, // CLTParameters clt_parameters,
quadCLTs[ref_index], // QuadCLT ref_scene,
mb_ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
ref_pXpYD, // double [][] ref_pXpYD,
reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors_ref, // double [][] mb_vectors, // now [2][ntiles];
debugLevel); // int debugLevel)
ers_reference.addScene(ts,
scenes_xyzatr[nscene][0], // ref_index
scenes_xyzatr[nscene][1],
dxyzatr_dt[nscene][0],
dxyzatr_dt[nscene][1]
);
} else { // if (nscene == ref_index)
double [][] mb_vectors = null;
scene_xyz_pre = ers_reference.getSceneXYZ(ts);
scene_atr_pre = ers_reference.getSceneATR(ts);
double [][] scene_xyzatr = new double[][] {scene_xyz_pre, scene_atr_pre};
double [][] inv_scene = ErsCorrection.invertXYZATR(scene_xyzatr);
double [] avg_weights = new double [2*avg_len+1];
int n_pre = nscene + avg_len;
int n_post = nscene - avg_len;
if (n_post < earliest_scene) n_post = earliest_scene;
if (n_pre > last_scene) n_pre = last_scene;
double s0=0, sx=0, sx2=0;
double [][] sy = new double[2][3], sxy = new double [2][3];
for (int n_other = n_post; n_other <= n_pre; n_other++) {
int ix = n_other - nscene; //+/-i
int i = ix + avg_len; // >=0
avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_len + 1));
double [][] other_xyzatr = ers_reference.getSceneXYZATR(quadCLTs[n_other].getImageName());
double [][] other_rel = (n_other != ref_index)?ErsCorrection.combineXYZATR(other_xyzatr,inv_scene):
(new double[2][3]);
double w = avg_weights[i];
s0 += w;
sx += w * ix;
sx2 += w * ix * ix;
for (int j = 0; j < other_rel.length; j++) {
for (int k = 0; k < other_rel[j].length; k++) {
sy [j][k] += w * other_rel[j][k];
sxy[j][k] += w * other_rel[j][k] * ix;
}
}
}
boolean fpn_disable = false; // estimate they are too close
double angle_per_pixel = ers_reference.getCorrVector().getTiltAzPerPixel();
//ers_reference
double [][] diff_pull = new double [2][3];
for (int j = 0; j < diff_pull.length; j++) {
for (int k = 0; k < diff_pull[j].length; k++) {
diff_pull[j][k]=(sy[j][k]*sx2 - sxy[j][k]*sx) / (s0*sx2 - sx*sx);
}
}
double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull);
if (mb_en) {
double [][] dxyzatr_dt_scene = dxyzatr_dt[nscene];
mb_vectors = OpticalFlow.getMotionBlur(
quadCLTs[ref_index], // QuadCLT ref_scene,
quadCLTs[nscene], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
scene_xyz_pre, // double [] camera_xyz,
scene_atr_pre, // double [] camera_atr,
dxyzatr_dt_scene[0], // double [] camera_xyz_dt,
dxyzatr_dt_scene[1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
double [] lma_rms = new double[2];
scenes_xyzatr[nscene] = Interscene.adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters,
false, // boolean initial_adjust,
fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs[ref_index], // QuadCLT reference_QuadCLT,
interscene_ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
ref_pXpYD, // double [][] pXpYD_ref, // pXpYD for the reference scene
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
tp_tasks_ref[0], // TpTask[] tp_tasks_ref, // only (main if MB correction) tasks for FPN correction
quadCLTs[nscene], // QuadCLT scene_QuadCLT,
scene_xyz_pre, // xyz
scene_atr_pre, // atr
scene_pull[0], // double [] scene_xyz_pull, // if both are not null, specify target values to pull to
scene_pull[1], // double [] scene_atr_pull,
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
lma_rms, // double [] rms, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors, // double [][] mb_vectors, // now [2][ntiles];
clt_parameters.imp.debug_level); // 1); // -1); // int debug_level);
boolean adjust_OK = scenes_xyzatr[nscene] != null;
if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments
if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
fail_reason[0] = FAIL_REASON_ROLL;
adjust_OK = false;
}
if (max_zoom_diff > 0) { // ignore if set to
if (Math.abs(scenes_xyzatr[nscene][0][2]) > max_z_change) { // NaN OK - will not fail
fail_reason[0] = FAIL_REASON_ZOOM;
adjust_OK = false;
}
}
}
// FAIL_REASON_ROLL
handle_failure: {
if (!adjust_OK) {
boolean OK = false;
System.out.println("LMA failed at nscene = "+nscene+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA))) {
if (fpn_skip) {
System.out.println("fpn_skip is set, just skipping this scene");
failed_scenes[nscene] = true;
num_failed++;
scenes_xyzatr[nscene] = new double[][] {
scene_xyz_pre, // for now restore original, pre-adjustment
scene_atr_pre // for now restore original, pre-adjustment
};
break handle_failure;
// continue; // to next scene
} else {
System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
// set this and all previous to null
}
}
// all other reasons lead to failure
earliest_scene = nscene + 1;
if (debugLevel > -4) {
System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + ts+
" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
} }
// set this and all previous to null // set this and all previous to null
for (; scene_index >= 0 ; scene_index--) { for (; nscene >= 0 ; nscene--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName(), null); ers_reference.addScene(quadCLTs[nscene].getImageName()); // remove
quadCLTs[nscene] = null; // completely remove early scenes?
} }
break; break;
} }
*/ }
// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes // overwrite old data
ers_reference.addScene(scene_QuadClt.getImageName(), // undo velocities->ers scaling
scenes_xyzatr[scene_index][0], double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
scenes_xyzatr[scene_index][1], clt_parameters,
ZERO3, // ers_scene.getErsXYZ_dt(), quadCLTs[nscene].getErsCorrection().getErsXYZATR_dt(
ZERO3 // ers_scene.getErsATR_dt() ));
// ers_reference.addScene(ts,
// scenes_xyzatr[nscene][0],
// scenes_xyzatr[nscene][1],
// quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(), // same as dxyzatr_dt[nscene][0], just keep for future adjustments?
// quadCLTs[nscene].getErsCorrection().getErsATR_dt() // same as dxyzatr_dt[nscene][1], just keep for future adjustments?
// );
ers_reference.addScene(ts,
scenes_xyzatr[nscene][0],
scenes_xyzatr[nscene][1],
adjusted_xyzatr_dt[0],
adjusted_xyzatr_dt[1]
); );
if (lma_rms[0] > maximal_series_rms) { if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0]; maximal_series_rms = lma_rms[0];
} }
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+ System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+ quadCLTs[ref_index].getImageName() + "/" + ts+
" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms); " Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
} }
} // for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--) } // if (nscene == ref_index) else
if ((ref_index - earliest_scene + 1) < min_num_scenes) { } // for (int nscene = ref_index; nscene > earliest_scene; nscene--) {
System.out.println("Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
" < "+min_num_scenes+". Scrapping this series."); // TODO: after all scenes done, see if any of scenes_xyzatr[] is null, and if fpn_rematch - rematch
if (start_ref_pointers != null) { if (dbg_mb_img != null) {
start_ref_pointers[0] = earliest_scene; String [] dbg_mb_titles = new String[quadCLTs.length];
for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
dbg_mb_titles[i] = quadCLTs[i].getImageName();
} }
return -1; ShowDoubleFloatArrays.showArrays(
dbg_mb_img,
tilesX * 2,
tilesY,
true,
quadCLTs[ref_index].getImageName()+"-MOTION_BLUR",
dbg_mb_titles);
} }
if (earliest_scene > 0) {
System.out.println("setInitialOrientations(): Not all scenes matched, earliest useful scene = "+earliest_scene+ if (debugLevel > -4) {
" (total number of scenes = "+(ref_index - earliest_scene + 1)+
").Maximal RMSE was "+maximal_series_rms);
} else if (debugLevel > -4) {
System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms); System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
} }
quadCLTs[ref_index].set_orient(1); // first orientation
quadCLTs[ref_index].set_accum(0); // reset accumulations ("build_interscene") number quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
null, // String path, // full name with extension or w/o path to use x3d directory null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); debugLevel+1);
return earliest_scene; return earliest_scene;
} }
public static void readjustShowCorrected(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
double [][] dxyzatr_dt_nscene,
double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
double mb_max_gain,
double [] mb_ref_disparity,
int ref_index,
int nscene,
int debug_scene, // = 68
int debugLevel) {
boolean show_corrected = false;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
if (nscene == debug_scene) {
System.out.println("2.nscene = "+nscene);
System.out.println("3.nscene = "+nscene);
}
dbg_mb_img[nscene] = new double [tilesX*tilesY*2];
Arrays.fill(dbg_mb_img[nscene],Double.NaN);
String ts = quadCLTs[nscene].getImageName();
double [] mb_scene_xyz = (nscene != ref_index)? ers_reference.getSceneXYZ(ts):ZERO3;
double [] mb_scene_atr = (nscene != ref_index)? ers_reference.getSceneATR(ts):ZERO3;
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
mb_ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene_QuadClt,
quadCLTs[ref_index]); // final QuadCLT reference_QuadClt)
double [][] motion_blur = OpticalFlow.getMotionBlur( // dxyzatr_dt[][] should not be scaled here
quadCLTs[ref_index], // QuadCLT ref_scene,
quadCLTs[nscene], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
mb_scene_xyz, // double [] camera_xyz,
mb_scene_atr, // double [] camera_atr,
dxyzatr_dt_nscene[0], // double [] camera_xyz_dt,
dxyzatr_dt_nscene[1], // double [] camera_atr_dt,
-1, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
for (int nTile = 0; nTile < motion_blur[0].length; nTile++) {
int tx = nTile % tilesX;
int ty = nTile / tilesX;
dbg_mb_img[nscene][tx + tilesX * (ty*2 +0)] = mb_tau * motion_blur[0][nTile];
dbg_mb_img[nscene][tx + tilesX * (ty*2 +1)] = mb_tau * motion_blur[1][nTile];
}
while (show_corrected) {
ImagePlus imp_mbc = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
false, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc.show();
ImagePlus imp_mbc_merged = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
true, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED-MERGED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc_merged.show();
show_corrected = false; // manually set/reset
}
}
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,
...@@ -1210,7 +2070,6 @@ public class Interscene { ...@@ -1210,7 +2070,6 @@ public class Interscene {
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain); System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
boolean fail_on_zoom_roll=false; // it should fail on initial adjustment boolean fail_on_zoom_roll=false; // it should fail on initial adjustment
int avg_len = clt_parameters.imp.avg_len; int avg_len = clt_parameters.imp.avg_len;
// boolean test_motion_blur = true;//false
// Set up velocities from known coordinates, use averaging // Set up velocities from known coordinates, use averaging
double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr}; double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
...@@ -1224,20 +2083,8 @@ public class Interscene { ...@@ -1224,20 +2083,8 @@ public class Interscene {
((dbg_scale_dt[0][0] > 0)?"x":((dbg_scale_dt[0][0] < 0) ? "X":"0"))+ ((dbg_scale_dt[0][0] > 0)?"x":((dbg_scale_dt[0][0] < 0) ? "X":"0"))+
((dbg_scale_dt[0][1] > 0)?"y":((dbg_scale_dt[0][1] < 0) ? "Y":"0"))+ ((dbg_scale_dt[0][1] > 0)?"y":((dbg_scale_dt[0][1] < 0) ? "Y":"0"))+
((dbg_scale_dt[0][2] > 0)?"z":((dbg_scale_dt[0][2] < 0) ? "Z":"0")); ((dbg_scale_dt[0][2] > 0)?"z":((dbg_scale_dt[0][2] < 0) ? "Z":"0"));
// boolean ers_readjust_enable = true; // debugging ers during readjust 11.02.2022
// boolean ers_use_xyz = false; // true; // false; // enable when ready 11.02.2022
// int [][] dbg_scale_dt = {{0,0,0},{0, 0, 0}}; // no_ers - causes singular matrices (not alwasy), but works
// int [][] dbg_scale_dt = {{0,0,0},{1,-1, 1}}; // aTr
// int [][] dbg_scale_dt = {{0,0,0},{1, 1,-1}}; // atR
// int [][] dbg_scale_dt = {{0,0,0},{1, 1, 1}}; // atr
// int [][] dbg_scale_dt = {{1,1,1},{1, 1, 1}}; // atrxyz
// int [][] dbg_scale_dt = {{1,1,1},{1, 1,-1}}; // atRxyz
// int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,1}}; // atrXYZ
// int [][] dbg_scale_dt = {{-1,-1,-1},{1, 1,-1}}; // atRXYZ
boolean mb_en = clt_parameters.imp.mb_en; boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
// double mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
int margin = clt_parameters.imp.margin; int margin = clt_parameters.imp.margin;
double second_margin = 1.1; // allow slightly larger offset for readjsutments (to be limited by the first pass) double second_margin = 1.1; // allow slightly larger offset for readjsutments (to be limited by the first pass)
double boost_max_short = 5.0; // 2.0; // Some bug? double boost_max_short = 5.0; // 2.0; // Some bug?
...@@ -1260,10 +2107,7 @@ public class Interscene { ...@@ -1260,10 +2107,7 @@ public class Interscene {
System.out.println("reAdjustPairsLMAInterscene(): min_offset="+min_max[0]+", max_offset="+min_max[1]); System.out.println("reAdjustPairsLMAInterscene(): min_offset="+min_max[0]+", max_offset="+min_max[1]);
double [] disparity_raw = new double [tilesX * tilesY]; double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity); Arrays.fill(disparity_raw,clt_parameters.disparity);
/// double [][] combo_dsn_final = quadCLTs[ref_index].readDoubleArrayFromModelDirectory(
/// "-INTER-INTRA-LMA", // String suffix,
/// 0, // int num_slices, // (0 - all)
/// null); // int [] wh);
double [][] combo_dsn_final = quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky double [][] combo_dsn_final = quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
double [][] dls = { // Update to use FG? Or FG/no BG? double [][] dls = { // Update to use FG? Or FG/no BG?
...@@ -1277,7 +2121,6 @@ public class Interscene { ...@@ -1277,7 +2121,6 @@ public class Interscene {
dls, // double [][] dls dls, // double [][] dls
quadCLTs[ref_index], // QuadCLT scene, quadCLTs[ref_index], // QuadCLT scene,
debugLevel); debugLevel);
// double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) { if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ? interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
...@@ -1319,8 +2162,8 @@ public class Interscene { ...@@ -1319,8 +2162,8 @@ public class Interscene {
// should have at least next or previous non-null // should have at least next or previous non-null
int debug_scene = 69; // -68; // -8; int debug_scene = 69; // -68; // -8;
double maximal_series_rms = 0.0; double maximal_series_rms = 0.0;
double [][] mb_vectors_ref = null; /// double [][] mb_vectors_ref = null;
TpTask[][] tp_tasks_ref = null; /// TpTask[][] tp_tasks_ref = null;
if (debug_ers) { if (debug_ers) {
System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'"); System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
} }
...@@ -1345,10 +2188,8 @@ public class Interscene { ...@@ -1345,10 +2188,8 @@ public class Interscene {
} }
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName()); dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
if (debug_ers) { if (debug_ers) {
// double est_pix = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true); // All 4 variants - just for testing. Normally should try center first and use
// double est_pix1 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,1); // quad if there is no significant movement
// double est_pix2 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,2);
// double est_pix3 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,3);
double est_center = quadCLTs[ref_index].estimateAverageShift( double est_center = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0, scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1, scenes_xyzatr[nscene], // double [][] xyzatr1,
...@@ -1377,7 +2218,6 @@ public class Interscene { ...@@ -1377,7 +2218,6 @@ public class Interscene {
nscene, nscene,
scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2], scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2], scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2],
// est_pix, est_pix1, est_pix2, est_pix3,
est_center, est_quad, est_center_rectilinear, est_quad_rectilinear)); est_center, est_quad, est_center_rectilinear, est_quad_rectilinear));
} }
} }
...@@ -1391,13 +2231,7 @@ public class Interscene { ...@@ -1391,13 +2231,7 @@ public class Interscene {
scenes_xyzatr, // double [][][] scenes_xyzatr, scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range half_run_range); // double half_run_range
break; break;
// case 2: // read from ims
default: // do nothing - already read default: // do nothing - already read
// dxyzatr_dt = new double [quadCLTs.length][][];
// for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// // read from map (ts)
// // divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
// }
} }
if (debug_ers) { if (debug_ers) {
...@@ -1412,7 +2246,6 @@ public class Interscene { ...@@ -1412,7 +2246,6 @@ public class Interscene {
} }
double max_z_change = Double.NaN; // only applicable for drone images double max_z_change = Double.NaN; // only applicable for drone images
if (max_zoom_diff > 0) { // ignore if set to if (max_zoom_diff > 0) { // ignore if set to
// double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
max_z_change = avg_z * max_zoom_diff; max_z_change = avg_z * max_zoom_diff;
if (fail_on_zoom_roll) { if (fail_on_zoom_roll) {
if (debugLevel > -3) { if (debugLevel > -3) {
...@@ -1433,51 +2266,37 @@ public class Interscene { ...@@ -1433,51 +2266,37 @@ public class Interscene {
scene_seq[indx++] = i; scene_seq[indx++] = i;
} }
} }
boolean rectilinear = false;
double max_quad = 10; // use quad if center offset is less than this
TpTask[][] tp_tasks_ref = new TpTask[2][];
double [][] pXpYD_ref = new double [tilesX*tilesY][];
// for (int nscene = ref_index; nscene >= earliest_scene; nscene--) {
for (int nscene:scene_seq) { for (int nscene:scene_seq) {
if (debugLevel > -4) { if (nscene == debug_scene) {
// double est_pix = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true); System.out.println("nscene = "+nscene);
// double est_pix1 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,1); System.out.println("nscene = "+nscene);
// double est_pix2 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,2); }
// double est_pix3 = quadCLTs[ref_index].estimatePixelShift(scenes_xyzatr[ref_index], scenes_xyzatr[nscene], true,3); double est_shift = quadCLTs[ref_index].estimateAverageShift(
double est_center = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
false); // boolean rectilinear)
double est_quad = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
false); // boolean rectilinear)
double est_center_rectilinear = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0, scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1, scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z, avg_z, // double average_z,
false, // boolean use_rot, false, // boolean use_rot,
true); // boolean rectilinear) rectilinear); // boolean rectilinear)
double est_quad_rectilinear = quadCLTs[ref_index].estimateAverageShift( if (est_shift < max_quad) {
est_shift = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0, scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1, scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z, avg_z, // double average_z,
true, // boolean use_rot, true, // boolean use_rot,
true); // boolean rectilinear) rectilinear); // boolean rectilinear)
// only check quad if center is below thershold (not to get probed too far - overlap)
// rectilinear - OK, error is small.
System.out.println("nscene="+nscene+": est offset "+
est_center+"pix "+est_quad+"pix "+est_center_rectilinear+"pix "+est_quad_rectilinear+"pix ");
} }
if (nscene == debug_scene) { if (debugLevel > -4) {
System.out.println("nscene = "+nscene); System.out.println("nscene="+nscene+": est offset "+
System.out.println("nscene = "+nscene); est_shift+"pix");
} }
// just checking it is not isolated // just checking it is not isolated
if ((quadCLTs[nscene] == null) || if ((quadCLTs[nscene] == null) ||
// (scenes_xyzatr[nscene] == null) || // no velocity even with averaging
((nscene != ref_index) && ((nscene != ref_index) &&
((ers_reference.getSceneXYZ(quadCLTs[nscene].getImageName())== null) || ((ers_reference.getSceneXYZ(quadCLTs[nscene].getImageName())== null) ||
(ers_reference.getSceneATR(quadCLTs[nscene].getImageName())== null)))) { (ers_reference.getSceneATR(quadCLTs[nscene].getImageName())== null)))) {
...@@ -1485,7 +2304,6 @@ public class Interscene { ...@@ -1485,7 +2304,6 @@ public class Interscene {
break; break;
} }
String ts = quadCLTs[nscene].getImageName(); String ts = quadCLTs[nscene].getImageName();
double [] scene_xyz_pre = ZERO3; double [] scene_xyz_pre = ZERO3;
double [] scene_atr_pre = ZERO3; double [] scene_atr_pre = ZERO3;
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!) quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
...@@ -1505,7 +2323,12 @@ public class Interscene { ...@@ -1505,7 +2323,12 @@ public class Interscene {
debug_scene, // = 68 int debug_scene, // = 68 debug_scene, // = 68 int debug_scene, // = 68
debugLevel); // int debugLevel); debugLevel); // int debugLevel);
} }
final boolean[] param_select = clt_parameters.ilp.ilma_lma_select;
final double [] param_regweights = clt_parameters.ilp.ilma_regularization_weights;
final double max_rms = clt_parameters.imp.max_rms;
if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes
/*
mb_vectors_ref = null; mb_vectors_ref = null;
if (mb_en) { if (mb_en) {
mb_vectors_ref = OpticalFlow.getMotionBlur( mb_vectors_ref = OpticalFlow.getMotionBlur(
...@@ -1531,6 +2354,7 @@ public class Interscene { ...@@ -1531,6 +2354,7 @@ public class Interscene {
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors_ref, // double [][] mb_vectors, // now [2][ntiles]; mb_vectors_ref, // double [][] mb_vectors, // now [2][ntiles];
debugLevel); // int debugLevel) debugLevel); // int debugLevel)
*/
ers_reference.addScene(ts, ers_reference.addScene(ts,
scenes_xyzatr[nscene][0], // ref_index scenes_xyzatr[nscene][0], // ref_index
scenes_xyzatr[nscene][1], scenes_xyzatr[nscene][1],
...@@ -1578,6 +2402,7 @@ public class Interscene { ...@@ -1578,6 +2402,7 @@ public class Interscene {
} }
} }
double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull); double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull);
/*
if (mb_en) { if (mb_en) {
double [][] dxyzatr_dt_scene = dxyzatr_dt[nscene]; double [][] dxyzatr_dt_scene = dxyzatr_dt[nscene];
...@@ -1592,7 +2417,9 @@ public class Interscene { ...@@ -1592,7 +2417,9 @@ public class Interscene {
0, // int shrink_gaps, // will gaps, but not more that grow by this 0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level) debugLevel); // int debug_level)
} }
*/
double [] lma_rms = new double[2]; double [] lma_rms = new double[2];
/*
scenes_xyzatr[nscene] = Interscene.adjustPairsLMAInterscene( scenes_xyzatr[nscene] = Interscene.adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
false, // boolean initial_adjust, false, // boolean initial_adjust,
...@@ -1619,6 +2446,37 @@ public class Interscene { ...@@ -1619,6 +2446,37 @@ public class Interscene {
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors, // double [][] mb_vectors, // now [2][ntiles]; mb_vectors, // double [][] mb_vectors, // now [2][ntiles];
clt_parameters.imp.debug_level); // 1); // -1); // int debug_level); clt_parameters.imp.debug_level); // 1); // -1); // int debug_level);
*/
scenes_xyzatr[nscene] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, //CLTParameters clt_parameters,
use_lma_dsi, //,boolean use_lma_dsi,
fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
tp_tasks_ref, // TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
pXpYD_ref, // double [][] pXpYD_ref, // should be se or at least double [num_tiles][] if tp_tasks_ref[0] == null
// will be recalculated when tp_tasks_ref[0] == null, but for reference frame
ref_index, // int nscene0, // may be == ref_index
nscene, // int nscene1, // compares to nscene0
interscene_ref_disparity,// double [] ref_disparity, // null or alternative reference disparity
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[ref_index],// double [][] scene0_xyzatr,
scene_xyzatr, // double [][] scene1_xyzatr,
scene_pull, // double [][] scene1_xyzatr_pull,
param_select, // boolean[] param_select,
param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
max_rms, // double max_rms,
mb_en, // boolean mb_en,
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
clt_parameters.imp.debug_level); // int debugLevel)
boolean adjust_OK = scenes_xyzatr[nscene] != null; boolean adjust_OK = scenes_xyzatr[nscene] != null;
if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments
if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) { if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
...@@ -1869,7 +2727,8 @@ public class Interscene { ...@@ -1869,7 +2727,8 @@ public class Interscene {
@Deprecated
// Trying to replace with universal adjustDiffPairsLMAInterscene()
public static double[][] adjustPairsLMAInterscene( public static double[][] adjustPairsLMAInterscene(
CLTParameters clt_parameters, CLTParameters clt_parameters,
boolean use_lma_dsi, boolean use_lma_dsi,
...@@ -4178,6 +5037,10 @@ public class Interscene { ...@@ -4178,6 +5037,10 @@ public class Interscene {
ims_ortho); ims_ortho);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu); double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu}; double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
false); // boolean scale)
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) { for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
...@@ -4308,10 +5171,12 @@ public class Interscene { ...@@ -4308,10 +5171,12 @@ public class Interscene {
// a (around Y),t (around X), r (around Z) // a (around Y),t (around X), r (around Z)
sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]); sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]);
*/ */
double [][] cam_dxyzatr = scene.getDxyzatrIms(clt_parameters, false); // raw, do not scale // double [][] cam_dxyzatr = scene.getDxyzatrIms(clt_parameters, false); // raw, do not scale
sb.append("\t"+cam_dxyzatr[0][0]+ "\t"+cam_dxyzatr[0][1]+ "\t"+cam_dxyzatr[0][2]); // sb.append("\t"+cam_dxyzatr[0][0]+ "\t"+cam_dxyzatr[0][1]+ "\t"+cam_dxyzatr[0][2]);
sb.append("\t"+cam_dxyzatr[1][0]+ "\t"+cam_dxyzatr[1][1]+ "\t"+cam_dxyzatr[1][2]); // sb.append("\t"+cam_dxyzatr[1][0]+ "\t"+cam_dxyzatr[1][1]+ "\t"+cam_dxyzatr[1][2]);
sb.append("\t"+scenes_dxyzatr[nscene][0][0]+ "\t"+scenes_dxyzatr[nscene][0][1]+ "\t"+scenes_dxyzatr[nscene][0][2]);
sb.append("\t"+scenes_dxyzatr[nscene][1][0]+ "\t"+scenes_dxyzatr[nscene][1][1]+ "\t"+scenes_dxyzatr[nscene][1][2]);
//scenes_dxyzatr
// add lpf // add lpf
sb.append("\n"); sb.append("\n");
} }
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ e17a0659
...@@ -4975,12 +4975,14 @@ public class OpticalFlow { ...@@ -4975,12 +4975,14 @@ public class OpticalFlow {
boolean ims_use = center_reference || clt_parameters.imp.ims_use; // center works with IMS only boolean ims_use = center_reference || clt_parameters.imp.ims_use; // center works with IMS only
/// int center_index=ref_index; /// int center_index=ref_index;
int ref_index = last_index; // old versions int ref_index = last_index; // old versions
boolean compensate_ims_rotation = false;
if (force_initial_orientations && !reuse_video) { if (force_initial_orientations && !reuse_video) {
boolean OK = false; boolean OK = false;
int es1 = -1; int es1 = -1;
if (center_reference) { if (center_reference) {
es1 = Interscene.setInitialOrientationsCenterIms( es1 = Interscene.setInitialOrientationsCenterIms(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes, // int min_num_scenes, min_num_scenes, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters, colorProcParameters, // final ColorProcParameters colorProcParameters,
debayerParameters, // EyesisCorrectionParameters.DebayerParameters debayerParameters, debayerParameters, // EyesisCorrectionParameters.DebayerParameters debayerParameters,
...@@ -5007,6 +5009,7 @@ public class OpticalFlow { ...@@ -5007,6 +5009,7 @@ public class OpticalFlow {
} else if (ims_use) { } else if (ims_use) {
earliest_scene = Interscene.setInitialOrientationsIms( earliest_scene = Interscene.setInitialOrientationsIms(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes, // int min_num_scenes, min_num_scenes, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters, colorProcParameters, // final ColorProcParameters colorProcParameters,
quadCLTs, // final QuadCLT[] quadCLTs, // quadCLTs, // final QuadCLT[] quadCLTs, //
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ e17a0659
...@@ -125,6 +125,7 @@ public class QuadCLTCPU { ...@@ -125,6 +125,7 @@ public class QuadCLTCPU {
public static final boolean USE_PRE_2021 = false; // temporary public static final boolean USE_PRE_2021 = false; // temporary
public static final int THREADS_MAX = 100; public static final int THREADS_MAX = 100;
public static final double [] ZERO3 = {0.0,0.0,0.0};
// public GPUTileProcessor.GpuQuad gpuQuad = null; // public GPUTileProcessor.GpuQuad gpuQuad = null;
...@@ -205,7 +206,90 @@ public class QuadCLTCPU { ...@@ -205,7 +206,90 @@ public class QuadCLTCPU {
public void setQuatCorr(double[] quat) { public void setQuatCorr(double[] quat) {
quat_corr = quat; quat_corr = quat;
} }
public double [][] getDxyzatrIms(
/**
* Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS velocities (currently
* only linear are used). Refined scene poses may be used as pull targets
* with free orientations (and angular velocities for ERS).
* Result poses are {0,0,0} for the reference frame.
*
* Assuming correct IMU angular velocities and offset linear ones.
*
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence (for timestamps)
* @param xyzatr current scenes [[x,y,z],[a,t,r]] in reference scene frame
* @param dxyzatr IMU-provided linear and angular velocities in reference
* scene frame
* @param ref_index reference scene index
* @param early_index earliest (lowest) scene index to use
* @param last_index last (highest) scene index to use
* @return refined array of scene poses [[x,y,z],[a,t,r]] (in reference scene frame),
* same indices as input
*/
public static double [][][] refineFromImsVelocities(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] dxyzatr,
int ref_index,
int early_index,
int last_index
) {
double [][] xyz_integ = new double [quadCLTs.length][3];
double [] tim = new double [quadCLTs.length];
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];
for (int nscene = early_index; nscene <= last_index; nscene++) {
double t = quadCLTs[nscene].getTimeStamp();
double x = t - t00; // from early_index
tim[nscene] = x;
s0 +=1.0;
sx += x;
sx2 += x*x;
double dt = (t-t0)/2; // half from previous
t0 = t;
for (int i = 0; i < 3; i++) {
double y = 0;
if (nscene > early_index) {
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene-1][0][i]);
y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
}
sy[i] += y;
sxy[i] += x*y;
}
}
double denom = sx2 * s0 - sx * sx;
double [] a = new double[3], b = new double[3], ref_offs = new double[3];
for (int i = 0; i < 3; i++) {
a[i] = (sxy[i] * s0 - sx * sy[i])/denom;
b[i] = (sy[i] * sx2 - sx * sxy[i])/denom;
ref_offs[i] = b[i] + a[i] * tim[ref_index] + xyz_integ[ref_index][i];
}
double [][][] xyzatr_out = new double [quadCLTs.length][][];
for (int nscene = early_index; nscene <= last_index; nscene++) {
xyzatr_out[nscene] = new double[3][2];
for (int i = 0; i < 3; i++) {
xyzatr_out[nscene][0][i] = b[i] + a[i] * tim[nscene] + xyz_integ[nscene][i] - ref_offs[i];
xyzatr_out[nscene][1][i] = xyzatr[nscene][1][i]; // for now - just copy old, maybe will add smth.
}
}
return xyzatr_out;
}
/**
* Get linear and angular velocities (camera frame) from
* this scene IMS data
* @param clt_parameters configuration parameters (including
* scales from IMS to linear for linear and angular velocities
* @param scale if true, multiply by scales from he configuration parameters)
* @return linear and angular velocities in camera frame
*/
protected double [][] getDxyzatrIms(
CLTParameters clt_parameters, CLTParameters clt_parameters,
boolean scale) { boolean scale) {
double [] ims_ortho = clt_parameters.imp.ims_ortho; double [] ims_ortho = clt_parameters.imp.ims_ortho;
...@@ -219,16 +303,106 @@ public class QuadCLTCPU { ...@@ -219,16 +303,106 @@ public class QuadCLTCPU {
double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false); double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false); double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
// a (around Y),t (around X), r (around Z) // a (around Y),t (around X), r (around Z)
double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}}; double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
if (scale) { if (scale) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
dxyzatyr[0][i]*=clt_parameters.imp.ims_scale_xyz[i]; dxyzatr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
dxyzatyr[1][i]*=clt_parameters.imp.ims_scale_atr[i]; dxyzatr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
} }
} }
return dxyzatyr; return dxyzatr;
} }
/**
* Get linear and angular velocities (camera frame) from
* scenes' IMS data
* @param clt_parameters configuration parameters (including
* scales from IMS to linear for linear and angular velocities
* @param quadCLTs array of scenes with IMS data loaded
* @param scale if true, multiply by scales from he configuration parameters)
* @return linear and angular velocities in camera frame for each scene that has IMS data
*/
public static double [][][] getDxyzatrIms(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
boolean scale) {
double [][][] dxyzatr = new double [quadCLTs.length][][];
for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
dxyzatr[i] = quadCLTs[i].getDxyzatrIms(
clt_parameters,
scale);
}
return dxyzatr;
}
/**
* Get offset and rotation of each scene from the sequence relative to this
* scene using IMS data. Linear offsets may be too big for the fix, so they
* need running correction from the last matched scene, scenes should start from
* the closest to this one and move gradually farther away.
* @param clt_parameters Configuration parameters, including IMS to camera rotation
* @param quadCLTs array of scenes
* @param quat_corr optional quaternion correcting IMS orientation (when known) or null
* @param debugLevel debug level
* @return per-scene array of [[x,y,z],[a,t,r]] in camera frame for initial fitting.
* Orientation may be later used to pull LMA. Result for reference (this) scene may
* be small values, but not exactly zeros.
*/
public double [][][] getXyzatrIms(CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
double [] quat_corr, // only applies to rotations - verify!
int debugLevel){
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
// scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
Did_ins_2 d2_ref = did_ins_2;
// apply correction to orientation - better not to prevent extra loops.
// And it only applies to rotations
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
ims_mount_atr,
ims_ortho);
if (quat_corr != null) {
cam_quat_ref_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_ref_enu, false);
if (debugLevel > -3) {
System.out.println("getXyzatrIms(): Applying attitude correction from quaternion ["+
quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
}
}
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][] last_corr_xyzatr = {ZERO3,ZERO3};
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
QuadCLT scene = quadCLTs[nscene];
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up
double [] cam_quat_enu =Imx5.quaternionImsToCam(
d2.getQEnu(),
ims_mount_atr,
ims_ortho);
if (quat_corr != null) {
cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
}
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
// Offset in the reference scene frame, not in the current scene
cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
enu, // absolute offset (East, North, Up) in meters
false);
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] ims_scene_xyzatr_enu = {cam_xyz_enu, scene_abs_atr_enu }; // try with xyz?
// set initial approximation from IMS, subtract reference XYZATR
// predicted by IMU from the reference scene
double [][] pose_ims = ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu));
scenes_xyzatr[nscene] = pose_ims;
}
return scenes_xyzatr;
}
public double [] getLla() { public double [] getLla() {
return did_ins_2.lla; return did_ins_2.lla;
} }
...@@ -355,10 +529,6 @@ public class QuadCLTCPU { ...@@ -355,10 +529,6 @@ public class QuadCLTCPU {
scene1.getImageName()); scene1.getImageName());
} }
//QuadCLTCPU [] scenes
// TODO: Use dsi[] instead
// @Deprecated
// public boolean[] blue_sky = null;
public void inc_orient() {num_orient++;} public void inc_orient() {num_orient++;}
public void inc_accum() {num_accum++;} public void inc_accum() {num_accum++;}
public void set_orient(int num) {num_orient = num;} public void set_orient(int num) {num_orient = num;}
...@@ -417,30 +587,20 @@ public class QuadCLTCPU { ...@@ -417,30 +587,20 @@ public class QuadCLTCPU {
/** /**
* Estimate average pixel offset between 2 scenes for avoiding FPN. This scene * Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
* should have DSI to estimate average distance * should have DSI to estimate average distance
* Use estimateAverageShift() instead - much more accurate. This one underestimates
* up to twice (not understood)
*
* @param xyzatr0 - first scene [[x,y,z],[a,t,r]] * @param xyzatr0 - first scene [[x,y,z],[a,t,r]]
* @param xyzatr1 - second scene [[x,y,z],[a,t,r]] * @param xyzatr1 - second scene [[x,y,z],[a,t,r]]
* @param use_lma * @param use_lma
* @return * @return
*/ */
@Deprecated
public double estimatePixelShift( public double estimatePixelShift(
double [][] xyzatr0, double [][] xyzatr0,
double [][] xyzatr1, double [][] xyzatr1,
boolean use_lma boolean use_lma
) { ) {
return estimatePixelShift(
xyzatr0,
xyzatr1,
use_lma,
0);
}
public double estimatePixelShift(
double [][] xyzatr0,
double [][] xyzatr1,
boolean use_lma,
int mode
) {
boolean sign_x = (mode & 1) != 0;
boolean sign_y = (mode & 2) != 0;
double z = getAverageZ(use_lma); double z = getAverageZ(use_lma);
if (Double.isNaN(z)) { if (Double.isNaN(z)) {
System.out.println("estimatePixelShift(): failed estimating distance - no DSI or too few DSI tiles."); System.out.println("estimatePixelShift(): failed estimating distance - no DSI or too few DSI tiles.");
...@@ -452,8 +612,8 @@ public class QuadCLTCPU { ...@@ -452,8 +612,8 @@ public class QuadCLTCPU {
for (int i = 0; i<2;i++) for (int j=0; j < 3; j++) { for (int i = 0; i<2;i++) for (int j=0; j < 3; j++) {
dxyzatr[i][j] = xyzatr1[i][j]-xyzatr0[i][j]; dxyzatr[i][j] = xyzatr1[i][j]-xyzatr0[i][j];
} }
double eff_az = dxyzatr[1][0] + (sign_x?-1:1) * dxyzatr[0][0] / z; double eff_az = dxyzatr[1][0] + dxyzatr[0][0] / z;
double eff_tl = dxyzatr[1][1] + (sign_y?-1:1) * dxyzatr[0][1] / z; double eff_tl = dxyzatr[1][1] + dxyzatr[0][1] / z;
double pix_x = -eff_az*aztl_pix_per_rad; double pix_x = -eff_az*aztl_pix_per_rad;
double pix_y = eff_tl*aztl_pix_per_rad; double pix_y = eff_tl*aztl_pix_per_rad;
double pix_roll = dxyzatr[1][2] * roll_pix_per_rad; double pix_roll = dxyzatr[1][2] * roll_pix_per_rad;
...@@ -462,6 +622,20 @@ public class QuadCLTCPU { ...@@ -462,6 +622,20 @@ public class QuadCLTCPU {
return est_pix; return est_pix;
} }
/**
* Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
* should have DSI to estimate average distance.
* Difference with rectilinear is small so both are OK.
* @param xyzatr0 first scene pose (double[3][2]) for reference scene
* @param xyzatr1 second scene pose
* @param average_z average distance (altitude AGL for downward images) in meters
* @param use_rot when false - compare only the center, when true - average 4 samples
* at 25% of width/height. May be needed for pure rotation (no center
* shift), but may fail if interscene shift exceeds 25%. So try first
* for center and use 4 samples only if center does not move.
* @param rectilinear Ignore lens distortions
* @return interscene shift in pixels
*/
public double estimateAverageShift( public double estimateAverageShift(
double [][] xyzatr0, double [][] xyzatr0,
double [][] xyzatr1, double [][] xyzatr1,
......
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