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
Hide 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
......@@ -109,14 +109,16 @@ import loci.formats.FormatException;
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 ORIENTATION_LOGS_SUFFIX = "-ORIENTATION.log";
public static final String INTERFRAME_SUFFIX = "-INTERFRAME";
public static final String FIELD_CALIBRATION_SUFFIX = "-FIELD_CALIBRATION";
public static final String SYSTEM_OUT_LOG_SUFFIX = "-SYSTEM_OUT.log";
public static final String CONFIGURATION_EXTENSION = ".corr-xml";
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";
public static final String SYSTEM_OUT_LOG_SUFFIX = "-SYSTEM_OUT.log";
public static final String CONFIGURATION_EXTENSION = ".corr-xml";
public static final String [] DSI_SUFFIXES = {"-INTER-INTRA-LMA","-INTER-INTRA","-DSI_MAIN"};
public static int INDEX_INTER_LMA = 0;
......@@ -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/QuaternionLma.java
View file @ 9e5d5d56
......@@ -25,6 +25,7 @@
package com.elphel.imagej.tileprocessor;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
......@@ -62,8 +63,9 @@ public class QuaternionLma {
private double [] x_vector = null;
private double [] y_vector = null;
private double [] y_inv_vector = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private double xyz_weight; // weight of all xyz, samples (weight of rotations - pure_weight- xyz_weight
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] axis = null;
......@@ -81,12 +83,29 @@ public class QuaternionLma {
}
public double [] getLastRms() {
return last_rms;
return getRms4(last_rms);
}
public double [] getInitialRms() {
return initial_rms;
return getRms4(initial_rms);
}
public double [] getGoodOrBadRms() {
return getRms4(good_or_bad_rms);
}
private double [] getRms4(double [] rms3) { // rms, rms_pure, xyz_rms
double [] rms4 = new double [4];
Arrays.fill(rms4, Double.NaN);
System.arraycopy(rms3, 0, rms4, 0, Math.min(rms3.length, 3));
if (rms3.length > 2 ) {
rms4[3]=(rms3[1]*pure_weight - rms3[2]*xyz_weight)/(pure_weight - xyz_weight);
}
return rms4;
}
public double [] getAxis() {
return axis;
......@@ -161,7 +180,7 @@ public class QuaternionLma {
debugYfX ( "GNSS-", // String pfx,
x_vector); // double [] data)
}
xyz_weight = 0;
}
public void prepareLMA(
......@@ -203,6 +222,7 @@ public class QuaternionLma {
weights[samples * N] = 1.0 - pure_weight;
y_vector[samples * N] = 1.0;
last_jt = new double [parameters_vector.length][];
xyz_weight = pure_weight;
}
......@@ -228,7 +248,7 @@ public class QuaternionLma {
debug_level); // final int debug_level);
return;
}
if (mode != MODE_XYZQ) {
if (mode != MODE_XYZQ) { // == MODE_XYZQ_LOCAL
prepareLMAMode3(
mode, // int mode,
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
......@@ -252,7 +272,11 @@ public class QuaternionLma {
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
double sw = 0;
xyz_weight = 0;
for (int i = 0; i < N; i++) {
double sample_weight = ((vect_x[i]== null) || (vect_y[i]== null)) ? 0.0:((vect_w != null)? vect_w[i] : 1.0);
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
y_vector[samples * i + j] = 0.0;
......@@ -265,31 +289,34 @@ public class QuaternionLma {
}
} else {
// xyz
double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
// quaternions
// double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
Rotation rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
Rotation rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);
// Translation componets
for (int j = 0; j < 3; j++) {
x_vector[samples_x * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
weights[samples * i + j] = w;
sw += w;
weights[samples * i + j] = tw;
sw += tw;
xyz_weight += tw;
}
x_vector[samples_x * i + 3] = rot_x.getQ0();
x_vector[samples_x * i + 4] = rot_x.getQ1();
x_vector[samples_x * i + 5] = rot_x.getQ2();
x_vector[samples_x * i + 6] = rot_x.getQ3();
w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
// Rotation componets
// w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
if (samples < samples_x) {
y_vector[samples * i + 3] = rot_y.getQ1();
y_vector[samples * i + 4] = rot_y.getQ2();
y_vector[samples * i + 5] = rot_y.getQ3();
for (int j = 0; j < 3; j++) {
weights[samples * i + 3 + j] = w;
sw += w;
weights[samples * i + 3 + j] = rw;
sw += rw;
}
} else {
y_vector[samples * i + 3] = rot_y.getQ0();
......@@ -297,14 +324,15 @@ public class QuaternionLma {
y_vector[samples * i + 5] = rot_y.getQ2();
y_vector[samples * i + 6] = rot_y.getQ3();
for (int j = 1; j < 4; j++) {
weights[samples * i + 3 + j] = w;
sw += w;
weights[samples * i + 3 + j] = rw;
sw += rw;
}
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
xyz_weight *= k;
if (extra_samples > 0) {
double w = (1.0 - pure_weight)/parameters_vector.length;
for (int i = 0; i < parameters_vector.length; i++) {
......@@ -319,8 +347,8 @@ public class QuaternionLma {
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
return;
}
public void prepareLMA43(
int mode,
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
......@@ -341,6 +369,7 @@ public class QuaternionLma {
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
double sw = 0;
xyz_weight = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
......@@ -354,7 +383,10 @@ public class QuaternionLma {
}
} else {
// xyz
double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
double sample_weight = (vect_w != null)? vect_w[i] : 1.0;
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
// double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
// quaternions (both normalized)
Rotation rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
......@@ -363,8 +395,9 @@ public class QuaternionLma {
for (int j = 0; j < 3; j++) {
x_vector[samples_x * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
weights[samples * i + j] = w;
sw += w;
weights[samples * i + j] = tw;
sw += tw;
xyz_weight += tw;
}
x_vector[samples_x * i + 3] = rot_x.getQ0();
x_vector[samples_x * i + 4] = rot_x.getQ1();
......@@ -373,15 +406,16 @@ public class QuaternionLma {
y_vector[samples * i + 3] = rot_y.getQ1();
y_vector[samples * i + 4] = rot_y.getQ2();
y_vector[samples * i + 5] = rot_y.getQ3();
w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
// w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 3; j++) {
weights[samples * i + 3 + j] = w;
sw += w;
weights[samples * i + 3 + j] = rw;
sw += rw;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
xyz_weight *= k;
if (extra_samples>0) {
weights [samples * N] = 1.0 - pure_weight;
y_vector[samples * N] = 1.0;
......@@ -416,10 +450,13 @@ public class QuaternionLma {
y_inv_vector = new double [samples_x * N + extra_samples];
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
xyz_weight = 0;
// double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
double sw = 0;
for (int i = 0; i < N; i++) {
double sample_weight = ((vect_x[i]== null) || (vect_y[i]== null)) ? 0.0:((vect_w != null)? vect_w[i] : 1.0);
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
x_vector [samples * i + j] = 0.0;
......@@ -462,22 +499,24 @@ public class QuaternionLma {
System.arraycopy(inv_y[0], 0, y_inv_vector, samples_x * i, 3);
System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3, 4);
y_vector[samples_x * i + 3] = 1.0; // no rotation
double wt = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
// removed 02/13/2026 - what was 0.75?
// double wt = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
// double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 3; j++) {
weights[samples * i + j] = wt;
sw += wt;
weights[samples * i + j] = tw; //wt;
sw += tw; // wt;
xyz_weight += tw; // wt;
}
// for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
for (int j = 0; j < 4; j++) {// 0 - q0, near 1.0
weights[samples * i + 3 + j] = wr;
sw += wr;
weights[samples * i + 3 + j] = rw; // wr;
sw += rw; //wr;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
xyz_weight *= k;
if (extra_samples > 0) {
weights [samples * N] = 1.0 - pure_weight;
y_inv_vector[samples * N] = 1.0;
......@@ -503,14 +542,14 @@ public class QuaternionLma {
final int debug_level) {
if (mode != MODE_COMBO) {
prepareLMAMode4(
mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w, // double [] vect_w, // one per scene
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debug_level); // final int debug_level);
mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w, // double [] vect_w, // one per scene
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debug_level); // final int debug_level);
return;
}
......@@ -671,6 +710,8 @@ public class QuaternionLma {
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
xyz_weight = 0;
return;
}
// TODO: Consider adding differences between x and y for regularization (or it won't work)
......@@ -1622,11 +1663,13 @@ public class QuaternionLma {
private double [] getYminusFxWeighted(
final double [] fx,
final double [] rms_fp // null or [2]
) {
final double [] rms_fp, // null or [2]
boolean noNaNs) {
final double [] wymfw = new double [fx.length];
double s_rms=0;
double s_rms=0, sxyz_rms=0;
double rms_pure=Double.NaN;
double rms_pure_xyz=Double.NaN;
boolean use_xyz_weight = (mode==MODE_XYZQ) || (mode==MODE_XYZQ_LOCAL) || (mode==MODE_XYZ4Q3);
// int num_comp = use_6dof? 7 : 3;
for (int i = 0; i < fx.length; i++) {
double d = y_vector[i] - fx[i];
......@@ -1634,22 +1677,39 @@ public class QuaternionLma {
if (Double.isNaN(wd)) {
System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
if (noNaNs) {
System.out.println("getYminusFxWeighted(): return null as noNaNs is true");
if (rms_fp != null) {
rms_fp[0] = Double.NaN;
rms_fp[1] = Double.NaN;;
}
return null;
}
wd = 0.0;
d = 0.0;
}
int comp_index = i % samples;
if (i == (samples * N)) {
rms_pure = Math.sqrt(s_rms/pure_weight);;
rms_pure = Math.sqrt(s_rms/pure_weight);
rms_pure_xyz = Math.sqrt(sxyz_rms/xyz_weight);
}
wymfw[i] = wd;
s_rms += d * wd;
double wd2 = d * wd;
s_rms += wd2; // d * wd;
if (use_xyz_weight && (comp_index < 3)) { // xyz
sxyz_rms += wd2;
}
}
double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
if (Double.isNaN(rms_pure)) {
rms_pure=rms;
rms_pure_xyz = Math.sqrt(sxyz_rms/xyz_weight);
}
if (rms_fp != null) {
rms_fp[0] = rms;
rms_fp[1] = rms_pure;
rms_fp[2] = rms_pure_xyz;
}
return wymfw;
}
......@@ -1716,8 +1776,21 @@ public class QuaternionLma {
return -1; // false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
switch (mode) {
case MODE_XYZQ:
case MODE_XYZQ_LOCAL:
case MODE_XYZ4Q3:
double [] good_or_bad4 = getGoodOrBadRms();
double [] initial_rms4 = getInitialRms();
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0] +
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+"), "+
"XYZ RMS="+good_or_bad4[2]+" ("+initial_rms4[2]+"), ATR RMS="+good_or_bad4[3]+" ("+initial_rms4[3]+"), "+
"lambda="+lambda);
break;
default:
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
}
if (rslt[1]) {
break;
......@@ -1748,7 +1821,20 @@ public class QuaternionLma {
}
boolean show_intermediate = true;
if (show_intermediate && (debug_level > 0)) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
switch (mode) {
case MODE_XYZQ:
case MODE_XYZQ_LOCAL:
case MODE_XYZ4Q3:
double [] last_rms4 = getLastRms();
double [] initial_rms4 = getInitialRms();
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+"), "+
"XYZ RMS="+last_rms4[2]+" ("+initial_rms4[2]+"), ATR RMS="+last_rms4[3]+" ("+initial_rms4[3]+"), "+
"lambda="+lambda);
break;
default:
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+"), "+
"lambda="+lambda);
}
}
if (debug_level > 2){
String [] lines1 = printOldNew(false); // boolean allvectors)
......@@ -1760,7 +1846,24 @@ public class QuaternionLma {
if (debug_level > 0) {
if ((debug_level > 1) || last_run) { // (iter == 1) || last_run) {
if (!show_intermediate) {
System.out.println("LMA: iter="+iter+", full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
switch (mode) {
case MODE_XYZQ:
case MODE_XYZQ_LOCAL:
case MODE_XYZ4Q3:
double [] last_rms4 = getLastRms();
double [] initial_rms4 = getInitialRms();
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+"), "+
"XYZ RMS="+last_rms4[2]+" ("+initial_rms4[2]+"), ATR RMS="+last_rms4[3]+" ("+initial_rms4[3]+"), "+
"lambda="+lambda);
break;
default:
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+"), "+
"lambda="+lambda);
}
/*
System.out.println("LMA: iter="+iter+", full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+
" ("+initial_rms[1]+") + lambda="+lambda);
*/
}
String [] lines = printOldNew(false); // boolean allvectors)
for (String line : lines) {
......@@ -1826,10 +1929,11 @@ public class QuaternionLma {
double lambda,
double rms_diff,
int debug_level) {
boolean noNaNs = true;
boolean [] rslt = {false,false};
// maybe the following if() branch is not needed - already done in prepareLMA !
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
last_rms = new double[2];
last_rms = new double[3]; // [2] - for XYZ
if (debug_level > 1) {
System.out.println("lmaStep(): first step");
}
......@@ -1837,16 +1941,16 @@ public class QuaternionLma {
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
if (debug_level > 0) {
if (debug_level > 3) {
debugYfX ( "fx0-", // String pfx,
fx); // double [] data)
}
if (debug_level > 2) {
if (debug_level > 4) {
debugYfX ( "ffx0-", // String pfx,
dbg_data); // double [] data)
}
if (debug_level > 1) {
if (debug_level > 5) { // remove?
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
......@@ -1857,7 +1961,9 @@ public class QuaternionLma {
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
last_rms, // final double [] rms_fp // null or [2]
noNaNs); // boolean noNaNs)
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
......@@ -1933,10 +2039,11 @@ public class QuaternionLma {
new_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
double [] rms = new double[2];
double [] rms = new double[3];
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
rms); // final double [] rms_fp // null or [2]
rms, // final double [] rms_fp // null or [2]
noNaNs); // boolean noNaNs)
if (debug_level > 2) {
/*
dbgYminusFx(this.last_ymfx, "next y-fX");
......@@ -1944,12 +2051,12 @@ public class QuaternionLma {
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
// if (last_ymfx == null) {
// return new boolean[] {false,false}; // null; // need to re-init/restart LMA
// }
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
if ((rms[0] < this.last_rms[0]) && (last_ymfx != null)) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
......@@ -1973,7 +2080,9 @@ public class QuaternionLma {
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
this.last_rms); // final double [] rms_fp // null or [2]
this.last_rms, // final double [] rms_fp // null or [2]
noNaNs); // boolean noNaNs)
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
......
src/main/java/com/elphel/imagej/tileprocessor/SeriesInfinityCorrection.java
View file @ 9e5d5d56
......@@ -3,11 +3,16 @@ package com.elphel.imagej.tileprocessor;
import java.io.File;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Calendar;
import java.util.HashMap;
import java.util.HashSet;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
......@@ -21,7 +26,8 @@ public class SeriesInfinityCorrection {
CLTParameters clt_parameters,
QuadCLT [] refCLTs,
QuadCLT index_CLT, // normally - just the last in quadCLTs
boolean save_series_orient,
boolean save_sequence_orient,
boolean process_segments,
double [][] orient_rslt,
final int debugLevel) {
// boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
......@@ -82,43 +88,117 @@ public class SeriesInfinityCorrection {
double [] dist_visual = new double[refCLTs.length];
double [] dist_ims2 = new double[refCLTs.length];
double [] disp_corrs= new double[refCLTs.length];
for (int nref = 0; nref < refCLTs.length; nref++) {
QuadCLT ref_scene = refCLTs[nref];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
String [] fl_ts = ref_scene.getFirstLastTimestamps();
boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
String name_ref = ref_scene.getImageName();
QuadCLT[] quadCLTs = new QuadCLT[names.length];
int ref_index = -1;
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
if (names[nscene].equals(name_ref)) {
ref_index = nscene;
quadCLTs[nscene] = ref_scene;
} else {
quadCLTs[nscene] = scene_map.get(names[nscene]);
int [] ref_indices = new int [refCLTs.length];
double [][] quats = process_segments? (new double [refCLTs.length][]) : null;
QuadCLT[][] quadCLTss = new QuadCLT[refCLTs.length][];
for (int nref = 0; nref < refCLTs.length; nref++) {
QuadCLT ref_scene = refCLTs[nref];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
String [] fl_ts = ref_scene.getFirstLastTimestamps();
boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
String name_ref = ref_scene.getImageName();
QuadCLT[] quadCLTs = new QuadCLT[names.length];
int ref_index = -1;
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
if (names[nscene].equals(name_ref)) {
ref_index = nscene;
quadCLTs[nscene] = ref_scene;
} else {
quadCLTs[nscene] = scene_map.get(names[nscene]);
}
}
if (ref_index < 0) {
System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
continue;
}
ref_indices[nref] = ref_index;
quadCLTss[nref] = quadCLTs;
if (ref_scene.getImageName().equals("1763233239_531146")) {
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
}
// just for testing - adjusting per segment
if (quats != null) {
quats [nref] = QuadCLTCPU.adjustImuOrient(
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
0, // int earliest_scene,
quadCLTs.length - 1, // int last_index,
save_sequence_orient, // boolean save_details,
debugLevel); // int debugLevel);
}
continue; // just to put a break point
}
if (ref_index < 0) {
System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
continue;
}
QuadCLTCPU.adjustImuOrient(
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
0, // int earliest_scene,
quadCLTs.length - 1, // int last_index,
save_series_orient, // boolean save_details,
debugLevel); // int debugLevel);
continue; // just to put a break point
}
return null;
// adjusting for all series
double [] quat = adjustImuOrient(
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo,
quadCLTss, // QuadCLT[][] quadCLTss,
ref_indices, // int [] ref_indices,
index_CLT, // QuadCLT index_scene, // where to put result
save_sequence_orient, // boolean save_details,
debugLevel+1); // int debugLevel)
// save per-segment quaternion and angles, and the result one
if (save_sequence_orient && (quats != null)) {
saveQuternionCorrection(
quats, // double [][] quats,
quat, // double [] quat,
refCLTs, // QuadCLT [] refCLTs,
index_CLT, // QuadCLT index_CLT,
orient_combo, // boolean orient_combo,
(debugLevel>-3)); // boolean debug)
}
return quat;
}
public static void saveQuternionCorrection(
double [][] quats,
double [] quat,
QuadCLT [] refCLTs,
QuadCLT index_CLT,
boolean orient_combo,
boolean debug) {
double [][] quats_all = new double[quats.length+1][];
QuadCLT [] allCLTs = new QuadCLT [quats_all.length];
allCLTs[0] = index_CLT;
quats_all[0] = quat;
for (int i = 0; i < quats.length; i++) {
allCLTs[i+1] = refCLTs[i];
quats_all[i+1] = quats[i];
}
StringBuffer sb = new StringBuffer();
sb.append("timestamp\tQ0\tQ1\tQ2\tQ3\tscale\tA\tT\tR\tA(deg)\tT(deg)\tR(deg)\n");
for (int nscene = 0; nscene < quats_all.length; nscene++) {
double [] q = quats_all[nscene];
Rotation rot = new Rotation(q[0],q[1],q[2],q[3], false); // no normalization - see if can be scaled
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
double quat_scale = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
for (int i = 0; i < 3; i++) degrees[i] = angles[i]*180/Math.PI;
sb.append(allCLTs[nscene].getImageName()+"\t");
sb.append(rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\t");
sb.append(quat_scale+"\t");
sb.append(angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\t");
sb.append(degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
}
String suffix = orient_combo ? QuadCLTCPU.IMU_CALIB_SUM_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_SUMMARY_SUFFIX;
Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
CalibrationFileManagement.saveStringToFile (
path.toString(),
sb.toString());
if (debug) {
System.out.println("Summary orientation correction data saved to "+path.toString());
}
}
public static double getInfinityMuliSeries(
CLTParameters clt_parameters,
QuadCLT [] refCLTs,
......@@ -126,7 +206,6 @@ public class SeriesInfinityCorrection {
double [][] infinity_rslt,
final int debugLevel) {
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
for (QuadCLT ref_scene:refCLTs) {
ref_scene.restoreAnyDSI(debugLevel);
}
......@@ -327,7 +406,558 @@ public class SeriesInfinityCorrection {
return;
}
public static double[] adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[][] quadCLTss,
int [] ref_indices,
QuadCLT index_scene, // where to put result
boolean save_details,
int debugLevel) {
boolean orient_by_move = clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
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 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
boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
boolean adjust_accl = clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
boolean apply_accl = clt_parameters.imp.apply_accl; // apply IMU velocities scales
double quat_max_change = clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
double quat_min_lin = clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
double transl_cost = clt_parameters.imp.wser_orient_transl; // AGL corrected, 0.5 - equal weights, 0.0 - rotation only, 1.0 - translation-only
int num_refs = ref_indices.length;
double [][][][] pimu_xyzatrs = new double[num_refs][][][];
double [][][][] xyzatrs = new double[num_refs][][][];
ErsCorrection [] ers_refs = new ErsCorrection[num_refs];
// calculate average Z for the sequence (does not need to be very accurate
double sum_avgz_w = 0,sum_avgz=0;
for (int nref = 0; nref < num_refs; nref++) {
int last_index = quadCLTss[nref].length-1;
QuadCLT[] quadCLTs = quadCLTss[nref];
int ref_index = ref_indices[nref];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
pimu_xyzatrs[nref] = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
null, // quat_corr, // double [] quat_corr,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
0, // earliest_scene, // final int early_index,
last_index, // last_index, //(quadCLTs.length -1) // int last_index,
debugLevel); // final int debugLevel
xyzatrs[nref] = new double [quadCLTss[nref].length][][];
for (int nscene = 0; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatrs[nref][nscene] = ers_ref.getSceneXYZATR(ts);
}
ers_refs[nref] = ers_ref;
double w = 1.0;
sum_avgz_w += w;
sum_avgz += w * quadCLTs[ref_index].getAverageZ(true); // in meters
}
double avg_z = sum_avgz/sum_avgz_w;
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;
double translation_agl = 1.0 / (avg_z + 1.0);
if (!orient_by_move) {
translation_agl = 0.0;
} else if (!orient_by_rot) {
translation_agl = 1.0;
}
// Apply transl_cost
double translation_weight = (translation_agl * transl_cost) / (translation_agl * transl_cost + (1-translation_agl)* (1-transl_cost));
// boolean orient_combo,
// boolean save_details
if (debugLevel > -3) {
System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", translation_agl="+translation_agl+", translation_weight="+translation_weight);
}
String suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX) : null;
double [][][][] rotated_xyzatrs = rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTss, // QuadCLT[][] quadCLTss,
xyzatrs, // double [][][][] xyzatrs,
pimu_xyzatrs, // double [][][][] ims_xyzatrs,
ref_indices, // int [] ref_indices,
rms, // double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
index_scene, // QuadCLT index_scene, // where to put result
suffix, // String suffix,
debug_lev); // int debugLevel
if (rotated_xyzatrs != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
/*
saveStringInModelDirectory(
sb.tiString, // String string,
IMU_CALIB_LOGS_SUFFIX); // String suffix)
*/
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Applying correction to the IMS mount orientation:\n");
/// sb.append("getNumOrient()="+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
/// sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
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=");
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]);
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
double [] new_degrees = new double[3];
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");
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) {
for (int i = 0; i < new_ims_mount_atr.length; i++) {
if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
apply_imu_orient = false;
sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
" exceeds the specified limit ("+quat_max_change+")\n");
sb.append("*** Orientation update is disabled.\n");
}
}
}
if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
} else {
sb.append("*** IMU mount angles calculated, but not applied ***\n");
}
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(String.format(
"Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
sb.append(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
double [] omega_corr = null;
// Work later on adjust_gyro and adjust_accl;
/*
if (adjust_gyro) {
omega_corr = getOmegaCorrections(
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
rotated_xyzatr, // double [][][] rotated_xyzatr,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
debugLevel); // int debugLevel
if (omega_corr != null) {
double [] used_omegas = clt_parameters.imp.get_pimu_offs()[1]; // returns in atr order
double [] new_omegas = new double[3];
for (int i = 0; i < 3; i++) {
new_omegas[i] = used_omegas[i] - omega_corr[i];
}
sb.append(String.format(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
used_omegas[0],used_omegas[1],used_omegas[2]));
sb.append(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
omega_corr[0], omega_corr[1], omega_corr[2]));
sb.append(String.format(
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
if (apply_gyro) {
clt_parameters.imp.set_pimu_omegas(new_omegas);
sb.append(String.format(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
sb.append(String.format(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
}
} else {
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
}
double [] acc_corr = null;
if (adjust_accl) {
acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
rotated_xyzatr, // double [][][] rotated_xyzatr,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
quat_min_lin, // double min_range,
debugLevel); // int debugLevel
if (acc_corr != null) {
double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
double [] new_accl_corr = used_accl_corr.clone();
int num_corr=0;
for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
num_corr++;
}
sb.append(String.format(
"Used velocities scales = [%9f, %9f, %9f]\n",
used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
sb.append(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]\n",
acc_corr[0], acc_corr[1], acc_corr[2]));
sb.append(String.format(
"New velocities scales = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
if (apply_accl && (num_corr>0)) {
clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
sb.append(String.format(
"Applied new velocities scales = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
sb.append(String.format(
"New velocities scales are not applied = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
} else {
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
//
}
*/
sb.append("------------------------\n\n");
index_scene.appendStringInModelDirectory(sb.toString(),QuadCLTCPU.IMU_CALIB_LOGS_SUFFIX); // String suffix)
if (debugLevel > -3) {
System.out.print(sb.toString());
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
return quat;
}
public static double [][][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight, // 1.0 - pure translation, 0.0 - pure rotation
QuadCLT[][] quadCLTss,
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
int [] ref_indices,
double [] rms, // null or double[5];
double [] quaternion, // null or double[4]
QuadCLT index_scene, // where to put result
String suffix,
int debugLevel) {
final boolean use3 = false; // true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
boolean use_inv = false; //
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
use3, // boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
avg_height, // double avg_height,
translation_weight, // double translation_weight,
quadCLTss, // QuadCLT[][] quadCLTss,
xyzatrs, // double [][][][] xyzatrs,
ims_xyzatrs, // double [][][][] ims_xyzatrs,
rms, // double [] rms, // null or double[5];
debugLevel); // int debugLevel
if (quat == null) {
return null;
}
if (quaternion != null) {
System.arraycopy(quat, 0, quaternion, 0, 4);
}
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
int num_refs = ims_xyzatrs.length;
double [][][][] rotated_xyzatrs = new double [num_refs][][][];
for (int nref = 0; nref < num_refs; nref++) {
int num_scenes = ims_xyzatrs[nref].length;
rotated_xyzatrs[nref] = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
double [] rotated_xyz = new double[3];
Rotation rot_invert = rot.revert();
for (int nscene = 0; nscene < num_scenes; nscene++) {
rot.applyTo(ims_xyzatrs[nref][nscene][0], rotated_xyz);
// ???
Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatrs[nref][nscene][1][0], xyzatrs[nref][nscene][1][1], xyzatrs[nref][nscene][1][2]);
Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][nscene][1][2]);
Rotation rotation_atr,rotation_atr2;
if (use_inv) { // should not be used
rotation_atr = rot_invert.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
} else {
rotation_atr = rot.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
}
rotated_xyzatrs[nref][nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
}
if (suffix != null) {
Path orient_path = Paths.get(index_scene.getX3dDirectory(true)).
resolve(index_scene.getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
saveRotateImsMultiDetails(
rot, // Rotation rot,
xyzatrs, // double [][][] xyzatr,
ims_xyzatrs, // double [][][] ims_xyzatr,
rotated_xyzatrs, // double [][][] rotated_xyzatr,
orient_path.toString()); // String path) {
if (debugLevel > -3) {
System.out.println("Orientation details saved to "+orient_path.toString());
}
}
return rotated_xyzatrs;
}
public static void saveRotateImsMultiDetails(
Rotation rot,
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
double [][][][] rotated_xyzatrs,
String path) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
StringBuffer sb = new StringBuffer();
sb.append("quat\t"+rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\n");
sb.append("ATR(rad)\t"+angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\n");
sb.append("ATR(deg)\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
sb.append(
"N\tX\tY\tZ\tA\tT\tR\t"+
"PIMU-X\tPIMU-Y\tPIMU-Z\tPIMU-A\tPIMU-T\tPIMU-R\t"+
"ROT-X\tROT-Y\tROT-Z\tROT-A\tROT-T\tROT-R\n");
int num_refs = xyzatrs.length;
for (int nref = 0; nref < num_refs; nref++) {
int num_scenes = xyzatrs[nref].length;
for (int nscene = 0; nscene < num_scenes; nscene++) {
sb.append(String.format("%3d"+
"\t%f\t%f\t%f\t%f\t%f\t%f"+
"\t%f\t%f\t%f\t%f\t%f\t%f"+
"\t%f\t%f\t%f\t%f\t%f\t%f\n",
nscene,
xyzatrs[nref][nscene][0][0],xyzatrs[nref][nscene][0][1],xyzatrs[nref][nscene][0][2],
xyzatrs[nref][nscene][1][0],xyzatrs[nref][nscene][1][1],xyzatrs[nref][nscene][1][2],
ims_xyzatrs[nref][nscene][0][0],ims_xyzatrs[nref][nscene][0][1],ims_xyzatrs[nref][nscene][0][2],
ims_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][nscene][1][2],
rotated_xyzatrs[nref][nscene][0][0],rotated_xyzatrs[nref][nscene][0][1],rotated_xyzatrs[nref][nscene][0][2],
rotated_xyzatrs[nref][nscene][1][0],rotated_xyzatrs[nref][nscene][1][1],rotated_xyzatrs[nref][nscene][1][2]));
}
}
CalibrationFileManagement.saveStringToFile (
path,
sb.toString());
}
/**
* Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS positions (currently
* only linear are used). Refined scene poses may be used as pull targets
* with free orientations (and angular velocities for ERS).
* Result poses are {0,0,0} for the reference frame.
*
* Assuming correct IMU angular velocities and offset linear ones.
*
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence (for timestamps)
* @param xyzatr current scenes [[x,y,z],[a,t,r]] in reference scene frame
* @param ims_xyzatr IMU-derived (integrated) scene position and orientation in reference
* scene frame (only position is currently used)
* @param ref_index reference scene index
* @param early_index earliest (lowest) scene index to use
* @param last_index last (highest) scene index to use
* @return quaternion componets
*/
public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters,
int quat_lma_mode,
boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
double avg_height,
double translation_weight,
QuadCLT[][] quadCLTss,
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
double [] rms, // null or double[5];
int debugLevel) {
boolean use_scale_y = false;
// final boolean use3 = (quat_lma_mode == 3); // true;
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;
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_damping = clt_parameters.imp.quat_max_damping;
int num_samples = 0;
for (int nref = 0; nref < xyzatrs.length;nref++) {
num_samples += xyzatrs[nref].length;
}
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
// double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
double [][][] vect_y = new double [num_samples][][]; // camera XYZATR
double [][][] vect_x = new double [num_samples][][]; // IMS XYZATR
int vindx = 0;
for (int nref = 0; nref < xyzatrs.length;nref++) {
for (int nscene = 0; nscene < xyzatrs[nref].length; nscene++) {
vect_x[vindx] = ims_xyzatrs[nref][nscene];
vect_y[vindx++] = xyzatrs[nref][nscene];
}
}
// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
double [][][] vect_y_scaled;
if (use_scale_y) {
vect_y_scaled= QuaternionLma.scaleXYZ(
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
null); // new int [] {early_index, last_index}); // int [] first_last){ // null - all
} else {
vect_y_scaled = vect_y;
}
if ((quat_lma_mode == QuaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
0.0, // reg_w, // double reg_w, // regularization weight [0..1)
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
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]);
double reg_w_old = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
if (debugLevel > -3) {
System.out.println("====1 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
reg_w, // double reg_w, // regularization weight [0..1)
quat0, // double [] quat0,
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)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
translation_weight, // double translation_weight, // 0.0 ... 1.0;
0.0, // reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
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]);
double reg_w_old = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
if (debugLevel > -3) {
System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
translation_weight, // double translation_weight, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debugLevel); // int debug_level)
}
} else {
quaternionLma.prepareLMA(
vect_x, // quat_lma_xyz, // double [][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [][] vect_w, all same weight
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debugLevel); // 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,
debugLevel); // 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 ((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];
}
}
}
}
}
return quaternionLma.getQuaternion();
}
}
/*
SetChannels [] set_channels=quadCLT_main.setChannels(debugLevel);
......
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