Commit c2c4ce6e authored by Andrey Filippov's avatar Andrey Filippov

Tweaking fine adjustment

parent 24b642b0
......@@ -39,6 +39,7 @@ public class AlignmentCorrection {
static final int INDEX_10_WEIGHT = 1;
static final int INDEX_10_DISPARITY = 0;
static final int INDEX_14_DISPARITY = 0;
static final double DISP_SCALE = 2.0;
QuadCLT qc;
......@@ -144,7 +145,7 @@ public class AlignmentCorrection {
0.5*(offsets[1][0] - offsets[0][0]), //(dx1 - dx0)/2; 0.5/magic is already applied
0.5*(offsets[3][1] - offsets[2][1]), //(dy3 - dy2)/2;
0.25* (offsets[0][0] + offsets[1][0] - offsets[2][1] - offsets[3][1]), //(dx0 + dx1 -dy2 - dy3)/4;
use_disparity ? (0.125* (offsets[0][0] + offsets[1][0] + offsets[2][1] + offsets[3][1])) : 0.0 // only when disparity is known to be 0
use_disparity ? (DISP_SCALE* 0.125* (offsets[0][0] + offsets[1][0] + offsets[2][1] + offsets[3][1])) : 0.0 // only when disparity is known to be 0
};
return y;
}
......@@ -216,7 +217,7 @@ public class AlignmentCorrection {
{ 0.25, 0.0 , -0.25, 0.0 , -0.25, 0.0 , 0.25, 0.0 }, // mv4 = (dx1 - dx0)/2 = (x3 - x2 + x0 - x1) / 2
{ 0.0 , 0.25, 0.0 , -0.25, 0.0 , -0.25, 0.0 , 0.25 }, // mv5 = (dy3 - dy2)/2 = (y3 - y1 + y0 - y2) / 2
{-0.125, 0.125, 0.125, 0.125, -0.125, -0.125, 0.125, -0.125 }, // mv6 = (dx0 + dx1 -dy2 - dy3)/4 = (x1 - x0 + x3 - x2 - y2 + y0 - y3 + y1)/4
{-0.0625, -0.0625, 0.0625,-0.0625,-0.0625, 0.0625, 0.0625, 0.0625}};// mv7 = (dx0 + dx1 +dy2 + dy3)/8= (x1 - x0 + x3 - x2 + y2 - y0 + y3 - y1)/8
{-0.0625*DISP_SCALE, -0.0625*DISP_SCALE, 0.0625*DISP_SCALE,-0.0625*DISP_SCALE,-0.0625*DISP_SCALE, 0.0625*DISP_SCALE, 0.0625*DISP_SCALE, 0.0625*DISP_SCALE}};// mv7 = (dx0 + dx1 +dy2 + dy3)/8= (x1 - x0 + x3 - x2 + y2 - y0 + y3 - y1)/8
return dMismatch_dXY;
}
......@@ -397,7 +398,8 @@ public class AlignmentCorrection {
use_poly, // final boolean use_poly,
clt_parameters.fcorr_inf_quad, // final boolean use_quadratic,
clt_parameters.fcorr_inf_vert, // final boolean use_vertical,
clt_parameters.ly_inf_en, // final boolean use_disparity, // for infinity - if true, restores differences in the direction of disparity that was subtracted during measurement)
// now disaprity is already restored
false, //clt_parameters.ly_inf_en, // final boolean use_disparity, // for infinity - if true, restores differences in the direction of disparity that was subtracted during measurement)
// For ly_inf_en need to make sure that programmed disparity was 0.0, so
clt_parameters.ly_inf_disp, //final boolean allow_dispatity,
clt_parameters,
......@@ -913,7 +915,7 @@ public class AlignmentCorrection {
double [][] dbg_xy = null;
if (clt_parameters.show_extrinsic && (debugLevel > -2)) { // TODO: Add clt_parameters
// dbg_xy = new double [9][num_tiles];
dbg_xy = new double [10][num_tiles];
dbg_xy = new double [18][num_tiles];
}
for (Sample s: samples_list){
int tileX = s.tile % tilesX;
......@@ -1045,6 +1047,10 @@ B = |+dy0 -dy1 -2*dy3 |
}
dbg_xy[9][s.tile] += s.weight*(-xy[0]-xy[1]+xy[2]-xy[3]-xy[4]+xy[5]+xy[6]+xy[7])/8;
dbg_xy[8][s.tile] += s.weight;
for (int i = 0; i < dxy.length;i++) {
dbg_xy[2* i +10][s.tile] += dxy[i][0] * s.weight;
dbg_xy[2* i +11][s.tile] += dxy[i][1] * s.weight;
}
}
......@@ -1093,18 +1099,17 @@ B = |+dy0 -dy1 -2*dy3 |
if (dbg_xy != null){
for (int nTile = 0; nTile < num_tiles; nTile++){
if (dbg_xy[8][nTile] > 0.0){
for (int i = 0; i< 8; i++) {
for (int i = 0; i < dbg_xy.length; i++) if (i != 8){
dbg_xy[i][nTile] /= dbg_xy[8][nTile];
}
dbg_xy[9][nTile] /= dbg_xy[8][nTile];
} else {
for (int i = 0; i< 8; i++) {
for (int i = 0; i < dbg_xy.length; i++) if (i != 8){
dbg_xy[i][nTile] = Double.NaN;
dbg_xy[9][nTile] = Double.NaN;
}
}
}
String [] titles = {"x0", "y0", "x1", "y1", "x2", "y2", "x3","y3","weight","~disp"};
String [] titles = {"x0", "y0", "x1", "y1", "x2", "y2", "x3","y3","weight","~disp","dx0","dy0","dx1","dy1","dx2","dy2","dx3","dy3"};
(new showDoubleFloatArrays()).showArrays(
dbg_xy,
tilesX,
......@@ -1730,44 +1735,6 @@ B = |+dy0 -dy1 -2*dy3 |
return scans;
}
public double [][][] lazyEyeCorrectionStep(
final boolean use_poly, // Use polynomial correction, false - correct tilt/azimuth/roll of each sensor
final double fcorr_radius,
final double min_strength_in,
final double max_diff,
// final double comp_strength_var,
final int max_iterations,
final double max_coeff_diff,
final double far_pull, // = 0.2; // 1; // 0.5;
final double strength_pow,
final double lazyEyeCompDiff, // clt_parameters.fcorr_disp_diff
final int lazyEyeSmplSide, // = 2; // Sample size (side of a square)
final int lazyEyeSmplNum, // = 3; // Number after removing worst (should be >1)
final double lazyEyeSmplRms, // = 0.1; // Maximal RMS of the remaining tiles in a sample
final double lazyEyeDispVariation, // maximal full disparity difference between tgh tile and 8 neighborxs
final int smplSide, // = 2; // Sample size (side of a square)
final int smplNum, // = 3; // Number after removing worst (should be >1)
final double smplRms, // = 0.1; // Maximal RMS of the remaining tiles in a sample
// histogram parameters
final int hist_smpl_side, // 8 x8 masked, 16x16 sampled
final double hist_disp_min,
final double hist_disp_step,
final int hist_num_bins,
final double hist_sigma,
final double hist_max_diff,
final int hist_min_samples,
final boolean hist_norm_center, // if there are more tiles that fit than min_samples, replace with
final double inf_fraction, // fraction of the weight for the infinity tiles
EyesisCorrectionParameters.CLTParameters clt_parameters,
double [][] scans_14,
int tilesX,
double magic_coeff, // still not understood coefficient that reduces reported disparity value. Seems to be around 8.5
int debugLevel)
{
return null;
}
public double [][][] lazyEyeCorrection(
final boolean use_poly, // Use polynomial correction, false - correct tilt/azimuth/roll of each sensor
final boolean restore_disp_inf, // Restore subtracted disparity for scan #0 (infinity)
......@@ -1842,7 +1809,7 @@ B = |+dy0 -dy1 -2*dy3 |
}
// Add disparity to dx0, dx1, dy2, dy3 pairs
if (restore_disp_inf) {
if ( restore_disp_inf) { // ==clt_parameters.inf_restore_disp
for (int nTile = 0; nTile < num_tiles; nTile++) if (scans[INDEX_10_WEIGHT][nTile] > 0){
for (int i = 0; i < INDICES_10_DISP.length; i++) {
scans[INDICES_10_DISP[i]][nTile] += scans[INDEX_10_DISPARITY][nTile];
......@@ -2106,8 +2073,8 @@ B = |+dy0 -dy1 -2*dy3 |
}
// static final int INDEX_10_WEIGHT = 1;
if (debugLevel > -1) { // 0) {
System.out.println("test123");
if ((debugLevel > -1) && (hist_smpl_side > 0)) { // 0) {
String [] prefixes = {"disparity", "strength", "dx0", "dy0", "dx1", "dy1", "dx2", "dy2", "dx3", "dy3"};
String [] titles = new String [2 * NUM_SLICES];
for (int i = 0; i < NUM_SLICES; i++){
......@@ -2158,11 +2125,12 @@ B = |+dy0 -dy1 -2*dy3 |
}
(new showDoubleFloatArrays()).showArrays(dbg_img, tilesX1, tilesY1, true, "inf_and_ly8",titles);
}
System.out.println("test1234");
// create list for infinity data
// /clt_parameters.ly_inf_en,
ArrayList<Sample> inf_samples_list;
if (clt_parameters.ly_inf_en) {
// if (clt_parameters.ly_inf_en) {
if (true) { //clt_parameters.ly_inf_en) {
inf_samples_list = selectInfinityTiles(
clt_parameters.fcorr_radius, // final double fcorr_radius,
clt_parameters.fcorr_inf_vert,// final boolean use_vertical,
......@@ -2211,8 +2179,8 @@ B = |+dy0 -dy1 -2*dy3 |
for (Sample s: inf_samples_list) {
s.weight *= weights[0];
}
} else {
inf_samples_list = new ArrayList<Sample>(); // do not use infinity at all
// } else {
// inf_samples_list = new ArrayList<Sample>(); // do not use infinity at all
}
///-----
......@@ -2291,18 +2259,22 @@ B = |+dy0 -dy1 -2*dy3 |
if (!use_poly && (mismatch_list != null)){
double [] old_new_rms = new double[1];
boolean apply_extrinsic = true;
int solveCorr_debug = ((clt_parameters.lym_iter == 1) && (clt_parameters.ly_par_sel != 0))? 2 : debugLevel;
GeometryCorrection.CorrVector corr_vector = solveCorr (
clt_parameters.ly_inf_en, // boolean use_disparity, // if true will ignore disparity data even if available (was false)
clt_parameters.ly_combo_en, // boolean use_other_extr, // adjust other extrinsic parameters that do not influence disparity, common roll and zoom
// clt_parameters.ly_combo_en, // boolean use_other_extr, // adjust other extrinsic parameters that do not influence disparity, common roll and zoom
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_com_roll, // boolean common_roll, // Enable common roll (valid for high disparity range only)
clt_parameters.ly_focalLength, // boolean corr_focalLength, // Correct scales (focal length temperature? variations)
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)
mismatch_list, // ArrayList<Mismatch> mismatch_list,
qc.geometryCorrection, // GeometryCorrection geometryCorrection,
qc.geometryCorrection.getCorrVector(), // GeometryCorrection.CorrVector corr_vector,
old_new_rms, // double [] old_new_rms, // should be double[2]
// 2); // debugLevel); // 2); // 1); // int debugLevel)
debugLevel); // 2); // 1); // int debugLevel)
solveCorr_debug); // debugLevel); // 2); // 1); // int debugLevel)
//TODO: ** Put 2 here to debug derivative images (diff_dmv_dsym - does not match yet, probably different "addition" of angles)
if (debugLevel > -1){
......@@ -2487,7 +2459,7 @@ B = |+dy0 -dy1 -2*dy3 |
double [] dbg_a_vector= null;
geometryCorrection.getPortsCoordinatesAndDerivatives(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
1E-8, //6, // double delta, // 1e-6
1E-9, // 1E-8, //6, // double delta, // 1e-6
corr_vector, // CorrVector corr_vector,
deriv_dbg, // j_partial_debug, //
pXY[0], // double px,
......@@ -2812,11 +2784,14 @@ B = |+dy0 -dy1 -2*dy3 |
public GeometryCorrection.CorrVector solveCorr (
boolean use_disparity, // adjust disparity-related extrinsics
boolean use_other_extr, // adjust other extrinsic parameters that do not influence disparity, common roll and zoom
// boolean use_other_extr, // adjust other extrinsic parameters that do not influence disparity, common roll and zoom
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
boolean common_roll, // Enable common roll (valid for high disparity range only)
boolean corr_focalLength, // Correct scales (focal length temperature? variations)
int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
ArrayList<Mismatch> mismatch_list,
......@@ -2825,6 +2800,7 @@ B = |+dy0 -dy1 -2*dy3 |
double [] old_new_rms, // should be double[2]
int debugLevel)
{
boolean dbg_images = debugLevel > 0; // 1;
boolean has_disparity = force_convergence; // force false;
// See if at least some data has disparities to be adjusted
if (use_disparity) {
......@@ -2839,9 +2815,14 @@ B = |+dy0 -dy1 -2*dy3 |
// force_convergence, // boolean disparity_only,
// force_convergence && has_disparity, // boolean use_disparity,
has_disparity, // boolean use_disparity,
use_other_extr, // boolean use_other_extr,
// use_other_extr, // boolean use_other_extr,
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);
corr_focalLength, // boolean corr_focalLength);
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
double [][] jta = getJacobianTransposed( // gets transposed jacobian for 8*num_tiles points (port coordinates) and symmetrical parameters
par_mask, // boolean [] par_mask,
......@@ -2878,7 +2859,6 @@ B = |+dy0 -dy1 -2*dy3 |
Matrix y_minus_fx_weighted = new Matrix(y_minus_fx_a_weighted, y_minus_fx_a_weighted.length);
double [][] jtja = getJTJ(jta_mv, weights);
Matrix jtj = new Matrix(jtja); // getJTJ(jta, weights)); // less operations than jt.times(jt.transpose());
boolean dbg_images = debugLevel>1;
int dbg_decimate = 64; // just for the debug image
int dbg_width = qc.tp.getTilesX()*qc.tp.getTileSize();
int dbg_height = qc.tp.getTilesY()*qc.tp.getTileSize();
......@@ -2886,7 +2866,7 @@ B = |+dy0 -dy1 -2*dy3 |
int dbg_oheight = dbg_height/dbg_decimate;
int dbg_length = dbg_owidth*dbg_oheight;
String [] dbg_titles_sym= {"sym0","sym1","sym2","sym3","sym4","sym5","sroll0","sroll1","sroll2","sroll3", "zoom0", "zoom1", "zoom2"};
String [] dbg_titles_xy= {"x0","y0","x1","y1","x2","y2","x3","y3"};
String [] dbg_titles_xy= {"dx0","dy0","dx1","dy1","dx2","dy2","dx3","y3"};
String [] dbg_titles_mv= {"dy0","dy1","dx2","dx3","dx1-dx0","dy3-dy2","dh-dv","dh+dv"};
double [][] dbg_xy = null; // jacobian dmv/dsym
double [][] dbg_mv = null; // jacobian dmv/dsym
......
......@@ -1081,6 +1081,9 @@ public class Correlation2d {
for (int pair = 0; pair < corrs.length; pair++) if ((corrs[pair] != null) && (((1 << pair) & pair_mask) != 0)) {
// for (int np = 0; np < num_pairs; np++) {
int this_mask = 1 << pair;
if (debug_level > -1) {
System.out.println(String.format("mismatchPairs(), np = %d pairs mask = 0x%x", np, this_mask));
}
Correlations2dLMA lma=corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
......@@ -1089,7 +1092,7 @@ public class Correlation2d {
xcenter, // double xcenter, // preliminary center x in pixels for largest baseline
vasw_pwr, // double vasw_pwr, // value as weight to this power,
// debug_level-3, // int debug_level,
debug_level-1, // int debug_level,
debug_level, // -1, // int debug_level,
tileX, // int tileX, // just for debug output
tileY); //int tileY
if ((lma == null) || (lma.getPoly() == null)) {
......@@ -1238,7 +1241,7 @@ public class Correlation2d {
boolean are_ortho = (imn ^ imx) == 1;
double corr = overcorrection * (mx - mn);
int isel = bg ? imn : imx;
int isel = bg ? imn : imx; // isel may be -1 !!!
int iother = bg ? imx : imn;
int iortho = isel ^ 1;
......@@ -1249,7 +1252,7 @@ public class Correlation2d {
System.out.println(String.format("eff_strength = [%8.5f, %8.5f, %8.5f, %8.5f]", eff_strength[0], eff_strength[1], eff_strength[2], eff_strength[3]));
System.out.println(String.format("width_d = [%8.5f, %8.5f, %8.5f, %8.5f]", width_d[0], width_d[1], width_d[2], width_d[3]));
}
if (!strong[isel]) {
if (!strong[isel] || (isel <0)) {
corr = Double.NaN;
if (debug) System.out.println("Direction with "+(bg?"min":"max")+" disparity is not strong enough -> no correction");
} else if (width_d[isel] > max_hwidth) {
......
......@@ -643,9 +643,11 @@ public class Correlations2dLMA {
// modify to reuse Samples and apply polynomial approximation to resolve x0,y0 and strength?
public double [] getMaxXYPoly( // get interpolated maximum coordinates using 2-nd degree polynomial
/// double outside, // how much solution may be outside of the samples
boolean debug
) {
double [][][] mdata = new double[samples.size()][3][];
/// double min_x = Double.NaN, max_x = Double.NaN, min_y = Double.NaN, max_y =Double.NaN , min_v = Double.NaN;
for (int i = 0; i < mdata.length; i++) {
Sample s = samples.get(i);
mdata[i][0] = new double [2];
......@@ -655,6 +657,21 @@ public class Correlations2dLMA {
mdata[i][1][0] = s.v;
mdata[i][2] = new double [1];
mdata[i][2][0] = s.w;
/*
if (i == 0) {
min_x = s.x;
max_x = min_x;
min_y = s.y;
max_y = min_y;
min_v = s.v;
} else {
if (s.x > max_x) max_x = s.x;
else if (s.x < min_x) min_x = s.x;
if (s.y > max_y) max_y = s.y;
else if (s.y < min_y) min_y = s.y;
if (s.v < min_v) min_v = s.x;
}
*/
}
double [] rslt = (new PolynomialApproximation()).quadraticMaxV2dX2Y2XY( // 9 elements - Xc, Yx, f(x,y), A, B, C, D, E, F (from A*x^2 + B*y^2 +C*x*y+...)
mdata,
......@@ -662,11 +679,15 @@ public class Correlations2dLMA {
debug? 4:0);
this.poly_coeff = rslt;
if (rslt == null) {
if ((rslt == null) || (rslt[2] < 0.0) || // negative strength
(rslt[3] >= 0.0) || // x: min, not max
(rslt[4] >= 0.0)) { // y: min, not max
this.poly_coeff = null;
this.poly_xyvwh = null;
return null;
}
// if ()
// calculate width_x and width_y
double hwx = Double.NaN, hwy = Double.NaN;
if ((rslt[2] > 0.0) && (rslt[3] <0.0) && (rslt[4] <0.0)) {
......
......@@ -2449,13 +2449,16 @@ public class EyesisCorrectionParameters {
public double ih_max_diff = 0.1; // Keep samples within this difference from farthest maximum
public int ih_min_samples = 10; // Minimal number of remaining samples
public boolean ih_norm_center = true; // Replace samples with a single average with equal weight
public boolean inf_restore_disp = true; // Add disparity back to d{x,y}[i] (debug feature)
// Lazy eye parameters
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 boolean ly_combo_en = true; // Adjust other that disparity-related extrinsics
public boolean ly_aztilt_en = true; // Adjust azimuths and tilts
public boolean ly_diff_roll_en = true; // Adjust differential rolls (3 of 4 angles)
public boolean ly_focalLength= true; // Correct scales (focal length temperature? variations)
public boolean ly_com_roll= false; // Enable common roll (valid for high disparity range only)
public int ly_par_sel = 0; // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use checkbox selections above)
public double ly_inf_frac = 0.5; // Relative weight of infinity calibration data
public boolean ly_inf_disp= false; // Correct disparity for infinity tiles
public boolean ly_inf_force= false; // Force convergence correction during extrinsic, even with no infinity data
......@@ -3135,12 +3138,17 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"ih_max_diff", this.ih_max_diff +"");
properties.setProperty(prefix+"ih_min_samples", this.ih_min_samples+"");
properties.setProperty(prefix+"ih_norm_center", this.ih_norm_center+"");
properties.setProperty(prefix+"inf_restore_disp", this.inf_restore_disp+"");
properties.setProperty(prefix+"ly_on_scan", this.ly_on_scan+"");
properties.setProperty(prefix+"ly_inf_en", this.ly_inf_en+"");
properties.setProperty(prefix+"ly_combo_en", this.ly_combo_en+"");
properties.setProperty(prefix+"ly_aztilt_en", this.ly_aztilt_en+"");
properties.setProperty(prefix+"ly_diff_roll_en", this.ly_diff_roll_en+"");
properties.setProperty(prefix+"ly_focalLength", this.ly_focalLength+"");
properties.setProperty(prefix+"ly_com_roll", this.ly_com_roll+"");
properties.setProperty(prefix+"ly_par_sel", this.ly_par_sel+"");
properties.setProperty(prefix+"ly_inf_frac", this.ly_inf_frac +"");
properties.setProperty(prefix+"ly_inf_disp", this.ly_inf_disp+"");
......@@ -3767,12 +3775,17 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"ih_max_diff")!=null) this.ih_max_diff=Double.parseDouble(properties.getProperty(prefix+"ih_max_diff"));
if (properties.getProperty(prefix+"ih_min_samples")!=null) this.ih_min_samples=Integer.parseInt(properties.getProperty(prefix+"ih_min_samples"));
if (properties.getProperty(prefix+"ih_norm_center")!=null) this.ih_norm_center=Boolean.parseBoolean(properties.getProperty(prefix+"ih_norm_center"));
if (properties.getProperty(prefix+"inf_restore_disp")!=null) this.inf_restore_disp=Boolean.parseBoolean(properties.getProperty(prefix+"inf_restore_disp"));
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_combo_en")!=null) this.ly_combo_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_combo_en"));
if (properties.getProperty(prefix+"ly_aztilt_en")!=null) this.ly_aztilt_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_aztilt_en"));
if (properties.getProperty(prefix+"ly_diff_roll_en")!=null) this.ly_diff_roll_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_diff_roll_en"));
if (properties.getProperty(prefix+"ly_focalLength")!=null) this.ly_focalLength=Boolean.parseBoolean(properties.getProperty(prefix+"ly_focalLength"));
if (properties.getProperty(prefix+"ly_com_roll")!=null) this.ly_com_roll=Boolean.parseBoolean(properties.getProperty(prefix+"ly_com_roll"));
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_inf_frac")!=null) this.ly_inf_frac=Double.parseDouble(properties.getProperty(prefix+"ly_inf_frac"));
if (properties.getProperty(prefix+"ly_inf_disp")!=null) this.ly_inf_disp=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_disp"));
......@@ -4434,14 +4447,19 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Keep samples within this difference from farthest maximum", this.ih_max_diff, 3);
gd.addNumericField("Minimal number of remaining samples", this.ih_min_samples, 0);
gd.addCheckbox ("Replace samples with a single average with equal weight", this.ih_norm_center);
gd.addCheckbox ("Add disparity back to d{x,y}[i] (debug feature)", this.inf_restore_disp);
gd.addTab ("Lazy eye", "Lazy eye parameters");
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");
gd.addCheckbox ("Adjust other extrinsics that do not influence disparity", this.ly_combo_en,"disable if only disparity is critical");
gd.addCheckbox ("Adjust azimuths and tilts", this.ly_aztilt_en,"Adjust azimuths and tilts excluding those that change disparity");
gd.addCheckbox ("Adjust differential rolls", this.ly_diff_roll_en,"Adjust differential rolls (3 of 4 rolls, keeping average roll)");
gd.addCheckbox ("Correct scales (focal length temperature? variations)", this.ly_focalLength);
gd.addCheckbox ("Enable common roll adjustment (valid for high disparity range scans only)", this.ly_com_roll);
gd.addNumericField("Manual parameter mask selection (0 use checkboxes above)", this.ly_par_sel, 0, 5,"",
"bit 0 - sym0, bit1 - sym1, ...");
gd.addNumericField("Relative weight of infinity calibration data", this.ly_inf_frac, 3);
gd.addCheckbox ("Correct disparity for infinity tiles )has to disable until code fixed)", this.ly_inf_disp);
gd.addCheckbox ("Force convergence correction during extrinsic, even with no infinity data", this.ly_inf_force);
......@@ -5144,12 +5162,16 @@ public class EyesisCorrectionParameters {
this.ih_max_diff= gd.getNextNumber();
this.ih_min_samples= (int) gd.getNextNumber();
this.ih_norm_center= gd.getNextBoolean();
this.inf_restore_disp= gd.getNextBoolean();
this.ly_on_scan= gd.getNextBoolean();
this.ly_inf_en= gd.getNextBoolean();
this.ly_combo_en= gd.getNextBoolean();
this.ly_aztilt_en= gd.getNextBoolean();
this.ly_diff_roll_en= gd.getNextBoolean();
this.ly_focalLength= gd.getNextBoolean();
this.ly_com_roll= gd.getNextBoolean();
this.ly_par_sel= (int) gd.getNextNumber();
this.ly_inf_frac= gd.getNextNumber();
this.ly_inf_disp= gd.getNextBoolean();
......
......@@ -104,17 +104,25 @@ public class GeometryCorrection {
// boolean disparity_only,
// boolean use_disparity,
boolean use_disparity,
boolean use_other_extr,
// boolean use_other_extr,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength)
boolean corr_focalLength,
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
return (new CorrVector()).getParMask(
use_disparity, // disparity_only,
use_other_extr, // boolean use_other_extr,
// use_disparity,
// use_other_extr, // boolean use_other_extr,
use_aztilts, // Adjust azimuths and tilts excluding disparity
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,
corr_focalLength);
corr_focalLength,
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
}
......@@ -361,6 +369,38 @@ public class GeometryCorrection {
@Override
public String toString()
{
String s;
double [] sym_vect = toSymArray(null);
double [] v = new double [vector.length];
double [] sv = new double [vector.length];
for (int i = 0; i < ROLL_INDEX; i++){
v[i] = vector[i]*1000.0*focalLength/pixelSize; // tilt and azimuth
sv[i] = sym_vect[i]*1000.0*focalLength/pixelSize; // all 3 angles
}
for (int i = ROLL_INDEX; i < LENGTH_ANGLES; i++){
v[i] = vector[i]*1000.0*distortionRadius/pixelSize; // rolls
sv[i] = sym_vect[i]*1000.0*distortionRadius/pixelSize; // combined rolls
}
for (int i = LENGTH_ANGLES; i < LENGTH; i++){
v[i] = vector[i]*1000.0*distortionRadius/pixelSize; // zooms
sv[i] = sym_vect[i]*1000.0*distortionRadius/pixelSize; // zooms
}
s = String.format("tilt (up): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of he image center)\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) );
s += String.format("azimuth (right): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of he image center)\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) );
s += String.format("roll (CW): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift at the imge half-width from the center)\n" , v[6], v[7], v[8], v[9] );
s += String.format("diff zoom (in): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift at the imge half-width from the center)\n" , v[10], v[11], v[12], -(v[10] + v[11] + v[12]) );
s += "Symmetrical vector:\n";
s += " |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘| 6: common roll 7:(r0-r3)/2, |- +| |- -| |- +|\n";
s += " 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖| 8:(r1-r2)/2 9:(r0+r3-r1-r2)/4 10: |- +| 11: |+ +| 12: |+ -|\n";
s += String.format(" 0:%9.6fpx 1:%8.5fpx 2:%8.5fpx 3:%8.5fpx 4:%8.5fpx 5:%8.5fpx 6:%8.5fpx 7:%8.5fpx 8:%8.5fpx 9:%8.5fpx 10: %8.5fpx 11:%8.5fpx 12:%8.5fpx\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9], sv[10], sv[11], sv[12] );
return s;
}
public String toStringDegrees()
{
String s;
double [] sym_vect = toSymArray(null);
......@@ -389,6 +429,10 @@ public class GeometryCorrection {
return s;
}
public void incrementVector(double [] incr,
double scale)
{
......@@ -513,31 +557,43 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
public boolean [] getParMask(
boolean use_disparity,
boolean use_other_extr,
// boolean use_other_extr,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
// boolean disparity_only,
// boolean use_disparity,
boolean common_roll,
boolean corr_focalLength)
boolean corr_focalLength,
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
// common_roll &= !disparity_only;
// corr_focalLength &= !disparity_only;
// use_disparity |= disparity_only;
boolean [] par_mask = {
use_disparity, //sym0
use_other_extr, //sym1
use_other_extr, //sym2
use_other_extr, //sym3
use_other_extr, //sym4
use_other_extr, //sym5
use_aztilts, //sym1
use_aztilts, //sym2
use_aztilts, //sym3
use_aztilts, //sym4
use_aztilts, //sym5
common_roll, //sym6 // common roll
use_other_extr, //sym7
use_other_extr, //sym8
use_other_extr, //sym9
use_diff_rolls, //sym7
use_diff_rolls, //sym8
use_diff_rolls, //sym9
corr_focalLength, //sym10
corr_focalLength, //sym11
corr_focalLength //sym12
};
if (manual_par_sel != 0) {
for (int i = 0; i < par_mask.length; i++) {
par_mask[i] = ((manual_par_sel >> i) & 1) != 0;
}
System.out.println("*** Using manual parameter mask, overwriting boolean flags:");
for (int i = 0; i < par_mask.length; i++) {
System.out.println("Sym"+i+": "+par_mask[i]);
}
}
return par_mask;
}
......@@ -1178,6 +1234,13 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
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
* 2) apply disparity
* 3) apply rotations and zoom
* 4) re-apply distortion
* 5) return port center X and Y
*/
double [][] pXY = new double [numSensors][2];
......@@ -1195,15 +1258,21 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
// non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * this.rXY[i][1];
// TODO: convert to true rotations
// 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;
// calculate back to distorted
// 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
......@@ -1214,12 +1283,18 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
rri *= ri;
rD2rND += a[j]*(rri - 1.0); // Fixed
}
// Get port pixel coordiantes 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];
if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
......@@ -1329,7 +1404,6 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
Matrix [] corr_rots_m = cv_delta_m.getRotMatrices(); // get array of per-sensor rotation matrices
double [][] rslt_p = getPortsCoordinatesAndDerivatives(
// dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_rots_p, // Matrix [] rots,
null, // Matrix [][] deriv_rots,
null, // boolean calc_deriv,
......@@ -1338,7 +1412,6 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
disparity // double disparity
);
double [][] rslt_m = getPortsCoordinatesAndDerivatives(
// dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_rots_m, // Matrix [] rots,
null, // Matrix [][] deriv_rots,
null, // boolean calc_deriv,
......
......@@ -2222,7 +2222,8 @@ public class ImageDtt {
if (tile_lma_debug_level > -1) {
System.out.println("debug12348973590");
}
double [] mismatch_result;
double [] mismatch_result = null;
boolean need_CM = true;
if (imgdtt_params.ly_poly) {
mismatch_result = corr2d.mismatchPairs( // returns x-xcenter, y, strength (sign same as disparity)
imgdtt_params, // ImageDttParameters imgdtt_params,
......@@ -2233,16 +2234,48 @@ public class ImageDtt {
tile_lma_debug_level,// int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
} else {
// check if got crazy poly, then retry with CM
boolean has_NaN = false;
boolean need_dbg = false;
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
if (Double.isNaN(mismatch_result[3*dir + 0]) || Double.isNaN(mismatch_result[3*dir + 1])) {
has_NaN = true;
} else if ((mismatch_result[3*dir + 2] != 0.0) &&
((Math.abs(mismatch_result[3*dir + 0]) > imgdtt_params.ly_crazy_poly) ||
(Math.abs(mismatch_result[3*dir + 1]) > imgdtt_params.ly_crazy_poly))) {
mismatch_result[3*dir + 2] = 0;
has_NaN = true;
need_dbg = true;
}
}
need_CM = imgdtt_params.ly_poly_backup && has_NaN;
if (need_dbg && (imgdtt_params.lma_debug_level > 0)) {
System.out.println("Failed polynomial mismatch for tileX="+tileX+", tileY="+tileY);
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
System.out.println(String.format("%d: dxy[%d]=%f8.5, dxy[%d]=%f8.5 strength=%7.5f",
dir, (dir*2+0), mismatch_result[dir*3 + 0], (dir*2 + 1), mismatch_result[dir*3 + 1], mismatch_result[dir*3 + 2]));
}
}
}
// TODO: use magic_scale for CM?
if (need_CM) { // if poly was off or gave crazy poly
mismatch_result = corr2d.mismatchPairsCM( // returns x-xcenter, y, strength (sign same as disparity)
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
all_pairs, // int pair_mask, // which pairs to process
-disparity, // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // radius, // positive - within that distance, negative - within 2*(-radius)+1 square
max_corr_radius, // imgdtt_params.ortho_vasw_pwr, // radius, // positive - within that distance, negative - within 2*(-radius)+1 square
tile_lma_debug_level,// int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
if (imgdtt_params.ly_poly && (imgdtt_params.lma_debug_level > 0)) {
System.out.println("Corrected by CM failed polynomial mismatch for tileX="+tileX+", tileY="+tileY);
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
System.out.println(String.format("%d: dxy[%d]=%f8.5, dxy[%d]=%f8.5 strength=%7.5f",
dir, (dir*2+0), mismatch_result[dir*3 + 0], (dir*2 + 1), mismatch_result[dir*3 + 1], mismatch_result[dir*3 + 2]));
}
}
}
if (tile_lma_debug_level > 0) {
System.out.println("Lazy eye mismatch:");
......
......@@ -45,6 +45,8 @@ public class ImageDttParameters {
public int enhortho_width = 2; // reduce weight of center correlation pixels from center (0 - none, 1 - center, 2 +/-1 from center)
public double enhortho_scale = 0.0; // 0.2; // multiply center correlation pixels (inside enhortho_width)
public boolean ly_poly = false; // Use polynomial when measuring mismatch (false - use center of mass)
public double ly_crazy_poly = 1.0; // Maximal allowed mismatch difference calculated as polynomial maximum
public boolean ly_poly_backup = true; // Use CM offset measuremets if poly failed
public boolean fo_correct = true; // correct far objects by comparing orthogonal correlations
public boolean fo_far = false;// Use smallest disparity (false - largest)
......@@ -126,6 +128,9 @@ public class ImageDttParameters {
gd.addNumericField("Multiply center correlation pixels (inside enhortho_width) (1.0 - disables enh_ortho)", this.enhortho_scale, 3);
gd.addCheckbox ("Use polynomial when measuring mismatch (false - use center of mass)", this.ly_poly);
gd.addNumericField("Maximal allowed mismatch difference calculated as polynomial maximum", this.ly_crazy_poly,3,6,"px",
"Use CM method as a back-up, if polynomial maximum is far off");
gd.addCheckbox ("Use CM offset measuremets if poly failed", this.ly_poly_backup);
gd.addMessage("Far objects correction");
gd.addCheckbox ("Try to correct far objects (make them closer) by hor/vert comparison", this.fo_correct);
......@@ -251,6 +256,8 @@ public class ImageDttParameters {
this.enhortho_scale= gd.getNextNumber();
this.ly_poly = gd.getNextBoolean();
this.ly_crazy_poly= gd.getNextNumber();
this.ly_poly_backup = gd.getNextBoolean();
this.fo_correct = gd.getNextBoolean();
this.fo_far = gd.getNextBoolean();
......@@ -336,6 +343,8 @@ public class ImageDttParameters {
properties.setProperty(prefix+"min_corr", this.min_corr +"");
properties.setProperty(prefix+"ly_poly", this.ly_poly +"");
properties.setProperty(prefix+"ly_crazy_poly", this.ly_crazy_poly +"");
properties.setProperty(prefix+"ly_poly_backup", this.ly_poly_backup +"");
properties.setProperty(prefix+"fo_correct", this.fo_correct +"");
properties.setProperty(prefix+"fo_far", this.fo_far +"");
......@@ -411,6 +420,9 @@ public class ImageDttParameters {
if (properties.getProperty(prefix+"fo_correct")!=null) this.fo_correct=Boolean.parseBoolean(properties.getProperty(prefix+"fo_correct"));
if (properties.getProperty(prefix+"ly_poly")!=null) this.ly_poly=Boolean.parseBoolean(properties.getProperty(prefix+"ly_poly"));
if (properties.getProperty(prefix+"ly_crazy_poly")!=null) this.ly_crazy_poly=Double.parseDouble(properties.getProperty(prefix+"ly_crazy_poly"));
if (properties.getProperty(prefix+"ly_poly_backup")!=null) this.ly_poly_backup=Boolean.parseBoolean(properties.getProperty(prefix+"ly_poly_backup"));
if (properties.getProperty(prefix+"fo_far")!=null) this.fo_far=Boolean.parseBoolean(properties.getProperty(prefix+"fo_far"));
if (properties.getProperty(prefix+"fo_min_strength")!=null) this.fo_min_strength=Double.parseDouble(properties.getProperty(prefix+"fo_min_strength"));
if (properties.getProperty(prefix+"fo_min_eff")!=null) this.fo_min_eff=Double.parseDouble(properties.getProperty(prefix+"fo_min_eff"));
......@@ -491,6 +503,8 @@ public class ImageDttParameters {
idp.enhortho_scale = this.enhortho_scale;
idp.ly_poly = this.ly_poly;
idp.ly_crazy_poly = this.ly_crazy_poly;
idp.ly_poly_backup = this.ly_poly_backup;
idp.fo_correct = this.fo_correct;
idp.fo_far = this.fo_far;
......
......@@ -3852,14 +3852,31 @@ public class QuadCLT {
double [][] inf_ds1 = {
disparity_map[ImageDtt.DISPARITY_INDEX_CM],
mismatch_strength, //disparity_map[ ImageDtt.DISPARITY_STRENGTH_INDEX],
clt_mismatch[0], // dx0
clt_mismatch[1], // dy0 +
clt_mismatch[3], // dx1
clt_mismatch[4], // dy1 +
clt_mismatch[6], // dx2 +
clt_mismatch[7], // dy2
clt_mismatch[9], // dx3 +
clt_mismatch[10]};// dy3
clt_mismatch[0].clone(), // dx0
clt_mismatch[1].clone(), // dy0 +
clt_mismatch[3].clone(), // dx1
clt_mismatch[4].clone(), // dy1 +
clt_mismatch[6].clone(), // dx2 +
clt_mismatch[7].clone(), // dy2
clt_mismatch[9].clone(), // dx3 +
clt_mismatch[10].clone()};// dy3
// restore full d{xy}[i] with subtracted disparity - debugging (otherwise clone() above is not neded)
// Add disparity to dx0, dx1, dy2, dy3 pairs
if (clt_parameters.inf_restore_disp) {
if (debugLevel > -2) {
System.out.println("---- Adding disparity to d{xy}[i] ---");
}
for (int nTile = 0; nTile < inf_ds1[0].length; nTile++) if (inf_ds1[1][nTile] > 0){ // strength
for (int i = 0; i < AlignmentCorrection.INDICES_10_DISP.length; i++) {
inf_ds1[AlignmentCorrection.INDICES_10_DISP[i]][nTile] += inf_ds1[AlignmentCorrection.INDEX_10_DISPARITY][nTile];
}
}
} else {
if (debugLevel > -2) {
System.out.println("---- d{xy}[i] have disparity canceled, xy_mismatch will only reflect residualvalues---");
}
}
String [] titles1 = {"disp_cm", "strength", "dx0", "dy0", "dx1", "dy1", "dx2", "dy2", "dx3", "dy3"};
inf_ds = inf_ds1;
titles = titles1;
......@@ -5010,6 +5027,10 @@ public class QuadCLT {
}
AlignmentCorrection ac = new AlignmentCorrection(this);
// includes both infinity correction and mismatch correction for the same infinity tiles
//FIXME: Here disparity now should be restored in dxy...
double [][][] new_corr = ac.infinityCorrection(
clt_parameters.ly_poly, // final boolean use_poly,
clt_parameters.fcorr_inf_strength, // final double min_strenth,
......
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