Skip to content

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

Improving mount correction, implementing gyro (omegas) correction

parent 76f96ef8
Show whitespace changes
Inline Side-by-side
Showing with 244 additions and 21 deletions
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 54e00356
...@@ -114,6 +114,10 @@ public class IntersceneMatchParameters { ...@@ -114,6 +114,10 @@ public class IntersceneMatchParameters {
public int min_num_orient = 2; // make from parameters, should be >= 1 public int min_num_orient = 2; // make from parameters, should be >= 1
public int min_num_interscene = 2; // make from parameters, should be >= 1 public int min_num_interscene = 2; // make from parameters, should be >= 1
public boolean adjust_imu_orient = false;// adjust IMU misalignment to the camera public boolean adjust_imu_orient = false;// adjust IMU misalignment to the camera
public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
public boolean adjust_gyro = false;// adjust qyro omegas offsets
public boolean apply_gyro = true; // apply adjusted qyro omegas offsets
public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
public boolean use_quat_corr = true; // Use orientation correction everywhere if available public boolean use_quat_corr = true; // Use orientation correction everywhere if available
...@@ -451,6 +455,11 @@ public class IntersceneMatchParameters { ...@@ -451,6 +455,11 @@ public class IntersceneMatchParameters {
public int quat_num_iter = 20; public int quat_num_iter = 20;
public double quat_reg_w = 0.25; public double quat_reg_w = 0.25;
public double quat_max_ratio = 5.0; // max derivatives difference
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
public boolean stereo_merge = true; public boolean stereo_merge = true;
public int stereo_gap = 32; // pixels between right and left frames public int stereo_gap = 32; // pixels between right and left frames
public double stereo_intereye = 63.5; // mm public double stereo_intereye = 63.5; // mm
...@@ -522,6 +531,12 @@ public class IntersceneMatchParameters { ...@@ -522,6 +531,12 @@ public class IntersceneMatchParameters {
return new double [][] {new double[3], return new double [][] {new double[3],
{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR} {-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
} }
public void set_pimu_omegas(double [] atr) {
pimu_gyr_offs[2] = - atr[0];
pimu_gyr_offs[1] = - atr[1];
pimu_gyr_offs[0] = atr[2];
}
public void dialogQuestions(GenericJTabbedDialog gd) { public void dialogQuestions(GenericJTabbedDialog gd) {
// gd.addMessage ("Scene parameters selection"); // gd.addMessage ("Scene parameters selection");
// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching"); // gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
...@@ -640,6 +655,12 @@ public class IntersceneMatchParameters { ...@@ -640,6 +655,12 @@ public class IntersceneMatchParameters {
"Minimal required number of re-calculations of the interscene-accumulated DSI."); "Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient, gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
"Adjust IMU misalignment to the camera."); "Adjust IMU misalignment to the camera.");
gd.addCheckbox ("Adjust IMU orientation", this.apply_imu_orient,
"Apply IMU misalignment to the camera if adjusted.");
gd.addCheckbox ("Adjust gyro offsets", this.adjust_gyro,
"Adjust qyro omegas offsets.");
gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
"Apply adjusted qyro omegas offsets.");
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr, gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
"Calculate camera orientation correction from predicted by IMS ."); "Calculate camera orientation correction from predicted by IMS .");
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr, gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
...@@ -1275,6 +1296,14 @@ public class IntersceneMatchParameters { ...@@ -1275,6 +1296,14 @@ public class IntersceneMatchParameters {
gd.addNumericField("Regularization weight", this.quat_reg_w, 3,5,"", gd.addNumericField("Regularization weight", this.quat_reg_w, 3,5,"",
"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1."); "Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");
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 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",
"Do not use translation for IMS mount adjustment if it is too small.");
gd.addNumericField("Minimal rotation for mount calibration", this.quat_min_rot, 3,5,"rad",
"Do not use rotations for IMS mount adjustment if it is too small.");
gd.addTab("Stereo/Video","Parameters for stereo video generation"); gd.addTab("Stereo/Video","Parameters for stereo video generation");
gd.addMessage ("Stereo"); gd.addMessage ("Stereo");
...@@ -1425,6 +1454,9 @@ public class IntersceneMatchParameters { ...@@ -1425,6 +1454,9 @@ public class IntersceneMatchParameters {
this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1; this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1; this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
this.adjust_imu_orient = gd.getNextBoolean(); this.adjust_imu_orient = gd.getNextBoolean();
this.apply_imu_orient = gd.getNextBoolean();
this.adjust_gyro = gd.getNextBoolean();
this.apply_gyro = gd.getNextBoolean();
this.calc_quat_corr = gd.getNextBoolean(); this.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean(); this.apply_quat_corr = gd.getNextBoolean();
this.use_quat_corr = gd.getNextBoolean(); this.use_quat_corr = gd.getNextBoolean();
...@@ -1716,6 +1748,11 @@ public class IntersceneMatchParameters { ...@@ -1716,6 +1748,11 @@ public class IntersceneMatchParameters {
this.quat_num_iter = (int) gd.getNextNumber(); this.quat_num_iter = (int) gd.getNextNumber();
this.quat_reg_w = gd.getNextNumber(); this.quat_reg_w = gd.getNextNumber();
this.quat_max_ratio = gd.getNextNumber();
this.quat_max_change = gd.getNextNumber();
this.quat_min_transl = gd.getNextNumber();
this.quat_min_rot = gd.getNextNumber();
if (stereo_views.length > 0) { if (stereo_views.length > 0) {
int i = gd.getNextChoiceIndex(); int i = gd.getNextChoiceIndex();
if (i > 0) { if (i > 0) {
...@@ -1856,6 +1893,9 @@ public class IntersceneMatchParameters { ...@@ -1856,6 +1893,9 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int
properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
properties.setProperty(prefix+"adjust_imu_orient", this.adjust_imu_orient+""); // boolean properties.setProperty(prefix+"adjust_imu_orient", this.adjust_imu_orient+""); // boolean
properties.setProperty(prefix+"apply_imu_orient", this.apply_imu_orient+""); // boolean
properties.setProperty(prefix+"adjust_gyro", this.adjust_gyro+""); // boolean
properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean
properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
...@@ -2157,6 +2197,11 @@ public class IntersceneMatchParameters { ...@@ -2157,6 +2197,11 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"quat_num_iter", this.quat_num_iter+""); // int properties.setProperty(prefix+"quat_num_iter", this.quat_num_iter+""); // int
properties.setProperty(prefix+"quat_reg_w", this.quat_reg_w+""); // double 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_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
properties.setProperty(prefix+"stereo_merge", this.stereo_merge+""); // boolean properties.setProperty(prefix+"stereo_merge", this.stereo_merge+""); // boolean
properties.setProperty(prefix+"stereo_gap", this.stereo_gap+""); // int properties.setProperty(prefix+"stereo_gap", this.stereo_gap+""); // int
properties.setProperty(prefix+"stereo_intereye", this.stereo_intereye+""); // double properties.setProperty(prefix+"stereo_intereye", this.stereo_intereye+""); // double
...@@ -2252,6 +2297,9 @@ public class IntersceneMatchParameters { ...@@ -2252,6 +2297,9 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient")); if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene")); if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
if (properties.getProperty(prefix+"adjust_imu_orient")!=null) this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient")); if (properties.getProperty(prefix+"adjust_imu_orient")!=null) this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient"));
if (properties.getProperty(prefix+"apply_imu_orient")!=null) this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
if (properties.getProperty(prefix+"adjust_gyro")!=null) this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
if (properties.getProperty(prefix+"apply_gyro")!=null) this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr")); if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr")); if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr")); if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
...@@ -2560,6 +2608,11 @@ public class IntersceneMatchParameters { ...@@ -2560,6 +2608,11 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"quat_num_iter")!=null) this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter")); if (properties.getProperty(prefix+"quat_num_iter")!=null) this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
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_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_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"));
if (properties.getProperty(prefix+"stereo_merge")!=null) this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge")); if (properties.getProperty(prefix+"stereo_merge")!=null) this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
if (properties.getProperty(prefix+"stereo_gap")!=null) this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap")); if (properties.getProperty(prefix+"stereo_gap")!=null) this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
if (properties.getProperty(prefix+"stereo_intereye")!=null) this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye")); if (properties.getProperty(prefix+"stereo_intereye")!=null) this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye"));
...@@ -2675,6 +2728,9 @@ public class IntersceneMatchParameters { ...@@ -2675,6 +2728,9 @@ public class IntersceneMatchParameters {
imp.min_num_orient = this.min_num_orient; imp.min_num_orient = this.min_num_orient;
imp.min_num_interscene = this.min_num_interscene; imp.min_num_interscene = this.min_num_interscene;
imp.adjust_imu_orient = this.adjust_imu_orient; imp.adjust_imu_orient = this.adjust_imu_orient;
imp.apply_imu_orient = this.apply_imu_orient;
imp.adjust_gyro = this.adjust_gyro;
imp.apply_gyro = this.apply_gyro;
imp.calc_quat_corr = this.calc_quat_corr; imp.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr; imp.apply_quat_corr = this.apply_quat_corr;
imp.use_quat_corr = this.use_quat_corr; imp.use_quat_corr = this.use_quat_corr;
...@@ -2972,6 +3028,11 @@ public class IntersceneMatchParameters { ...@@ -2972,6 +3028,11 @@ public class IntersceneMatchParameters {
imp.quat_num_iter = this.quat_num_iter; imp.quat_num_iter = this.quat_num_iter;
imp.quat_reg_w = this.quat_reg_w; imp.quat_reg_w = this.quat_reg_w;
imp.quat_max_ratio = this.quat_max_ratio;
imp.quat_max_change = this.quat_max_change;
imp.quat_min_transl = this.quat_min_transl;
imp.quat_min_rot = this.quat_min_rot;
imp.stereo_merge = this.stereo_merge; imp.stereo_merge = this.stereo_merge;
imp.stereo_gap = this.stereo_gap; imp.stereo_gap = this.stereo_gap;
imp.stereo_intereye = this.stereo_intereye; imp.stereo_intereye = this.stereo_intereye;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 54e00356
...@@ -4659,7 +4659,10 @@ public class OpticalFlow { ...@@ -4659,7 +4659,10 @@ public class OpticalFlow {
// skip completely if no color or mono, tiff or video // skip completely if no color or mono, tiff or video
boolean adjust_imu_orient = clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS boolean adjust_imu_orient = clt_parameters.imp.adjust_imu_orient; // 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 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
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 54e00356
...@@ -266,6 +266,8 @@ public class QuadCLTCPU { ...@@ -266,6 +266,8 @@ public class QuadCLTCPU {
int num_iter = clt_parameters.imp.quat_num_iter; // 20; int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false; boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25; double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio; // 0.25;
double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma(); QuaternionLma quaternionLma = new QuaternionLma();
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
...@@ -291,6 +293,22 @@ public class QuadCLTCPU { ...@@ -291,6 +293,22 @@ public class QuadCLTCPU {
} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) || } else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) || (quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) { (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);
if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
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) {
System.out.println("==== Calculated reg_w = "+reg_w);
}
quaternionLma.prepareLMA( quaternionLma.prepareLMA(
quat_lma_mode, // int mode, quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x, vect_x, // double [][][] vect_x,
...@@ -300,6 +318,7 @@ public class QuadCLTCPU { ...@@ -300,6 +318,7 @@ public class QuadCLTCPU {
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1 reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0, quat0, // double [] quat0,
debugLevel); // int debug_level) debugLevel); // int debug_level)
}
} else { } else {
/* /*
double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
...@@ -394,9 +413,6 @@ public class QuadCLTCPU { ...@@ -394,9 +413,6 @@ public class QuadCLTCPU {
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]); xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV, Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]); ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]);
// Rotation rotation_atr = rot.applyTo(scene_atr);
// Rotation rotation_atr = rot.applyTo(ims_atr);
// Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
Rotation rotation_atr,rotation_atr2; Rotation rotation_atr,rotation_atr2;
if (use_inv) { if (use_inv) {
rotation_atr = rot_invert.applyTo(ims_atr); rotation_atr = rot_invert.applyTo(ims_atr);
...@@ -407,7 +423,6 @@ public class QuadCLTCPU { ...@@ -407,7 +423,6 @@ public class QuadCLTCPU {
} }
rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(), rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)}; rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
/// rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
} }
if (debugLevel > -1) { if (debugLevel > -1) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
...@@ -449,6 +464,9 @@ public class QuadCLTCPU { ...@@ -449,6 +464,9 @@ public class QuadCLTCPU {
int last_index, int last_index,
int debugLevel int debugLevel
) { ) {
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
double [][][] pimu_xyzatr = QuadCLT.integratePIMU( double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs, quadCLTs, // final QuadCLT[] quadCLTs,
...@@ -500,20 +518,129 @@ public class QuadCLTCPU { ...@@ -500,20 +518,129 @@ public class QuadCLTCPU {
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat( double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr, ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q) quat); // double [] corr_q)
if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr); clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
}
} else {
System.out.println ("*** IMU mount angles calculated, but not applied ***");
}
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3]; double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI; for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]"); System.out.println(String.format(
System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]"); "Using ATR(rad)=[%9f, %9f, %9f]", new_atr[0], new_atr[1], new_atr[2]));
System.out.println(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]", degrees[0], degrees[1], degrees[2]));
double [] omega_corr = null;
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];
}
if (debugLevel > -1) {
System.out.println(String.format(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]", used_omegas[0],used_omegas[1],used_omegas[2]));
System.out.println(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]", omega_corr[0], omega_corr[1], omega_corr[2]));
System.out.println(String.format(
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
}
if (apply_gyro) {
clt_parameters.imp.set_pimu_omegas(new_omegas);
if (debugLevel > -3) {
System.out.println(String.format(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
System.out.println ("*** Need to save the main configuration file ***"); System.out.println ("*** Need to save the main configuration file ***");
} }
} else {
if (debugLevel > -3) {
System.out.println(String.format(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
}
}
} else {
System.out.println("*** Adjustment of the gyro omegas failed ***");
}
}
} else { } else {
System.out.println ("*** Failed to calculate IMS mount correction! ***"); System.out.println ("*** Failed to calculate IMS mount correction! ***");
} }
} }
public static double [] getOmegaCorrections(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
double [][][] rotated_xyzatr,
QuadCLT[] quadCLTs,
int ref_index,
int early_index,
int last_index,
int debugLevel
) {
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
double [] timestamps = new double [quadCLTs.length];
double [][][] data = new double[3][last_index-early_index+1][2];
double ts_ref = quadCLTs[ref_index].getTimeStamp();
for (int nscene = early_index; nscene <= last_index; nscene++) {
timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
xyzatr[nscene] = ers_ref.getSceneXYZATR(quadCLTs[nscene].getImageName());
Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
Rotation imu_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
rotated_xyzatr[nscene][1][0],
rotated_xyzatr[nscene][1][1],
rotated_xyzatr[nscene][1][2]);
Rotation diff_rot = imu_atr.applyInverseTo(scene_atr); // left rotate IMU to match scene
double rel_ts = timestamps[nscene] - ts_ref;
double [] diff_atr = diff_rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
for (int i = 0; i < 3; i++) {
data[i][nscene-early_index][0] = rel_ts; // for all xyz the same
data[i][nscene-early_index][1] = diff_atr[i];
}
}
PolynomialApproximation pa= new PolynomialApproximation(-1); // debugLevel);
double [][] coeffs = new double [3][2];
for (int i = 0; i < 3; i++) {
coeffs[i] = pa.polynomialApproximation1d(data[i], 1); // linear
}
if (debugLevel > -1) {
System.out.println("Azimuth omega correction: " +coeffs[0][1]+" ("+coeffs[0][0]+")");
System.out.println(" Tilt omega correction: " +coeffs[1][1]+" ("+coeffs[1][0]+")");
System.out.println(" Roll omega correction: " +coeffs[2][1]+" ("+coeffs[2][0]+")");
if (debugLevel > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
"N","rt","dA","dT","dR","lA","lT","lR"));
for (int nscene = early_index; nscene <= last_index; nscene++) {
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",
nscene, data[0][nscene-early_index][0], // timestamps[nscene]-timestamps[ref_index],
data[0][nscene-early_index][1],
data[1][nscene-early_index][1],
data[2][nscene-early_index][1],
coeffs[0][0]+coeffs[0][1] * data[0][nscene-early_index][0],
coeffs[1][0]+coeffs[1][1] * data[1][nscene-early_index][0],
coeffs[2][0]+coeffs[2][1] * data[2][nscene-early_index][0]));
}
}
}
return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};
}
/** /**
* Refine scene poses (now only linear) from currently adjusted poses * Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS velocities (currently * (adjusted using IMS orientations) and IMS velocities (currently
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 54e00356
...@@ -221,12 +221,13 @@ public class QuaternionLma { ...@@ -221,12 +221,13 @@ public class QuaternionLma {
debug_level); // final int debug_level); debug_level); // final int debug_level);
return; return;
} }
//MODE_XYZQ
N = vect_x.length; N = vect_x.length;
this.mode = mode; this.mode = mode;
samples = 3+ quat0.length; samples = 3+ quat0.length;
samples_x = 7; samples_x = 7;
pure_weight = 1.0 - reg_w; pure_weight = 1.0 - reg_w;
int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN; int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples_x * N]; x_vector = new double [samples_x * N];
y_vector = new double [samples * N + extra_samples]; y_vector = new double [samples * N + extra_samples];
weights = new double [samples * N + extra_samples]; weights = new double [samples * N + extra_samples];
...@@ -285,9 +286,12 @@ public class QuaternionLma { ...@@ -285,9 +286,12 @@ public class QuaternionLma {
} }
double k = (pure_weight)/sw; double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k; for (int i = 0; i < weights.length; i++) weights[i] *= k;
if (extra_samples>0) { if (extra_samples > 0) {
weights [samples * N] = 1.0 - pure_weight; double w = (1.0 - pure_weight)/parameters_vector.length;
y_vector[samples * N] = 1.0; for (int i = 0; i < parameters_vector.length; i++) {
weights [samples * N + i] = w;
y_vector[samples * N] = 0.0; // or target value
}
} }
last_jt = new double [parameters_vector.length][]; last_jt = new double [parameters_vector.length][];
if (debug_level > 0) { if (debug_level > 0) {
...@@ -387,7 +391,7 @@ public class QuaternionLma { ...@@ -387,7 +391,7 @@ public class QuaternionLma {
samples = 7; samples = 7;
samples_x = 7; samples_x = 7;
pure_weight = 1.0 - reg_w; pure_weight = 1.0 - reg_w;
int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN; int extra_samples = 0; // (reg_w > 0)? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples * N]; x_vector = new double [samples * N];
y_vector = new double [samples * N + extra_samples]; y_vector = new double [samples * N + extra_samples];
y_inv_vector = new double [samples_x * N + extra_samples]; y_inv_vector = new double [samples_x * N + extra_samples];
...@@ -1130,6 +1134,14 @@ public class QuaternionLma { ...@@ -1130,6 +1134,14 @@ public class QuaternionLma {
} }
} }
} }
if (weights.length > N*samples) {
for (int i = 0; i < vector3.length; i++) {
fx[samples*N + i] = vector3[i];
if (jt != null) {
jt[i][samples*N + i] = 1.0;
}
}
}
return fx; return fx;
} }
...@@ -1563,6 +1575,26 @@ public class QuaternionLma { ...@@ -1563,6 +1575,26 @@ public class QuaternionLma {
return rslt[0]? iter : -1; return rslt[0]? iter : -1;
} }
public double [] getMinMaxDiag(
int debug_level){
double [][] jt = new double [parameters_vector.length][];
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
Matrix wjtjlambda = new Matrix(getWJtJlambda(
0, // *10, // temporary
jt)); // double [][] jt)
double [] mn_mx= {Double.NaN,Double.NaN};
for (int i = 0; i < parameters_vector.length; i++) {
double d = wjtjlambda.get(i,i);
if (!(d > mn_mx[0])) mn_mx[0] = d;
if (!(d < mn_mx[1])) mn_mx[1] = d;
}
return mn_mx;
}
private boolean [] lmaStep( private boolean [] lmaStep(
double lambda, double lambda,
double rms_diff, double rms_diff,
......
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