Commit 48429931 authored by Andrey Filippov's avatar Andrey Filippov

before implementing interscene ERS correction

parent ba6fb0e3
......@@ -182,6 +182,7 @@ public class CLTParameters {
public int lylw_par_sel = 0; // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use checkbox selections above)
public double ly_marg_fract = 0.2; // part of half-width, and half-height to reduce weights
public double ly_rad_to_hdiag = 0.8; // Limit by radius too (1.0 - radius is half diagonal, same margins, use min)
public boolean ly_on_scan = true; // Calculate and apply lazy eye correction after disparity scan (poly or extrinsic)
public boolean ly_inf_en = true; // Simultaneously correct disparity at infinity (both poly and extrinsic)
public int ly_min_forced = 20; // Minimal number of clusters with forced disparity to use it
......@@ -193,13 +194,35 @@ public class CLTParameters {
public boolean ly_ers_forw= true; // Enable ERS correction of the camera linear movement in z direction
public boolean ly_ers_side= false; // true; // Enable ERS correction of the camera linear movement in x direction
public boolean ly_ers_vert= false; // true; // Enable ERS correction of the camera linear movement in y direction
public int ly_par_sel = 0; // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use checkbox selections above)
public double ly_weight_infinity = 0.3; // Total weight of infinity tiles fraction (0.0 - 1.0)
public double ly_weight_disparity = 0.0; // Disparity weight relative to the sum of 8 lazy eye values of the same tile
public double ly_weight_disparity_inf = 0.5; // Disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
public double ly_max_disparity_far = 5.0; // Reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
public double ly_max_disparity_use = 1000.0; // Disregard near objects (default 1000) for LY adjustment to avoid ERS influence
public double ly_inf_min_dfe = 1.75; // Clouds detection: minimal distance from bg_sel edge for non-zero weight
public double ly_inf_max_dfe = 5.0; // Clouds detection: saturation level for distance from bg_sel edge (<=0 - disable feature)
public boolean ly_inf_force_fine = false; // Use fine settings even for early adjustment cycles
public double ly_inf_min_broad = -1.0; // Minimal infinity disparity during early LY adjustments (broad)
public double ly_inf_max_broad = 0.5; // Maximal infinity disparity during early LY adjustments (broad)
public double ly_inf_min_narrow = -0.2; // Minimal infinity disparity during late LY adjustments (narrow)
public double ly_inf_max_narrow = 0.05; // Maximal infinity disparity during late LY adjustments (narrow
public boolean ly_moving_en = false; // enable filtering areas with potentially moving objects
public boolean ly_moving_apply = false; // apply filtering areas with potentially moving objects
public double ly_moving_sigma = 1.0; // blurring sigma for moving objects = 1.0;
public double ly_max_mov_disparity = 75.0; // disparity limit for moving objects detection = 75.0;
public double ly_rad_to_hdiag_mov = 0.7; // radius to half-diagonal ratio to remove high-distortion corners = 0.7 ; // 0.8
public double ly_max_mov_average = 0.2; // do not attempt to detect moving objects if ERS is not accurate for terrain = .25;
public double ly_mov_min_L2 = 0.75; // threshold for moving objects = 0.75;
public int ly_debug_level = 0; // LY debug level
public boolean ly_right_left= true; // equalize weights of right/left FoV (use with horizon in both halves and gross infinity correction)
public boolean ly_inf_tilt= true; // select infinity tiles form right/left tilted (false - from average)
public int ly_per_quad = 10; // minimal tiles per quadrant (not counting the worst) tp proceed
public double ly_per_quad_r = 0.003; // minimal relative tiles per quadrant (not counting the worst) tp proceed
public int ly_inf = 10; // minimal number of tiles at infinity to proceed
......@@ -231,7 +254,7 @@ public class CLTParameters {
public double lym_overexp = 0.0001; // Any (near) saturated pixels - discard tile (see sat_level also)
public boolean lym_update_disp = true; // Update target disparity after each step
public int lym_iter = 25; // Maximal number of iterations
private double lym_change = 0.5e-5; // Parameter vector difference to exit 4e-6 - OK
private double lym_change = 5e-5; // Parameter vector difference to exit 4e-6 - OK
private double lym_change_aux = 1e-4; // same for aux camera (currntly)lwir
public double lym_poly_change = 0.002; // Parameter vector difference to exit from polynomial correction
......@@ -1043,6 +1066,8 @@ public class CLTParameters {
properties.setProperty(prefix+"lylw_par_sel", this.lylw_par_sel+"");
properties.setProperty(prefix+"ly_marg_fract", this.ly_marg_fract+"");
properties.setProperty(prefix+"ly_rad_to_hdiag", this.ly_rad_to_hdiag+"");
properties.setProperty(prefix+"ly_on_scan", this.ly_on_scan+"");
properties.setProperty(prefix+"ly_inf_en", this.ly_inf_en+"");
properties.setProperty(prefix+"ly_min_forced", this.ly_min_forced+"");
......@@ -1054,14 +1079,34 @@ public class CLTParameters {
properties.setProperty(prefix+"ly_ers_forw", this.ly_ers_forw+"");
properties.setProperty(prefix+"ly_ers_side", this.ly_ers_side+"");
properties.setProperty(prefix+"ly_ers_vert", this.ly_ers_vert+"");
properties.setProperty(prefix+"ly_par_sel", this.ly_par_sel+"");
properties.setProperty(prefix+"ly_debug_level", this.ly_debug_level+"");
properties.setProperty(prefix+"ly_weight_infinity", this.ly_weight_infinity+"");
properties.setProperty(prefix+"ly_weight_disparity", this.ly_weight_disparity+"");
properties.setProperty(prefix+"ly_weight_disparity_inf", this.ly_weight_disparity_inf+"");
properties.setProperty(prefix+"ly_max_disparity_far", this.ly_max_disparity_far+"");
properties.setProperty(prefix+"ly_max_disparity_use", this.ly_max_disparity_use+"");
properties.setProperty(prefix+"ly_inf_min_dfe", this.ly_inf_min_dfe+"");
properties.setProperty(prefix+"ly_inf_max_dfe", this.ly_inf_max_dfe+"");
properties.setProperty(prefix+"ly_inf_force_fine", this.ly_inf_force_fine+"");
properties.setProperty(prefix+"ly_inf_min_broad", this.ly_inf_min_broad+"");
properties.setProperty(prefix+"ly_inf_max_broad", this.ly_inf_max_broad+"");
properties.setProperty(prefix+"ly_inf_min_narrow", this.ly_inf_min_narrow+"");
properties.setProperty(prefix+"ly_inf_max_narrow", this.ly_inf_max_narrow+"");
properties.setProperty(prefix+"ly_moving_en", this.ly_moving_en+"");
properties.setProperty(prefix+"ly_moving_apply", this.ly_moving_apply+"");
properties.setProperty(prefix+"ly_moving_sigma", this.ly_moving_sigma+"");
properties.setProperty(prefix+"ly_max_mov_disparity", this.ly_max_mov_disparity+"");
properties.setProperty(prefix+"ly_rad_to_hdiag_mov", this.ly_rad_to_hdiag_mov+"");
properties.setProperty(prefix+"ly_max_mov_average", this.ly_max_mov_average+"");
properties.setProperty(prefix+"ly_mov_min_L2", this.ly_mov_min_L2+"");
properties.setProperty(prefix+"ly_debug_level", this.ly_debug_level+"");
properties.setProperty(prefix+"ly_right_left", this.ly_right_left+"");
properties.setProperty(prefix+"ly_inf_tilt", this.ly_inf_tilt+"");
properties.setProperty(prefix+"ly_per_quad", this.ly_per_quad +"");
properties.setProperty(prefix+"ly_per_quad_r", this.ly_per_quad_r +"");
......@@ -1816,6 +1861,7 @@ public class CLTParameters {
if (properties.getProperty(prefix+"lylw_par_sel")!=null) this.lylw_par_sel=Integer.parseInt(properties.getProperty(prefix+"lylw_par_sel"));
if (properties.getProperty(prefix+"ly_marg_fract")!=null) this.ly_marg_fract=Double.parseDouble(properties.getProperty(prefix+"ly_marg_fract"));
if (properties.getProperty(prefix+"ly_rad_to_hdiag")!=null) this.ly_rad_to_hdiag=Double.parseDouble(properties.getProperty(prefix+"ly_rad_to_hdiag"));
if (properties.getProperty(prefix+"ly_on_scan")!=null) this.ly_on_scan=Boolean.parseBoolean(properties.getProperty(prefix+"ly_on_scan"));
if (properties.getProperty(prefix+"ly_inf_en")!=null) this.ly_inf_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_en"));
if (properties.getProperty(prefix+"ly_min_forced")!=null) this.ly_min_forced=Integer.parseInt(properties.getProperty(prefix+"ly_min_forced"));
......@@ -1827,13 +1873,34 @@ public class CLTParameters {
if (properties.getProperty(prefix+"ly_ers_forw")!=null) this.ly_ers_forw=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_forw"));
if (properties.getProperty(prefix+"ly_ers_side")!=null) this.ly_ers_side=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_side"));
if (properties.getProperty(prefix+"ly_ers_vert")!=null) this.ly_ers_vert=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_vert"));
if (properties.getProperty(prefix+"ly_par_sel")!=null) this.ly_par_sel=Integer.parseInt(properties.getProperty(prefix+"ly_par_sel"));
if (properties.getProperty(prefix+"ly_weight_infinity")!=null) this.ly_weight_infinity=Double.parseDouble(properties.getProperty(prefix+"ly_weight_infinity"));
if (properties.getProperty(prefix+"ly_weight_disparity")!=null) this.ly_weight_disparity=Double.parseDouble(properties.getProperty(prefix+"ly_weight_disparity"));
if (properties.getProperty(prefix+"ly_weight_disparity_inf")!=null) this.ly_weight_disparity_inf=Double.parseDouble(properties.getProperty(prefix+"ly_weight_disparity_inf"));
if (properties.getProperty(prefix+"ly_max_disparity_far")!=null) this.ly_max_disparity_far=Double.parseDouble(properties.getProperty(prefix+"ly_max_disparity_far"));
if (properties.getProperty(prefix+"ly_max_disparity_use")!=null) this.ly_max_disparity_use=Double.parseDouble(properties.getProperty(prefix+"ly_max_disparity_use"));
if (properties.getProperty(prefix+"ly_inf_min_dfe")!=null) this.ly_inf_min_dfe=Double.parseDouble(properties.getProperty(prefix+"ly_inf_min_dfe"));
if (properties.getProperty(prefix+"ly_inf_max_dfe")!=null) this.ly_inf_max_dfe=Double.parseDouble(properties.getProperty(prefix+"ly_inf_max_dfe"));
if (properties.getProperty(prefix+"ly_inf_force_fine")!=null) this.ly_inf_force_fine=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_force_fine"));
if (properties.getProperty(prefix+"ly_inf_min_broad")!=null) this.ly_inf_min_broad=Double.parseDouble(properties.getProperty(prefix+"ly_inf_min_broad"));
if (properties.getProperty(prefix+"ly_inf_max_broad")!=null) this.ly_inf_max_broad=Double.parseDouble(properties.getProperty(prefix+"ly_inf_max_broad"));
if (properties.getProperty(prefix+"ly_inf_min_narrow")!=null) this.ly_inf_min_narrow=Double.parseDouble(properties.getProperty(prefix+"ly_inf_min_narrow"));
if (properties.getProperty(prefix+"ly_inf_max_narrow")!=null) this.ly_inf_max_narrow=Double.parseDouble(properties.getProperty(prefix+"ly_inf_max_narrow"));
if (properties.getProperty(prefix+"ly_moving_en")!=null) this.ly_moving_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_moving_en"));
if (properties.getProperty(prefix+"ly_moving_apply")!=null) this.ly_moving_apply=Boolean.parseBoolean(properties.getProperty(prefix+"ly_moving_apply"));
if (properties.getProperty(prefix+"ly_moving_sigma")!=null) this.ly_moving_sigma=Double.parseDouble(properties.getProperty(prefix+"ly_moving_sigma"));
if (properties.getProperty(prefix+"ly_max_mov_disparity")!=null) this.ly_max_mov_disparity=Double.parseDouble(properties.getProperty(prefix+"ly_max_mov_disparity"));
if (properties.getProperty(prefix+"ly_rad_to_hdiag_mov")!=null) this.ly_rad_to_hdiag_mov=Double.parseDouble(properties.getProperty(prefix+"ly_rad_to_hdiag_mov"));
if (properties.getProperty(prefix+"ly_max_mov_average")!=null) this.ly_max_mov_average=Double.parseDouble(properties.getProperty(prefix+"ly_max_mov_average"));
if (properties.getProperty(prefix+"ly_mov_min_L2")!=null) this.ly_mov_min_L2=Double.parseDouble(properties.getProperty(prefix+"ly_mov_min_L2"));
if (properties.getProperty(prefix+"ly_par_sel")!=null) this.ly_par_sel=Integer.parseInt(properties.getProperty(prefix+"ly_par_sel"));
if (properties.getProperty(prefix+"ly_debug_level")!=null) this.ly_debug_level=Integer.parseInt(properties.getProperty(prefix+"ly_debug_level"));
if (properties.getProperty(prefix+"ly_right_left")!=null) this.ly_right_left=Boolean.parseBoolean(properties.getProperty(prefix+"ly_right_left"));
if (properties.getProperty(prefix+"ly_inf_tilt")!=null) this.ly_inf_tilt=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_tilt"));
if (properties.getProperty(prefix+"ly_per_quad")!=null) this.ly_per_quad=Integer.parseInt(properties.getProperty(prefix+"ly_per_quad"));
if (properties.getProperty(prefix+"ly_per_quad_r")!=null) this.ly_per_quad_r=Double.parseDouble(properties.getProperty(prefix+"ly_per_quad_r"));
......@@ -2637,8 +2704,8 @@ public class CLTParameters {
gd.addMessage ("--- other LMA parameters ---");
gd.addNumericField("Relative weight margins (0.0 - all 1.0, 1.0 sin^2", this.ly_marg_fract, 8,3,"",
"Reduce weigt of peripheral tiles");
gd.addNumericField("Relative weight margins (0.0 - all 1.0, 1.0 sin^2)", this.ly_marg_fract, 3,5,"","Reduce weigt of peripheral tiles");
gd.addNumericField("Vignette corners (relative to diagonal), same fading as margins above", this.ly_rad_to_hdiag, 3,5,"","1.0 - half diagonal, 0.8 - half width");
gd.addCheckbox ("Calculate and apply lazy eye correction after disparity scan (poly or extrinsic), may repeat", this.ly_on_scan);
gd.addCheckbox ("Adjust disparity using objects at infinity by changing individual tilt and azimuth ", this.ly_inf_en," disable if there are no really far objects in the scene");
......@@ -2653,14 +2720,60 @@ public class CLTParameters {
gd.addCheckbox ("Enable ERS correction of the camera vertical motion", this.ly_ers_vert);
gd.addNumericField("Manual parameter mask selection (0 use checkboxes above)", this.ly_par_sel, 0, 5,"",
"bit 0 - sym0, bit1 - sym1, ...");
gd.addMessage ("--- Relarive weights for LY samples (infinity/near, disparity/lazy eye, ...)---");
gd.addNumericField("Total weight of infinity tiles fraction (0.0 - 1.0)", this.ly_weight_infinity, 3,5,"pix",
"The remaining weight goes to all objects and finate distance");
gd.addNumericField("Disparity weight relative to the sum of 8 lazy eye values of the same tile for near object", this.ly_weight_disparity, 3,5,"pix",
"Should probably be zero");
gd.addNumericField("Disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity", this.ly_weight_disparity_inf, 3,5,"pix",
"Now 0.5");
gd.addNumericField("Disparity threshold to reduce weights of near tiles", this.ly_max_disparity_far, 1,5,"pix",
"Near objects' (disparity > max_disparity_far) weights are scaled by sqrt(max_disparity_far/disparity)");
gd.addNumericField("Disregard near objects (default 1000) for LY adjustment to avoid ERS influence", this.ly_max_disparity_use, 1,5,"pix",
"Testing extrinsic (no ERS) adjustment only");
gd.addMessage ("--- Clouds in the sky detection (reduces weights for far objects, such as mountain ridges) ---");
gd.addNumericField("Minimal distance from background selection edge for positive weighths", this.ly_inf_min_dfe, 3,5,"clusters",
"Values close to the larger integer cause first positive value smaller than next increments");
gd.addNumericField("Maximal (saturation) distance from background selection edge (0.0 - disable clouds)", this.ly_inf_max_dfe, 3,5,"clusters",
"Cluters farther from the edge will have the same weight. If <= 0 - disable clouds in the sky boosting.");
gd.addMessage ("--- Early/late lazy eye adjustments infinity margins ---");
gd.addCheckbox ("Use narrow-band settings even for early adjustment cycles", this.ly_inf_force_fine,
"Unchecked - start with broad range, switch to narrow in later cysles");
gd.addNumericField("Low disaprity limit for infinity during early passes (broad)", this.ly_inf_min_broad, 3,5,"pix",
"Early adjustment passes should tolerate large disp[arity tilts");
gd.addNumericField("High disaprity limit for infinity during early passes (broad)", this.ly_inf_max_broad, 3,5,"pix",
"Early adjustment passes should tolerate large disp[arity tilts");
gd.addNumericField("Low disaprity limit for infinity during late passes (narrow)", this.ly_inf_min_narrow, 3,5,"pix",
"Late adjustments should have tighter margins, and the negative (this) should be more tolerant than the positive one");
gd.addNumericField("High disaprity limit for infinity during late passes (narrow)", this.ly_inf_max_narrow, 3,5,"pix",
"Late adjustments should have tighter margins, and the positive (this) should be more tighter than the negatgive one");
gd.addMessage ("--- Detection and removal of potentially moving objects from adjustment data set ---");
gd.addCheckbox ("Enable moving object detection", this.ly_moving_en,
"Should be enabled when camera egomotion is approximately determined");
gd.addCheckbox ("Apply detected moving objects", this.ly_moving_apply,
"Uncheck for dry-run");
gd.addNumericField("Low-pass sigma to blur moving objects detection", this.ly_moving_sigma, 3,5,"pix",
"1.0 seems to be a good value");
gd.addNumericField("Do not detect very close objects", this.ly_max_mov_disparity, 3,5,"pix",
"More tests are needed with close objects");
gd.addNumericField("Do not detect objects in peripheral (high lens distortion) areas", this.ly_rad_to_hdiag_mov, 3,5,"",
"As a ratio to half diagonal. 0.8 corresponds to half-width");
gd.addNumericField("Do not try to detect until the camera egomotion is determoined", this.ly_max_mov_average, 3,5,"",
"After adjustment the evatage value is about 1.1");
gd.addNumericField("Threshold value for potentially moving objects (lower - terrain)", this.ly_mov_min_L2, 3,5,"",
"0.5 .. 0.75, needs more testing");
gd.addMessage ("---");
gd.addNumericField("Debug level for lazy eye/ers processing", this.ly_debug_level, 0, 5,"",
"Active when global debug level > -1, 1 - min, 2 - lma steps, 3 - images");
gd.addCheckbox ("Equalize weights of right/left FoV", this.ly_right_left,
"Use this mode use with horizon visible in both FoV halves when gross infinity correction is needed");
gd.addCheckbox ("Compensate for right/left tilt at infinity", this.ly_inf_tilt,
"When selectig true infinity tiles, compare to tilted avarage (false use average of all infinity tiles)");
gd.addNumericField("Minimal tiles per quadrant (not counting the worst) tp proceed", this.ly_per_quad, 0);
gd.addNumericField("Minimal tiles per quadrant (not counting the worst) tp proceed - fraction of all tiles", this.ly_per_quad_r, 3);
......@@ -2671,7 +2784,7 @@ public class CLTParameters {
gd.addNumericField("Relative weight of infinity calibration data", this.ly_inf_frac, 3);
gd.addNumericField("Maximal disparity to be treated as infinity when adjusting with the rig data", this.ly_inf_max_disparity, 8,3,"pix",
gd.addNumericField("Maximal disparity to be treated as infinity when adjusting with the rig data", this.ly_inf_max_disparity, 3,5,"pix",
"Only used in guided (by rig data) mode");
gd.addCheckbox ("Correct disparity for infinity tiles )has to disable until code fixed)", this.ly_inf_disp);
......@@ -2683,22 +2796,22 @@ public class CLTParameters {
gd.addNumericField("Number after removing worst (should be >1)", this.ly_smpl_num, 0);
gd.addMessage ("Maximal measured relative disparity = "+ (0.8*disp_scan_step)+" (0.8 * disp_scan_step)");
// gd.addNumericField("Maximal measured relative disparity", this.ly_meas_disp, 3);
gd.addNumericField("Maximal RMS of the remaining tiles in a sample", this.ly_smpl_rms, 5);
gd.addNumericField("Maximal full disparity difference to 8 neighbors", this.ly_disp_var, 8,5,"pix",
gd.addNumericField("Maximal RMS of the remaining tiles in a sample", this.ly_smpl_rms, 5,8,"");
gd.addNumericField("Maximal full disparity difference to 8 neighbors", this.ly_disp_var, 5,8,"pix",
"Full allowed mismatch is a sum of absolute and disparity times relative");
gd.addNumericField("Maximal relative full disparity difference to 8 neighbors", this.ly_disp_rvar, 8,5,"",
gd.addNumericField("Maximal relative full disparity difference to 8 neighbors", this.ly_disp_rvar, 5,8,"",
"Full allowed mismatch is a sum of absolute and disparity times relative");
gd.addNumericField("Maximal full disparity difference to 8 neighbors with GT", this.ly_disp_var_gt, 8,5,"pix",
gd.addNumericField("Maximal full disparity difference to 8 neighbors with GT", this.ly_disp_var_gt, 5,8,"pix",
"Full allowed mismatch is a sum of absolute and disparity times relative (relaxed when ground truth is available)");
gd.addNumericField("Maximal relative full disparity difference to 8 neighbors with GT", this.ly_disp_rvar_gt, 8,5,"",
gd.addNumericField("Maximal relative full disparity difference to 8 neighbors with GT", this.ly_disp_rvar_gt, 5,8,"",
"Full allowed mismatch is a sum of absolute and disparity times relative (relaxed when ground truth is available)");
gd.addNumericField("Reduce weight of higher disparity tiles", this.ly_norm_disp, 5);
gd.addNumericField("Reduce weight of higher disparity tiles", this.ly_norm_disp, 5,8,"");
gd.addMessage ("--- Lazy eye multi-step fitting ---");
gd.addNumericField("Any (near) saturated pixels - discard tile (see sat_level also)", this.lym_overexp, 10);
gd.addNumericField("Any (near) saturated pixels - discard tile (see sat_level also)", this.lym_overexp, 10,12,"");
gd.addCheckbox ("Update target disparity after each step", this.lym_update_disp);
gd.addNumericField("Maximal number of iterations", this.lym_iter, 0);
gd.addNumericField("Parameter vector difference to exit (main camera)", this.lym_change, 10);
gd.addNumericField("Parameter vector difference to exit (aux camera)", this.lym_change_aux, 10);
gd.addNumericField("Parameter vector difference to exit (main camera)", this.lym_change, 10,12,"");
gd.addNumericField("Parameter vector difference to exit (aux camera)", this.lym_change_aux, 10,12,"");
gd.addNumericField("Parameter vector difference to exit from polynomial correction", this.lym_poly_change, 10);
......@@ -3544,6 +3657,7 @@ public class CLTParameters {
this.lylw_par_sel= (int) gd.getNextNumber();
this.ly_marg_fract= gd.getNextNumber();
this.ly_rad_to_hdiag= gd.getNextNumber();
this.ly_on_scan= gd.getNextBoolean();
this.ly_inf_en= gd.getNextBoolean();
this.ly_min_forced= (int) gd.getNextNumber();
......@@ -3556,9 +3670,33 @@ public class CLTParameters {
this.ly_ers_side= gd.getNextBoolean();
this.ly_ers_vert= gd.getNextBoolean();
this.ly_par_sel= (int) gd.getNextNumber();
this.ly_weight_infinity= gd.getNextNumber();
this.ly_weight_disparity= gd.getNextNumber();
this.ly_weight_disparity_inf=gd.getNextNumber();
this.ly_max_disparity_far= gd.getNextNumber();
this.ly_max_disparity_use= gd.getNextNumber();
this.ly_inf_min_dfe= gd.getNextNumber();
this.ly_inf_max_dfe= gd.getNextNumber();
this.ly_inf_force_fine= gd.getNextBoolean();
this.ly_inf_min_broad= gd.getNextNumber();
this.ly_inf_max_broad= gd.getNextNumber();
this.ly_inf_min_narrow= gd.getNextNumber();
this.ly_inf_max_narrow= gd.getNextNumber();
this.ly_moving_en= gd.getNextBoolean();
this.ly_moving_apply= gd.getNextBoolean();
this.ly_moving_sigma= gd.getNextNumber();
this.ly_max_mov_disparity= gd.getNextNumber();
this.ly_rad_to_hdiag_mov= gd.getNextNumber();
this.ly_max_mov_average= gd.getNextNumber();
this.ly_mov_min_L2= gd.getNextNumber();
this.ly_debug_level= (int) gd.getNextNumber();
this.ly_right_left= gd.getNextBoolean();
this.ly_inf_tilt= gd.getNextBoolean();
this.ly_per_quad= (int) gd.getNextNumber();
this.ly_per_quad_r= gd.getNextNumber();
......
......@@ -5010,6 +5010,9 @@ private Panel panel1,
} else if (label.equals("LIST extrinsics")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
if (EYESIS_CORRECTIONS_AUX == null) {
EYESIS_CORRECTIONS_AUX = new EyesisCorrections(SYNC_COMMAND.stopRequested,CORRECTION_PARAMETERS.getAux());
}
listExtrinsics();
return;
/* ======================================================================== */
......@@ -6531,7 +6534,10 @@ private Panel panel1,
if (dir!=null) {
System.out.println("top directory = "+dir);
}
return MLStats.listExtrinsics(dir); // , mask);
return MLStats.listExtrinsics(
dir, // ); // , mask);
EYESIS_CORRECTIONS,
EYESIS_CORRECTIONS_AUX);
}
public boolean mlRecalc() {
......
......@@ -1461,7 +1461,7 @@ public class Corr2dLMA {
}
public double [][] lmaDisparityStrength( // restored from git
public double [][] lmaDisparityStrength1( // restored from git
double lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
double lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
double lma_min_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
......@@ -1510,7 +1510,7 @@ public class Corr2dLMA {
public double [][] lmaDisparityStrength0(
public double [][] lmaDisparityStrength(
double lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
double lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
double lma_min_max_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
......@@ -1554,7 +1554,7 @@ public class Corr2dLMA {
if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue;
}
// strength = Math.sqrt(strength * Math.sqrt(abc[tile][0] * abc[tile][2])); // / area ); // new strength
strength = Math.sqrt(strength * Math.sqrt(abc[tile][0] * abc[tile][2])); // / area ); // new strength
ds[tile][0] = disparity;
ds[tile][1] = (strength * lma_str_scale) + lma_str_offset;
}
......
......@@ -220,12 +220,14 @@ public class ExtrinsicAdjustment {
public GeometryCorrection.CorrVector solveCorr (
double marg_fract, // part of half-width, and half-height to reduce weights
boolean use_disparity, // adjust disparity-related extrinsics
double inf_min_disparity, // minimal disparity for infinity
double inf_max_disparity, // minimal disparity for infinity
double inf_min_disparity, // minimal disparity for infinity (from average, possibly tilted)
double inf_max_disparity, // maximal disparity for infinity (from average, possibly tilted)
double inf_min_disp_abs, // minimal disparity for infinity (absolute)
double inf_max_disp_abs, // maximal disparity for infinity (absolute)
boolean en_infinity_tilt, // select infinity tiles form right/left tilted (false - from average)
boolean infinity_right_left, // balance weights between right and left halves of infinity
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
// boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
// // data, using just radial distortions
int min_num_forced, // minimal number of clusters with forced disparity to use it
boolean common_roll, // Enable common roll (valid for high disparity range only)
boolean corr_focalLength, // Correct scales (focal length temperature? variations)
......@@ -235,11 +237,24 @@ public class ExtrinsicAdjustment {
boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
// add balancing-related here?
int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
double weight_disparity,
double weight_lazyeye,
double weight_infinity, // 0.3, total weight of infinity tiles fraction (0.0 - 1.0)
double weight_disparity, // 0.0 disparity weight relative to the sum of 8 lazy eye values of the same tile
double weight_disparity_inf,// 0.5 disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
double max_disparity_far, // 5.0 reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use, // 5.0 (default 1000)disable near objects completely - use to avoid ERS
double min_dfe, // = 1.75;
double max_dfe, // = 5.0; // <=0 - disable feature
// moving objects filtering
boolean moving_en, // enable filtering areas with potentially moving objects
boolean moving_apply, // apply filtering areas with potentially moving objects
double moving_sigma, // blurring sigma for moving objects = 1.0;
double max_mov_disparity, // disparity limit for moving objects detection = 75.0;
double rad_to_hdiag_mov, // radius to half-diagonal ratio to remove high-distortion corners = 0.7 ; // 0.8
double max_mov_average, // do not attempt to detect moving objects if ERS is not accurate for terrain = .25;
double mov_min_L2, // threshold for moving objects = 0.75;
double [][] measured_dsxy_in, //
boolean [] force_disparity_in, // boolean [] force_disparity,
// GeometryCorrection geometryCorrection,
boolean use_main, // corr_rots_aux != null;
GeometryCorrection.CorrVector corr_vector_meas,
double [] old_new_rms, // should be double[2]
......@@ -254,15 +269,14 @@ public class ExtrinsicAdjustment {
this.measured_dsxy[i][INDX_DIFF] = -this.measured_dsxy[i][INDX_DIFF]; // invert disparity sign
}
this.force_disparity = force_disparity_in;
boolean dbg_images = debugLevel > 0; // 2; // -3; // 2; // 1;
boolean dbg_images = debugLevel > 0; // 2; // -3; // 2; // 1; manually set here
double rad_to_hdiag = 0.8;
weight_window = getWeightWindow(
marg_fract,
rad_to_hdiag); // limit corners
if (dbg_images) {
if (dbg_images && (debugLevel > 0)) { // disabled
(new ShowDoubleFloatArrays()).showArrays(
weight_window,
clustersX,
......@@ -280,12 +294,12 @@ public class ExtrinsicAdjustment {
corr_vector,
true); // boolean set_dydisp)
if (dbg_images) {
if (dbg_images && (debugLevel > 0)) { // disabled
showX0Y0(
x0y0, // double[][] data,
"nondistorted X0Y0");// String title);
}
/*
this.par_mask = geometryCorrection.getParMask(
use_disparity, // has_disparity, // boolean use_disparity,
use_aztilts, // Adjust azimuths and tilts excluding disparity
......@@ -297,141 +311,126 @@ public class ExtrinsicAdjustment {
ers_side, // Enable ERS correction of the camera linear movement in x direction
ers_vert, // Enable ERS correction of the camera linear movement in y direction
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
*/
boolean [] filtered_infinity = null;
double [] dfe = null;
if (max_dfe > 0) {
dfe = distanceFromEdge (
force_disparity,
measured_dsxy,
min_dfe, // 1.75
max_dfe, // 5.0
dbg_images && (debugLevel > 0)); // disabled
}
// boolean en_tilt = true;
// double min_infinity_abs = -1.0;
// double max_infinity_abs = 0.5;
if (use_disparity) {
filtered_infinity = filterInfinity(
measured_dsxy, // double [][] measured_dsxy,
force_disparity, // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
dfe, // double [] distance_from_edge,
en_infinity_tilt, // boolean en_tilt, // consider right/left infinity tilt (will be disabled if more than *abs difference over width)
min_num_forced, // int min_in_half,
inf_min_disp_abs, // double min_infinity_abs,
inf_max_disp_abs, // double max_infinity_abs,
inf_min_disparity, // double min_infinity,
inf_max_disparity); // double max_infinity
}
/*
this.weights = getWeights(
/*
this.weights = getWeights( // will ignore window for infinity (already used for selection)
measured_dsxy, // double [][] measured_dsxy,
(use_disparity? force_disparity: null), // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
filtered_infinity, // boolean [] filtered_infinity,
dfe, // double [] distance_from_edge,// to reduce weight of the mountain ridge, increase clouds (or null)
min_num_forced, // int min_num_forced,
weight_disparity, // double weight_disparity,
weight_lazyeye); // double weight_lazyeye);
*/
double weight_infinity = 0.3; // // total weight of infinity tiles fraction (0.0 - 1.0)
weight_disparity = 0.0; // disparity weight relative to the sum of 8 lazy eye values of the same tile
double weight_disparity_inf = 0.5; // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
double max_disparity_far = 5.0; // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use = 5.0 ; // temporarily to avoid ERS - disable near objects completely
double [][] clouds = detectClouds(
measured_dsxy,
force_disparity); // same dimension as dsdn, true if disparity should be controlled
String [] dbg_titles = {"clouds", "disparity","strength"};
(new ShowDoubleFloatArrays()).showArrays(
clouds,
clustersX,
clustersY,
true,
"clouds",
dbg_titles);
this.weights = getWeights( // will ignore window for infinity (already used for selection)
infinity_right_left, // boolean infinity_right_left, // each halve should have > min_num_forced, will calculate separate average
weight_infinity, // double weight_infinity, // total weight of infinity tiles fraction (0.0 - 1.0)
weight_disparity, // double weight_disparity, // disparity weight relative to the sum of 8 lazy eye values of the same tile
weight_disparity_inf, // double weight_disparity_inf, // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
max_disparity_far, // double max_disparity_far) // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
max_disparity_use);
*/
int [] inf_stat = setWeights( // number right, number left
measured_dsxy, // double [][] measured_dsxy,
(use_disparity? force_disparity: null), // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
filtered_infinity, // boolean [] filtered_infinity,
dfe, // double [] distance_from_edge,// to reduce weight of the mountain ridge, increase clouds (or null)
min_num_forced, // int min_num_forced,
infinity_right_left, // boolean infinity_right_left, // each halve should have > min_num_forced, will calculate separate average
weight_infinity, // double weight_infinity, // total weight of infinity tiles fraction (0.0 - 1.0)
weight_disparity, // double weight_disparity, // disparity weight relative to the sum of 8 lazy eye values of the same tile
weight_disparity_inf, // double weight_disparity_inf, // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
max_disparity_far, // double max_disparity_far) // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
max_disparity_use);
if (use_disparity) {
if (inf_stat[0] < min_num_forced) {
System.out.println("Too few infinity tiles ("+inf_stat[0]+"<"+(min_num_forced)+") to adjust disparity");
// disable parameters...all extrinsic
use_disparity = false;
use_aztilts = false;
use_diff_rolls = false;
common_roll = false;
corr_focalLength = false;
} else if (inf_stat[1] < min_num_forced/2) {
System.out.println("Too few infinity tiles ("+inf_stat[1]+"<"+(min_num_forced/2)+") in one half to balance right/left");
// disable parameters: all extrinsic but disparity (S0)
use_aztilts = false;
use_diff_rolls = false;
common_roll = false;
corr_focalLength = false;
}
}
// verify some parameters still remain?
double min_fg_disp = 0.00; // 25.0; // minimal disparity to boost foreground objects
double min_rel_over = 0.25; // minimal relative disparity over average for a row to boost foreground objects
int min_num_fg = min_num_forced;
double fg_boost_fraction = 0.5;
if (min_fg_disp > 0.0) { // not needed
boolean [] select_ers = selectERS(
measured_dsxy, // double [][] measured_dsxy,
null, // boolean [] selection, // or null
min_fg_disp, // double min_fg_disp,
min_rel_over); // double min_rel_over)
boostERS(
this.weights, // double [] weights, // will be updated
select_ers, // boolean [] fg,
min_num_fg, // int min_num_fg,
fg_boost_fraction); // double fg_boost_fraction)
}
double max_ers_disparity = 175.0;
limitDisparity(
measured_dsxy, // double [][] measured_dsxy,
this.weights, // double [] weights, // will be updated
max_ers_disparity); // double max_disparity
// remove moving objects here
if (debugLevel > -10) { // temporary
dbgYminusFxWeight(
this.last_ymfx,
this.weights, // will use current weights
"Initial y-fX");
double [][] ers_tilt_az = getMovingObjects(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
null ); // double [] fx
if (ers_tilt_az != null) {
showMovingObjects(
"Moving_objects", // String title,
ers_tilt_az);
double moving_sigma = 1.0;
double [] ers_blured = showMovingObjects( // uses weights for bluring
"Moving_objects_sigma"+moving_sigma, // String title,
ers_tilt_az,
moving_sigma, // double sigmaX,
0.01); // double accuracy
System.out.println("Moving objects");
double [] cluster_weight = getClusterWeight();
double max_mov_disparity = 75.0;
double rad_to_hdiag_mov = 0.7 ; // 0.8
boolean [] moving_en= getMovingObjectsSel(
measured_dsxy, // to use target_disparity
cluster_weight,
max_mov_disparity, // disable higher disparity
rad_to_hdiag_mov); // 0.8 limit by radius too (1.0 - radius is half diagonal, same margins, use min)
int clusters = clustersX * clustersY;
double sw = 0.0;
double mov_avg = 0.0;
for (int cluster = 0; cluster < clusters; cluster++) {
if (moving_en[cluster] && !Double.isNaN(ers_blured[cluster])) {
mov_avg += ers_blured[cluster] * cluster_weight[cluster];
sw += cluster_weight[cluster];
}
this.par_mask = geometryCorrection.getParMask(
use_disparity, // has_disparity, // boolean use_disparity,
use_aztilts, // Adjust azimuths and tilts excluding disparity
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,// boolean common_roll,
corr_focalLength, // boolean corr_focalLength);
ers_rot, // boolean ers_rot, // Enable ERS correction of the camera rotation
ers_forw, // Enable ERS correction of the camera linear movement in z direction
ers_side, // Enable ERS correction of the camera linear movement in x direction
ers_vert, // Enable ERS correction of the camera linear movement in y direction
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
if (this.par_mask == null) {
System.out.println("All parameters disabled, nothing to adjust");
return null;
}
mov_avg /= sw; // clusters;
System.out.println("Average moving object detection value = "+mov_avg+
" (use to abandon detection if too high?");
// double min_l2 = 1.0 * mov_avg;
double min_l2 = 2.50 * mov_avg; // 1.5 * mov_avg;
boolean [] moving_maybe = extractMovingObjects(
"Moving_objects_filtering", // String title,
ers_blured, //double [] ers_blured, // has NaNs
moving_en, // boolean [] moving_en
min_l2, // double min_l2); // minimal value (can use average or fraction of it?
4, // int shrink, // prevents growing outer border (==4)
2); // int grow) { // located moving areas
int num_moving = 0;
for (int cluster = 0; cluster < clusters; cluster++) {
if (moving_maybe[cluster]) num_moving++;
}
System.out.println("Number of clusters to remove from LMA fitting = "+num_moving);
System.out.println();
blockSelectedClusters(
if (moving_en) { // debugLevel > -10) { // temporary
String title_moving = (debugLevel > -3) ? "Moving_objects": null;
String title_moving_extra = (debugLevel > -2) ? "Moving_objects_filtering" : null;
boolean debug_text = (debugLevel > -3);
boolean [] moving_maybe = selectMovingObjects(
moving_sigma, // double sigma, // = 1.0;
max_mov_disparity, // double max_mov_disparity, // = 75.0;
rad_to_hdiag_mov, // double rad_to_hdiag_mov, // = 0.7 ; // 0.8
max_mov_average, // double max_mov_average, // = .25;
mov_min_L2, // double min_l2, // = 0.75;
title_moving, // String title, // "Moving_objects"
title_moving_extra, // String title_extra, // "Moving_objects_filtering"
debug_text // boolean debug_text
);
if (moving_apply) {
blockSelectedClusters( // will do nothing for null
measured_dsxy, // double [][] measured_dsxy,
this.weights, // double [] weights, // will be updated
moving_maybe); // boolean [] prohibit
}
}
if (dbg_images) { // temporary
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
}
double lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
......@@ -455,6 +454,7 @@ public class ExtrinsicAdjustment {
return lma_OK? corr_vector : null;
}
private double [][] getXYNondistorted(
GeometryCorrection.CorrVector corr_vector,
boolean set_dydisp){
......@@ -721,31 +721,104 @@ public class ExtrinsicAdjustment {
// Make total weight of disparity (forced) samples - weight_disparity, weight of all others (8 per cluster) weight_lazyeye;
private double [] distanceFromEdge (
boolean [] force_disparity,
double [][] measured_dsxy,
double min_dfe,
double max_dfe,
boolean debug) {
TileNeibs tn = new TileNeibs(clustersX, clustersY);
int [] idfe = tn.distanceFromEdge(force_disparity);
double [] dfe = new double [idfe.length];
int nft = 0;
double aw = 0.0;
for (int i = 0; i < idfe.length; i++) if ((idfe[i] > min_dfe) && (measured_dsxy[i]!= null)){
nft++;
double d = Math.min(idfe[i] - min_dfe, max_dfe); // max_dfe
aw += d;
}
if (nft > 0) {
aw /= nft;
for (int i = 0; i < idfe.length; i++) if ((idfe[i] > min_dfe) && (measured_dsxy[i]!= null)){
double d = Math.min(idfe[i] - min_dfe, max_dfe); // max_dfe
dfe[i] = d / aw;
}
if (debug) {
(new ShowDoubleFloatArrays()).showArrays(
dfe,
clustersX,
clustersY,
"dist_from_edge_"+min_dfe+"-"+max_dfe);
}
}
return dfe;
}
private boolean [] filterInfinity(
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
double [] distance_from_edge,
boolean en_tilt, // consider right/left infinity tilt (will be disabled if more than *abs difference over width)
int min_in_half,
double min_infinity_abs,
double max_infinity_abs,
double min_infinity,
double max_infinity
) {
int clusters = clustersX * clustersY;
double half_width = 0.5 * clustersX;
boolean [] true_infinity = new boolean[clusters];
double sw = 0.0;
double swd = 0.0;
double s0 = 0.0;
double sx = 0.0;
double sx2 = 0.0;
double sy = 0.0;
double sxy= 0.0;
int nright = 0;
int nleft = 0;
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && force_disparity[cluster]){
double s = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH] * weight_window[cluster]; // cluster weight
sw += s;
swd += s * measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
if (distance_from_edge != null) {
s *= distance_from_edge[cluster];
}
if (sw > 0.0) {
swd /= sw; // average value
min_infinity += swd;
max_infinity += swd;
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && force_disparity[cluster]){
if (s > 0.0) {
double d = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
if (( d >= min_infinity_abs) && ( d <= max_infinity_abs)) {
double x = cluster % clustersX;
s0 += s;
sx += s * x;
sx2 += s * x * x;
sy += s * d;
sxy += s *x * d;
if (x >= half_width) nright ++;
else nleft ++;
}
}
}
if (s0 > 0.0) {
double dnm = s0 * sx2 - sx*sx;
double a = 0.0;
double b = sy/s0;
if (en_tilt && (nright >= min_in_half) && (nleft >= min_in_half)) {
a = (sxy * s0 - sy * sx) / dnm;
b = (sy * sx2 - sxy * sx) / dnm;
if ((Math.abs(a) * clustersX) > (max_infinity_abs - min_infinity_abs)/2) {
System.out.println("Infinity tilt too high ("+a+" > "+((max_infinity_abs - min_infinity_abs)/2)+
"), disabling tilt");
a = 0.0;
b = sy/s0;
}
}
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && force_disparity[cluster]){
if ((distance_from_edge == null) || (distance_from_edge[cluster] > 0.0)) {
double x = cluster % clustersX;
double d = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] - (a * x + b) ;
true_infinity[cluster] = (d >= min_infinity) && (d <= max_infinity);
}
}
}
return true_infinity;
}
......@@ -856,6 +929,8 @@ public class ExtrinsicAdjustment {
}
}
// Create mask so that moving objects are only allowed inside permitted area
private boolean [] getMovingObjectsSel(
double [][] measured_dsxy, // to use target_disparity
double [] cluster_weights,
......@@ -904,6 +979,9 @@ public class ExtrinsicAdjustment {
double [] weights,
boolean [] prohibit
) {
if (prohibit == null) {
return;
}
double sw = 0.0;
double sw_en = 0.0;
int clusters = clustersX * clustersY;
......@@ -1052,17 +1130,18 @@ public class ExtrinsicAdjustment {
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
boolean [] filtered_infinity, // tiles known to be infinity
double [] distance_from_edge,// to reduce weight of the mountain ridge, increase clouds (or null)
int min_num_forced, // if number of forced samples exceeds this, zero out weights of non-forced
boolean infinity_right_left, // each halve should have > min_num_forced, will calculate separate average
double weight_infinity, // total weight of infinity tiles fraction (0.0 - 1.0)
double weight_disparity, // disparity weight relative to the sum of 8 lazy eye values of the same tile
double weight_disparity_inf, // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
double max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use) // do not process near objects to avoid ERS
{
// weight_disparity =.5; // FIXME: Fix!
int clusters = clustersX * clustersY;
double [] weights = new double [clusters * POINTS_SAMPLE];
double [] strength = new double [clusters];
double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly
boolean [] disable = new boolean [clusters];
if (filtered_infinity != null) {
for (int cluster = 0; cluster < clusters; cluster++) {
......@@ -1072,21 +1151,25 @@ public class ExtrinsicAdjustment {
double sw = 0.0;
double swf = 0.0;
int num_forced = 0;
// if (force_disparity != null) for (int cluster = 0; cluster < clusters; cluster++) if (force_disparity[cluster])num_forced ++;
// boolean use_forced = num_forced >= min_num_forced;
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){
double s =getClustWeight(
double [] sdl =getClustWeight(
cluster,
measured_dsxy,
force_disparity, // same dimension as dsdn, true if disparity should be controlled
distance_from_edge,
max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
max_disparity_use);
strength[cluster] = s;
if ((force_disparity != null) && (force_disparity[cluster])) {
swf += s;
strength[0][cluster] = weight_disparity_inf * sdl[0];
strength[1][cluster] = (1.0 - weight_disparity_inf) * sdl[1];
swf += strength[0][cluster] + strength[1][cluster];
num_forced++;
} else {
strength[0][cluster] = weight_disparity * sdl[0];
strength[1][cluster] = (1.0 - weight_disparity) * sdl[1];
}
sw += s;
sw += strength[0][cluster] + strength[1][cluster];
}
if (sw <= 0.0) {
return null;
......@@ -1097,47 +1180,198 @@ public class ExtrinsicAdjustment {
use_forced = false;
}
if (use_forced) {
// disparity weight non-zero only for infinity, non-disparity - equal scale for all
k_inf_disp = weight_disparity_inf * weight_infinity / swf;
k_inf_ly = (1.0 - weight_disparity_inf) * weight_infinity / swf / (POINTS_SAMPLE - 1);
k_other_disp = weight_disparity * (1.0 - weight_infinity) / (sw - swf);
k_other_ly = (1.0 - weight_disparity) * (1.0 - weight_infinity) / (sw - swf) / (POINTS_SAMPLE - 1);
k_inf_disp = weight_infinity / swf;
k_inf_ly = weight_infinity / swf / (POINTS_SAMPLE - 1);
k_other_disp = (1.0 - weight_infinity) / (sw - swf);
k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (POINTS_SAMPLE - 1);
} else {
k_other_disp = weight_disparity / sw;
k_other_ly = (1.0 - weight_disparity) / sw / (POINTS_SAMPLE - 1);
k_other_disp = 1.0 / sw;
k_other_ly = 1.0 / sw / (POINTS_SAMPLE - 1);
k_inf_disp = k_other_disp;
k_inf_ly = k_other_ly;
}
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){
double s =strength[cluster];
double sd = strength[0][cluster];
double sly =strength[1][cluster];
if ((force_disparity != null) && force_disparity[cluster]) {
weights[cluster * POINTS_SAMPLE + 0] = s * k_inf_disp;
weights[cluster * POINTS_SAMPLE + 0] = sd * k_inf_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = s * k_inf_ly;
weights[cluster * POINTS_SAMPLE + i] = sly * k_inf_ly;
}
} else {
weights[cluster * POINTS_SAMPLE + 0] = s * k_other_disp;
weights[cluster * POINTS_SAMPLE + 0] = sd * k_other_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = s * k_other_ly;
weights[cluster * POINTS_SAMPLE + i] = sly * k_other_ly;
}
}
}
this.pure_weight = 1.0;
return weights;
}
private double getClustWeight(
private int [] setWeights( // number right, number left
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
boolean [] filtered_infinity, // tiles known to be infinity
double [] distance_from_edge,// to reduce weight of the mountain ridge, increase clouds (or null)
int min_num_forced, // if number of forced samples exceeds this, zero out weights of non-forced
boolean infinity_right_left, // each halve should have > min_num_forced, will calculate separate average
double weight_infinity, // total weight of infinity tiles fraction (0.0 - 1.0)
double weight_disparity, // disparity weight relative to the sum of 8 lazy eye values of the same tile
double weight_disparity_inf, // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
double max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use) // do not process near objects to avoid ERS
{
int clusters = clustersX * clustersY;
double half_width = 0.5 * clustersX;
double [] weights = new double [clusters * POINTS_SAMPLE];
double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly
boolean [] disable = new boolean [clusters];
if (filtered_infinity != null) {
for (int cluster = 0; cluster < clusters; cluster++) {
disable[cluster] = force_disparity[cluster] && !filtered_infinity[cluster];
}
}
double sw = 0.0;
double swf = 0.0;
int num_forced_right = 0;
int num_forced_left = 0;
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){
double [] sdl =getClustWeight(
cluster,
measured_dsxy,
force_disparity, // same dimension as dsdn, true if disparity should be controlled
distance_from_edge,
max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
max_disparity_use);
if ((force_disparity != null) && (force_disparity[cluster])) {
strength[0][cluster] = weight_disparity_inf * sdl[0];
strength[1][cluster] = (1.0 - weight_disparity_inf) * sdl[1];
swf += strength[0][cluster] + strength[1][cluster];
double x = cluster % clustersX;
if (x >= half_width) num_forced_right ++;
else num_forced_left ++;
} else {
strength[0][cluster] = weight_disparity * sdl[0];
strength[1][cluster] = (1.0 - weight_disparity) * sdl[1];
}
sw += strength[0][cluster] + strength[1][cluster];
}
int num_forced = num_forced_right + num_forced_left;
int num_forced_min = (num_forced_right > num_forced_left) ? num_forced_left : num_forced_right;
if (sw <= 0.0) {
return null;
}
boolean use_forced = num_forced >= min_num_forced;
infinity_right_left &= (num_forced_min > (min_num_forced/2)); // do not try to balance if at least one half has too few
double k_inf_disp, k_inf_ly, k_other_disp, k_other_ly;
if (sw <= swf) {
use_forced = false;
}
if (use_forced) {
k_inf_disp = weight_infinity / swf;
k_inf_ly = weight_infinity / swf / (POINTS_SAMPLE - 1);
k_other_disp = (1.0 - weight_infinity) / (sw - swf);
k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (POINTS_SAMPLE - 1);
} else {
k_other_disp = 1.0 / sw;
k_other_ly = 1.0 / sw / (POINTS_SAMPLE - 1);
k_inf_disp = k_other_disp;
k_inf_ly = k_other_ly;
}
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){
double sd = strength[0][cluster];
double sly =strength[1][cluster];
if ((force_disparity != null) && force_disparity[cluster]) {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_inf_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_inf_ly;
}
} else {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_other_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_other_ly;
}
}
}
// optionally balance right/left for the infinity
if (infinity_right_left && (force_disparity != null)) {
double swdr = 0.0;
double swdl = 0.0;
double swlyr = 0.0;
double swlyl = 0.0;
for (int cluster = 0; cluster < clusters; cluster++) {
if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){
double x = cluster % clustersX;
double wd = weights[cluster * POINTS_SAMPLE + 0];
double wly = 0;
for (int i = 1; i < POINTS_SAMPLE; i++) {
wly += weights[cluster * POINTS_SAMPLE + i];
}
if (x >= half_width) {
swdr += wd;
swlyr += wly;
} else {
swdl += wd;
swlyl += wly;
}
}
}
double kwdr = (swdr+swdl)/(2*swdr);
double kwdl = (swdr+swdl)/(2*swdl);
double kwlyr = (swlyr+swlyl)/(2*swlyr);
double kwlyl = (swlyr+swlyl)/(2*swlyl);
for (int cluster = 0; cluster < clusters; cluster++) {
if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){
double x = cluster % clustersX;
if (x >= half_width) {
weights[cluster * POINTS_SAMPLE + 0] *= kwdr;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] *= kwlyr;
}
} else {
weights[cluster * POINTS_SAMPLE + 0] *= kwdl;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] *= kwlyl;
}
}
}
}
}
this.pure_weight = 1.0;
this.weights = weights;
return new int [] {num_forced, num_forced_min};
}
private double [] getClustWeight( // {weight for disparity, weight for ly}
int cluster,
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
double [] distance_from_edge,
double max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use) // do not process near objects to avoid ERS
{
double s = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH];
double s1 = Double.NaN;
if (distance_from_edge != null) { // use vignetting for infinity too
s *= weight_window[cluster];
if ((force_disparity != null) && force_disparity[cluster]) {
s1 = s;
s *= distance_from_edge[cluster];
}
} else { // do not use vignetting for infinity ?
if ((force_disparity == null) || (!force_disparity[cluster])) {
s *= weight_window[cluster];
}
}
double d = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET];
if (d > max_disparity_far) {
s *= Math.sqrt(max_disparity_far/d);
......@@ -1145,7 +1379,10 @@ public class ExtrinsicAdjustment {
if (d > max_disparity_use) {
s = 0.0;
}
return s;
if (Double.isNaN(s1)) {
s1 = s;
}
return new double[] {s,s1};
}
......@@ -1404,6 +1641,126 @@ public class ExtrinsicAdjustment {
titles);
}
private boolean [] selectMovingObjects(
double sigma, // = 1.0;
double max_mov_disparity, // = 75.0;
double rad_to_hdiag_mov, // = 0.7 ; // 0.8
double max_mov_average, // = .25;
double min_l2, // = 0.75;
String title, // "Moving_objects"
String title_extra, // "Moving_objects_filtering"
boolean debug_text
) {
int clusters = clustersX * clustersY;
double max_reasonable = 100.0;
double [][] ers_tilt_az = getMovingObjects( // will output null if ERS tilt or roll are disabled
corr_vector, // GeometryCorrection.CorrVector corr_vector,
null ); // double [] fx
boolean [] moving_maybe = null;
if (ers_tilt_az != null) {
double [] l2_moving = new double [clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
if (ers_tilt_az[cluster] != null) {
l2_moving[cluster] = Math.sqrt(0.5*(ers_tilt_az[cluster][0]*ers_tilt_az[cluster][0] + ers_tilt_az[cluster][1]*ers_tilt_az[cluster][1]));
if (l2_moving[cluster] > max_reasonable) {
l2_moving[cluster] = Double.NaN;
}
} else {
l2_moving[cluster] = Double.NaN;
}
}
double [] ers_blured = l2_moving.clone();
double [] cluster_weights = getClusterWeight();
if (sigma > 0.0){
ers_blured = (new DoubleGaussianBlur()).blurWithNaN(
ers_blured, // double[] pixels,
cluster_weights, // null, // double [] in_weight, // or null
clustersX,// int width,
clustersY,// int height,
sigma, // double sigmaX,
sigma, // double sigmaY,
0.01); // double accuracy
}
// get area permitted for moving objects (not too close to the edges)
boolean [] moving_en= getMovingObjectsSel(
measured_dsxy, // to use target_disparity
cluster_weights,
max_mov_disparity, // disable higher disparity
rad_to_hdiag_mov); // 0.8 limit by radius too (1.0 - radius is half diagonal, same margins, use min)
double sw = 0.0;
double mov_avg = 0.0;
for (int cluster = 0; cluster < clusters; cluster++) {
if (moving_en[cluster] && !Double.isNaN(ers_blured[cluster])) {
mov_avg += ers_blured[cluster] * cluster_weights[cluster];
sw += cluster_weights[cluster];
}
}
mov_avg /= sw; // clusters;
if (debug_text ) {
System.out.println("Average moving object detection value = "+mov_avg+
", max_mov_average ="+ max_mov_average);
}
if (mov_avg <= max_mov_average) {
// double min_l2 = 1.0 * mov_avg;
// double min_l2 = 5.0 * mov_avg; //4.0 * mov_avg; //3.50 * mov_avg; // 2.50 * mov_avg; // 1.5 * mov_avg; // make absolute?
moving_maybe = extractMovingObjects(
title_extra, // String title,
ers_blured, //double [] ers_blured, // has NaNs
moving_en, // boolean [] moving_en
min_l2, // double min_l2); // minimal value (can use average or fraction of it?
4, // int shrink, // prevents growing outer border (==4)
2); // int grow) { // located moving areas
int num_moving = 0;
for (int cluster = 0; cluster < clusters; cluster++) {
if (moving_maybe[cluster]) num_moving++;
}
if (debug_text ) {
System.out.println("Number of clusters to remove from LMA fitting = "+num_moving);
}
}
if (title != null) {
String [] titles = {"vert", "hor", "amplitude", "blured", "mask","maybe"};
double [][] dbg_img = new double [titles.length][] ; // [clusters];
dbg_img[0] = new double [clusters];
dbg_img[1] = new double [clusters];
dbg_img[2] = new double [clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
if (ers_tilt_az[cluster] != null) {
dbg_img[0][cluster] = ers_tilt_az[cluster][0];
dbg_img[1][cluster] = ers_tilt_az[cluster][1];
dbg_img[2][cluster] = l2_moving[cluster];
} else {
dbg_img[0][cluster] = Double.NaN;
dbg_img[1][cluster] = Double.NaN;
dbg_img[2][cluster] = Double.NaN;
}
}
dbg_img[3] = ers_blured;
dbg_img[4] = new double [clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
dbg_img[4][cluster] = moving_en[cluster] ? 1.0:0.0;
}
if (moving_maybe != null) {
dbg_img[5] = new double [clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
dbg_img[5][cluster] = moving_maybe[cluster] ? 1.0:0.0;
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
clustersX,
clustersY,
true,
title,
titles);
}
}
return moving_maybe;
}
private double [][] getMovingObjects(
GeometryCorrection.CorrVector corr_vector,
double [] fx // or null;
......@@ -1431,15 +1788,6 @@ public class ExtrinsicAdjustment {
return ers_tilt_az;
}
private void showMovingObjects(
String title,
double [][] ers_tilt_az) {
showMovingObjects(
title,
ers_tilt_az,
0, // double sigma,
0);
}
private double [] getClusterWeight() { // sum == 1.0;
int clusters = clustersX * clustersY;
......@@ -1460,11 +1808,24 @@ public class ExtrinsicAdjustment {
return cluster_weights;
}
private void showMovingObjects(
String title,
double [][] ers_tilt_az) {
showMovingObjects(
title,
ers_tilt_az,
0, // double sigma,
0,
true);
}
private double [] showMovingObjects(
String title,
double [][] ers_tilt_az,
double sigma,
double accuracy
double accuracy,
boolean debug
)
{
int clusters = clustersX * clustersY;
......@@ -1868,52 +2229,11 @@ public class ExtrinsicAdjustment {
// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector);
if (debug_level > -10) { // temporary
if (debug_level > -1) { // temporary
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
/*
double [][] ers_tilt_az = getMovingObjects(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
null ); // double [] fx
showMovingObjects(
"Moving_objects", // String title,
ers_tilt_az);
double moving_sigma = 1.0;
double [] ers_blured = showMovingObjects(
"Moving_objects_sigma"+moving_sigma, // String title,
ers_tilt_az,
moving_sigma, // double sigmaX,
0.01); // double accuracy
System.out.println("Moving objects");
double mov_avg = 0.0;
int clusters = clustersX * clustersY;
for (int cluster = 0; cluster < clusters; cluster++) {
if (!Double.isNaN(ers_blured[cluster])) mov_avg += ers_blured[cluster];
}
mov_avg /= clusters;
System.out.println("Average moving object detection value = "+mov_avg+
" (use to abandon detection if too high?");
double min_l2 = 1.0 * mov_avg;
boolean [] moving_maybe = extractMovingObjects(
"Moving_objects_filtering", // String title,
ers_blured, //double [] ers_blured, // has NaNs
min_l2, // double min_l2); // minimal value (can use average or fraction of it?
4, // int shrink, // prevents growing outer border (==4)
2); // int grow) { // located moving areas
int num_moving = 0;
for (int cluster = 0; cluster < clusters; cluster++) {
if (moving_maybe[cluster]) num_moving++;
}
System.out.println("Number of clusters to remove from LMA fitting = "+num_moving);
System.out.println();
*/
}
if (last_ymfx == null) {
......@@ -2017,7 +2337,8 @@ public class ExtrinsicAdjustment {
this.corr_vector = new_vector.clone();
if (debug_level > 2) {
System.out.print("New vector: "+new_vector.toString());
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
/// for (int np = 0; np < vector.length; np++) {
/// System.out.print(this.vector[np]+" ");
/// }
......
......@@ -599,6 +599,19 @@ public class GeometryCorrection {
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
if (!use_disparity &&
!use_aztilts &&
!use_diff_rolls &&
!common_roll &&
!corr_focalLength &&
!ers_rot &&
!ers_forw &&
!ers_side &&
!ers_vert &&
(manual_par_sel == 0)) {
return null;
}
return (new CorrVector()).getParMask(
use_disparity, // disparity_only,
// use_other_extr, // boolean use_other_extr,
......@@ -1798,9 +1811,9 @@ public class GeometryCorrection {
vector [IMU_INDEX + 0] = imu[0];
vector [IMU_INDEX + 1] = imu[1];
vector [IMU_INDEX + 2] = imu[2];
vector [IMU_INDEX + 3] = imu[0];
vector [IMU_INDEX + 4] = imu[1];
vector [IMU_INDEX + 5] = imu[2];
vector [IMU_INDEX + 3] = imu[3];
vector [IMU_INDEX + 4] = imu[4];
vector [IMU_INDEX + 5] = imu[5];
}
......@@ -1868,7 +1881,7 @@ public class GeometryCorrection {
if (indx < LENGTH_ANGLES) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // rolls
if (indx < IMU_INDEX) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // zooms
if (indx < (IMU_INDEX+2)) return sym_vect[indx]* (inPix? (1000.0*focalLength /pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < (IMU_INDEX+3)) return sym_vect[indx]* (inPix? (1000.0*focalLength /pixelSize): 1.0); // omega_roll
if (indx < (IMU_INDEX+3)) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // omega_roll
if (indx < LENGTH) return sym_vect[indx]* (inPix? (1000.0): 1.0); // m/s mm/s
return Double.NaN;
}
......@@ -2257,7 +2270,6 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
boolean ers_side, // Enable ERS correction of the camera linear movement in x direction
boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
boolean [] par_mask = {
use_disparity, //sym0
......@@ -3263,9 +3275,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
dpXci_pYci_imu_lin[0][0] = -wdisparity / k; // dpx/ dworld_X
dpXci_pYci_imu_lin[1][1] = wdisparity / k; // dpy/ dworld_Y
dpXci_pYci_imu_lin[0][2] = (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z
dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
/// ers_Xci += delta_t* (dpXci_pYci_imu_lin[0][0] * imu[3] + dpXci_pYci_imu_lin[0][2] * imu[5]);
/// ers_Yci += delta_t* (dpXci_pYci_imu_lin[1][1] * imu[4] + dpXci_pYci_imu_lin[1][2] * imu[5]);
// dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
dpXci_pYci_imu_lin[1][2] = -(xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
ers_x += dpXci_pYci_imu_lin[0][0] * imu[3] + dpXci_pYci_imu_lin[0][2] * imu[5];
ers_y += dpXci_pYci_imu_lin[1][1] * imu[4] + dpXci_pYci_imu_lin[1][2] * imu[5];
}
......@@ -3357,256 +3368,6 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
}
public double [][] getPortsCoordinatesAndDerivativesOld( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [][] pXYderiv, // if not null, should be double[8][]
double [][] disp_dist, //
double px,
double py,
double disparity)
{
// String dbg_s = corr_vector.toString();
/* Starting with required tile center X, Y and nominal distortion, for each sensor port:
* 1) unapply common distortion (maybe for different - master camera)
* 2) apply disparity
* 3) apply rotations and zoom
* 4) re-apply distortion
* 5) return port center X and Y
* line_time
*/
double [] imu = null;
if (disp_dist != null) {
imu = extrinsic_corr.getIMU(); // currently it is common for all channels
if ((deriv_rots == null) && ((imu[0] != 0.0) || (imu[1] != 0.0) ||(imu[2] != 0.0))){
deriv_rots = extrinsic_corr.getRotDeriveMatrices();
}
}
if ((disp_dist == null) && (pXYderiv != null)) {
disp_dist = new double [numSensors][4];
}
double [][] rXY = getRXY(use_rig_offsets); // may include rig offsets
double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * gc_main.pixelCorrectionWidth;
double pYcd = py - 0.5 * gc_main.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*gc_main.pixelSize; // distorted radius in a virtual center camera
double rND2R=gc_main.getRByRDist(rD/gc_main.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
// next radial distortion coefficients are for this, not master camera (may be the same)
double [] rad_coeff={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
double fl_pix = focalLength/(0.001*pixelSize); // focal length in pixels - this camera
double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
double [] xyz = (disparity > 0) ? getWorldCoordinates( // USED in lwir
px, // double px,
py, // double py,
disparity, // double disparity,
true) : null; // boolean correctDistortions)
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * rXY[i][1];
// rectilinear, end of dealing with possibly other (master) camera, below all is for this camera distortions
// Convert a 2-d non-distorted vector to 3d at fl_pix distance in z direction
double [][] avi = {{pXci0}, {pYci0},{fl_pix}};
Matrix vi = new Matrix(avi); // non-distorted sensor channel view vector in pixels (z -along the common axis)
// Apply port-individual combined rotation/zoom matrix
Matrix rvi = rots[i].times(vi);
// get back to the projection plane by normalizing vector
double norm_z = fl_pix/rvi.get(2, 0);
double pXci = rvi.get(0, 0) * norm_z;
double pYci = rvi.get(1, 0) * norm_z;
// Re-apply distortion
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
double ri = rNDi* ri_scale; // relative to distortion radius
double rD2rND = 1.0;
double rri = 1.0;
for (int j = 0; j < rad_coeff.length; j++){
rri *= ri;
rD2rND += rad_coeff[j]*(rri - 1.0); // Fixed
}
// Get port pixel coordinates by scaling the 2d vector with Rdistorted/Dnondistorted coefficient)
double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND;
pXY[i][0] = pXid + this.pXY0[i][0];
pXY[i][1] = pYid + this.pXY0[i][1];
// used when calculating derivatives, TODO: combine calculations !
double drD2rND_dri = 0.0;
Matrix drvi_daz = null;
Matrix drvi_dtl = null;
Matrix drvi_drl = null;
double dpXci_dazimuth = 0.0;
double dpYci_dazimuth = 0.0;
double dpXci_dtilt = 0.0;
double dpYci_dtilt = 0.0;
double dpXci_droll = 0.0;
double dpYci_droll = 0.0;
if ((disp_dist != null) || (pXYderiv != null)) {
rri = 1.0;
for (int j = 0; j < rad_coeff.length; j++){
drD2rND_dri += rad_coeff[j] * (j+1) * rri;
rri *= ri;
}
if (deriv_rots != null) {
// needed for derivatives and IMU
drvi_daz = deriv_rots[i][0].times(vi);
drvi_dtl = deriv_rots[i][1].times(vi);
drvi_drl = deriv_rots[i][2].times(vi);
dpXci_dazimuth = drvi_daz.get(0, 0) * norm_z - pXci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpYci_dazimuth = drvi_daz.get(1, 0) * norm_z - pYci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpXci_dtilt = drvi_dtl.get(0, 0) * norm_z - pXci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpYci_dtilt = drvi_dtl.get(1, 0) * norm_z - pYci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpXci_droll = drvi_drl.get(0, 0) * norm_z - pXci * drvi_drl.get(2, 0) / rvi.get(2, 0);
dpYci_droll = drvi_drl.get(1, 0) * norm_z - pYci * drvi_drl.get(2, 0) / rvi.get(2, 0);
}
}
double delta_t = 0.0;
double [][] dpXci_pYci_imu_lin = new double[2][3]; // null
if (disp_dist != null) {
disp_dist[i] = new double [4]; // dx/d_disp, dx_d_ccw_disp
// Not clear - what should be in Z direction before rotation here?
double [][] add0 = {
{-rXY[i][0], rXY[i][1], 0.0},
{-rXY[i][1], -rXY[i][0], 0.0},
{ 0.0, 0.0, 0.0}}; // what is last element???
Matrix dd0 = new Matrix(add0);
Matrix dd1 = rots[i].times(dd0).getMatrix(0, 1,0,1).times(norm_z); // get top left 2x2 sub-matrix
// now first column of 2x2 dd1 - x, y components of derivatives by disparity, second column - derivatives by ortho to disparity (~Y in 2d correlation)
// unity vector in the direction of radius
double c_dist = pXci/rNDi;
double s_dist = pYci/rNDi;
double [][] arot2= {
{c_dist, s_dist},
{-s_dist, c_dist}};
Matrix rot2 = new Matrix(arot2); // convert from non-distorted X,Y to parallel and perpendicular (CCW) to the radius
double [][] ascale_distort = {
{rD2rND + ri* drD2rND_dri, 0 },
{0, rD2rND}};
Matrix scale_distort = new Matrix(ascale_distort); // scale component parallel to radius as distortion derivative, perpendicular - as distortion
Matrix dd2 = rot2.transpose().times(scale_distort).times(rot2).times(dd1);
disp_dist[i][0] = dd2.get(0, 0);
disp_dist[i][1] = dd2.get(0, 1);
disp_dist[i][2] = dd2.get(1, 0); // d_py/d_disp
disp_dist[i][3] = dd2.get(1, 1);
// ERS linear does not yet use per-port rotations, probably not needed
if ((imu != null) &&((imu[0] != 0.0) || (imu[1] != 0.0) ||(imu[2] != 0.0) ||(imu[3] != 0.0) ||(imu[4] != 0.0) ||(imu[5] != 0.0))) {
delta_t = dd2.get(1, 0) * disparity * line_time; // positive for top cameras, negative - for bottom
double ers_Xci = delta_t* (dpXci_dtilt * imu[0] + dpXci_dazimuth * imu[1] + dpXci_droll * imu[2]);
double ers_Yci = delta_t* (dpYci_dtilt * imu[0] + dpYci_dazimuth * imu[1] + dpYci_droll * imu[2]);
if (xyz != null) {
double k = SCENE_UNITS_SCALE * this.disparityRadius;
double wdisparity = disparity;
double dwdisp_dz = (k * this.focalLength / (0.001*this.pixelSize)) / (xyz[2] * xyz[2]);
dpXci_pYci_imu_lin[0][0] = -wdisparity / k; // dpx/ dworld_X
dpXci_pYci_imu_lin[1][1] = wdisparity / k; // dpy/ dworld_Y
dpXci_pYci_imu_lin[0][2] = (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z
dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
ers_Xci += delta_t* (dpXci_pYci_imu_lin[0][0] * imu[3] + dpXci_pYci_imu_lin[0][2] * imu[5]);
ers_Yci += delta_t* (dpXci_pYci_imu_lin[1][1] * imu[4] + dpXci_pYci_imu_lin[1][2] * imu[5]);
}
pXY[i][0] += ers_Xci * rD2rND; // added correction to pixel X
pXY[i][1] += ers_Yci * rD2rND; // added correction to pixel Y
} else {
imu = null;
}
// TODO: calculate derivatives of pX, pY by 3 imu omegas
}
if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
Matrix drvi_dzm = deriv_rots[i][3].times(vi);
double dpXci_dzoom = drvi_dzm.get(0, 0) * norm_z - pXci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
double dpYci_dzoom = drvi_dzm.get(1, 0) * norm_z - pYci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
double dri_dazimuth = ri_scale / rNDi* (pXci * dpXci_dazimuth + pYci * dpYci_dazimuth);
double dri_dtilt = ri_scale / rNDi* (pXci * dpXci_dtilt + pYci * dpYci_dtilt);
double dri_dzoom = ri_scale / rNDi* (pXci * dpXci_dzoom + pYci * dpYci_dzoom);
double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
double drD2rND_dzoom = drD2rND_dri * dri_dzoom; // new
double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXci * drD2rND_dazimuth;
double dpYid_dazimuth = dpYci_dazimuth * rD2rND + pYci * drD2rND_dazimuth;
double dpXid_dtilt = dpXci_dtilt * rD2rND + pXci * drD2rND_dtilt;
double dpYid_dtilt = dpYci_dtilt * rD2rND + pYci * drD2rND_dtilt;
double dpXid_droll = dpXci_droll * rD2rND;
double dpYid_droll = dpYci_droll * rD2rND;
double dpXid_dzoom = dpXci_dzoom * rD2rND + pXci * drD2rND_dzoom; // new second term
double dpYid_dzoom = dpYci_dzoom * rD2rND + pYci * drD2rND_dzoom; // new second term
// assuming drD2rND_imu* is zero (rD2rND does not depend on imu_*
// hope it will not be needed, as derivatives are used only for filed calibration, handled differently
if (imu != null) {
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+0] = delta_t * rD2rND * dpXci_dtilt; // * imu[0];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+0] = delta_t * rD2rND * dpYci_dtilt; // * imu[0];
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+1] = delta_t * rD2rND * dpXci_dazimuth; // * imu[1];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+1] = delta_t * rD2rND * dpYci_dazimuth; // * imu[1];
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+2] = delta_t * rD2rND * dpYci_droll; // * imu[2];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+2] = delta_t * rD2rND * dpYci_droll; // * imu[2];
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+3] = delta_t * rD2rND * dpXci_pYci_imu_lin[0][0]; // * imu[3];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+4] = delta_t * rD2rND * dpXci_pYci_imu_lin[1][1]; // * imu[5];
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+5] = delta_t * rD2rND * dpXci_pYci_imu_lin[0][2]; // * imu[5];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+5] = delta_t * rD2rND * dpXci_pYci_imu_lin[1][2]; // * imu[5];
}
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dpYid_dazimuth;
pXYderiv[2 * i + 0][CorrVector.ZOOM_INDEX+i] = dpXid_dzoom;
pXYderiv[2 * i + 1][CorrVector.ZOOM_INDEX+i] = dpYid_dzoom;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dpYid_dazimuth;
pXYderiv[2 * i + 0][CorrVector.ZOOM_INDEX+j] = -dpXid_dzoom;
pXYderiv[2 * i + 1][CorrVector.ZOOM_INDEX+j] = -dpYid_dzoom;
}
}
pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dpXid_droll;
pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dpYid_droll;
}
}
return pXY;
}
public double [][] getPortsCoordinatesAndDerivativesDbg( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
......@@ -3840,9 +3601,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
dpXci_pYci_imu_lin[0][0] = -wdisparity / k; // dpx/ dworld_X
dpXci_pYci_imu_lin[1][1] = wdisparity / k; // dpy/ dworld_Y
dpXci_pYci_imu_lin[0][2] = (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z
dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
/// ers_Xci += delta_t* (dpXci_pYci_imu_lin[0][0] * imu[3] + dpXci_pYci_imu_lin[0][2] * imu[5]);
/// ers_Yci += delta_t* (dpXci_pYci_imu_lin[1][1] * imu[4] + dpXci_pYci_imu_lin[1][2] * imu[5]);
// dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
dpXci_pYci_imu_lin[1][2] = -(xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
System.out.println("ncam="+i+
": dpXci_pYci_imu_lin[0][0]="+dpXci_pYci_imu_lin[0][0]+
", dpXci_pYci_imu_lin[0][2]="+dpXci_pYci_imu_lin[0][2]);
......@@ -4050,7 +3810,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
dpXci_pYci_imu_lin[0][0] = -wdisparity / k; // dpx/ dworld_X // TODO: Change sign - here and in the other similar place!
dpXci_pYci_imu_lin[1][1] = wdisparity / k; // dpy/ dworld_Y
dpXci_pYci_imu_lin[0][2] = (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z
dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
// Should match GPU and normal (distorted) calculations
// dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
dpXci_pYci_imu_lin[1][2] = -(xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
}
double delta_t = 0.0;
// TODO: ignoring rotations - add it?
......@@ -4117,162 +3879,6 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
return pXYND;
}
/*
// Does not account for rotated camera when calculating delta_t
public double [] getPortsNonDistortedCoordinatesAndDerivativesOld( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [][] pXYNDderiv, // if not null, should be double[8][]
double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
double [] imu,
double [] xyz, // world XYZ for ERS correction
double px,
double py,
double disparity)
{
double [][] rXY = getRXY(use_rig_offsets); // may include rig offsets
double [] pXYND = new double [numSensors * 2];
double pXcd = px - 0.5 * gc_main.pixelCorrectionWidth;
double pYcd = py - 0.5 * gc_main.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*gc_main.pixelSize; // distorted radius in a virtual center camera
double rND2R=gc_main.getRByRDist(rD/gc_main.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
// next radial distortion coefficients are for this, not master camera (may be the same)
// double [] rad_coeff={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
double fl_pix = focalLength/(0.001*pixelSize); // focal length in pixels - this camera
// double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * rXY[i][1];
// rectilinear, end of dealing with possibly other (master) camera, below all is for this camera distortions
// Convert a 2-d non-distorted vector to 3d at fl_pix distance in z direction
double [][] avi = {{pXci0}, {pYci0},{fl_pix}};
Matrix vi = new Matrix(avi); // non-distorted sensor channel view vector in pixels (z -along the common axis)
// Apply port-individual combined rotation/zoom matrix
Matrix rvi = rots[i].times(vi);
// get back to the projection plane by normalizing vector
double norm_z = fl_pix/rvi.get(2, 0);
double pXci = rvi.get(0, 0) * norm_z;
double pYci = rvi.get(1, 0) * norm_z;
pXYND[2 * i + 0] = pXci;
pXYND[2 * i + 1] = pYci;
// used when calculating derivatives, TODO: combine calculations !
// double drD2rND_dri = 0.0;
Matrix drvi_daz = null;
Matrix drvi_dtl = null;
Matrix drvi_drl = null;
double dpXci_dazimuth = 0.0;
double dpYci_dazimuth = 0.0;
double dpXci_dtilt = 0.0;
double dpYci_dtilt = 0.0;
double dpXci_droll = 0.0;
double dpYci_droll = 0.0;
if (deriv_rots != null) {
// needed for derivatives and IMU
drvi_daz = deriv_rots[i][0].times(vi);
drvi_dtl = deriv_rots[i][1].times(vi);
drvi_drl = deriv_rots[i][2].times(vi);
dpXci_dazimuth = drvi_daz.get(0, 0) * norm_z - pXci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpYci_dazimuth = drvi_daz.get(1, 0) * norm_z - pYci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpXci_dtilt = drvi_dtl.get(0, 0) * norm_z - pXci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpYci_dtilt = drvi_dtl.get(1, 0) * norm_z - pYci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpXci_droll = drvi_drl.get(0, 0) * norm_z - pXci * drvi_drl.get(2, 0) / rvi.get(2, 0);
dpYci_droll = drvi_drl.get(1, 0) * norm_z - pYci * drvi_drl.get(2, 0) / rvi.get(2, 0);
}
double [][] dpXci_pYci_imu_lin = new double[2][3]; // null
if (xyz != null) { // verify by delta? common for all channels
// restore disparity back from the world coordinates to make it a constant
double k = SCENE_UNITS_SCALE * this.disparityRadius;
double wdisparity = -(k * this.focalLength / (0.001*this.pixelSize)) / xyz[2];
double dwdisp_dz = (k * this.focalLength / (0.001*this.pixelSize)) / (xyz[2] * xyz[2]);
// double wpXc = xyz[0] * wdisparity / k; // pixels
// double wpYc =-xyz[1] * wdisparity / k; // pixels
dpXci_pYci_imu_lin[0][0] = -wdisparity / k; // dpx/ dworld_X // TODO: Change sign - here and in the other similar place!
dpXci_pYci_imu_lin[1][1] = wdisparity / k; // dpy/ dworld_Y
dpXci_pYci_imu_lin[0][2] = (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z
dpXci_pYci_imu_lin[1][2] = (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z
}
double delta_t = 0.0;
// TODO: ignoring rotations - add it?
if ((dy_ddisparity != null) && (imu != null)) {
delta_t = dy_ddisparity[i] * disparity * line_time; // positive for top cameras, negative - for bottom
double ers_Xci = delta_t* (dpXci_dtilt * imu[0] + dpXci_dazimuth * imu[1] + dpXci_droll * imu[2] +
dpXci_pYci_imu_lin[0][0] * imu[3] + dpXci_pYci_imu_lin[0][2] * imu[5]);
double ers_Yci = delta_t* (dpYci_dtilt * imu[0] + dpYci_dazimuth * imu[1] + dpYci_droll * imu[2]+
dpXci_pYci_imu_lin[1][1] * imu[4] + dpXci_pYci_imu_lin[1][2] * imu[5]);
pXYND[2 * i + 0] += ers_Xci; // added correction to pixel X
pXYND[2 * i + 1] += ers_Yci; // added correction to pixel Y
// does not account on {dpXci_dazimuth, dpYci_dazimuth, dpXci_dtilt, dpYci_dtilt, dpXci_droll, dpYci_droll}
// /{d_azimuth, d_tilt, d_roll} when calculating derivatives in the presence of the ERS
}
if (pXYNDderiv != null) {
pXYNDderiv[2 * i] = new double [CorrVector.LENGTH];
pXYNDderiv[2 * i+1] = new double [CorrVector.LENGTH];
Matrix drvi_dzm = deriv_rots[i][3].times(vi);
double dpXci_dzoom = drvi_dzm.get(0, 0) * norm_z - pXci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
double dpYci_dzoom = drvi_dzm.get(1, 0) * norm_z - pYci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
if (imu != null) {
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+0] = delta_t * dpXci_dtilt; // * imu[0];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+0] = delta_t * dpYci_dtilt; // * imu[0];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+1] = delta_t * dpXci_dazimuth; // * imu[1];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+1] = delta_t * dpYci_dazimuth; // * imu[1];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+2] = delta_t * dpXci_droll; // * imu[2];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+2] = delta_t * dpYci_droll; // * imu[2];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+3] = delta_t * dpXci_pYci_imu_lin[0][0]; // * imu[3];
// pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+3] = delta_t * dpXci_pYci_imu_lin[1][0]; // * imu[3]; // 0
// pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+4] = delta_t * dpXci_pYci_imu_lin[0][1]; // * imu[4]; // 0
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+4] = delta_t * dpXci_pYci_imu_lin[1][1]; // * imu[5];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+5] = delta_t * dpXci_pYci_imu_lin[0][2]; // * imu[5];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+5] = delta_t * dpXci_pYci_imu_lin[1][2]; // * imu[5];
}
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYNDderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dpXci_dtilt;
pXYNDderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dpYci_dtilt;
pXYNDderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dpXci_dazimuth;
pXYNDderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dpYci_dazimuth;
pXYNDderiv[2 * i + 0][CorrVector.ZOOM_INDEX+i] = dpXci_dzoom;
pXYNDderiv[2 * i + 1][CorrVector.ZOOM_INDEX+i] = dpYci_dzoom;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYNDderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dpXci_dtilt;
pXYNDderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dpYci_dtilt;
pXYNDderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dpXci_dazimuth;
pXYNDderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dpYci_dazimuth;
pXYNDderiv[2 * i + 0][CorrVector.ZOOM_INDEX+j] = -dpXci_dzoom;
pXYNDderiv[2 * i + 1][CorrVector.ZOOM_INDEX+j] = -dpYci_dzoom;
}
}
pXYNDderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dpXci_droll;
pXYNDderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dpYci_droll;
}
}
return pXYND;
}
*/
/**
* Calculate pixel common "idealized" coordinates of the auxiliary camera image tile matching the specified tile (px,py) of the idealized
* "center" (still distorted) image of the main camera and generic disparity, measured in pixels
......
......@@ -9,7 +9,7 @@ import com.elphel.imagej.gpu.GPUTileProcessor;
//import Jama.Matrix;
public class ImageDtt extends ImageDttCPU {
public boolean debug_strengths = false; // true;
private final GPUTileProcessor.GpuQuad gpuQuad;
public ImageDtt(
......@@ -2303,11 +2303,9 @@ public class ImageDtt extends ImageDttCPU {
final double [][] lazy_eye_data = new double [clustersY*clustersX][];
final int gpu_corr_rad = transform_size - 1;
final int nClustersInChn=clustersX * clustersY;
/// final int clustSize = tileStep*tileStep;
final int debug_clustX = debug_tileX / tileStep;
final int debug_clustY = debug_tileY / tileStep;
/// final int bg_extra = (tileSizeBg - tileStep + 1) / 2; // add overlap around each bg cluster
// calculate which tiles to use for each cluster
// will generate sparse array for cluster central tiles to match CPU software
......@@ -2322,7 +2320,6 @@ public class ImageDtt extends ImageDttCPU {
final AtomicInteger ai = new AtomicInteger(0);
final double shiftX = 0.0;
final double shiftY = 0.0;
// final int super_radius = tileSizeBg; // rename? 0 - none, 1 - 3x3, 2 - 5x5, ...
// TODO: Maybe calculate full energy in each TD tile for normalization
// First pass merge correlation result for each cluster
for (int ithread = 0; ithread < threads.length; ithread++) {
......@@ -2439,7 +2436,6 @@ public class ImageDtt extends ImageDttCPU {
}
}
// tile_op[centerY][centerX] = task_val;
num_in_cluster [clustY][clustX] = num_good_tiles;
disparity_array_center[clustY][clustX] = avg;
......@@ -2523,7 +2519,7 @@ public class ImageDtt extends ImageDttCPU {
}
}
}
final double [][] dbg_img = new double[19][clustersX*clustersY];
final double [][] dbg_img = debug_strengths? (new double[19][clustersX*clustersY]):null;
// Second pass - process normalized per-cluster correlations
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
......@@ -2549,8 +2545,7 @@ public class ImageDtt extends ImageDttCPU {
int clustX = tileX/tileStep;
int clustY = tileY/tileStep;
int nclust = clustX + clustY * clustersX;
dbg_img[0][nclust] = 1.0;
/// boolean debugCluster1 = (Math.abs(clustX - debug_clustX) < 10) && (Math.abs(clustY - debug_clustY) < 10);
if (dbg_img != null) dbg_img[0][nclust] = 1.0;
double [] disp_str = null;
for (int indx_pair = 0; indx_pair < num_pairs; indx_pair++) {
......@@ -2614,7 +2609,7 @@ public class ImageDtt extends ImageDttCPU {
tileY); // int tileY
*/
if (lma2 != null) {
dbg_img[1][nclust] = 1.0;
if (dbg_img != null) dbg_img[1][nclust] = 1.0;
// was for single tile
disp_str = lma2.lmaDisparityStrength(
imgdtt_params.lmas_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
......@@ -2629,14 +2624,13 @@ public class ImageDtt extends ImageDttCPU {
double [][] ds_dbg = {disp_str};
lma2.printStats(ds_dbg,1);
}
//was for multi-tile
// int nCluster = clustY * clustersX + clustX;
double [][] ddnd = lma2.getDdNd();
// double [] stats = lma2.getStats (1); // num_in_cluster[clustY][clustX]);
double [] stats = lma2.getStats (num_in_cluster[clustY][clustX]);
if (dbg_img != null) {
dbg_img[2][nclust] = stats[0];
dbg_img[3][nclust] = stats[1];
dbg_img[4][nclust] = stats[2];
}
double [][] lma_ds = lma2.lmaDisparityStrengthLY( // [1][2]
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
......@@ -2647,6 +2641,8 @@ public class ImageDtt extends ImageDttCPU {
1.0, // imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
0.0); // imgdtt_params.lma_str_offset); // convert lma-generated strength to match previous ones - add to result
double strengh_k = 1.0; // 0.2*Math.sqrt(num_in_cluster[clustY][clustX]); // approximately matching old/multitile
if (dbg_img != null) {
double [][] dbg_ext_stat = lma2.lmaGetExtendedStats(
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
......@@ -2659,9 +2655,9 @@ public class ImageDtt extends ImageDttCPU {
dbg_img[5+ii][nclust] = dbg_ext_stat[0][ii];
}
dbg_img[16][nclust] = num_in_cluster[clustY][clustX];
double strengh_k = 1.0; // 0.2*Math.sqrt(num_in_cluster[clustY][clustX]); // approximately matching old/multitile
dbg_img[17][nclust] = strengh_k * lma_ds[0][1] * num_in_cluster[clustY][clustX];
dbg_img[18][nclust] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr;
}
if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) {
lazy_eye_data[nclust] = new double [ExtrinsicAdjustment.INDX_LENGTH];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_STRENGTH] = strengh_k * lma_ds[0][1] * num_in_cluster[clustY][clustX];
......@@ -2695,12 +2691,15 @@ public class ImageDtt extends ImageDttCPU {
};
}
startAndJoin(threads);
if (dbg_img != null) {
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
clustersX,
clustersY,
true,
"ly_dbg"); // name+"-CORR2D-D"+clt_parameters.disparity,
}
if (super_radius == 0) {
return lazy_eye_data; // no processing of clouds in the sky
}
......@@ -2708,7 +2707,6 @@ public class ImageDtt extends ImageDttCPU {
final double [][] lazy_eye_data_final = new double [clustersY*clustersX][];
final int [][] num_in_cluster_final = new int [clustersY][clustersX]; // only in cluster centers
final float [][][][] fcorr_td_super = new float [tilesY][tilesX][][]; // sparse, only in cluster centers
// final float [][][][] fcorr_td_centers = new float [tilesY][tilesX][][]; // sparse, only in cluster centers
final double [][][][] disp_dist_super = new double [clustersY][clustersX][][];
final double [][][] clust_pY_super = new double [clustersY][clustersX][];
final double [][][] pxpy_super = new double [clustersY][clustersX][];
......@@ -2833,25 +2831,8 @@ public class ImageDtt extends ImageDttCPU {
final float [][] fcorr2D_super = gpuQuad.getCorr2D(gpu_corr_rad); // int corr_rad);
final int corr_length_super = fcorr2D_super[0].length + 0;// all correlation tiles have the same size
final int num_tiles_super = corr_indices_super.length / num_pairs;
/* Reuse same as already calculated
final double [][] corr_wnd = Corr2dLMA.getCorrWnd(
transform_size,
imgdtt_params.lma_wnd);
final double [] corr_wnd_inv_limited = (imgdtt_params.lma_min_wnd <= 1.0)? new double [corr_wnd.length * corr_wnd[0].length]: null;
if (corr_wnd_inv_limited != null) {
double inv_pwr = imgdtt_params.lma_wnd_pwr - (imgdtt_params.lma_wnd - 1.0); // compensate for lma_wnd
for (int i = imgdtt_params.lma_hard_marg; i < (corr_wnd.length - imgdtt_params.lma_hard_marg); i++) {
for (int j = imgdtt_params.lma_hard_marg; j < (corr_wnd.length - imgdtt_params.lma_hard_marg); j++) {
corr_wnd_inv_limited[i * (corr_wnd.length) + j] = 1.0/Math.max(Math.pow(corr_wnd[i][j],
inv_pwr),
imgdtt_params.lma_min_wnd);
}
}
}
*/
final double [][] dbg_img2 = debug_strengths? (new double[19][clustersX*clustersY]):null;
final double [][] dbg_img2 = new double[19][clustersX*clustersY];
// Fourth pass - process normalized per-cluster correlations for low-contrast infinity tiles (clouds in the sky)
ai.set(0);
// TODO: 2) calculate lazy_eye_data_final
......@@ -2879,7 +2860,9 @@ public class ImageDtt extends ImageDttCPU {
int clustX = tileX/tileStep;
int clustY = tileY/tileStep;
int nclust = clustX + clustY * clustersX;
if (dbg_img2 != null) {
dbg_img2[0][nclust] = 1.0;
}
double [] disp_str = null;
for (int indx_pair = 0; indx_pair < num_pairs; indx_pair++) {
......@@ -2943,7 +2926,6 @@ public class ImageDtt extends ImageDttCPU {
tileY); // int tileY
*/
if (lma2 != null) {
dbg_img2[1][nclust] = 1.0;
// was for single tile
disp_str = lma2.lmaDisparityStrength(
imgdtt_params.lmas_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
......@@ -2958,14 +2940,8 @@ public class ImageDtt extends ImageDttCPU {
double [][] ds_dbg = {disp_str};
lma2.printStats(ds_dbg,1);
}
//was for multi-tile
// int nCluster = clustY * clustersX + clustX;
double [][] ddnd = lma2.getDdNd();
// double [] stats = lma2.getStats (1); // num_in_cluster_final[clustY][clustX]);
double [] stats = lma2.getStats (num_in_cluster_final[clustY][clustX]);
dbg_img2[2][nclust] = stats[0];
dbg_img2[3][nclust] = stats[1];
dbg_img2[4][nclust] = stats[2];
double k = 2.5;
double [][] lma_ds = lma2.lmaDisparityStrengthLY( // [1][2]
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
......@@ -2975,6 +2951,13 @@ public class ImageDtt extends ImageDttCPU {
k* imgdtt_params.lma_max_area, //double lma_max_area, // maximal half-area (if > 0.0)
1.0, // imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
0.0); // imgdtt_params.lma_str_offset); // convert lma-generated strength to match previous ones - add to result
double strengh_k = 1.0; // 0.2*Math.sqrt(num_in_cluster[clustY][clustX]); // approximately matching old/multitile
strengh_k /= (2 * super_radius + 1)*(2 * super_radius + 1);
if (dbg_img2 != null) {
dbg_img2[1][nclust] = 1.0;
dbg_img2[2][nclust] = stats[0];
dbg_img2[3][nclust] = stats[1];
dbg_img2[4][nclust] = stats[2];
double [][] dbg_ext_stat = lma2.lmaGetExtendedStats(
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
......@@ -2987,11 +2970,10 @@ public class ImageDtt extends ImageDttCPU {
for (int ii = 0; ii < dbg_ext_stat[0].length; ii++) {
dbg_img2[5+ii][nclust] = dbg_ext_stat[0][ii];
}
double strengh_k = 1.0; // 0.2*Math.sqrt(num_in_cluster[clustY][clustX]); // approximately matching old/multitile
strengh_k /= (2 * super_radius + 1)*(2 * super_radius + 1);
dbg_img2[16][nclust] = num_in_cluster_final[clustY][clustX];
dbg_img2[17][nclust] = strengh_k * lma_ds[0][1] * num_in_cluster_final[clustY][clustX];
dbg_img2[18][nclust] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr;
}
if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) {
lazy_eye_data_final[nclust] = new double [ExtrinsicAdjustment.INDX_LENGTH];
......@@ -3025,6 +3007,7 @@ public class ImageDtt extends ImageDttCPU {
};
}
startAndJoin(threads);
if (dbg_img2 != null) {
(new ShowDoubleFloatArrays()).showArrays(
dbg_img2,
clustersX,
......@@ -3048,8 +3031,7 @@ public class ImageDtt extends ImageDttCPU {
clustersY,
true,
"ly_dbg_combo");
}
return lazy_eye_data_final;
}
......
......@@ -45,6 +45,7 @@ import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.cameras.EyesisCorrectionParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.correction.EyesisCorrections;
import com.elphel.imagej.jp4.JP46_Reader_camera;
import ij.IJ;
......@@ -555,18 +556,23 @@ public class MLStats {
}
public static boolean listExtrinsics(String dir) // , String mask)
public static boolean listExtrinsics(
String dir, //) // , String mask)
EyesisCorrections eyesis_corrections_main,
EyesisCorrections eyesis_corrections_aux)
{
Path path= Paths.get(dir);
boolean inPixels = true;
boolean inMrad = false;
boolean showATR = true;
boolean showZooms = true;
boolean showERS = true;
boolean showSym = true;
boolean showRigATR = true;
boolean showRigZoom = true;
boolean showRigAngle = true;
boolean showRigBaseline = false;
boolean showAux = true;
String mask = ".*EXTRINSICS\\.corr-xml";
GenericDialog gd = new GenericDialog("Select file mask and output format");
......@@ -575,11 +581,13 @@ public class MLStats {
gd.addCheckbox("Show results in mrad (false in arcseconds)", inMrad);
gd.addCheckbox("Show azimuths, tilts, rolls", showATR);
gd.addCheckbox("Show zooms", showZooms);
gd.addCheckbox("Show ERS", showERS);
gd.addCheckbox("Show symmetric angles", showSym);
gd.addCheckbox("Show rig azimuth, tilt, roll", showRigATR);
gd.addCheckbox("Show rig zoom", showRigZoom);
gd.addCheckbox("Show rig angle", showRigAngle);
gd.addCheckbox("Show rig baseline", showRigBaseline);
gd.addCheckbox("Show aux camera and rig", showAux);
gd.showDialog ();
if (gd.wasCanceled()) return false;
mask = gd.getNextString();
......@@ -587,19 +595,30 @@ public class MLStats {
inMrad = gd.getNextBoolean();
showATR = gd.getNextBoolean();
showZooms = gd.getNextBoolean();
showERS = gd.getNextBoolean();
showSym = gd.getNextBoolean();
showRigATR = gd.getNextBoolean();
showRigZoom = gd.getNextBoolean();
showRigAngle = gd.getNextBoolean();
showRigBaseline = gd.getNextBoolean();
showAux = gd.getNextBoolean();
if (!showAux) {
showRigATR = false;
showRigZoom = false;
showRigAngle = false;
showRigBaseline = false;
}
String units = inPixels ? "pix":(inMrad?"mil":"\"");
String zunits = inPixels ? "pix":(inMrad?"mil":"\"");
String ers_lin_units = inPixels ? "mm/s":"m/s";
double scale = inPixels ? 1.0 : (inMrad?1000.0:(180.0/Math.PI*60*60)); //leave pixels as is, convert radians to arc-sec
String fmt = "\t"+(inPixels ? "%8.4f":(inMrad?"%8.4f":"%8.2f"));
String fmt_angle = "\t%8.3f";
String fmt_len = "\t%8.3f";
int num_sym = showZooms? GeometryCorrection.CorrVector.LENGTH:GeometryCorrection.CorrVector.ZOOM_INDEX;
int num_sym = showZooms? GeometryCorrection.CorrVector.IMU_INDEX:GeometryCorrection.CorrVector.ZOOM_INDEX; // update
if (showERS) num_sym = GeometryCorrection.CorrVector.LENGTH;
class ModVerString{
String model;
String version;
......@@ -632,6 +651,12 @@ public class MLStats {
num_col+=4;
}
if (showERS) {
header+=String.format("\ters tilt (%s/s)\ters az (%s/s)\ters roll (%s/s)", units, units, units);
header+=String.format("\ters Vx (%s)\ters Vy (%s)\ters Vz (%s)", ers_lin_units, ers_lin_units, ers_lin_units);
num_col+=6;
}
if (showSym) {
for (int i = 0; i < num_sym; i++) {
header+=String.format("\tsym%02d-m", i);
......@@ -639,7 +664,7 @@ public class MLStats {
num_col+=num_sym;
}
if (showAux) {
if (showATR) {
header+=String.format("\taz a0 (%s)\taz a1 (%s)\taz a2 (%s)\taz a3 (%s)"+
"\ttl a0 (%s)\ttl a1 (%s) \ttl a2 (%s)\ttl a3 (%s)"+
......@@ -651,12 +676,18 @@ public class MLStats {
header+=String.format("\tzm a0 (%s)\tzm a1 (%s)\tzm a2 (%s)\tzm a3 (%s)", zunits, zunits, zunits, zunits);
num_col+=4;
}
if (showERS) {
header+=String.format("\ters tilt (%s/s) a\ters az (%s/s) a\ters roll (%s/s) a", units, units, units);
header+=String.format("\ters Vx (%s)\ters Vy (%s)\ters Vz (%s)", ers_lin_units, ers_lin_units, ers_lin_units);
num_col+=6;
}
if (showSym) {
for (int i = 0; i < num_sym; i++) {
header+=String.format("\tsym%02d-a", i);
}
num_col+=num_sym;
}
}
if (showRigATR) {
header+=String.format("\trig azmth (%s)\trig tilt(%s)\trig roll (%s)", units, units, units);
num_col+=3;
......@@ -712,13 +743,13 @@ public class MLStats {
QuadCLT qcm = new QuadCLT(
QuadCLT.PREFIX, // String prefix,
properties, // Properties properties,
null, // EyesisCorrections eyesisCorrections,
eyesis_corrections_main, // null, // EyesisCorrections eyesisCorrections,
null // EyesisCorrectionParameters.CorrectionParameters correctionsParameters
);
QuadCLT qca = new QuadCLT(
QuadCLT.PREFIX_AUX, // String prefix,
properties, // Properties properties,
null, // EyesisCorrections eyesisCorrections,
eyesis_corrections_aux, //null, // EyesisCorrections eyesisCorrections,
null // EyesisCorrectionParameters.CorrectionParameters correctionsParameters
);
System.out.println(indx+": model:"+model+", version:"+version+", name: "+name);
......@@ -778,6 +809,17 @@ public class MLStats {
}
}
if (showERS) { // main camera
double [] v = new double[6];
for (int i = 0; i < v.length ; i++) {
v[i] = scale * cvm.getExtrinsicParameterValue(i+GeometryCorrection.CorrVector.IMU_INDEX, inPixels);
sb.append(String.format(fmt,v[i])); // zooms
fmts [ncol] = fmt;
stats[ncol ][0]+=v[i];
stats[ncol++][1]+=v[i]*v[i];
}
}
if (showSym) {
for (int i = 0; i < num_sym; i++) {
double v = scale * cvm.getExtrinsicSymParameterValue(i, inPixels);
......@@ -788,7 +830,7 @@ public class MLStats {
}
}
if (showAux) {
if (showATR) { // aux camera
double [] v = new double[4];
for (int i = 0; i <4; i++) {
......@@ -837,6 +879,17 @@ public class MLStats {
stats[ncol++][1]+=v[i]*v[i];
}
}
if (showERS) { // main camera
double [] v = new double[6];
for (int i = 0; i < v.length ; i++) {
v[i] = scale * cva.getExtrinsicParameterValue(i+GeometryCorrection.CorrVector.IMU_INDEX, inPixels);
sb.append(String.format(fmt,v[i])); // zooms
fmts [ncol] = fmt;
stats[ncol ][0]+=v[i];
stats[ncol++][1]+=v[i]*v[i];
}
}
if (showSym) {
for (int i = 0; i < num_sym; i++) {
double v = scale * cva.getExtrinsicSymParameterValue(i, inPixels);
......@@ -846,6 +899,7 @@ public class MLStats {
stats[ncol++][1]+=v*v;
}
}
}
if (showRigATR) {
int [] indices = {
GeometryCorrection.RigOffset.AUX_AZIMUTH_INDEX,
......
......@@ -4836,16 +4836,20 @@ public class QuadCLTCPU {
double [] old_new_rms = new double[2];
boolean apply_extrinsic = (clt_parameters.ly_corr_scale != 0.0);
double inf_min_disparity = clt_parameters.ly_inf_force_fine? clt_parameters.ly_inf_min_narrow :clt_parameters.ly_inf_min_broad;
double inf_max_disparity = clt_parameters.ly_inf_force_fine? clt_parameters.ly_inf_max_narrow :clt_parameters.ly_inf_max_broad;
GeometryCorrection.CorrVector corr_vector = ea.solveCorr (
clt_parameters.ly_marg_fract, // double marg_fract, // part of half-width, and half-height to reduce weights
clt_parameters.ly_inf_en, // boolean use_disparity, // adjust disparity-related extrinsics
// 1.0 - to skip filtering infinity
-1.0, //double inf_min_disparity, // minimal disparity for infinity
1.0, // double inf_max_disparity, // minimal disparity for infinity
inf_min_disparity, // -0.5, // -1.0, //double inf_min_disparity, // minimal disparity for infinity
inf_max_disparity, // 0.05, // 1.0, // double inf_max_disparity, // minimal disparity for infinity
clt_parameters.ly_inf_min_broad, // inf_min_disp_abs, // minimal disparity for infinity (absolute)
clt_parameters.ly_inf_max_broad, // maximal disparity for infinity (absolute)
clt_parameters.ly_inf_tilt, // boolean en_infinity_tilt, // select infinity tiles form right/left tilted (false - from average)
clt_parameters.ly_right_left, // boolean infinity_right_left, // balance weights between right and left halves of infinity
clt_parameters.ly_aztilt_en, // boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
clt_parameters.ly_diff_roll_en,//boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
// clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
clt_parameters.ly_min_forced, // int min_num_forced, // minimal number of clusters with forced disparity to use it
// data, using just radial distortions
clt_parameters.ly_com_roll, //boolean common_roll, // Enable common roll (valid for high disparity range only)
......@@ -4856,8 +4860,24 @@ public class QuadCLTCPU {
clt_parameters.ly_ers_vert, // boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
// add balancing-related here?
clt_parameters.ly_par_sel, // int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
1.0, // double weight_disparity,
1.0, // double weight_lazyeye,
clt_parameters.ly_weight_infinity, //0.3, // double weight_infinity, // 0.3, total weight of infinity tiles fraction (0.0 - 1.0)
clt_parameters.ly_weight_disparity, //0.0, // double weight_disparity, // 0.0 disparity weight relative to the sum of 8 lazy eye values of the same tile
clt_parameters.ly_weight_disparity_inf,//0.5, // double weight_disparity_inf,// 0.5 disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
clt_parameters.ly_max_disparity_far, //5.0, // double max_disparity_far, // 5.0 reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
clt_parameters.ly_max_disparity_use, //5.0, // double max_disparity_use, // 5.0 (default 1000)disable near objects completely - use to avoid ERS
clt_parameters.ly_inf_min_dfe, //1.75,// double min_dfe, // = 1.75;
clt_parameters.ly_inf_max_dfe, //5.0, // double max_dfe, // = 5.0; // <=0 - disable feature
// moving objects filtering
clt_parameters.ly_moving_en, // boolean moving_en, // enable filtering areas with potentially moving objects
clt_parameters.ly_moving_apply, // boolean moving_apply, // apply filtering areas with potentially moving objects
clt_parameters.ly_moving_sigma, // double moving_sigma, // blurring sigma for moving objects = 1.0;
clt_parameters.ly_max_mov_disparity, // double max_mov_disparity, // disparity limit for moving objects detection = 75.0;
clt_parameters.ly_rad_to_hdiag_mov, // double rad_to_hdiag_mov, // radius to half-diagonal ratio to remove high-distortion corners = 0.7 ; // 0.8
clt_parameters.ly_max_mov_average, // double max_mov_average, // do not attempt to detect moving objects if ERS is not accurate for terrain = .25;
clt_parameters.ly_mov_min_L2, // double mov_min_L2, // threshold for moving objects = 0.75;
dsxy, // double [][] measured_dsxy,
null, // boolean [] force_disparity, // boolean [] force_disparity,
false, // boolean use_main, // corr_rots_aux != null;
......@@ -7710,18 +7730,7 @@ public class QuadCLTCPU {
bg_use[nTile] = true;
}
}
/*
// grow bg_use, but inside bg_sel;
int bg_grow = 9; //2*2;
tp.growTiles(
bg_grow, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
bg_use, // boolean [] tiles,
null); // boolean [] prohibit)
for (int nTile = 0 ; nTile < bg_use.length; nTile++) {
bg_use[nTile] &= bg_sel[nTile];
}
*/
if (true) { //!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) {
if (!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) { // if (true)
String [] dbg_titles = {"fdisp", "fstr", "disp", "str", "overexp","sel","use"};
double [][] ddd = {filtered_bgnd_disp_strength[0],filtered_bgnd_disp_strength[1],null,bg_str,bg_overexp, null, null};
ddd[5] = new double [bg_sel.length];
......@@ -7738,7 +7747,7 @@ public class QuadCLTCPU {
"filtered_bgnd_disp_strength",dbg_titles);
}
int num_bg = tp.clt_3d_passes.get(bg_scan).setTileOpDisparity( // other minimal strength?
bg_use, // bg_sel, // bg_use, // boolean [] selection, measure all that can be bg
bg_sel, // bg_use, // bg_sel, // bg_use, // boolean [] selection, measure all that can be bg
null); // double [] disparity); // null for 0
// Prepare measurement of combo-scan - remove low strength and what was used for background
......@@ -7780,8 +7789,7 @@ public class QuadCLTCPU {
tp.threadsMax, // maximal number of threads to launch
false, // updateStatus,
debugLevelInner - 1);
//// if (!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-3))
{
if (!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-3)) {
tp.showScan(
tp.clt_3d_passes.get(bg_scan), // CLTPass3d scan, badly filtered?
"bg_scan_post"); //String title)
......@@ -7828,7 +7836,7 @@ public class QuadCLTCPU {
combo_use[nTile] = true;
}
}
if (true) { // !batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) {
if ( !batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) { // true
String [] dbg_titles = {"fdisp", "fstr", "disp", "str", "overexp","sel","use"};
double [][] ddd = {filtered_combo_scand_isp_strength[0],filtered_combo_scand_isp_strength[1],combo_disp,combo_str,combo_overexp, null, null};
ddd[5] = new double [combo_use.length];
......@@ -7852,8 +7860,8 @@ public class QuadCLTCPU {
if (debugLevel > -3) { // -1
System.out.println("Updated number of lazy eye tiles = " + num_combo1+" (was "+num_combo+")");
}
// if (!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) {
if (clt_parameters.show_extrinsic && (debugLevel >-3)) {
if (!batch_mode && clt_parameters.show_extrinsic && (debugLevel >-1)) {
//// if (clt_parameters.show_extrinsic && (debugLevel >-3)) {
String [] titles = {"bgnd_disp","bgnd_str","combo_disp","combo_str","bg_sel","bg_use","combo_use"};
double [] dbg_bg_sel = new double [bg_sel.length];
double [] dbg_bg_use = new double [bg_sel.length];
......@@ -7931,7 +7939,9 @@ public class QuadCLTCPU {
boolean apply_extrinsic = (clt_parameters.ly_corr_scale != 0.0);
CLTPass3d scan = tp.clt_3d_passes.get(combo_scan);
// for the second half of runs (always for single run) - limit infinity min/max
if (debugLevel > 9) {
ea.showInput(scan.getLazyEyeData(),"first_data");
}
boolean debug_actual_LY_derivs = debugLevel > 9; // true
......@@ -7953,6 +7963,11 @@ public class QuadCLTCPU {
// 1.0 - to skip filtering infinity
inf_min, //double inf_min_disparity, // minimal disparity for infinity
inf_max, // double inf_max_disparity, // minimal disparity for infinity
clt_parameters.ly_inf_min_broad, // inf_min_disp_abs, // minimal disparity for infinity (absolute)
clt_parameters.ly_inf_max_broad, // maximal disparity for infinity (absolute)
clt_parameters.ly_inf_tilt, // boolean en_infinity_tilt, // select infinity tiles form right/left tilted (false - from average)
clt_parameters.ly_right_left, // boolean infinity_right_left, // balance weights between right and left halves of infinity
clt_parameters.ly_aztilt_en, // boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
clt_parameters.ly_diff_roll_en, // boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
// clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
......@@ -7966,8 +7981,23 @@ public class QuadCLTCPU {
clt_parameters.ly_ers_vert, // boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
// add balancing-related here?
clt_parameters.ly_par_sel, // int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
0.5, // double weight_disparity,
1.0, // double weight_lazyeye,
clt_parameters.ly_weight_infinity, //0.3, // double weight_infinity, // 0.3, total weight of infinity tiles fraction (0.0 - 1.0)
clt_parameters.ly_weight_disparity, //0.0, // double weight_disparity, // 0.0 disparity weight relative to the sum of 8 lazy eye values of the same tile
clt_parameters.ly_weight_disparity_inf,//0.5, // double weight_disparity_inf,// 0.5 disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
clt_parameters.ly_max_disparity_far, //5.0, // double max_disparity_far, // 5.0 reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
clt_parameters.ly_max_disparity_use, //5.0, // double max_disparity_use, // 5.0 (default 1000)disable near objects completely - use to avoid ERS
clt_parameters.ly_inf_min_dfe, //1.75,// double min_dfe, // = 1.75;
clt_parameters.ly_inf_max_dfe, //5.0, // double max_dfe, // = 5.0; // <=0 - disable feature
// moving objects filtering
clt_parameters.ly_moving_en, // boolean moving_en, // enable filtering areas with potentially moving objects
clt_parameters.ly_moving_apply, // boolean moving_apply, // apply filtering areas with potentially moving objects
clt_parameters.ly_moving_sigma, // double moving_sigma, // blurring sigma for moving objects = 1.0;
clt_parameters.ly_max_mov_disparity, // double max_mov_disparity, // disparity limit for moving objects detection = 75.0;
clt_parameters.ly_rad_to_hdiag_mov, // double rad_to_hdiag_mov, // radius to half-diagonal ratio to remove high-distortion corners = 0.7 ; // 0.8
clt_parameters.ly_max_mov_average, // double max_mov_average, // do not attempt to detect moving objects if ERS is not accurate for terrain = .25;
clt_parameters.ly_mov_min_L2, // double mov_min_L2, // threshold for moving objects = 0.75;
scan.getLazyEyeData(), // dsxy, // double [][] measured_dsxy,
scan.getLazyEyeForceDisparity(), // null, // boolean [] force_disparity, // boolean [] force_disparity,
false, // boolean use_main, // corr_rots_aux != null;
......
......@@ -386,14 +386,60 @@ public class TileNeibs{
}
public int [] distanceFromEdge(
boolean [] tiles) {
int [] dfe = new int [tiles.length];
for (int i = 0; i < tiles.length; i++) dfe[i] = tiles[i] ? -1 : 0;
ArrayList<Integer> front = new ArrayList<Integer>();
int dist = 0;
for (boolean has_empty = true; has_empty;) {
has_empty = false;
for (int start_indx = 0; start_indx < tiles.length; start_indx++) if (dfe[start_indx] < 0){
has_empty = true;
// see if it has wave front around
boolean has_front_near = false;
for (int d = 0; (d < dirs) && !has_front_near; d++){
int ipx1 = getNeibIndex(start_indx, d);
if ((ipx1 >= 0) && (dfe[ipx1] == dist)) {
has_front_near = true;
}
}
if (has_front_near) {
dfe[start_indx] = dist+1;
front.add(start_indx);
// build wave front (may be several)
while (!front.isEmpty()) {
int ipx = front.remove(0);// get oldest element
for (int d = 0; (d < dirs) && !has_front_near; d++){
int ipx1 = getNeibIndex(ipx, d);
if ((ipx1 >= 0) && (dfe[ipx1] < 0)) { // fresh cell
// does it has front neighbor?
for (int d1 = 0; (d1 < dirs) && !has_front_near; d1++){
int ipx2 = getNeibIndex(ipx1, d1);
if ((ipx2 >= 0) && (dfe[ipx2] == dist)) { // old front cell
dfe[ipx1] = dist+1;
front.add(ipx1);
break;
}
}
}
}
}
}
}
dist++;
}
return dfe;
}
/**
* Enumerate clusters on rectangular area
* @param tiles selected tiles, size should be sizeX * sizeY
* @param ordered if true, order tiles from largest to smallest5
* @return integer array, where 0 is unused, 1+ cluster it belongs to
*/
public int [] enumerateClusters(
boolean [] tiles,
boolean ordered)
......
......@@ -8325,11 +8325,14 @@ if (debugLevel > -100) return true; // temporarily !
"pre-adjust-extrinsic-scan-"+s); //String title)
}
}
double inf_min = -1.0;
double inf_max = 1.0;
if (num_adjust_main >= (adjust_main/2)) {
inf_min = -0.2;
inf_max = 0.05;
double inf_min = clt_parameters.ly_inf_min_broad; // -0.5;
double inf_max = clt_parameters.ly_inf_max_broad; // 0.5;
if (clt_parameters.ly_inf_force_fine || (num_adjust_main >= (adjust_main/2))) {
inf_min = clt_parameters.ly_inf_min_narrow; // -0.2;
inf_max = clt_parameters.ly_inf_max_narrow; // 0.05;
System.out.println("Late adjustment, using narrow band infinity detection, inf_min="+inf_min+", inf_max="+inf_max);
} else {
System.out.println("Early adjustment, using wide band infinity detection, inf_min="+inf_min+", inf_max="+inf_max);
}
boolean ok = quadCLT_main.extrinsicsCLT(
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
......@@ -8407,7 +8410,7 @@ if (debugLevel > -100) return true; // temporarily !
quadCLT_main.tp.clt_3d_passes.get( quadCLT_main.tp.clt_3d_passes.size() -1),
false); // boolean force_final);
if (debugLevel > -5){
if (debugLevel > -1) { //-5){
int scan_index = quadCLT_main.tp.clt_3d_passes.size() -1;
quadCLT_main.tp.showScan(
quadCLT_main.tp.clt_3d_passes.get(scan_index), // CLTPass3d scan,
......@@ -8508,6 +8511,18 @@ if (debugLevel > -100) return true; // temporarily !
}
if (num_restored < 2) {
System.out.println("No DSI from the main camera is available. Please re-run with 'clt_batch_explore' enabled to generate it");
if (quadCLT_main.correctionsParameters.clt_batch_save_extrinsics) {
saveProperties(
null, // String path, // full name with extension or w/o path to use x3d directory
null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
}
if (quadCLT_main.correctionsParameters.clt_batch_save_all) {
saveProperties(
null, // String path, // full name with extension or w/o path to use x3d directory
properties, // Properties properties, // if null - will only save extrinsics)
debugLevel);
}
continue; // skipping to the next file
}
}
......@@ -8682,6 +8697,8 @@ if (debugLevel > -100) return true; // temporarily !
debugLevel); // final int debugLevel)
}
if (quadCLT_main.correctionsParameters.clt_batch_save_extrinsics) {
saveProperties(
null, // String path, // full name with extension or w/o path to use x3d directory
......
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