Skip to content

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

debugging inconsistent pose fitting

parent e2a311fc
Hide whitespace changes
Inline Side-by-side
Showing with 222 additions and 1443 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ a675f808
...@@ -190,7 +190,7 @@ public class Interscene { ...@@ -190,7 +190,7 @@ public class Interscene {
// to include ref scene photometric calibration // to include ref scene photometric calibration
// added 05.21.2024: skip if was spawn already // added 05.21.2024: skip if was spawn already
if (changed || (quadCLTs[adjusted_scene_index] == null)) { if (changed || (quadCLTs[adjusted_scene_index] == null)) {
quadCLTs[adjusted_scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( quadCLTs[adjusted_scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( // sets quat_corr in all from quadCLTs[ref_index] ? just one?
set_channels[adjusted_scene_index].set_name, set_channels[adjusted_scene_index].set_name,
clt_parameters, clt_parameters,
colorProcParameters, // colorProcParameters, //
...@@ -242,20 +242,20 @@ public class Interscene { ...@@ -242,20 +242,20 @@ public class Interscene {
if (!reused_overlap) { if (!reused_overlap) {
// 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, compensate_ims_rotation, // final boolean compensate_ims_rotation,
inertial_only, // final boolean inertial_only, inertial_only, // final boolean inertial_only,
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, //
ref_index, // final int ref_index, ref_index, // final int ref_index,
set_channels, // final SetChannels [] set_channels, set_channels, // final SetChannels [] set_channels,
batch_mode, // final boolean batch_mode, batch_mode, // final boolean batch_mode,
cent_index, // int earliest_scene, cent_index, // int earliest_scene,
start_ref_pointers1, // int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index start_ref_pointers1, // int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index
threadsMax, // final int threadsMax, // int threadsMax, threadsMax, // final int threadsMax, // int threadsMax,
updateStatus, // final boolean updateStatus, updateStatus, // final boolean updateStatus,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
if (cent_index < 0) { if (cent_index < 0) {
System.out.println("setInitialOrientationsIms() first half failed. Consider more graceful bail out."); System.out.println("setInitialOrientationsIms() first half failed. Consider more graceful bail out.");
start_ref_pointers[0] = start_ref_pointers1[0]; start_ref_pointers[0] = start_ref_pointers1[0];
...@@ -271,7 +271,7 @@ public class Interscene { ...@@ -271,7 +271,7 @@ public class Interscene {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// ref_index,// ref_indx, ref_index, // -1, // ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
cent_index, // earliest_scene, // int earliest_scene, cent_index, // earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -305,7 +305,7 @@ public class Interscene { ...@@ -305,7 +305,7 @@ public class Interscene {
// TODO: add {start,end} pointers to quadCLTs[cent_index] // TODO: add {start,end} pointers to quadCLTs[cent_index]
// TODO: add pointer to center to ref_index // TODO: add pointer to center to ref_index
// check quadCLTs[cent_index].dsi here // check quadCLTs[cent_index].dsi here
quadCLTs[cent_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN" quadCLTs[cent_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN", uses quat_corr
set_channels[cent_index].set_name, set_channels[cent_index].set_name,
clt_parameters, clt_parameters,
colorProcParameters, // colorProcParameters, //
...@@ -322,20 +322,20 @@ public class Interscene { ...@@ -322,20 +322,20 @@ 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,
inertial_only, // final boolean inertial_only,
compensate_ims_rotation, // final boolean compensate_ims_rotation, compensate_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes_half, // int min_num_scenes, inertial_only, // final boolean inertial_only,
colorProcParameters, // final ColorProcParameters colorProcParameters, min_num_scenes_half, // int min_num_scenes,
quadCLTs, // final QuadCLT[] quadCLTs, // colorProcParameters, // final ColorProcParameters colorProcParameters,
cent_index, // final int ref_index, quadCLTs, // final QuadCLT[] quadCLTs, //
set_channels, // final SetChannels [] set_channels, cent_index, // final int ref_index,
batch_mode, // final boolean batch_mode, set_channels, // final SetChannels [] set_channels,
earliest_scene, // int earliest_scene, batch_mode, // final boolean batch_mode,
start_ref_pointers2, // int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index earliest_scene, // int earliest_scene,
threadsMax, // final int threadsMax, // int threadsMax, start_ref_pointers2, // int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index
updateStatus, // final boolean updateStatus, threadsMax, // final int threadsMax, // int threadsMax,
debugLevel); // final int debugLevel) updateStatus, // final boolean updateStatus,
debugLevel); // final int debugLevel)
if (earliest_scene2 < 0) { if (earliest_scene2 < 0) {
System.out.println("setInitialOrientationsIms() second half failed. Consider more graceful bail out."); System.out.println("setInitialOrientationsIms() second half failed. Consider more graceful bail out.");
start_ref_pointers[0] = start_ref_pointers1[0]; start_ref_pointers[0] = start_ref_pointers1[0];
...@@ -350,7 +350,7 @@ public class Interscene { ...@@ -350,7 +350,7 @@ public class Interscene {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// cent_index,// ref_indx, cent_index, // -1, // ref_indx,
quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -387,7 +387,7 @@ public class Interscene { ...@@ -387,7 +387,7 @@ public class Interscene {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// cent_index,// ref_indx, cent_index, // -1, // ref_indx,
quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -499,7 +499,9 @@ public class Interscene { ...@@ -499,7 +499,9 @@ public class Interscene {
return 0; return 0;
} else { // if (readjust) { } else { // if (readjust) {
boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
System.out.println("invertInitialOrientation(): "+
", lma_use_Z="+lma_use_Z+", lma_use_R="+lma_use_R);
boolean[] param_select = boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always ErsCorrection.getParamSelect( // ZR - always
lma_use_Z, // boolean use_Z, lma_use_Z, // boolean use_Z,
...@@ -509,6 +511,11 @@ public class Interscene { ...@@ -509,6 +511,11 @@ public class Interscene {
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select; false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false, // boolean use_ERS_tilt); false, // boolean use_ERS_tilt);
false );// boolean use_ERS_roll); false );// boolean use_ERS_roll);
System.out.print("param_select=[");
for (int i = 0; i < param_select.length;i++) {
System.out.print(param_select[i]? "+":"-");
}
System.out.println("]");
RMSEStats rmse_stats = new RMSEStats(); RMSEStats rmse_stats = new RMSEStats();
RMSEStats rmse_stats_metric = clt_parameters.imp.eig_use ? (new RMSEStats()):null; RMSEStats rmse_stats_metric = clt_parameters.imp.eig_use ? (new RMSEStats()):null;
...@@ -994,7 +1001,7 @@ public class Interscene { ...@@ -994,7 +1001,7 @@ public class Interscene {
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation); System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation);
System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+" correct orientation offset"); System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+"correct orientation offset");
QuadCLTCPU.showQuatCorr(quat_corr, enu_corr_metric); // enu_corr_metric - linear correction QuadCLTCPU.showQuatCorr(quat_corr, enu_corr_metric); // enu_corr_metric - linear correction
} }
boolean test_quat_corr = debugLevel > 1000; boolean test_quat_corr = debugLevel > 1000;
...@@ -1566,6 +1573,11 @@ public class Interscene { ...@@ -1566,6 +1573,11 @@ public class Interscene {
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select; false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false, // boolean use_ERS_tilt); false, // boolean use_ERS_tilt);
false); // boolean use_ERS_roll); false); // boolean use_ERS_roll);
System.out.print("param_select=[");
for (int i = 0; i < param_select.length;i++) {
System.out.print(param_select[i]? "+":"-");
}
System.out.println("]");
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
boolean changed = center_CLT.isPhotometricUpdatedAndReset(); boolean changed = center_CLT.isPhotometricUpdatedAndReset();
...@@ -1920,7 +1932,8 @@ public class Interscene { ...@@ -1920,7 +1932,8 @@ public class Interscene {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
center_CLT,// ref_indx, -1, // ref_index,// ref_indx,
center_CLT,// ref_scene,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
ego_comment, // String comment); ego_comment, // String comment);
...@@ -2802,6 +2815,8 @@ public class Interscene { ...@@ -2802,6 +2815,8 @@ public class Interscene {
double mb_max_gain, double mb_max_gain,
boolean use_Z, boolean use_Z,
boolean use_R, boolean use_R,
boolean use_XY,
boolean use_AT,
boolean disable_ers, boolean disable_ers,
boolean disable_ers_y, boolean disable_ers_y,
boolean disable_ers_r, boolean disable_ers_r,
...@@ -2821,24 +2836,30 @@ public class Interscene { ...@@ -2821,24 +2836,30 @@ public class Interscene {
boolean test_motion_blur, boolean test_motion_blur,
int debugLevel) int debugLevel)
{ {
boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain+ System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain+
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+ ", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+", restore_imu="+restore_imu+
", use_Z="+use_Z+", use_R="+use_R); ", lma_xyzatr="+lma_xyzatr+", readjust_xy_ims="+readjust_xy_ims+", readjust_xy_ims="+ readjust_xy_ims+
", use_Z="+use_Z+", use_R="+use_R+", use_XY"+use_XY+", use_AT="+use_AT);
boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true; boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true;
boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
boolean lock_position = clt_parameters.imp.lock_position; boolean lock_position = clt_parameters.imp.lock_position;
double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain // double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain
// boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available // boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available
boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select : boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect( ErsCorrection.getParamSelect(
lma_xyzatr || use_Z, // boolean use_Z, lma_xyzatr || use_Z, // boolean use_Z,
lma_xyzatr || use_R, // boolean use_R, lma_xyzatr || use_R, // boolean use_R,
lma_xyzatr || ( !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0))), // false only in mode c): freeze X,Y// boolean use_XY lma_xyzatr || use_XY, // ( !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0))), // false only in mode c): freeze X,Y// boolean use_XY
lma_xyzatr || ( readjust_xy_ims || lpf_xy), // boolean use_AT, lma_xyzatr || use_AT, // ( readjust_xy_ims || lpf_xy), // boolean use_AT,
!disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select; !disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
!disable_ers_y, // boolean use_ERS_tilt, !disable_ers_y, // boolean use_ERS_tilt,
!disable_ers_r); // boolean use_ERS_roll); !disable_ers_r); // boolean use_ERS_roll);
System.out.print("param_select=[");
for (int i = 0; i < param_select.length;i++) {
System.out.print(param_select[i]? "+":"-");
}
System.out.println("]");
if (lock_position) { if (lock_position) {
for (int i:ErsCorrection.DP_XYZ_INDICES) param_select[i] = false; for (int i:ErsCorrection.DP_XYZ_INDICES) param_select[i] = false;
for (int i:ErsCorrection.DP_AT_INDICES) param_select[i] = true; for (int i:ErsCorrection.DP_AT_INDICES) param_select[i] = true;
...@@ -3654,15 +3675,17 @@ public class Interscene { ...@@ -3654,15 +3675,17 @@ public class Interscene {
boolean test_motion_blur, boolean test_motion_blur,
int debugLevel) int debugLevel)
{ {
System.out.println("reAdjustPairsLMAIntersceneCuas(): using mb_max_gain="+mb_max_gain+ // boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true;
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+
", use_R="+use_R);
/// boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true;
boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
boolean lock_position = true; // clt_parameters.imp.lock_position; boolean lock_position = true; // clt_parameters.imp.lock_position;
double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain // double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain
// boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available // boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available
System.out.println("reAdjustPairsLMAIntersceneCuas(): using mb_max_gain="+mb_max_gain+
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+", restore_imu="+restore_imu+
", lma_xyzatr="+lma_xyzatr+
", use_R="+use_R);
boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select : boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect( ErsCorrection.getParamSelect(
lma_xyzatr, // boolean use_Z, lma_xyzatr, // boolean use_Z,
...@@ -7062,337 +7085,18 @@ public class Interscene { ...@@ -7062,337 +7085,18 @@ public class Interscene {
} }
public static void generateEgomotionTable( public static void generateEgomotionTable(
CLTParameters clt_parameters, CLTParameters clt_parameters,
QuadCLT [] quadCLTs, QuadCLT [] quadCLTs,
int ref_index, int ref_index,
int earliest_scene,
String path,
String comment,
int debugLevel) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
int num_processed = 0;
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
// for testing QuaternionLma
double [] rms = new double [2];
double [] enu_corr = new double[3];
double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
// ref_index, // int ref_index,
ref_scene, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel
if (debugLevel > -3) {
if (quatCorr == null) {
System.out.println("generateEgomotionTable(): quatCorr=null");
} else {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("generateEgomotionTable(): quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("generateEgomotionTable(): ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("generateEgomotionTable(): ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
System.out.println("generateEgomotionTable(): ENU corr=["+enu_corr[0]+", "+enu_corr[1]+", "+enu_corr[2]+"]");
}
}
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
String ts = scene.getImageName();
scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(ts);
if (scenes_xyzatr[nscene] != null) {
num_processed++;
}
}
boolean use_processed = num_processed > 1;
String header_ts="#\ttimestamp";
String header_img="\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)"+
"\tEVx(m/s)\tEVy(m/s)\tEVz(m/s)\tEVa(rad/s)\tEVt(rad/s)\tEVr(rad/s)";
String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
"\tlat\tlong\talt\tned0\tned1\tned2";
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"+
"\tINS->X\tINS->Y\tINS->Z"+
"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
"\traw_A_enu\traw_T_enu\traw_R_enu"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
String header_testpimu = "\timsX\timsY\timsZ\timsA\timsT\timsR"+
"\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR"+
"\tpimuX-C\tpimuY-C\tpimuZ-C\tpimuA-C\tpimuT-C\tpimuR-C";
String header = header_ts+(use_processed?header_img:"")+header_ins1+
header_ins2+header_ins2_extra+header_pimu+header_camv+header_testpimu;
StringBuffer sb = new StringBuffer();
double [][][] dxyzatr_dt = null;
if (use_processed) {
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
//ref_index,
quadCLTs[ref_index],// QuadCLT ref_scene, // may be one of scenes or center
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr, // 5.0
clt_parameters.ofp.lpf_series, // half_run_range); // double half_run_range
debugLevel); // int debugLevel);
}
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref); //
double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // velocities and omegas from IMU
clt_parameters, // CLTParameters clt_parameters,
quadCLTs); // , // QuadCLT[] quadCLTs,
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
null, // double [] quat_corr, // only applies to rotations - verify!
-1); // debugLevel) ; // int debugLevel)
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
(quadCLTs.length -1), // int last_index,
debugLevel // final int debugLevel
);
int cent_index = earliest_scene + (quadCLTs.length - earliest_scene) / 2;
double [][] cent_xyzatr = pimu_xyzatr[cent_index];
// int ref_index,
// int earliest_scene,
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (scene == null) {
for (int i = 0; i < 26; i++) {
sb.append("\t---");
}
sb.append("\n");
continue;
}
double timestamp = scene.getTimeStamp();
Did_ins_1 d1 = scene.did_ins_1;
Did_ins_2 d2 = scene.did_ins_2;
Did_pimu d3 = scene.did_pimu;
sb.append(nscene+"\t"+timestamp);
if (use_processed) {
double [] nan3 = new double[] {Double.NaN, Double.NaN,Double.NaN};
double [][] nan23 = new double[][] {nan3,nan3};
if (scenes_xyzatr[nscene] != null) {
if (scenes_xyzatr[nscene][0] == null) scenes_xyzatr[nscene][0]= nan3;
if (scenes_xyzatr[nscene][1] == null) scenes_xyzatr[nscene][1]= nan3;
if (dxyzatr_dt[nscene]== null) dxyzatr_dt[nscene] = nan23;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene]== null) scenes_xyzatr_dt[nscene] = nan23;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
// x,y,z - scene position in reference scene frame (reference scene is all 0-s)
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
// a,tilt,roll - scene orientation in reference scene frame (reference scene is all 0-s)
sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
// Vx, Vy, Vz - linear velocities calculated from coordinates by running average (now +/- 5 scenes
sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
// Va, Vt, Vr - angular velocities calculated from coordinates by running average (now +/- 5 scenes
sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
sb.append("\t"+scenes_xyzatr_dt[nscene][0][0]+"\t"+scenes_xyzatr_dt[nscene][0][1]+"\t"+scenes_xyzatr_dt[nscene][0][2]);
sb.append("\t"+scenes_xyzatr_dt[nscene][1][0]+"\t"+scenes_xyzatr_dt[nscene][1][1]+"\t"+scenes_xyzatr_dt[nscene][1][2]);
} else {
for (int i = 0; i < 12; i++) {
sb.append("\t---");
}
}
}
sb.append("\t"+d1.timeOfWeek);
sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[2]);
sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);
sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);
sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);
//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
sb.append("\t"+d2.timeOfWeek);
sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.lla[0]+ "\t"+d2.lla[1]+ "\t"+d2.lla[2]);
double [] double_theta = d1.getTheta();
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
double [] double_qn2b = d2.getQn2b();
double [] double_uvw = d2.getUvw();
double [] uvw_dir = Imx5.applyQuaternionTo(double_qn2b, double_uvw, false); // bad
double [] uvw_inv = Imx5.applyQuaternionTo(double_qn2b, double_uvw, true); // good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp]
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
//"\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"
sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu_raw =Imx5.quaternionImsToCam(d2.getQEnu(),
new double[3], // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
double [] cam_xyz_ned = test_xyz_ned(
Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
d2_ref.getQn2b(), // double[] quat_ned,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
double [] quat_lma_enu_xyz = (quatCorr != null) ?
Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
cam_xyz_enu;
//"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"
// NED without ims_mount_atr correction \tcam_X1\tcam_Y1\tcam_Z1
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
// NED with ims_mount_atr correction \tcam_X2\tcam_Y2\tcam_Z2
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
//"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"
sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); // \tned->X\tned->Y\tned->Z
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr \tenu->X\tenu->Y\tenu->Z
//"\tINS->X\tINS->Y\tINS->Z"
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr "\tINS->X\tINS->Y\tINS->Z"
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
double [] scene_rel_atr=ErsCorrection.combineXYZATR(
ims_scene_xyzatr,
ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
double [] scene_raw_atr_enu = Imx5.quatToCamAtr(cam_quat_enu_raw); // not corrected by ims_mount_atr
double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu))[1];
// "\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
sb.append("\t"+scene_abs_atr[0]+ "\t"+scene_abs_atr[1]+ "\t"+scene_abs_atr[2]); //
sb.append("\t"+scene_rel_atr[0]+ "\t"+scene_rel_atr[1]+ "\t"+scene_rel_atr[2]); //
// "\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"
sb.append("\t"+scene_abs_atr_enu[0]+ "\t"+scene_abs_atr_enu[1]+ "\t"+scene_abs_atr_enu[2]); //
sb.append("\t"+scene_rel_atr_enu[0]+ "\t"+scene_rel_atr_enu[1]+ "\t"+scene_rel_atr_enu[2]); //
// "\traw_A_enu\traw_T_enu\traw_R_enu"+
sb.append("\t"+scene_raw_atr_enu[0]+ "\t"+scene_raw_atr_enu[1]+ "\t"+scene_raw_atr_enu[2]); //
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
// velocities and omegas from IMU
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
sb.append(
// getXyzatrIms () "\timsX\timsY\timsZ\timsA\timsT\timsR"
"\t"+ims_xyzatr[nscene][0][0]+ "\t"+ims_xyzatr[nscene][0][1]+ "\t"+ims_xyzatr[nscene][0][2]+
"\t"+ims_xyzatr[nscene][1][0]+ "\t"+ims_xyzatr[nscene][1][1]+ "\t"+ims_xyzatr[nscene][1][2]+
// integratePIMU() "\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR";
"\t"+pimu_xyzatr[nscene][0][0]+"\t"+pimu_xyzatr[nscene][0][1]+"\t"+pimu_xyzatr[nscene][0][2]+
"\t"+pimu_xyzatr[nscene][1][0]+"\t"+pimu_xyzatr[nscene][1][1]+"\t"+pimu_xyzatr[nscene][1][2]);
double [][] cent_diff = ErsCorrection.combineXYZATR(
pimu_xyzatr[nscene],
ErsCorrection.invertXYZATR(cent_xyzatr));
sb.append(
// "\tpimuX-C\tpimuY-C\tpimuZ-C\tpimuA-C\tpimuT-C\tpimuR-C" // difference to the center. XYZ - wrong (fix it)?
"\t"+cent_diff[0][0]+ "\t"+cent_diff[0][1]+ "\t"+cent_diff[0][2]+
"\t"+cent_diff[1][0]+ "\t"+cent_diff[1][1]+ "\t"+cent_diff[1][2]);
// add lpf
sb.append("\n");
}
// test QuaternionLMA here
// Add another data
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
sb.append("New ATR mount (rad):[\t"+new_atr[0]+"\t"+new_atr[1]+"\t"+new_atr[2]+"]\n");
sb.append("New ATR mount (deg):[\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"]\n");
// double [] quatCorr
if (quatCorr == null) {
sb.append("quatCorr = null\n");
} else {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
sb.append("quatCorr=[\t"+quatCorr[0]+"\t"+quatCorr[1]+"\t"+quatCorr[2]+"\t"+quatCorr[3]+"]\n");
sb.append("ATR(rad)=[\t"+corr_angles[0]+"\t"+corr_angles[1]+"\t"+corr_angles[2]+"]\n");
sb.append("ATR(deg)=[\t"+corr_degrees[0]+"\t"+corr_degrees[1]+"\t"+corr_degrees[2]+"]\n");
}
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
CalibrationFileManagement.saveStringToFile (
path,
header+"\n"+sb.toString()+"\n"+footer);
}else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
public static void generateEgomotionTable(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
// int ref_index,
QuadCLT ref_scene, // may be one of quadCLTs or center_CLT QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
int earliest_scene, int earliest_scene,
String path, String path,
String comment, String comment,
int debugLevel) { int debugLevel) {
boolean always_ref_scene=true; // temporarily reverting to older version that uses lla. Maybe it is not that bad as it fuses IMU+GPS
if ((ref_index >=0) && !always_ref_scene) {
ref_scene = quadCLTs[ref_index];
}
QuadCLT master_scene = ref_scene; QuadCLT master_scene = ref_scene;
if (ref_scene.did_ins_2 == null) { if (ref_scene.did_ins_2 == null) {
if (debugLevel > -3) { if (debugLevel > -3) {
...@@ -7412,10 +7116,11 @@ public class Interscene { ...@@ -7412,10 +7116,11 @@ public class Interscene {
// for testing QuaternionLma // for testing QuaternionLma
double [] rms = new double [2]; double [] rms = new double [2];
double [] enu_corr = new double[3]; double [] enu_corr = new double[3];
double [] quatCorr = getQuaternionCorrection( double [] quatCorr = null;
quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// ref_index, // int ref_index, ref_index, // int ref_index,
ref_scene, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT ref_scene, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2]; rms, // double [] rms // null or double[2];
...@@ -7426,7 +7131,7 @@ public class Interscene { ...@@ -7426,7 +7131,7 @@ public class Interscene {
System.out.println("generateEgomotionTable(): quatCorr=null"); System.out.println("generateEgomotionTable(): quatCorr=null");
} else { } else {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):"); System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical) 3:");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
...@@ -7728,10 +7433,10 @@ public class Interscene { ...@@ -7728,10 +7433,10 @@ public class Interscene {
}else{ }else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900); new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
} }
return;
} }
@Deprecated @Deprecated
// adjusts by all 3 axis rotation // adjusts by all 3 axis rotation
public static double [] getQuaternionCorrection_Old( public static double [] getQuaternionCorrection_Old(
...@@ -7811,180 +7516,97 @@ public class Interscene { ...@@ -7811,180 +7516,97 @@ public class Interscene {
return quaternionLma.getQuaternion(); return quaternionLma.getQuaternion();
} }
} }
// relative to the GPS/compass
@Deprecated
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
double [] rms, // null or double[2];
double [] enu_corr,
int debugLevel
) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][];
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
quat_lma_xyz[nscene] = new double[3];
} else {
String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
}
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
}
double [] up_axis = Imx5.getUpAxis(
ims_mount_atr); // double [] ims_atr )
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
int debug_level = debugLevel;
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareCompassLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y,
null, // double [][] vect_w, all same weight
up_axis, // double [] vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
debug_level); // int debug_level)
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debug_level); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (rms != null) { // null or double[2];
double [] last_rms = quaternionLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = quaternionLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
}
}
}
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
}
double [][] camera_enu = quaternionLma.cameraToENU(quaternionLma.getQuaternion());
double sw = 0;
double [] swd = new double[3];
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
double w = 1.0;
sw += w;
for (int i = 0; i < swd.length; i++) {
swd[i]+=w*(camera_enu[nscene][i]-quat_lma_enu_xyz[nscene][i]);
}
}
for (int i = 0; i < swd.length; i++) {
swd[i] /= sw;
}
if (enu_corr== null) {
enu_corr=new double[3]; // will generate, just not return
}
double [] enu= Imx5.applyQuaternionTo(// metric E,N,U
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
swd,
true); // inverse
for (int i = 0; i < 3; i++) {
enu_corr[i] = enu[i];
}
if (debug_level > -3) {
System.out.println("GNSS correction in camera X,Y,Z = ["+swd[0]+"]["+swd[1]+"]["+swd[2]+"]");
System.out.println("GNSS correction in camera E,N,U = ["+enu_corr[0]+"]["+enu_corr[1]+"]["+enu_corr[2]+"]");
}
if (debug_level > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s",
"N","GNSS-X","GNSS-Y","GNSS-Z",
"CAM-X","CAM-Y","CAM-Z"));
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
quat_lma_enu_xyz[nscene][0],quat_lma_enu_xyz[nscene][1],quat_lma_enu_xyz[nscene][2],
camera_enu[nscene][0],camera_enu[nscene][1],camera_enu[nscene][2]));
}
}
return quaternionLma.getAxisQuat();
}
}
// relative to the GPS/compass // relative to the GPS/compass
/** /**
* Finding correspondence between scene XYZ (ers_reference.getSceneXYZ(ts)) and IMS axes ( * Estimate camera rotation around the IMS vertical so that GNSS-derived motion vectors match the
* Imx5.applyQuaternionTo( * camera-centric scene XYZ displacements.
* Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu, * <p>
* ims_mount_atr, // small misalignment, provided in clt_parameters * Behaviour:
* ims_ortho), // fixed 90-degree rotation * <ul>
* @param clt_parameters - configuration parameters * <li>If {@code ref_index >= 0}, the IMS-to-camera quaternion is derived from PIMU integration
* @param quadCLTs * for the reference scene (more accurate when the reference scene is part of {@code quadCLTs}).</li>
* @param ref_scene * <li>Otherwise the alignment falls back to ENU offsets computed from lat/long/alt differences.</li>
* @param earliest_scene * <li>{@code enu_corr}, when provided, is populated with the averaged ENU correction between
* @param rms * GNSS and camera coordinates.</li>
* @param enu_corr * <li>Outputs the quaternion that rotates camera axes into IMS axes with the solved compass
* @param debugLevel * correction applied.</li>
* @return * </ul>
*
* @param clt_parameters configuration parameters
* @param quadCLTs all scenes used for alignment
* @param ref_index index of the reference scene within {@code quadCLTs}; negative to force
* using {@code ref_scene_in} only
* @param ref_scene_in reference scene when it is not part of {@code quadCLTs}
* @param earliest_scene first scene index to consider
* @param rms optional; receives {@code [finalRms, prevRms, initialRms, prevInitialRms, iterations]}
* @param enu_corr optional; receives ENU correction (length 3)
* @param debugLevel debug verbosity
* @return quaternion that compensates IMS-to-camera yaw (null on failure)
*/ */
public static double [] getQuaternionCorrection( public static double [] getQuaternionCorrection(
CLTParameters clt_parameters, CLTParameters clt_parameters,
QuadCLT [] quadCLTs, QuadCLT [] quadCLTs,
QuadCLT ref_scene, // may be one of quadCLTs or center_CLT int ref_index,
QuadCLT ref_scene_in, // may be one of quadCLTs or center_CLT
int earliest_scene, int earliest_scene,
double [] rms, // null or double[2]; double [] rms, // null or double[2];
double [] enu_corr, double [] enu_corr,
int debugLevel int debugLevel
) { ) {
/*
it seems that integratePIMU provides more accurate results but it is not available when the
reference scene is not one of the quadCLTs, such as in counter-UAS application (it is the axis direction
while quadCLTs are around it.
*/
boolean use_integrate_pimu = ref_index >=0;
QuadCLT ref_scene = use_integrate_pimu ? quadCLTs[ref_index]:ref_scene_in;
boolean debug_table = (debugLevel > 0);
if (ref_scene.did_ins_2 == null) { if (ref_scene.did_ins_2 == null) {
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): ref_scene.did_ins_2 == null, using last scene for reference"); System.out.println("getQuaternionCorrection(): ref_scene.did_ins_2 == null, using last scene for reference");
} }
ref_scene = quadCLTs[quadCLTs.length-1]; ref_scene = quadCLTs[quadCLTs.length-1];
} }
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
// QuadCLT ref_scene = quadCLTs[ref_index];
// boolean is_center = ref_scene.hasCenterClt();
ErsCorrection ers_reference = ref_scene.getErsCorrection(); ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][]; double [][] quat_lma_xyz = new double [quadCLTs.length][];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][]; double [][] quat_lma_enu_xyz = new double [quadCLTs.length][];
Did_ins_2 d2_ref = ref_scene.did_ins_2; Did_ins_2 d2_ref = ref_scene.did_ins_2;
if (use_integrate_pimu) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
(quadCLTs.length -1), // int last_index,
debugLevel // final int debugLevel
);
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
quat_lma_enu_xyz[nscene] = pimu_xyzatr[nscene][0];
}
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): used PIMU integration for camera-to-ims rotation.");
}
} else {
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East, North, Up from the gps coordinates (including altitude)?
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo( // camera xyz (relative to ref) from rela
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
}
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): used lat/long/alt differences for camera-to-ims rotation.");
}
}
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) { for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene]; QuadCLT scene = quadCLTs[nscene];
if (scene == ref_scene) { if (scene == ref_scene) {
...@@ -7993,18 +7615,33 @@ public class Interscene { ...@@ -7993,18 +7615,33 @@ public class Interscene {
String ts = scene.getImageName(); String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts); quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
} }
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
} }
double [] up_axis = Imx5.getUpAxis( double [] up_axis = Imx5.getUpAxis(
ims_mount_atr); // double [] ims_atr ) ims_mount_atr); // double [] ims_atr )
if (debug_table) { // debug_level > 0) {
System.out.println(String.format("%3s"+
// "\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s" + "\t%9s\t%9s\t%9s", // + "\t%9s\t%9s\t%9s" + "\t%9s\t%9s\t%9s",
"N","GNSS-X","GNSS-Y","GNSS-Z",
"CAM-X","CAM-Y","CAM-Z"));
for (int nscene = 0; nscene < quat_lma_enu_xyz.length; nscene++) if ((quat_lma_enu_xyz[nscene] != null) && (quat_lma_xyz[nscene] != null)) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f"+
// "\t%9.5f\t%9.5f\t%9.5f"+
// "\t%9.5f\t%9.5f\t%9.5f"+
// "\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
quat_lma_enu_xyz[nscene][0],quat_lma_enu_xyz[nscene][1],quat_lma_enu_xyz[nscene][2],
quat_lma_xyz[nscene][0],quat_lma_xyz[nscene][1],quat_lma_xyz[nscene][2]));
}
}
double lambda = clt_parameters.imp.quat_lambda; // 0.1; double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5; double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0; double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
...@@ -8079,7 +7716,7 @@ public class Interscene { ...@@ -8079,7 +7716,7 @@ public class Interscene {
System.out.println("GNSS correction in camera X,Y,Z = ["+swd[0]+"]["+swd[1]+"]["+swd[2]+"]"); System.out.println("GNSS correction in camera X,Y,Z = ["+swd[0]+"]["+swd[1]+"]["+swd[2]+"]");
System.out.println("GNSS correction in camera E,N,U = ["+enu_corr[0]+"]["+enu_corr[1]+"]["+enu_corr[2]+"]"); System.out.println("GNSS correction in camera E,N,U = ["+enu_corr[0]+"]["+enu_corr[1]+"]["+enu_corr[2]+"]");
} }
if (debug_level > 0) { if (debug_table) { // debug_level > 0) {
System.out.println(String.format("%3s"+ System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s"+ "\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s", "\t%9s\t%9s\t%9s",
...@@ -8099,9 +7736,6 @@ public class Interscene { ...@@ -8099,9 +7736,6 @@ public class Interscene {
} }
} }
static double [] test_xyz_ned( static double [] test_xyz_ned(
double [] ned, double [] ned,
double[] quat_ned, double[] quat_ned,
...@@ -8125,6 +7759,4 @@ public class Interscene { ...@@ -8125,6 +7759,4 @@ public class Interscene {
ims_ortho); ims_ortho);
return Imx5.applyQuaternionTo(cam_quat2, enu, false); return Imx5.applyQuaternionTo(cam_quat2, enu, false);
} }
} }
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ a675f808
...@@ -68,12 +68,6 @@ public class IntersceneMatchParameters { ...@@ -68,12 +68,6 @@ public class IntersceneMatchParameters {
public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418}; public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418};
public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1) public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1)
@Deprecated
public double [] ims_scale_xyz = {1.0,1.0,1.0}; // {1.1, 1.1, 1.1};
@Deprecated
public double [] ims_scale_atr = {1.0,1.0,1.0}; // {1.1, 1.1, 1.1};
public boolean fmg_initial_en = true; // enable IMS-based FPN mitigation for initial orientation public boolean fmg_initial_en = true; // enable IMS-based FPN mitigation for initial orientation
public boolean fmg_reorient_en = true; // enable IMS-based FPN mitigation for readjustmnet orientation public boolean fmg_reorient_en = true; // enable IMS-based FPN mitigation for readjustmnet orientation
public double fmg_distance = 10.0; // try to find other reference scene not closer than this pixels public double fmg_distance = 10.0; // try to find other reference scene not closer than this pixels
...@@ -1384,11 +1378,6 @@ min_str_neib_fpn 0.35 ...@@ -1384,11 +1378,6 @@ min_str_neib_fpn 0.35
gd.addStringField ("IMU accelerators gain", IntersceneMatchParameters.doublesToString(pimu_acc_offs), 80, gd.addStringField ("IMU accelerators gain", IntersceneMatchParameters.doublesToString(pimu_acc_offs), 80,
"IMU accelerators gain difference from 1. Negative values when IMU-reported velocities are smaller than actual"); "IMU accelerators gain difference from 1. Negative values when IMU-reported velocities are smaller than actual");
gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1' (not needed).");
gd.addMessage ("IMS-based FPN mitigation for interscene matching"); gd.addMessage ("IMS-based FPN mitigation for interscene matching");
gd.addCheckbox ("FPN mitigation for initial orientation", this.fmg_initial_en, gd.addCheckbox ("FPN mitigation for initial orientation", this.fmg_initial_en,
...@@ -3396,8 +3385,6 @@ min_str_neib_fpn 0.35 ...@@ -3396,8 +3385,6 @@ min_str_neib_fpn 0.35
this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3); this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3); this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.pimu_acc_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3); this.pimu_acc_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.fmg_initial_en = gd.getNextBoolean(); this.fmg_initial_en = gd.getNextBoolean();
this.fmg_reorient_en = gd.getNextBoolean(); this.fmg_reorient_en = gd.getNextBoolean();
this.fmg_distance = gd.getNextNumber(); this.fmg_distance = gd.getNextNumber();
...@@ -4582,8 +4569,6 @@ min_str_neib_fpn 0.35 ...@@ -4582,8 +4569,6 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz)); properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
properties.setProperty(prefix+"pimu_gyr_offs", IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs)); properties.setProperty(prefix+"pimu_gyr_offs", IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs));
properties.setProperty(prefix+"pimu_acc_offs", IntersceneMatchParameters.doublesToString(this.pimu_acc_offs)); properties.setProperty(prefix+"pimu_acc_offs", IntersceneMatchParameters.doublesToString(this.pimu_acc_offs));
properties.setProperty(prefix+"ims_scale_xyz", IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
properties.setProperty(prefix+"ims_scale_atr", IntersceneMatchParameters.doublesToString(this.ims_scale_atr));
properties.setProperty(prefix+"fmg_initial_en", this.fmg_initial_en + ""); // boolean properties.setProperty(prefix+"fmg_initial_en", this.fmg_initial_en + ""); // boolean
properties.setProperty(prefix+"fmg_reorient_en", this.fmg_reorient_en + ""); // boolean properties.setProperty(prefix+"fmg_reorient_en", this.fmg_reorient_en + ""); // boolean
properties.setProperty(prefix+"fmg_distance", this.fmg_distance+""); // double properties.setProperty(prefix+"fmg_distance", this.fmg_distance+""); // double
...@@ -5710,8 +5695,6 @@ min_str_neib_fpn 0.35 ...@@ -5710,8 +5695,6 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"ims_mount_xyz")!=null) this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3); if (properties.getProperty(prefix+"ims_mount_xyz")!=null) this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3); if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3);
if (properties.getProperty(prefix+"pimu_acc_offs")!=null) this.pimu_acc_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_acc_offs"),3); if (properties.getProperty(prefix+"pimu_acc_offs")!=null) this.pimu_acc_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_acc_offs"),3);
if (properties.getProperty(prefix+"ims_scale_xyz")!=null) this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
if (properties.getProperty(prefix+"ims_scale_atr")!=null) this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
if (properties.getProperty(prefix+"fmg_initial_en")!=null) this.fmg_initial_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_initial_en")); if (properties.getProperty(prefix+"fmg_initial_en")!=null) this.fmg_initial_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_initial_en"));
if (properties.getProperty(prefix+"fmg_reorient_en")!=null) this.fmg_reorient_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_reorient_en")); if (properties.getProperty(prefix+"fmg_reorient_en")!=null) this.fmg_reorient_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_reorient_en"));
if (properties.getProperty(prefix+"fmg_distance")!=null) this.fmg_distance=Double.parseDouble(properties.getProperty(prefix+"fmg_distance")); if (properties.getProperty(prefix+"fmg_distance")!=null) this.fmg_distance=Double.parseDouble(properties.getProperty(prefix+"fmg_distance"));
...@@ -6870,8 +6853,6 @@ min_str_neib_fpn 0.35 ...@@ -6870,8 +6853,6 @@ min_str_neib_fpn 0.35
imp.ims_mount_xyz = this.ims_mount_xyz.clone(); imp.ims_mount_xyz = this.ims_mount_xyz.clone();
imp.pimu_gyr_offs = this.pimu_gyr_offs.clone(); imp.pimu_gyr_offs = this.pimu_gyr_offs.clone();
imp.pimu_acc_offs = this.pimu_acc_offs.clone(); imp.pimu_acc_offs = this.pimu_acc_offs.clone();
imp.ims_scale_xyz = this.ims_scale_xyz.clone();
imp.ims_scale_atr = this.ims_scale_atr.clone();
imp.fmg_initial_en = this.fmg_initial_en; imp.fmg_initial_en = this.fmg_initial_en;
imp.fmg_reorient_en = this.fmg_reorient_en; imp.fmg_reorient_en = this.fmg_reorient_en;
imp.fmg_distance = this.fmg_distance; imp.fmg_distance = this.fmg_distance;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ a675f808
...@@ -4822,7 +4822,7 @@ public class OpticalFlow { ...@@ -4822,7 +4822,7 @@ public class OpticalFlow {
boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
// apply_quat_corr used inside getGroundIns() - used when generating output // apply_quat_corr used inside getGroundIns() - used when generating output
// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS // boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
boolean use_ims_rotation = clt_parameters.imp.use_quat_corr; // use internally (probably deprecated boolean use_ims_rotation = clt_parameters.imp.use_quat_corr; // use internally (probably deprecated - not)
boolean inertial_only = clt_parameters.imp.inertial_only; // use internally boolean inertial_only = clt_parameters.imp.inertial_only; // use internally
boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims) boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
...@@ -4979,7 +4979,7 @@ public class OpticalFlow { ...@@ -4979,7 +4979,7 @@ public class OpticalFlow {
// See if build_ref_dsi is needed // See if build_ref_dsi is needed
if (!build_ref_dsi) { if (!build_ref_dsi) {
// try reading full // try reading full
quadCLTs[last_index] = (QuadCLT) quadCLT_main.spawnNoModelQuadCLT( // will conditionImageSet quadCLTs[last_index] = (QuadCLT) quadCLT_main.spawnNoModelQuadCLT( // will conditionImageSet copies quat_corr from this
set_channels[last_index].set_name, set_channels[last_index].set_name,
clt_parameters, clt_parameters,
colorProcParameters, // colorProcParameters, //
...@@ -5296,7 +5296,7 @@ public class OpticalFlow { ...@@ -5296,7 +5296,7 @@ public class OpticalFlow {
if (center_CLT == null) { // does not work in CUAS mode if (center_CLT == null) { // does not work in CUAS mode
ref_blue_sky = quadCLTs[last_index].getDoubleBlueSky(); // new ref_blue_sky = quadCLTs[last_index].getDoubleBlueSky(); // new
if (!bypass_last_index) { if (!bypass_last_index) {
quadCLTs[last_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN" quadCLTs[last_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN", sets quat_corr, sets it in quadCLTs[last_index]
set_channels[last_index].set_name, set_channels[last_index].set_name,
clt_parameters, clt_parameters,
colorProcParameters, // colorProcParameters, //
...@@ -5423,7 +5423,7 @@ public class OpticalFlow { ...@@ -5423,7 +5423,7 @@ public class OpticalFlow {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// ref_index,// ref_indx, ref_index, //-1, //ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -5718,7 +5718,7 @@ public class OpticalFlow { ...@@ -5718,7 +5718,7 @@ public class OpticalFlow {
} }
} }
if (!done_sfm) { // first pass or sfm failed. Always here in CUAS mode if (!done_sfm) { // first pass or sfm failed. Always here in CUAS mode
boolean compensate_dsi = true; boolean compensate_dsi = false; // true;
// should skip scenes w/o orientation 06/29/2022 // should skip scenes w/o orientation 06/29/2022
// TODO: Implement for CUAS // TODO: Implement for CUAS
...@@ -5838,7 +5838,28 @@ public class OpticalFlow { ...@@ -5838,7 +5838,28 @@ public class OpticalFlow {
!batch_mode, // boolean test_motion_blur, !batch_mode, // boolean test_motion_blur,
debugLevel) ; // int debugLevel) debugLevel) ; // int debugLevel)
}else { }else {
boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull
boolean lma_use_XY = ( !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)));
boolean lma_use_AT = ( readjust_xy_ims || lpf_xy);
if (clt_parameters.imp.air_mode_en) { if (clt_parameters.imp.air_mode_en) {
int min_adjust_atr = 2;
if (master_CLT.getNumOrient() < min_adjust_atr) {
disable_ers = true;
disable_ers_y = true;
disable_ers_r = true;
lma_xyzatr = false;
lma_use_Z = false;
lma_use_R = false;
lma_use_XY = true;
lma_use_AT = false;
}
// boolean disable_ers = (master_CLT.getNumOrient() < mb_ers_index);
// boolean disable_ers_y = (master_CLT.getNumOrient() < mb_ers_y_index);
// boolean disable_ers_r = (master_CLT.getNumOrient() < mb_ers_r_index);
// boolean lma_xyzatr = (master_CLT.getNumOrient() == mb_all_index);
// boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
// boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean apply_disparity_ims = clt_parameters.imp.air_disp_corr; boolean apply_disparity_ims = clt_parameters.imp.air_disp_corr;
// So far the same parameters. Modify - use IMS A,T,R,(Z ?), adjust X,Y, (Z?), same as for initial adjustment // So far the same parameters. Modify - use IMS A,T,R,(Z ?), adjust X,Y, (Z?), same as for initial adjustment
earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
...@@ -5846,6 +5867,8 @@ public class OpticalFlow { ...@@ -5846,6 +5867,8 @@ public class OpticalFlow {
mb_max_gain, // double mb_max_gain, mb_max_gain, // double mb_max_gain,
lma_use_Z, // boolean use_Z, lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R, lma_use_R, // boolean use_R,
lma_use_XY, // boolean use_XY,
lma_use_AT, // boolean use_AT,
disable_ers, // boolean disable_ers, disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y, disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r, disable_ers_r, // boolean disable_ers_r,
...@@ -5935,6 +5958,8 @@ public class OpticalFlow { ...@@ -5935,6 +5958,8 @@ public class OpticalFlow {
mb_max_gain, // double mb_max_gain, mb_max_gain, // double mb_max_gain,
lma_use_Z, // boolean use_Z, lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R, lma_use_R, // boolean use_R,
lma_use_XY, // boolean use_XY,
lma_use_AT, // boolean use_AT,
disable_ers, // boolean disable_ers, disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y, disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r, disable_ers_r, // boolean disable_ers_r,
...@@ -5980,7 +6005,7 @@ public class OpticalFlow { ...@@ -5980,7 +6005,7 @@ public class OpticalFlow {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// ref_index,// ref_indx, -1, // ref_indx,
master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -6181,10 +6206,15 @@ public class OpticalFlow { ...@@ -6181,10 +6206,15 @@ public class OpticalFlow {
if (calc_quat_corr) { if (calc_quat_corr) {
double [] quat_rms = new double [5]; double [] quat_rms = new double [5];
double [] enu_corr = new double[3]; double [] enu_corr = new double[3];
int corr_index = -1;
if ((ref_index >= 0) &&(quadCLTs[ref_index] == master_CLT)) {
corr_index = ref_index;
}
double [] quatCorr = Interscene.getQuaternionCorrection( double [] quatCorr = Interscene.getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
// ref_index, // int ref_index, corr_index, // ref_index, // int ref_index,
master_CLT, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT master_CLT, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
quat_rms, // double [] rms // null or double[2]; quat_rms, // double [] rms // null or double[2];
...@@ -6207,7 +6237,7 @@ public class OpticalFlow { ...@@ -6207,7 +6237,7 @@ public class OpticalFlow {
StringBuffer sb = new StringBuffer(); StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n"); sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
sb.append("Applying correction to the IMS to world orientation (rotating around IMS vertical):\n"); sb.append("Applying correction to the IMS to world orientation (rotating around IMS vertical) 1:\n");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
...@@ -6340,6 +6370,7 @@ public class OpticalFlow { ...@@ -6340,6 +6370,7 @@ public class OpticalFlow {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
-1, // ref_indx,
master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -6354,6 +6385,7 @@ public class OpticalFlow { ...@@ -6354,6 +6385,7 @@ public class OpticalFlow {
Interscene.generateEgomotionTable( Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
-1, // ref_indx,
master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
...@@ -10535,856 +10567,6 @@ public class OpticalFlow { ...@@ -10535,856 +10567,6 @@ public class OpticalFlow {
return zdiff; return zdiff;
} }
@Deprecated
public static double [][] intersceneExport(
CLTParameters clt_parameters,
boolean compensate_dsi,
int ref_index,
QuadCLT [] scenes_in,
ColorProcParameters colorProcParameters,
int debug_level
)
{
final boolean fsplit_en = clt_parameters.rig.fsplit_en; // true; // enable split of far tiles into FG/BG with LMA
final int fsplit_mode = clt_parameters.rig.fsplit_mode; // 1; // 0: disable, 1:avg->fg + fg->fg +fg->bg, 2: avg->fg+fg->bg, 3: avg->fg, avg->bg
final double fsplit_str = clt_parameters.rig.fsplit_str; // 0.4; // minimal strength of the tile to split
final int fsplit_neibs = clt_parameters.rig.fsplit_neibs; // 3; // minimal n umber of neighbors to consider split
final int fsplit_neibs_side =clt_parameters.rig.fsplit_neibs_side;// 2; // minimal number of on each side (for thin FG over BG)
final double fsplit_neib_str = clt_parameters.rig.fsplit_neib_str; // 0.4; // minimal strength of the neighbor to compare
final double fsplit_neib_diff = clt_parameters.rig.fsplit_neib_diff; // 1.5; // maximal disparity difference to neighbor to use
final double fsplit_disp = clt_parameters.rig.fsplit_disp; // 3.0; // maximal tile disparity to consider split.
final double fsplit_adiff = clt_parameters.rig.fsplit_adiff; // 0.08;// minimal tile (combo) absolute disparity difference from neighbors
final double fsplit_rdiff = clt_parameters.rig.fsplit_rdiff; // 0.1; // minimal tile (combo) relative disparity difference from neighbors
final double fsplit_kfg_min = clt_parameters.rig.fsplit_kfg_min; // 0.1; // minimal fraction of kfg and (1.0-kfg)
final double fsplit_min_ratio = clt_parameters.rig.fsplit_min_ratio;// 0.8; // minimal ratio of the weakest to strongest of fg and bg strengths
final double fsplit_min_diff = clt_parameters.rig.fsplit_min_diff; // 0.08;// minimal FG to BG disparity difference
final double fsplit_rms_gain = clt_parameters.rig.fsplit_rms_gain; // 1.1; // minimal LMA RMS improvement of splitting to FG and BG
final boolean fsplit_dbg = clt_parameters.rig.fsplit_dbg;
int fsplit_refines = 0;
if (fsplit_en) switch (fsplit_mode) {
case 1: fsplit_refines = 1; break;
case 2: fsplit_refines = 3; break;
}
QuadCLT [] scenes = scenes_in.clone();
ErsCorrection ers_reference = scenes[ref_index].getErsCorrection();
int indx_ref = scenes.length - 1; // Always added to the end even if out-of order
if (ref_index != indx_ref) {
System.arraycopy(
scenes_in,
ref_index+1,
scenes,
ref_index,
indx_ref-ref_index);
scenes[indx_ref] = scenes_in[ref_index];
}
final double scale_far_split_strength = 1.5;
// empirical correction for both lma and non-lma step
double corr_nonlma = 1.0; // 1.23;
double corr_lma = 1.0; // 1.23;
// reference scene is always added to the end, even is out of timestamp order
QuadCLT ref_scene = scenes[indx_ref]; // ordered by increasing timestamps
boolean generate_outlines = false; // true; // TODO: move to configs
System.out.println("intersceneExport(), scene timestamp="+ref_scene.getImageName());
// int num_scenes = scenes.length;
String [] combo_dsn_titles_full = COMBO_DSN_TITLES.clone(); // keep all titles and slices
String [] combo_dsn_titles = new String [COMBO_DSN_INDX_DISP_BG];
/*
for (int i = 0; i < combo_dsn_titles.length; i++) {
combo_dsn_titles[i] = combo_dsn_titles_full[i];
}
if (clt_parameters.rig.mll_max_refines_bg <= 0) {
combo_dsn_titles_full = combo_dsn_titles;
}
*/
double min_disp_change = clt_parameters.rig.mll_min_disp_change_pre; // 0.001; // stop re-measure when difference is below
final int max_refines_presplit =
clt_parameters.rig.mll_max_refines_bg +
clt_parameters.rig.mll_max_refines_lma +
clt_parameters.rig.mll_max_refines_pre;
final int max_refines = max_refines_presplit + fsplit_refines;
final int [] iter_indices = {
COMBO_DSN_INDX_DISP,
COMBO_DSN_INDX_STRENGTH,
COMBO_DSN_INDX_LMA,
COMBO_DSN_INDX_CHANGE}; // which to save for each iteration: {"disp", "strength","disp_lma","change"};
final int [] initial_indices = {
COMBO_DSN_INDX_DISP,
COMBO_DSN_INDX_STRENGTH,
COMBO_DSN_INDX_VALID}; // initial: "disp", "strength","num_valid"
double [][] combo_dsn = null;
// USE reference scene if no individual DSI are available double [][] dsrbg = scene.getDSRBG(); Probably null for non-reference
// make sure to use already integrated if available
double [][] combo_dsn0 =scenes[indx_ref].readComboDSI(true); // scenes[indx_ref].dsi and blue sky are NOT updated
if (combo_dsn0 == null) {
combo_dsn0 = prepareInitialComboDS( // 3
clt_parameters, // final CLTParameters clt_parameters,
scenes, // final QuadCLT [] scenes,
//// indx_ref, // final int indx_ref,
scenes[indx_ref], // final QuadCLT ref_scene,
debug_level-2); // final int debug_level);
} else {
// set blue sky from read from file ONLY if no BS was calculated for this instance
if ((combo_dsn0[COMBO_DSN_INDX_BLUE_SKY] != null) && !scenes[indx_ref].hasBlueSky()) {
if (debug_level > -2) {
System.out.println("There was no calculated Blue Sky, but a file had it. Will use the loaded one");
scenes[indx_ref].setBlueSky(combo_dsn0[COMBO_DSN_INDX_BLUE_SKY]);
}
}
combo_dsn0 = conditionComboDsnFinal(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
combo_dsn0, // double [][] combo_dsn_final, // dls,
scenes[indx_ref], // QuadCLT scene,
debug_level); // int debugLevel);
}
combo_dsn = new double[combo_dsn_titles.length - 1][];
for (int i = 0; i < initial_indices.length; i++) {
combo_dsn[initial_indices[i]] = combo_dsn0[i]; // "disp", "strength", <null>, "num_valid"
}
final int margin = 8;
final int tilesX = ref_scene.getTileProcessor().getTilesX();
final int tilesY = ref_scene.getTileProcessor().getTilesY();
final int tiles = tilesX * tilesY;
// uses 2 GB - change format
if (generate_outlines) { // debug_level > 100) { // add parameter?
int extra = 10; // pixels around largest outline
int scale = 4;
int line_width_outline = 1;
int line_width_corners = 3;
Color line_color_outline = new Color(0, 255, 0); // green
Color line_color_corners = new Color(0, 255, 255); // cyan
// generating scene outlines for results documentation
ImagePlus imp_outlines = generateSceneOutlines(
ref_scene, // QuadCLT ref_scene, // ordered by increasing timestamps
scenes, // QuadCLT [] scenes
extra, // int extra // add around largest outline
scale, // int scale,
line_width_outline, // int line_width_outline,
line_width_corners, // int line_width_corners,
line_color_outline, // Color line_color_outline,
line_color_corners // Color line_color_corners
);
ref_scene.saveImagePlusInModelDirectory(
"scene_outlines", // String suffix,
imp_outlines); // ImagePlus imp)
// imp_outlines.show();
}
final double [][] combo_dsn_change = new double [combo_dsn_titles.length] [tiles];
for (int i = 0; i < combo_dsn.length; i++) { // 4 elements: "disp", "strength","disp_lma","num_valid"
if (combo_dsn[i] != null) combo_dsn_change[i] = combo_dsn[i]; // all but change
}
if (debug_level > 0) {
ShowDoubleFloatArrays.showArrays(
combo_dsn_change,
tilesX,
tilesY,
true,
"combo_dsn-initial"+ref_scene.getImageName(),
combo_dsn_titles); // dsrbg_titles);
}
final int last_slices = combo_dsn_titles.length;
final int last_initial_slices = last_slices + initial_indices.length;
final boolean [] defined_tiles = new boolean [tiles];
for (int i = 0; i < defined_tiles.length; i++) {
defined_tiles[i] = !Double.isNaN(combo_dsn_change[COMBO_DSN_INDX_DISP][i]); // initially defined tiles
}
final double [][] refine_results = new double [last_slices + 4 * (max_refines + 1)][];
String [] refine_titles = new String [refine_results.length];
for (int i = 0; i < combo_dsn_titles.length; i++) {
refine_results[i] = combo_dsn_change[i]; // first 5 - references to 5-element combo_dsn_change
refine_titles[i] = combo_dsn_titles[i]+"-last"; // "disp", "strength","disp_lma","num_valid","change"
}
for (int i = 0; i < initial_indices.length; i++) {
refine_titles[last_slices + i] = combo_dsn_titles[initial_indices[i]]+"-initial"; // "disp", "strength","num_valid"
if (combo_dsn_change[initial_indices[i]] != null) {
refine_results[last_slices + i] = combo_dsn_change[initial_indices[i]].clone();
} else {
refine_results[last_slices + i] = new double [tiles];
}
}
for (int nrefine = 0; nrefine < max_refines; nrefine++) {
for (int i = 0; i < iter_indices.length; i++) {
refine_titles[last_initial_slices + i * max_refines + nrefine ] = combo_dsn_titles[iter_indices[i]]+"-"+nrefine;
}
}
double [] target_disparity = combo_dsn_change[COMBO_DSN_INDX_DISP].clone();
double [] target_disparity_orig = target_disparity.clone(); // will just use NaN/not NaN to restore tasks before second pass with LMA
// double [][] combo_dsn_final = new double [combo_dsn_titles.length][combo_dsn[0].length];
// double [][] combo_dsn_final =
// new double [(clt_parameters.rig.mll_max_refines_bg > 0)?
// combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
double [][] combo_dsn_final =
new double [COMBO_DSN_TITLES.length][combo_dsn[0].length];
//COMBO_DSN_INDX_SFM_GAIN
combo_dsn_final[COMBO_DSN_INDX_DISP]= combo_dsn[COMBO_DSN_INDX_DISP].clone();
for (int i = 1; i < combo_dsn_final.length; i++) {
if ( (i != COMBO_DSN_INDX_SFM_GAIN) &&
(i != COMBO_DSN_INDX_STRENGTH) &&
(i != COMBO_DSN_INDX_STRENGTH_BG)){
Arrays.fill(combo_dsn_final[i], Double.NaN);
}
}
// Save pair selection and minimize them for scanning, then restore;
int num_sensors =scenes[indx_ref].getNumSensors();
int save_pairs_selection = clt_parameters.img_dtt.getMcorr(num_sensors);
int save_bimax_combine_mode = clt_parameters.img_dtt.bimax_combine_mode;
boolean save_bimax_dual_only = clt_parameters.img_dtt.bimax_dual_only;
if (clt_parameters.rig.mll_max_refines_bg > 0) {
clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_FG; // initially refine FG
clt_parameters.img_dtt.bimax_dual_only = false; // not just dual only
if (debug_level > -3) {
System.out.println("intersceneExport(): set CAMEL_FG");
}
}
// FIXME: uncomment next
System.out.println ("++++ Uncomment next line (Done) ++++");
clt_parameters.img_dtt.setMcorr(num_sensors, 0 ); // remove all
clt_parameters.img_dtt.setMcorrNeib(num_sensors,true);
clt_parameters.img_dtt.setMcorrSq (num_sensors,true); // remove even more?
clt_parameters.img_dtt.setMcorrDia (num_sensors,true); // remove even more?
boolean save_run_lma = clt_parameters.correlate_lma;
clt_parameters.correlate_lma = false;
double [] disp_err = new double [combo_dsn[0].length]; // last time measure disparity error from correlation (scaled for CM)
double [] corr_scale = new double [combo_dsn[0].length]; // applied correction scale
boolean [] was_lma = new boolean [combo_dsn[0].length]; // previous measurement used LMA (not to match LMA/non-LMA measurements)
Arrays.fill(disp_err, Double.NaN);
Arrays.fill(corr_scale, 1.0);
double min_scale = 0.75, max_scale = 1.5; // reset correction scale to 1.0 if calculated is too big/small (jumping FG/BG)
double [][] dbg_corr_scale = null;
if (debug_level > 0) {
dbg_corr_scale = new double[max_refines][];
}
boolean [] selection = new boolean [target_disparity.length];
int nTiles = selection.length;
for (int i = 0; i < target_disparity.length; i++) {
selection[i] = !Double.isNaN(target_disparity[i]);
}
boolean [] selection_orig = selection.clone();
// Refine disparity map, each time recalculating each scene "projection" pixel X, pixel Y and
// disparity in each scene image set that correspond to the reference scene uniform grid tile.
// First use reduced number of pairs and no LMA, then continue with all pairs and LMA
boolean bg_refine= false;
final double [][] far_fgbg = new double [selection.length][];
final double [][] pre_split_ds = new double [2][];
double [][] far_fg_ds = null;
double [][] far_bg_ds = null;
boolean [] sel_split = new boolean [selection.length];
boolean [] far_split = null; // new boolean [nTiles];
double [][] far_fg_ds_merge = new double[2][];
double [][] far_bg_ds_merge = new double[2][];
double [] used_rms_lma = new double[selection.length]; // last rms from LMA, not including BG refine
double [] split_rms_lma = new double[selection.length];
Arrays.fill(used_rms_lma, Double.NaN);
boolean use_rms = true; // DISPARITY_STRENGTH_INDEX means LMA RMS (18/04/2023)
for (int nrefine = 0; nrefine < max_refines; nrefine++) {
if (nrefine == clt_parameters.rig.mll_max_refines_pre) {
min_disp_change = clt_parameters.rig.mll_min_disp_change_lma;
clt_parameters.img_dtt.setMcorr(num_sensors, save_pairs_selection); // restore
clt_parameters.correlate_lma = save_run_lma; // restore
selection = selection_orig.clone();
if (debug_level > -2) {
int num_tomeas = 0;
for (int nt = 0; nt < target_disparity.length; nt++) if (selection[nt]) { // (!Double.isNaN(target_disparity[nt])){
num_tomeas++;
}
System.out.println ("a. nrefine pass = "+nrefine+", remaining "+num_tomeas+" tiles to re-measure");
}
} else if (nrefine == (clt_parameters.rig.mll_max_refines_pre + clt_parameters.rig.mll_max_refines_lma)) {
// Processing BG - single pass
clt_parameters.img_dtt.bimax_dual_only = true; // Camel-max tiles only
clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_BG;
bg_refine= true;
selection = selection_orig.clone(); // re-select all original tiles
if (debug_level > -3) {
System.out.println("intersceneExport(): set CAMEL_BG, bimax_dual_only=true @"+nrefine);
}
} else if (nrefine == (clt_parameters.rig.mll_max_refines_pre + clt_parameters.rig.mll_max_refines_lma + 1)) {
clt_parameters.img_dtt.bimax_dual_only = false; // not just dual only
if (debug_level > -3) {
System.out.println("intersceneExport(): set bimax_dual_only=false @"+nrefine);
}
}
if (nrefine == (clt_parameters.rig.mll_max_refines_pre + clt_parameters.rig.mll_max_refines_lma + clt_parameters.rig.mll_max_bg_nearest)) {
clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_NEAREST;
if (debug_level > -3) {
System.out.println("intersceneExport(): set CAMEL_NEAREST @"+nrefine);
}
}
int split_pass = nrefine - max_refines_presplit;
double [][] avg_ds = new double[][] {
combo_dsn_final[COMBO_DSN_INDX_DISP],
combo_dsn_final[COMBO_DSN_INDX_STRENGTH]};
int split_src = -1;
if (split_pass >= 0) {
if (split_pass == 0) {
split_src= 0 ; // AVG
target_disparity=avg_ds[0];
} else if (split_pass == 1) {
split_src= 1 ; clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_FG;
target_disparity=far_fg_ds[0];
} else { // if (split_pass == 2) {
split_src= 2 ; clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_BG;
target_disparity=far_bg_ds[0];
}
// 04/11/2023: always outputs both FG and BG
clt_parameters.img_dtt.bimax_combine_mode = Correlation2d.CAMEL_FG;
sel_split = calcFarFgBg(
split_src, // final int split_src, // target disparity is for 0 - avg, 1 - fg, 2 - bg
fsplit_str, // final double fsplit_str, // minimal strength of the tile to split
fsplit_neibs, // final int fsplit_neibs, // minimal number of neighbors to consider split
fsplit_neibs_side,// final int fsplit_neibs_side,// minimal number of neighbors from each side to consider thin FG
fsplit_neib_str, // final double fsplit_neib_str, // minimal strength of the neighbor to use
fsplit_neib_diff,// final double fsplit_neib_diff,// maximal disparity difference to neighbor to use
fsplit_disp, // final double fsplit_disp, // maximal tile disparity to consider split, also for neibs.
fsplit_adiff, // final double fsplit_adiff, // minimal tile (combo) absolute disparity difference from neighbors
fsplit_rdiff, // final double fsplit_rdiff, // minimal tile (combo) relative disparity difference from neighbors
fsplit_kfg_min, // final double fsplit_kfg_min, // minimal fraction of kfg and (1.0-kfg)
far_fgbg, // final double [][] far_fgbg, // should be initialized to [nTiles][?]
avg_ds, // final double [][] avg_ds, // always defined, used for selection
far_fg_ds, // final double [][] fg_ds,
far_bg_ds, // final double [][] bg_ds,
far_split, // null, // final boolean [] selection, // may be null, does not apply to neighbors
tilesX, // final int tilesX,
fsplit_dbg? (ref_scene.getImageName()+"-FGBG-"+split_pass):null); // final String dbg_title);
//far_split
selection = sel_split; // for correlateInterscene()
if (split_src == 0) {
far_fg_ds = new double[2][];
far_bg_ds = new double[2][];
far_fg_ds[0] = new double[nTiles];
far_bg_ds[0] = new double[nTiles];
Arrays.fill(far_fg_ds[0], Double.NaN);
Arrays.fill(far_bg_ds[0], Double.NaN);
far_fg_ds_merge[0] = avg_ds[0].clone();
far_bg_ds_merge[0] = avg_ds[0].clone();
far_fg_ds_merge[1] = avg_ds[1].clone();
far_bg_ds_merge[1] = avg_ds[1].clone();
}
}
int mcorr_sel = ImageDttParameters.corrSelEncode(clt_parameters.img_dtt,num_sensors);
double [][] disparity_map =
correlateInterscene( // should skip scenes w/o orientation 06/29/2022
clt_parameters, // final CLTParameters clt_parameters,
scenes, // final QuadCLT [] scenes,
//// indx_ref, // final int indx_ref,
scenes[indx_ref],// final QuadCLT ref_scene,
target_disparity, // combo_dsn_change[combo_dsn_indx_disp], // final double [] disparity_ref, // disparity in the reference view tiles (Double.NaN - invalid)
selection, // final boolean [] selection, // may be null, if not null do not process unselected tiles
far_fgbg, // final double [][] far_fgbg, // null, or [nTile]{disp(fg)-disp(bg), str(fg)-str(bg)} hints for LMA FG/BG split
margin, // final int margin,
nrefine, // final int nrefine, // just for debug title
false, // ( nrefine == (max_refines - 1)) && clt_parameters.inp.show_final_2d, // final boolean show_2d_corr,
mcorr_sel, // final int mcorr_sel, // =
null, // final float [][][] accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
false, // final boolean no_map, // do not generate disparity_map (time-consuming LMA)
use_rms, // final boolean use_rms, // DISPARITY_STRENGTH_INDEX means LMA RMS (18/04/2023)
debug_level-8); // final int debug_level)
if (debug_level > 0) { //-3) {
ShowDoubleFloatArrays.showArrays(
disparity_map,
tilesX,
tilesY,
true,
"accumulated_disparity_map-"+nrefine,
ImageDtt.getDisparityTitles(ref_scene.getNumSensors(),ref_scene.isMonochrome()) // ImageDtt.DISPARITY_TITLES
);
}
// update disparities
double [] map_disparity = disparity_map[ImageDtt.DISPARITY_INDEX_CM]; // 2
// double [] map_strength = disparity_map[ImageDtt.DISPARITY_STRENGTH_INDEX]; // 10
double [] map_strength_lma = disparity_map[ImageDtt.DISPARITY_INDEX_POLY+1]; // 9
double [] map_strength = disparity_map[ImageDtt.DISPARITY_INDEX_CM+1]; // 3
if (use_rms && clt_parameters.correlate_lma) { // non-elegant way to build old DISPARITY_STRENGTH_INDEX
map_strength = map_strength.clone();
for (int i = 0; i < map_strength.length; i++) if (map_strength_lma[i] > 0.0) {
map_strength[i] = map_strength_lma[i];
}
}
double [] map_rms_lma = disparity_map[ImageDtt.DISPARITY_STRENGTH_INDEX]; // 8
double [] map_disparity_lma = disparity_map[ImageDtt.DISPARITY_INDEX_POLY]; // 8
// only for far split.
double [] map_far_fg_disparity = disparity_map[ImageDtt.DISPARITY_INDEX_POLY];
double [] map_far_fg_strength = disparity_map[ImageDtt.DISPARITY_INDEX_POLY+1];
double [] map_far_bg_disparity = disparity_map[ImageDtt.DISPARITY_INDEX_CM];
double [] map_far_bg_strength = disparity_map[ImageDtt.DISPARITY_INDEX_CM+1];
if (split_pass >= 0) {
if (far_split == null) {
far_split = new boolean [nTiles];
}
// single-threaded
if (split_src == 0) {
for (int tile = 0; tile < nTiles; tile++) {
if (!Double.isNaN(map_far_fg_disparity[tile]) && !Double.isNaN(map_far_bg_disparity[tile])) {
far_split[tile] = true;
far_fg_ds[0][tile] = target_disparity[tile] + map_far_fg_disparity[tile];
far_bg_ds[0][tile] = target_disparity[tile] + map_far_bg_disparity[tile];
far_fg_ds_merge[0][tile] = far_fg_ds[0][tile];
far_bg_ds_merge[0][tile] = far_bg_ds[0][tile];
far_fg_ds_merge[1][tile] = map_far_fg_strength[tile];
far_bg_ds_merge[1][tile] = map_far_bg_strength[tile];
}
}
} else if (split_src == 1) {
for (int tile = 0; tile < nTiles; tile++) {
if (!Double.isNaN(map_far_fg_disparity[tile])) {
far_split[tile] = true;
far_fg_ds[0][tile] = target_disparity[tile] + map_far_fg_disparity[tile];
far_fg_ds_merge[0][tile] = far_fg_ds[0][tile];
far_fg_ds_merge[1][tile] = map_far_fg_strength[tile];
}
}
} else if (split_src == 2) {
for (int tile = 0; tile < nTiles; tile++) {
if (!Double.isNaN(map_far_bg_disparity[tile])) {
far_split[tile] = true;
far_bg_ds[0][tile] = target_disparity[tile] + map_far_bg_disparity[tile];
far_bg_ds_merge[0][tile] = far_bg_ds[0][tile];
far_bg_ds_merge[1][tile] = map_far_bg_strength[tile];
}
}
}
far_fg_ds[1] = map_far_fg_strength;
far_bg_ds[1] = map_far_bg_strength;
if (debug_level > 0) { //-3) {
double [][] predicted_fg_ds = new double[2][nTiles];
double [][] predicted_bg_ds = new double[2][nTiles];
double [][] predicted_fg_ds_merge = new double[2][];
double [][] predicted_bg_ds_merge = new double[2][];
Arrays.fill(predicted_fg_ds[0], Double.NaN);
Arrays.fill(predicted_bg_ds[0], Double.NaN);
predicted_fg_ds_merge[0]= avg_ds[0].clone();
predicted_bg_ds_merge[0]= avg_ds[0].clone();
predicted_fg_ds_merge[1]= avg_ds[1].clone();
predicted_bg_ds_merge[1]= avg_ds[1].clone();
double [][] fg_predicted_err = new double[2][nTiles];
double [][] bg_predicted_err = new double[2][nTiles];
Arrays.fill(fg_predicted_err[0], Double.NaN);
Arrays.fill(bg_predicted_err[0], Double.NaN);
Arrays.fill(fg_predicted_err[1], Double.NaN);
Arrays.fill(bg_predicted_err[1], Double.NaN);
for (int tile = 0; tile < nTiles; tile++) {
if (far_fgbg[tile] != null) {
double fg_targ = far_fgbg[tile][0];
double bg_targ = far_fgbg[tile][1];
double kfg = far_fgbg[tile][2];
predicted_fg_ds[0][tile] = fg_targ + target_disparity[tile]; // disparity(FG)
predicted_bg_ds[0][tile] = bg_targ + target_disparity[tile]; // disparity(BG)
predicted_fg_ds[1][tile] = avg_ds[1][tile] * kfg; // strength(FG)
predicted_bg_ds[1][tile] = avg_ds[1][tile] * (1.0 - kfg); // strength(BG)
if (!Double.isNaN(far_fg_ds[0][tile]) && !Double.isNaN(far_fg_ds[0][tile])) {
fg_predicted_err[0][tile] = predicted_fg_ds[0][tile] - far_fg_ds[0][tile];
bg_predicted_err[0][tile] = predicted_bg_ds[0][tile] - far_bg_ds[0][tile];
fg_predicted_err[1][tile] = predicted_fg_ds[1][tile] - far_fg_ds[1][tile];
bg_predicted_err[1][tile] = predicted_bg_ds[1][tile] - far_bg_ds[1][tile];
}
predicted_fg_ds_merge[0][tile] = predicted_fg_ds[0][tile]; // disparity(FG)
predicted_bg_ds_merge[0][tile] = predicted_bg_ds[0][tile]; // disparity(BG)
predicted_fg_ds_merge[1][tile] = avg_ds[1][tile] * kfg; // strength(FG)
predicted_bg_ds_merge[1][tile] = avg_ds[1][tile] * (1.0 - kfg); // strength(BG)
split_rms_lma[tile] = map_rms_lma[tile];
}
}
String [] dbg_far_spit_titles={
"avg_disp", // 0
"fg_disp", // 1
"bg_disp", // 2
"fg-bg", // 3
"fg_disp_only", // 4
"bg_disp_only", // 5
"fg_disp_predicted_only",// 6
"bg_disp_predicted_only",// 7
"fg_disp_predicted_err", // 8
"bg_disp_predicted_err", // 9
"avg_str", // 10
"fg_str", // 11
"bg_str", // 12
"fg_str_only", // 13
"bg_str_only", // 14
"fg_str_predicted_only", // 15
"bg_str_predicted_only", // 16
"fg_str_predicted_err", // 17
"bg_str_predicted_err", // 18
"sel", // 19
"target_disparity", // 20
"rrms_single", // 21
"rrms_split", // 22
"rrms_gain", // 23
"str_ratio"}; // 24
double [][] dbg_img = new double[dbg_far_spit_titles.length][];
dbg_img[ 0] = avg_ds[0];
dbg_img[ 1] = far_fg_ds_merge[0];
dbg_img[ 2] = far_bg_ds_merge[0];
dbg_img[ 3] = new double [nTiles];
dbg_img[ 4] = far_fg_ds[0];
dbg_img[ 5] = far_bg_ds[0];
dbg_img[ 6] = predicted_fg_ds[0];
dbg_img[ 7] = predicted_bg_ds[0];
dbg_img[ 8] = fg_predicted_err[0];
dbg_img[ 9] = bg_predicted_err[0];
dbg_img[10] = avg_ds[1];
dbg_img[11] = far_fg_ds_merge[1];
dbg_img[12] = far_bg_ds_merge[1];
dbg_img[13] = far_fg_ds[1];
dbg_img[14] = far_bg_ds[1];
dbg_img[15] = predicted_fg_ds[1];
dbg_img[16] = predicted_bg_ds[1];
dbg_img[17] = fg_predicted_err[1];
dbg_img[18] = bg_predicted_err[1];
dbg_img[19] = new double [nTiles];
dbg_img[21] = new double [nTiles];
dbg_img[22] = new double [nTiles];
dbg_img[23] = new double [nTiles];
dbg_img[24] = new double [nTiles];
for (int tile = 0; tile < nTiles; tile++) {
// update index below !
dbg_img[19][tile] = (selection[tile] ? 0.5:0.0) + (far_split[tile] ? 0.6:0.0);
dbg_img[ 3][tile] = dbg_img[ 1][tile] - dbg_img[ 2][tile];
dbg_img[21][tile] = 1.0/used_rms_lma[tile];
dbg_img[22][tile] = 1.0/map_rms_lma[tile];
dbg_img[23][tile] = used_rms_lma[tile]/map_rms_lma[tile];
double sr = far_fg_ds[1][tile]/far_bg_ds[1][tile];
if (sr > 1.0) {
sr = 1.0/sr;
}
dbg_img[24][tile] = sr;
}
dbg_img[20] = target_disparity;
ShowDoubleFloatArrays.showArrays(
dbg_img,
tilesX,
tilesY,
true,
"far-split"+split_pass,
dbg_far_spit_titles);
}
} else { //if (split_pass >= 0) { // pre-far_fgbg disparity processing
int num_tomeas = 0; // number of tiles to measure
Arrays.fill(selection, false);
for (int nTile =0; nTile < combo_dsn_change[0].length; nTile++) {
if (defined_tiles[nTile]) { // originally defined, maybe not measured last time
if (!Double.isNaN(map_disparity[nTile])) { // re-measured
boolean is_lma = (map_disparity_lma != null) && !Double.isNaN(map_disparity_lma[nTile]);
if (is_lma) {
combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile] = map_disparity_lma[nTile] * corr_nonlma;
} else if (!Double.isNaN(map_disparity[nTile])) {
combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile] = map_disparity[nTile] / clt_parameters.ofp.magic_scale * corr_lma;
}
if (!Double.isNaN(combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile])) {
// will keep previous new_corr_scale value when no previous data or switching non-LMA/LMA. Should be initialized to 1.0
if (!Double.isNaN(disp_err[nTile]) && (is_lma == was_lma[nTile])) {
// current target_disparity: combo_dsn_change[combo_dsn_indx_disp][nTile]
double this_target_disparity = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile];
double previous_target_disparity = this_target_disparity - corr_scale[nTile] * disp_err[nTile];
double prev_disp_err = disp_err[nTile];
double this_disp_err = combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile];
double new_corr_scale = (previous_target_disparity - this_target_disparity) / (this_disp_err - prev_disp_err);
if ((new_corr_scale <= max_scale) && (new_corr_scale >= min_scale)) {
corr_scale[nTile] = new_corr_scale;
}
}
combo_dsn_change[COMBO_DSN_INDX_DISP][nTile] += combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile] * corr_scale[nTile];
was_lma[nTile] = is_lma;
combo_dsn_change[COMBO_DSN_INDX_STRENGTH][nTile] = map_strength[nTile]; // combine CM/LMA
if (bg_refine) {
combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile] = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile];
combo_dsn_final[COMBO_DSN_INDX_STRENGTH_BG][nTile] = combo_dsn_change[COMBO_DSN_INDX_STRENGTH][nTile];
combo_dsn_final[COMBO_DSN_INDX_LMA_BG][nTile] = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile];
if (map_disparity_lma != null) {
combo_dsn_final[COMBO_DSN_INDX_LMA_BG][nTile] = Double.isNaN(map_disparity_lma[nTile])? Double.NaN : combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile];
}
combo_dsn_final[COMBO_DSN_INDX_CHANGE_BG][nTile] = combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile];
} else {
combo_dsn_final[COMBO_DSN_INDX_DISP][nTile] = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile];
combo_dsn_final[COMBO_DSN_INDX_STRENGTH][nTile] = combo_dsn_change[COMBO_DSN_INDX_STRENGTH][nTile];
combo_dsn_final[COMBO_DSN_INDX_LMA][nTile] = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile];
if (map_disparity_lma != null) {
combo_dsn_final[COMBO_DSN_INDX_LMA][nTile] = Double.isNaN(map_disparity_lma[nTile])? Double.NaN : combo_dsn_final[COMBO_DSN_INDX_DISP][nTile];
// saving last LMA RMS to compare against split FG/BG to see if it improves (or significantly improves?)
if (!Double.isNaN(map_disparity_lma[nTile])) {
used_rms_lma[nTile] = map_rms_lma[nTile];
}
}
combo_dsn_final[COMBO_DSN_INDX_VALID][nTile] = combo_dsn[COMBO_DSN_INDX_VALID][nTile]; // not much sense
combo_dsn_final[COMBO_DSN_INDX_CHANGE][nTile] = combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile];
}
}
disp_err[nTile] = combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile];
if (Math.abs(combo_dsn_change[COMBO_DSN_INDX_CHANGE][nTile]) >= min_disp_change) {
target_disparity[nTile] = combo_dsn_change[COMBO_DSN_INDX_DISP][nTile] ;
selection[nTile] = true;
num_tomeas ++;
} else {
num_tomeas+=0;
}
} else { // originally defined, but not re-measured
num_tomeas+=0;
}
}
}
if (dbg_corr_scale != null) {
dbg_corr_scale[nrefine] = corr_scale.clone();
}
// Copy disparity to disparity_lma and just mask out tiles with no LMA data (keep all if LMA did not run at all)
System.arraycopy(combo_dsn_change[COMBO_DSN_INDX_DISP], 0, combo_dsn_change[COMBO_DSN_INDX_LMA], 0, combo_dsn_change[COMBO_DSN_INDX_DISP].length); // lma
if (map_disparity_lma != null) {
for (int i = 0; i < map_disparity_lma.length; i++) {
if (Double.isNaN(map_disparity_lma[i])) {
combo_dsn_change[COMBO_DSN_INDX_LMA][i] = Double.NaN;
}
}
}
for (int i = 0; i < iter_indices.length; i++) {
refine_results[last_initial_slices + (i * max_refines) + nrefine] = combo_dsn_change[iter_indices[i]].clone();
}
// clt_parameters.inp.show_final_2d, // final boolean show_2d_corr,
if (debug_level >0) {
ShowDoubleFloatArrays.showArrays(
combo_dsn_change,
tilesX,
tilesY,
true,
"combo_dsn-"+nrefine+"-"+ref_scene.getImageName(),
combo_dsn_titles); // dsrbg_titles);
}
if (debug_level > -2) {
System.out.println ("b. nrefine pass = "+nrefine+", clt_parameters.correlate_lma="+clt_parameters.correlate_lma+", remaining "+num_tomeas+" tiles to re-measure");
}
if (num_tomeas == 0) {
break;
}
} // else { //if (split_pass >= 0) {
} //for (int nrefine = 0; nrefine < max_refines; nrefine++) {
// Add duplicate of FG disparity and FG+BG disparity (FG where no BG) for visual comparison
if (true) { // clt_parameters.rig.mll_max_refines_bg > 0) {
for (int nTile =0; nTile < combo_dsn_change[0].length; nTile++) {
combo_dsn_final[COMBO_DSN_INDX_DISP_FG][nTile] = combo_dsn_final[COMBO_DSN_INDX_DISP][nTile];
if (Double.isNaN(combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile])) {
combo_dsn_final[COMBO_DSN_INDX_DISP_BG_ALL][nTile] = combo_dsn_final[COMBO_DSN_INDX_DISP][nTile];
} else {
combo_dsn_final[COMBO_DSN_INDX_DISP_BG_ALL][nTile] = combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile];
}
}
}
// add far split if available
boolean mod_cumul_disp = true;
boolean mod_cumul_bg_disp = true;
if ((far_fg_ds != null) && (far_fg_ds[0] != null)) {
for (int nTile =0; nTile < combo_dsn_change[0].length; nTile++) {
if (!Double.isNaN(far_fg_ds[0][nTile]) && !Double.isNaN(far_bg_ds[0][nTile])) {
// filtering
double fg_bg_diff = far_fg_ds[0][nTile] - far_bg_ds[0][nTile];
if (fg_bg_diff < fsplit_min_diff) {
continue;
}
double rms_gain = used_rms_lma[nTile] / split_rms_lma[nTile];
if (rms_gain < fsplit_rms_gain) {
continue;
}
double weak_strong_ratio = far_fg_ds[1][nTile] / far_bg_ds[1][nTile];
if (weak_strong_ratio > 1) {
weak_strong_ratio = 1.0 / weak_strong_ratio;
}
if (weak_strong_ratio < fsplit_min_ratio) {
continue;
}
combo_dsn_final[COMBO_DSN_INDX_DISP_FG][nTile] = far_fg_ds[0][nTile];
combo_dsn_final[COMBO_DSN_INDX_LMA][nTile] = far_fg_ds[0][nTile];
if (mod_cumul_disp) {
combo_dsn_final[COMBO_DSN_INDX_DISP][nTile] = far_fg_ds[0][nTile];
}
combo_dsn_final[COMBO_DSN_INDX_STRENGTH][nTile] = far_fg_ds[1][nTile] * scale_far_split_strength;
combo_dsn_final[COMBO_DSN_INDX_DISP_BG_ALL][nTile] = far_bg_ds[0][nTile];
combo_dsn_final[COMBO_DSN_INDX_LMA_BG][nTile] = far_bg_ds[0][nTile];
if (mod_cumul_bg_disp) {
combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile] = far_bg_ds[0][nTile];
}
combo_dsn_final[COMBO_DSN_INDX_STRENGTH_BG][nTile] = far_bg_ds[1][nTile] * scale_far_split_strength;
}
}
}
// add blue sky slice
/*
boolean [] blue_sky = ref_scene.getBlueSky();
double [] payload_blue_sky = combo_dsn_final[combo_dsn_final.length-1]; // last slice, length by titles
Arrays.fill(payload_blue_sky,Double.NaN);
if (blue_sky != null) {
for (int i = 0; i < tiles; i++) {
payload_blue_sky[i] = blue_sky[i] ? 1.0 : 0.0;
}
}
*/
double [] payload_blue_sky; // = combo_dsn_final[combo_dsn_final.length-1]; // last slice, length by titles
if (ref_scene.hasBlueSky()) {
payload_blue_sky = ref_scene.getDoubleBlueSky().clone();
} else {
payload_blue_sky = new double[tiles];
Arrays.fill(payload_blue_sky,Double.NaN);
}
combo_dsn_final[COMBO_DSN_INDX_BLUE_SKY] = payload_blue_sky;
// restore modified parameters
clt_parameters.img_dtt.bimax_combine_mode = save_bimax_combine_mode;
clt_parameters.img_dtt.bimax_dual_only = save_bimax_dual_only;
if (dbg_corr_scale != null) {
ShowDoubleFloatArrays.showArrays(
dbg_corr_scale,
tilesX,
tilesY,
true,
"Correction scales"
);
}
// Do above twice: with 40 pairs, no-lma and then with all pairs+LMA
if (debug_level > 1) {
ShowDoubleFloatArrays.showArrays(
combo_dsn_change,
tilesX,
tilesY,
true,
"combo_dsn_change-"+ref_scene.getImageName(),
combo_dsn_titles); // dsrbg_titles);
ShowDoubleFloatArrays.showArrays(
combo_dsn,
tilesX,
tilesY,
true,
"combo_dsn-"+ref_scene.getImageName(),
combo_dsn_titles); // dsrbg_titles);
ShowDoubleFloatArrays.showArrays(
combo_dsn_final,
tilesX,
tilesY,
true,
"combo_dsn-final-"+ref_scene.getImageName(),
combo_dsn_titles_full); // dsrbg_titles);
}
if (debug_level > 0) {
ShowDoubleFloatArrays.showArrays(
refine_results,
tilesX,
tilesY,
true,
"combo-"+max_refines+"-"+ref_scene.getImageName(),
refine_titles); // dsrbg_titles);
}
//noise_sigma_level
String rslt_suffix = "-INTER-INTRA-HISTORIC";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
// ref_index from the center is moved to the very end (indx_ref)
// was bug - used ref_index Maybe that was intentional?
if (compensate_dsi && !scenes[indx_ref].interDsiExists()) {
System.out.println("Will compensate pose Z as average disparity may change");
double z_corr = compensateDSI(
clt_parameters, // CLTParameters clt_parameters,
compensate_dsi, // boolean compensate_dsi, // false - do not apply, just calculate
scenes[indx_ref], // ref_index], // QuadCLT ref_scene,
combo_dsn_final[COMBO_DSN_INDX_LMA], // double [] disparity, // new disparity
debug_level); // int debug_level
System.out.println("Z correction = "+z_corr);
}
ref_scene.saveDoubleArrayInModelDirectory(
rslt_suffix, // String suffix,
refine_titles, // null, // String [] labels, // or null
refine_results, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
// fixing NaN in strengths. It is uses to return RMS in Not needed - NaN was from Arrays.fill(combo_dsn_final[i], Double.NaN);
// ImageDtt.clt_process_tl_correlations( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
// if (use_rms) nan_slices.add(DISPARITY_STRENGTH_INDEX); // will be used for RMS if LMA succeeded
// And NaN in strength cause (at least) NaN for double avg_z = quadCLTs[ref_index].getAverageZ(true);
// public static int COMBO_DSN_INDX_STRENGTH = 1; // strength, FG
// public static int COMBO_DSN_INDX_STRENGTH_BG = 6; // background strength
for (int slice: COMBO_DSN_NONNAN) { // new int[] {COMBO_DSN_INDX_STRENGTH,COMBO_DSN_INDX_STRENGTH_BG}) {
if (combo_dsn_final[slice] != null) {
for (int i = 0; i <combo_dsn_final[slice].length; i++) {
if (Double.isNaN(combo_dsn_final[slice][i])) {
combo_dsn_final[slice][i] = 0.0;
}
}
}
}
ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
// System.out.println("IntersceneAccumulate(), got previous scenes: "+sts.length);
if (debug_level > 1) { // tested OK
System.out.println("IntersceneAccumulate(): preparing image set...");
int nscenes = scenes.length;
// int indx_ref = nscenes - 1;
double [][][] all_scenes_xyzatr = new double [scenes.length][][]; // includes reference (last)
double [][][] all_scenes_ers_dt = new double [scenes.length][][]; // includes reference (last)
all_scenes_xyzatr[indx_ref] = new double [][] {ZERO3,ZERO3};
all_scenes_ers_dt[indx_ref] = new double [][] {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()};
for (int i = 0; i < nscenes; i++) if (i != indx_ref) {
String ts = scenes[i].getImageName();
all_scenes_xyzatr[i] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
all_scenes_ers_dt[i] = new double[][] {ers_reference.getSceneErsXYZ_dt(ts), ers_reference.getSceneErsATR_dt(ts)};
}
compareRefSceneTiles( // null pointer
"" , // String suffix,
true, // false, // boolean blur_reference,
all_scenes_xyzatr, // double [][][] scene_xyzatr, // does not include reference
all_scenes_ers_dt, // double [][][] scene_ers_dt, // does not include reference
scenes, // QuadCLT [] scenes,
8); // int iscale) // 8
}
// Moved to intersceneMlExport()
return combo_dsn_final;
}
public static double [][] intersceneExport( public static double [][] intersceneExport(
boolean save_result, // false - do not save boolean save_result, // false - do not save
int [] scene_range, // if null -> {0,scenes_in.length-1} int [] scene_range, // if null -> {0,scenes_in.length-1}
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ a675f808
...@@ -6138,7 +6138,7 @@ if (debugLevel < -100) { ...@@ -6138,7 +6138,7 @@ if (debugLevel < -100) {
return quadCLT; return quadCLT;
} }
public QuadCLT spawnNoModelQuadCLT( public QuadCLT spawnNoModelQuadCLT( // copies quat_corr from this
String set_name, String set_name,
CLTParameters clt_parameters, CLTParameters clt_parameters,
ColorProcParameters colorProcParameters, // ColorProcParameters colorProcParameters, //
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ a675f808
...@@ -224,10 +224,7 @@ public class QuadCLTCPU { ...@@ -224,10 +224,7 @@ public class QuadCLTCPU {
public String ims_last_path = null; public String ims_last_path = null;
public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames) public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames)
public double [] enu_corr_metric = null; // GNSS correction - add metrix offset to GNSS of the reference scene public double [] enu_corr_metric = null; // GNSS correction - add metrix offset to GNSS of the reference scene
@Deprecated
public double [][] pimu_offsets = new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
// latest (or any? first also?) scene in a sequence has a timestamp of the reference scene // latest (or any? first also?) scene in a sequence has a timestamp of the reference scene
public String timestamp_reference = null; public String timestamp_reference = null;
// only reference scene has a pair of first/last scene in a sequence // only reference scene has a pair of first/last scene in a sequence
...@@ -1665,11 +1662,11 @@ public class QuadCLTCPU { ...@@ -1665,11 +1662,11 @@ public class QuadCLTCPU {
prev_scene = nscene; prev_scene = nscene;
} }
} }
if (debugLevel>0) { if (debugLevel>0) { // null pointer at com.elphel.imagej.tileprocessor.QuadCLTCPU.integratePIMU(QuadCLTCPU.java:1676)
System.out.println(String.format( System.out.println(String.format(
"%3s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s", "%3s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
"N","X","Y","Z","A","T","R")); "N","X","Y","Z","A","T","R"));
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) { for (int nscene = 0; nscene < quadCLTs.length; nscene++) if ((quadCLTs[nscene] != null) && (xyzatr[nscene] != null)){
System.out.println(String.format( System.out.println(String.format(
"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f", "%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
nscene, nscene,
...@@ -1704,7 +1701,7 @@ public class QuadCLTCPU { ...@@ -1704,7 +1701,7 @@ public class QuadCLTCPU {
// scenes_xyzatr[ref_index] = new double[2][3]; // all zeros // scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
Did_ins_2 d2_ref = did_ins_2; Did_ins_2 d2_ref = did_ins_2;
if (d2_ref == null) { if (d2_ref == null) {
d2_ref=quadCLTs[quadCLTs.length-1].did_ins_2; d2_ref=quadCLTs[quadCLTs.length-1].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
...@@ -1724,12 +1721,12 @@ public class QuadCLTCPU { ...@@ -1724,12 +1721,12 @@ public class QuadCLTCPU {
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) { for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
QuadCLT scene = quadCLTs[nscene]; QuadCLT scene = quadCLTs[nscene];
Did_ins_2 d2 = scene.did_ins_2; Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up May have inacurate altitude
double [] cam_quat_enu =Imx5.quaternionImsToCam( double [] cam_quat_enu =Imx5.quaternionImsToCam( //per scene rotation from enu (from lla) to camera
d2.getQEnu(), d2.getQEnu(),
ims_mount_atr, ims_mount_atr,
ims_ortho); ims_ortho);
if (quat_corr != null) { if (quat_corr != null) { // with correction if exists
cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false); cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
} }
double [] cam_xyz_enu = Imx5.applyQuaternionTo( double [] cam_xyz_enu = Imx5.applyQuaternionTo(
...@@ -3198,7 +3195,7 @@ public class QuadCLTCPU { ...@@ -3198,7 +3195,7 @@ public class QuadCLTCPU {
} }
return geometryCorrection.getNumSensors(); return geometryCorrection.getNumSensors();
} }
public QuadCLTCPU( public QuadCLTCPU( // copies quat_corr
QuadCLTCPU qParent, QuadCLTCPU qParent,
String name String name
){ ){
...@@ -6878,12 +6875,6 @@ public class QuadCLTCPU { ...@@ -6878,12 +6875,6 @@ public class QuadCLTCPU {
if (this.enu_corr_metric != null) { if (this.enu_corr_metric != null) {
properties.setProperty(prefix+"enu_corr_metric", IntersceneMatchParameters.doublesToString(this.enu_corr_metric)); properties.setProperty(prefix+"enu_corr_metric", IntersceneMatchParameters.doublesToString(this.enu_corr_metric));
} }
if (this.pimu_offsets != null) {
properties.setProperty(prefix+"pimu_offsets_lin", IntersceneMatchParameters.doublesToString(this.pimu_offsets[0]));
properties.setProperty(prefix+"pimu_offsets_ang", IntersceneMatchParameters.doublesToString(this.pimu_offsets[1]));
}
return properties; return properties;
} }
...@@ -7065,14 +7056,7 @@ public class QuadCLTCPU { ...@@ -7065,14 +7056,7 @@ public class QuadCLTCPU {
if (properties.getProperty(prefix+"enu_corr_metric")!=null) { if (properties.getProperty(prefix+"enu_corr_metric")!=null) {
this.enu_corr_metric= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"enu_corr_metric"),3); this.enu_corr_metric= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"enu_corr_metric"),3);
} }
if (properties.getProperty(prefix+"pimu_offsets_lin")!=null) {
this.pimu_offsets[0]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_lin"),3);
}
if (properties.getProperty(prefix+"pimu_offsets_ang")!=null) {
this.pimu_offsets[1]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_ang"),3);
}
} }
public void setKernelImageFile(ImagePlus img_kernels){ // not used in lwir public void setKernelImageFile(ImagePlus img_kernels){ // not used in lwir
......
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