Skip to content

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

Working on orientation adjustment camera to IMS

parent b098f6c0
Expand all Show whitespace changes
Inline Side-by-side
Showing with 934 additions and 127 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 9e5d5d56
......@@ -7922,6 +7922,12 @@ public class Interscene {
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
if ((rms.length >= 6) && (last_rms.length > 2)){
rms[5] = last_rms[2];
if ((rms.length >= 7) && (last_rms.length > 3)){
rms[6] = last_rms[3];
}
}
}
}
}
......@@ -8212,6 +8218,12 @@ public class Interscene {
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
if ((rms.length >= 6) && (last_rms.length > 2)){
rms[5] = last_rms[2];
if ((rms.length >= 7) && (last_rms.length > 3)){
rms[6] = last_rms[3];
}
}
}
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 9e5d5d56
......@@ -1104,8 +1104,9 @@ min_str_neib_fpn 0.35
public double wser_inf_thresh = 0.5; // Undefined yet parameter to select which segments to use in infinity disparity adjustment
public boolean wser_orient_en = true; // per-series orientation dcamera-to-imu adjust enable
public double wser_orient_transl = 0.5; // weight of translations for orientation.
public double wser_orient_transl = 0.5; // weight of translations for orientation, AGL-corrected.
public boolean wser_orient_save = true; // Save per-series infinity adjustments data as CSV file
public boolean wser_orient_series = true; // Save per-series orientations and summary
public boolean wser_orient_apply = false; // Apply per-series infinity correction
public double wser_orient_thresh = 0.5; // Undefined yet parameter to select which segments to use in infinity disparity adjustment
......@@ -1138,7 +1139,8 @@ min_str_neib_fpn 0.35
public int quat_num_iter = 20;
public double quat_reg_w = 0.25;
public double quat_max_ratio = 5.0; // max derivatives difference
public double quat_max_ratio = 3.0; // 5.0; // max derivatives ratio
public double quat_max_damping = 0.5; // maximal regularization weight
public double quat_max_change = 0.1; // radians
public double quat_min_transl = 5.0; // meters to adjust by translation
public double quat_min_rot = 0.1; // radians to adjust by rotation
......@@ -1881,7 +1883,7 @@ min_str_neib_fpn 0.35
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
"Apply camera orientation correction from predicted by IMS when generating output files - used only for ground planes.");
gd.addCheckbox ("Use IMS orientation correction internally", this.use_quat_corr,
"Depreceted? Use camera orientation correction from predicted by IMS internally from the previous calculations.");
"Deprecated? Use camera orientation correction from predicted by IMS internally from the previous calculations.");
gd.addCheckbox ("Inertial only initial poses", this.inertial_only,
"Use only IMU (DID_PIMU) data for initial egomotion, not the GNSS-aided IMS output.");
......@@ -3253,8 +3255,10 @@ min_str_neib_fpn 0.35
"Per-series orientation dcamera-to-imu adjust enable.");
gd.addNumericField("Translations weight", this.wser_orient_transl, 5,7,"",
"Weight of translations for orientation (remaining - orintation).");
gd.addCheckbox ("Save per-series orientation correction", this.wser_orient_save,
"Save per-series orintation adjustments data as CSV file.");
gd.addCheckbox ("Save scene sequence orientation correction", this.wser_orient_save,
"Save scene sequence orintation adjustments data as CSV file.");
gd.addCheckbox ("Save per-series orientation correction", this.wser_orient_series,
"Save per-series orintation adjustments data as CSV files and their summary table in index scene folder.");
gd.addCheckbox ("Apply per-series orientation correction", this.wser_orient_apply,
"Apply per-series orientation correction.");
gd.addNumericField("Orientation adjustment thershold", this.wser_orient_thresh, 5,7,"",
......@@ -3318,6 +3322,8 @@ min_str_neib_fpn 0.35
gd.addNumericField("Maximal derivatives by axes ratio", this.quat_max_ratio, 3,5,"x",
"If difference is larger, add regularization to reduce it.");
gd.addNumericField("Maximal regularization fraction (<1)", this.quat_max_damping, 3,5,"x",
"If difference is larger, add regularization to reduce it.");
gd.addNumericField("Maximal correction angles change", this.quat_max_change, 3,5,"rad",
"Do not update corrections if any angle is above this limit. ");
gd.addNumericField("Minimal translation for mount calibration", this.quat_min_transl, 3,5,"m",
......@@ -4611,6 +4617,7 @@ min_str_neib_fpn 0.35
this.wser_orient_en = gd.getNextBoolean();
this.wser_orient_transl = gd.getNextNumber();
this.wser_orient_save = gd.getNextBoolean();
this.wser_orient_series = gd.getNextBoolean();
this.wser_orient_apply = gd.getNextBoolean();
this.wser_orient_thresh = gd.getNextNumber();
......@@ -4643,6 +4650,7 @@ min_str_neib_fpn 0.35
this.quat_reg_w = gd.getNextNumber();
this.quat_max_ratio = gd.getNextNumber();
this.quat_max_damping = gd.getNextNumber();
this.quat_max_change = gd.getNextNumber();
this.quat_min_transl = gd.getNextNumber();
this.quat_min_rot = gd.getNextNumber();
......@@ -5853,6 +5861,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"wser_orient_en", this.wser_orient_en+""); // boolean
properties.setProperty(prefix+"wser_orient_transl", this.wser_orient_transl+""); // double
properties.setProperty(prefix+"wser_orient_save", this.wser_orient_save+""); // boolean
properties.setProperty(prefix+"wser_orient_series", this.wser_orient_series+""); // boolean
properties.setProperty(prefix+"wser_orient_apply", this.wser_orient_apply+""); // boolean
properties.setProperty(prefix+"wser_orient_thresh", this.wser_orient_thresh+""); // double
......@@ -5884,6 +5893,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"quat_reg_w", this.quat_reg_w+""); // double
properties.setProperty(prefix+"quat_max_ratio", this.quat_max_ratio+""); // double
properties.setProperty(prefix+"quat_max_damping", this.quat_max_damping+""); // double
properties.setProperty(prefix+"quat_max_change", this.quat_max_change+""); // double
properties.setProperty(prefix+"quat_min_transl", this.quat_min_transl+""); // double
properties.setProperty(prefix+"quat_min_rot", this.quat_min_rot+""); // double
......@@ -7079,6 +7089,7 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"wser_orient_en")!=null) this.wser_orient_en=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_en"));
if (properties.getProperty(prefix+"wser_orient_transl")!=null) this.wser_orient_transl=Double.parseDouble (properties.getProperty(prefix+"wser_orient_transl"));
if (properties.getProperty(prefix+"wser_orient_save")!=null) this.wser_orient_save=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_save"));
if (properties.getProperty(prefix+"wser_orient_series")!=null) this.wser_orient_series=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_series"));
if (properties.getProperty(prefix+"wser_orient_apply")!=null) this.wser_orient_apply=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_apply"));
if (properties.getProperty(prefix+"wser_orient_thresh")!=null) this.wser_orient_thresh=Double.parseDouble (properties.getProperty(prefix+"wser_orient_thresh"));
......@@ -7110,6 +7121,7 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"quat_reg_w")!=null) this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));
if (properties.getProperty(prefix+"quat_max_ratio")!=null) this.quat_max_ratio=Double.parseDouble(properties.getProperty(prefix+"quat_max_ratio"));
if (properties.getProperty(prefix+"quat_max_damping")!=null) this.quat_max_damping=Double.parseDouble(properties.getProperty(prefix+"quat_max_damping"));
if (properties.getProperty(prefix+"quat_max_change")!=null) this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
if (properties.getProperty(prefix+"quat_min_transl")!=null) this.quat_min_transl=Double.parseDouble(properties.getProperty(prefix+"quat_min_transl"));
if (properties.getProperty(prefix+"quat_min_rot")!=null) this.quat_min_rot=Double.parseDouble(properties.getProperty(prefix+"quat_min_rot"));
......@@ -8284,6 +8296,7 @@ min_str_neib_fpn 0.35
imp.wser_orient_en = this.wser_orient_en;
imp.wser_orient_transl = this.wser_orient_transl;
imp.wser_orient_save = this.wser_orient_save;
imp.wser_orient_series = this.wser_orient_series;
imp.wser_orient_apply = this.wser_orient_apply;
imp.wser_orient_thresh = this.wser_orient_thresh;
......@@ -8316,6 +8329,7 @@ min_str_neib_fpn 0.35
imp.quat_reg_w = this.quat_reg_w;
imp.quat_max_ratio = this.quat_max_ratio;
imp.quat_max_damping = this.quat_max_damping;
imp.quat_max_change = this.quat_max_change;
imp.quat_min_transl = this.quat_min_transl;
imp.quat_min_rot = this.quat_min_rot;
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 9e5d5d56
......@@ -112,6 +112,8 @@ public class QuadCLTCPU {
public static final String IMU_CALIB_LOGS_SUFFIX = "-IMU_CALIB.log";
public static final String IMU_CALIB_DETAILS_SUFFIX = "-IMU_CALIB_DETAILS.csv";
public static final String IMU_CALIB_COMBO_SUFFIX = "-IMU_CALIB_COMBO.csv";
public static final String IMU_CALIB_SUMMARY_SUFFIX = "-IMU_CALIB_SUMMARY.csv";
public static final String IMU_CALIB_SUM_COMBO_SUFFIX = "-IMU_CALIB_SUMMARY_COMBO.csv";
public static final String ORIENTATION_LOGS_SUFFIX = "-ORIENTATION.log";
public static final String INTERFRAME_SUFFIX = "-INTERFRAME";
public static final String FIELD_CALIBRATION_SUFFIX = "-FIELD_CALIBRATION";
......@@ -699,9 +701,12 @@ public class QuadCLTCPU {
int ref_index,
int early_index,
int last_index,
double [] rms, // null or double[5];
double [] rms, // null or double[5]; now [7]?
int debugLevel
) {
if (quadCLTs[ref_index].getImageName().equals("1763233239_531146")) {
System.out.println("getRotationFromXYZATRCameraIms(): quadCLTs[ref_index].getImageName().getImageName()=="+quadCLTs[ref_index].getImageName());
}
boolean use_scale_y = false;
// final boolean use3 = (quat_lma_mode == 3); // true;
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
......@@ -712,8 +717,8 @@ public class QuadCLTCPU {
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio;
double quat_max_damping = clt_parameters.imp.quat_max_damping;
double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
......@@ -743,10 +748,18 @@ public class QuadCLTCPU {
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
if (debugLevel > -3) {
System.out.println("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
}
// if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
if (max_to_min > quat_max_ratio) {
reg_w = quat_max_damping * (max_to_min - quat_max_ratio) / max_to_min;
/*
double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
if (debugLevel > -1) {
*/
if (debugLevel > -3) {
System.out.println("==== Calculated reg_w = "+reg_w);
}
quaternionLma.prepareLMA(
......@@ -760,9 +773,9 @@ public class QuadCLTCPU {
debugLevel); // int debug_level)
}
} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) || // 1
(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) || // 3
(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) { // 6
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
......@@ -773,10 +786,18 @@ public class QuadCLTCPU {
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
if (debugLevel > -3) {
System.out.println("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
}
// if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
if (max_to_min > quat_max_ratio) {
reg_w = quat_max_damping * (max_to_min - quat_max_ratio) / max_to_min;
/*
double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
if (debugLevel > -1) {
*/
if (debugLevel > -3) {
System.out.println("==== Calculated reg_w = "+reg_w);
}
quaternionLma.prepareLMA(
......@@ -821,6 +842,12 @@ public class QuadCLTCPU {
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
if ((rms.length >= 6) && (last_rms.length > 2)){
rms[5] = last_rms[2];
if ((rms.length >= 7) && (last_rms.length > 3)){
rms[6] = last_rms[3];
}
}
}
}
}
......@@ -839,10 +866,14 @@ public class QuadCLTCPU {
int ref_index,
int early_index,
int last_index,
double [] rms, // null or double[5];
double [] rms, // null or double[5]; // now [7]?
double [] quaternion, // null or double[4]
String suffix,
int debugLevel) {
if (quadCLTs[ref_index].getImageName().equals("1763233239_531146")) {
System.out.println("quadCLTs[ref_index].getImageName().getImageName()=="+quadCLTs[ref_index].getImageName());
}
final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
boolean use_inv = false; //
double [] quat = getRotationFromXYZATRCameraIms(
......@@ -857,7 +888,7 @@ public class QuadCLTCPU {
ref_index, // int ref_index,
early_index, // int early_index,
last_index, // int last_index,
rms, // double [] rms, // null or double[5];
rms, // double [] rms, // null or double[5]; // now [7]?
debugLevel); // int debugLevel
if (quat == null) {
return null;
......@@ -933,6 +964,10 @@ public class QuadCLTCPU {
return rotated_xyzatr;
}
public static void saveRotateImsDetails(
Rotation rot,
int early_index,
......@@ -971,7 +1006,7 @@ public class QuadCLTCPU {
}
public static void adjustImuOrient(
public static double [] adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[] quadCLTs,
......@@ -984,7 +1019,7 @@ public class QuadCLTCPU {
boolean orient_by_rot = clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
if (!orient_by_move && !orient_by_rot) {
System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
return;
return null;
}
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted
......@@ -1010,7 +1045,7 @@ public class QuadCLTCPU {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
double [] rms = new double[5];
double [] rms = new double[7];
double [] quat = new double[4];
int quat_lma_mode = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
......@@ -1058,6 +1093,9 @@ public class QuadCLTCPU {
sb.append("avg_z= "+avg_z+" m\n");
sb.append("translation_weight= "+translation_weight+"\n");
sb.append("quat_lma_mode= "+quat_lma_mode+"\n");
sb.append("RMSe="); //rms[0]+","+rms[1]+","+rms[2]+","+rms[3]+","+rms[4]);
for (int i = 0; i < rms.length-1; i++) sb.append(rms[i]+",");
sb.append(rms[rms.length-1]+"\n");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
......@@ -1066,8 +1104,9 @@ public class QuadCLTCPU {
for (int i = 0; i < 3; i++) new_degrees[i]=new_ims_mount_atr[i]*180/Math.PI;
sb.append("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
sb.append("scale="+quat_scale+"\n");
sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]\n");
sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]\n");
double angle = Math.sqrt(corr_angles[0]*corr_angles[0] + corr_angles[1]*corr_angles[1] + corr_angles[2]*corr_angles[2]);
sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"] ("+ angle+")\n");
sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"] ("+ angle*180/Math.PI+")\n");
sb.append(" new ATR(rad)=["+new_ims_mount_atr[0]+", "+new_ims_mount_atr[1]+", "+new_ims_mount_atr[2]+"]\n");
sb.append(" new ATR(deg)=["+new_degrees[0]+", "+new_degrees[1]+", "+new_degrees[2]+"]\n");
if (apply_imu_orient) {
......@@ -1187,6 +1226,7 @@ public class QuadCLTCPU {
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
return quat;
}
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 9e5d5d56
......@@ -8673,6 +8673,7 @@ if (debugLevel > -100) return true; // temporarily !
boolean air_mode_en = clt_parameters.imp.air_mode_en; // fast airplane mode
boolean save_series_infinity = clt_parameters.imp.wser_inf_save; // true; // Save per-series infinity adjustments data as CSV file
boolean save_series_orient = clt_parameters.imp.wser_orient_save;
boolean process_segments = clt_parameters.imp.wser_orient_series;
if (run_pre_series) {
if (debugLevel > -4) {
System.out.println("===== Conditions met for all segment of the scene series, will run per-series calibrations =====");
......@@ -8697,13 +8698,14 @@ if (debugLevel > -100) return true; // temporarily !
// TODO: implement
// for testing running adjustImuOrient for the last segment in this series
double [][] orient_rslt = save_series_orient ? (new double [ref_scenes.length][]) : null;
double [] infinity_multi = SeriesInfinityCorrection.adjustImuOrientMulti(
double [] quatcorr_multi = SeriesInfinityCorrection.adjustImuOrientMulti(
clt_parameters, // CLTParameters clt_parameters,
ref_scenes, // QuadCLT [] quadCLTs,
index_scenes[0], // QuadCLT index_CLT, // normally - just the last in quadCLTs
save_series_orient, // boolean save_series_orient,
save_series_orient, // boolean save_sequence_orient,
process_segments, // boolean process_segments,
orient_rslt, // double [][] orient_rslt,
debugLevel); // int debugLevel)
debugLevel+1); // int debugLevel)
if (orient_rslt != null) {
}
}
......
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