Commit 6293946d authored by Andrey Filippov's avatar Andrey Filippov

Debugging LY adjustment, tested semi-manual OK

parent 71ac4a10
...@@ -292,6 +292,8 @@ public class CLTParameters { ...@@ -292,6 +292,8 @@ public class CLTParameters {
public double ly_norm_disp = 5.0; // Reduce weight of higher disparity tiles public double ly_norm_disp = 5.0; // Reduce weight of higher disparity tiles
// Lazy eye multi-step fitting // Lazy eye multi-step fitting
public String lym_dbg_path = ""; // read saved extrinsics_bgnd_combo.tif to bypass long preparation
public double lym_overexp = 0.0001; // Any (near) saturated pixels - discard tile (see sat_level also) 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 boolean lym_update_disp = true; // Update target disparity after each step
public int lym_iter = 25; // Maximal number of iterations public int lym_iter = 25; // Maximal number of iterations
...@@ -1188,6 +1190,9 @@ public class CLTParameters { ...@@ -1188,6 +1190,9 @@ public class CLTParameters {
properties.setProperty(prefix+"ly_disp_rvar_gt", this.ly_disp_rvar_gt +""); properties.setProperty(prefix+"ly_disp_rvar_gt", this.ly_disp_rvar_gt +"");
properties.setProperty(prefix+"ly_norm_disp", this.ly_norm_disp +""); properties.setProperty(prefix+"ly_norm_disp", this.ly_norm_disp +"");
properties.setProperty(prefix+"lym_overexp", this.lym_overexp +""); properties.setProperty(prefix+"lym_overexp", this.lym_overexp +"");
properties.setProperty(prefix+"lym_dbg_path", this.lym_dbg_path +"");
properties.setProperty(prefix+"lym_update_disp", this.lym_update_disp+""); properties.setProperty(prefix+"lym_update_disp", this.lym_update_disp+"");
properties.setProperty(prefix+"lym_iter", this.lym_iter+""); properties.setProperty(prefix+"lym_iter", this.lym_iter+"");
properties.setProperty(prefix+"lym_change", this.lym_change +""); properties.setProperty(prefix+"lym_change", this.lym_change +"");
...@@ -1997,6 +2002,7 @@ public class CLTParameters { ...@@ -1997,6 +2002,7 @@ public class CLTParameters {
if (properties.getProperty(prefix+"ly_disp_rvar_gt")!=null) this.ly_disp_rvar_gt=Double.parseDouble(properties.getProperty(prefix+"ly_disp_rvar_gt")); if (properties.getProperty(prefix+"ly_disp_rvar_gt")!=null) this.ly_disp_rvar_gt=Double.parseDouble(properties.getProperty(prefix+"ly_disp_rvar_gt"));
if (properties.getProperty(prefix+"ly_norm_disp")!=null) this.ly_norm_disp=Double.parseDouble(properties.getProperty(prefix+"ly_norm_disp")); if (properties.getProperty(prefix+"ly_norm_disp")!=null) this.ly_norm_disp=Double.parseDouble(properties.getProperty(prefix+"ly_norm_disp"));
if (properties.getProperty(prefix+"lym_overexp")!=null) this.lym_overexp=Double.parseDouble(properties.getProperty(prefix+"lym_overexp")); if (properties.getProperty(prefix+"lym_overexp")!=null) this.lym_overexp=Double.parseDouble(properties.getProperty(prefix+"lym_overexp"));
if (properties.getProperty(prefix+"lym_dbg_path")!=null) this.lym_dbg_path= (String) properties.getProperty(prefix+"lym_dbg_path");
if (properties.getProperty(prefix+"lym_update_disp")!=null) this.lym_update_disp=Boolean.parseBoolean(properties.getProperty(prefix+"lym_update_disp")); if (properties.getProperty(prefix+"lym_update_disp")!=null) this.lym_update_disp=Boolean.parseBoolean(properties.getProperty(prefix+"lym_update_disp"));
if (properties.getProperty(prefix+"lym_iter")!=null) this.lym_iter=Integer.parseInt(properties.getProperty(prefix+"lym_iter")); if (properties.getProperty(prefix+"lym_iter")!=null) this.lym_iter=Integer.parseInt(properties.getProperty(prefix+"lym_iter"));
if (properties.getProperty(prefix+"lym_change")!=null) this.lym_change=Double.parseDouble(properties.getProperty(prefix+"lym_change")); if (properties.getProperty(prefix+"lym_change")!=null) this.lym_change=Double.parseDouble(properties.getProperty(prefix+"lym_change"));
...@@ -2895,6 +2901,8 @@ public class CLTParameters { ...@@ -2895,6 +2901,8 @@ public class CLTParameters {
"Full allowed mismatch is a sum of absolute and disparity times relative (relaxed when ground truth is available)"); "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,8,""); gd.addNumericField("Reduce weight of higher disparity tiles", this.ly_norm_disp, 5,8,"");
gd.addMessage ("--- Lazy eye multi-step fitting ---"); gd.addMessage ("--- Lazy eye multi-step fitting ---");
gd.addStringField ("Read extrinsics_bgnd_combo instead of building it", this.lym_dbg_path, 80);
gd.addNumericField("Any (near) saturated pixels - discard tile (see sat_level also)", this.lym_overexp, 10,12,""); 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.addCheckbox ("Update target disparity after each step", this.lym_update_disp);
gd.addNumericField("Maximal number of iterations", this.lym_iter, 0); gd.addNumericField("Maximal number of iterations", this.lym_iter, 0);
...@@ -3833,6 +3841,8 @@ public class CLTParameters { ...@@ -3833,6 +3841,8 @@ public class CLTParameters {
this.ly_disp_var_gt= gd.getNextNumber(); this.ly_disp_var_gt= gd.getNextNumber();
this.ly_disp_rvar_gt= gd.getNextNumber(); this.ly_disp_rvar_gt= gd.getNextNumber();
this.ly_norm_disp= gd.getNextNumber(); this.ly_norm_disp= gd.getNextNumber();
this.lym_dbg_path = gd.getNextString();
this.lym_overexp= gd.getNextNumber(); this.lym_overexp= gd.getNextNumber();
this.lym_update_disp= gd.getNextBoolean(); this.lym_update_disp= gd.getNextBoolean();
this.lym_iter= (int) gd.getNextNumber(); this.lym_iter= (int) gd.getNextNumber();
......
...@@ -902,8 +902,10 @@ public class CLTPass3d{ ...@@ -902,8 +902,10 @@ public class CLTPass3d{
*/ */
public void updateDisparity() public void updateDisparity()
{ {
if (disparity_map != null) {
setTileOpDisparity(null, getDisparity(0)); setTileOpDisparity(null, getDisparity(0));
} }
}
public double [] getSecondMaxDiff ( public double [] getSecondMaxDiff (
......
...@@ -1101,8 +1101,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1101,8 +1101,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
} }
if (par_list.size() < par_mask.length) { if (par_list.size() < par_mask.length) {
int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8 int[] sub_pars = new int [par_list.size()];
from_sym = from_sym.getMatrix(0, par_mask.length,sub_pars); // remove masked columns for (int i = 0; i < sub_pars.length; i++) {
sub_pars[i] = par_list.get(i);
}
// int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
from_sym = from_sym.getMatrix(0, par_mask.length-1,sub_pars); // remove masked columns
} }
} }
Matrix pcd = new Matrix (port_coord_deriv); // rows: px0,py0,... px[n-1], py[n-1], columns: tar Matrix pcd = new Matrix (port_coord_deriv); // rows: px0,py0,... px[n-1], py[n-1], columns: tar
...@@ -1187,7 +1191,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1187,7 +1191,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
if (par_list.size() < par_mask.length) { if (par_list.size() < par_mask.length) {
int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8 int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
to_sym = to_sym.getMatrix(sub_pars, 0, par_mask.length); // remove masked rows to_sym = to_sym.getMatrix(sub_pars, 0, par_mask.length-1); // remove masked rows
} }
} }
Matrix tar = new Matrix(tar_array,tar_array.length); Matrix tar = new Matrix(tar_array,tar_array.length);
...@@ -1209,7 +1213,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1209,7 +1213,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
if (par_list.size() < par_mask.length) { if (par_list.size() < par_mask.length) {
int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8 int[] sub_pars = par_list.stream().mapToInt(i -> i).toArray(); // Java8
from_sym = from_sym.getMatrix(0, par_mask.length,sub_pars); // remove masked columns from_sym = from_sym.getMatrix(0, par_mask.length-1,sub_pars); // remove masked columns
} }
} }
Matrix sym = new Matrix(sym_array,sym_array.length); Matrix sym = new Matrix(sym_array,sym_array.length);
......
...@@ -102,11 +102,11 @@ public class ExtrinsicAdjustment { ...@@ -102,11 +102,11 @@ public class ExtrinsicAdjustment {
private CorrVector corr_vector = null; private CorrVector corr_vector = null;
private boolean [] par_mask = null; private boolean [] par_mask = null;
private boolean use_rig_offsets = false; private boolean use_rig_offsets = false;
private double [][] measured_dsxy = null; private double [][] measured_dsxy = null; // only set in solveCorr()
// private double [][] dy_ddisparity = null; // conveniently extracted from dsdn // private double [][] dy_ddisparity = null; // conveniently extracted from dsdn
private double [][] pY_offset = null; // conveniently extracted from dsdn - per-sensor average pY to calculate ERS offset private double [][] pY_offset = null; // conveniently extracted from dsdn - per-sensor average pY to calculate ERS offset (set in getXYNondistorted())
private double [][] x0y0 = null; // private double [][] x0y0 = null; //
private double[][] world_xyz = null; private double[][] world_xyz = null; // only set in solveCorr()
private double [] weight_window = null; // center area is more reliable private double [] weight_window = null; // center area is more reliable
public final GeometryCorrection geometryCorrection; public final GeometryCorrection geometryCorrection;
...@@ -296,7 +296,13 @@ public class ExtrinsicAdjustment { ...@@ -296,7 +296,13 @@ public class ExtrinsicAdjustment {
*/ */
private void showX0Y0(double [][] xy0, String title) { private void showX0Y0(double [][] xy0, String title) {
String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"}; int num_sensors = this.geometryCorrection.getNumSensors();
// String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"};
String [] titles = new String [ 2 * num_sensors];
for (int i = 0; i < num_sensors; i++) {
titles[2*i + 0] = "xnd-"+i;
titles[2*i + 1] = "ynd-"+i;
}
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double [][] pixels = new double [titles.length][clusters]; double [][] pixels = new double [titles.length][clusters];
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
...@@ -392,7 +398,7 @@ public class ExtrinsicAdjustment { ...@@ -392,7 +398,7 @@ public class ExtrinsicAdjustment {
world_xyz = getWorldXYZ(); // freeze world coordinates for measured pX,pY and disparity world_xyz = getWorldXYZ(); // freeze world coordinates for measured pX,pY and disparity
// calculate x,y non-distorted offsets for current correction vectors (to subtract from the new (modified) values // calculate x,y non-distorted offsets for current correction vectors (to subtract from the new (modified) values
// 0/0 in the center (in the optical center), in pixels // 0/0 in the center (in the optical center), in pixels
x0y0 = getXYNondistorted( x0y0 = getXYNondistorted( // Sets pY_offset[], needed for .getPortsDDNDDerivativesNew() (in getJacobianTransposed)
corr_vector, corr_vector,
true); // boolean set_dydisp) true); // boolean set_dydisp)
...@@ -418,7 +424,7 @@ public class ExtrinsicAdjustment { ...@@ -418,7 +424,7 @@ public class ExtrinsicAdjustment {
double [] dfe = null; double [] dfe = null;
if (max_dfe > 0) { if (max_dfe > 0) {
dfe = distanceFromEdge ( dfe = distanceFromEdge ( // all 0? too little, non-continuous
force_disparity, force_disparity,
measured_dsxy, measured_dsxy,
min_dfe, // 1.75 min_dfe, // 1.75
...@@ -434,7 +440,7 @@ public class ExtrinsicAdjustment { ...@@ -434,7 +440,7 @@ public class ExtrinsicAdjustment {
force_disparity, // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled force_disparity, // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
dfe, // double [] distance_from_edge, 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) 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, min_num_forced/4, // int min_in_half,
inf_min_disp_abs, // double min_infinity_abs, inf_min_disp_abs, // double min_infinity_abs,
inf_max_disp_abs, // double max_infinity_abs, inf_max_disp_abs, // double max_infinity_abs,
inf_min_disparity, // double min_infinity, inf_min_disparity, // double min_infinity,
...@@ -467,7 +473,7 @@ public class ExtrinsicAdjustment { ...@@ -467,7 +473,7 @@ public class ExtrinsicAdjustment {
max_disparity_far, // double max_disparity_far) // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity) max_disparity_far, // double max_disparity_far) // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
max_disparity_use); max_disparity_use);
if (use_disparity) { if (use_disparity) {
if (inf_stat[0] < min_num_forced) { if (inf_stat[0] < min_num_forced) { // 4
System.out.println("Too few infinity tiles ("+inf_stat[0]+"<"+(min_num_forced)+") to adjust disparity"); System.out.println("Too few infinity tiles ("+inf_stat[0]+"<"+(min_num_forced)+") to adjust disparity");
// disable parameters...all extrinsic // disable parameters...all extrinsic
use_disparity = false; use_disparity = false;
...@@ -475,7 +481,7 @@ public class ExtrinsicAdjustment { ...@@ -475,7 +481,7 @@ public class ExtrinsicAdjustment {
use_diff_rolls = false; use_diff_rolls = false;
common_roll = false; common_roll = false;
corr_focalLength = false; corr_focalLength = false;
} else if (inf_stat[1] < min_num_forced/2) { } else if ((inf_stat[1] < min_num_forced/4) || (inf_stat[1] < 1)) {
System.out.println("Too few infinity tiles ("+inf_stat[1]+"<"+(min_num_forced/2)+") in one half to balance right/left"); 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) // disable parameters: all extrinsic but disparity (S0)
use_aztilts = false; use_aztilts = false;
...@@ -821,7 +827,7 @@ public class ExtrinsicAdjustment { ...@@ -821,7 +827,7 @@ public class ExtrinsicAdjustment {
private boolean [] filterInfinity( private boolean [] filterInfinity(
double [][] measured_dsxy, double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
double [] distance_from_edge, double [] distance_from_edge, // all 0?
boolean en_tilt, // consider right/left infinity tilt (will be disabled if more than *abs difference over width) boolean en_tilt, // consider right/left infinity tilt (will be disabled if more than *abs difference over width)
int min_in_half, int min_in_half,
double min_infinity_abs, double min_infinity_abs,
...@@ -829,6 +835,9 @@ public class ExtrinsicAdjustment { ...@@ -829,6 +835,9 @@ public class ExtrinsicAdjustment {
double min_infinity, double min_infinity,
double max_infinity double max_infinity
) { ) {
if (min_in_half < 1) {
min_in_half = 1;
}
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double half_width = 0.5 * clustersX; double half_width = 0.5 * clustersX;
boolean [] true_infinity = new boolean[clusters]; boolean [] true_infinity = new boolean[clusters];
...@@ -1584,17 +1593,18 @@ public class ExtrinsicAdjustment { ...@@ -1584,17 +1593,18 @@ public class ExtrinsicAdjustment {
} }
if (graphic) { if (graphic) {
String [] titles3 = new String[num_pars * 3]; String [] titles3 = new String[num_pars * 3];
int rows = getRowsCols()[0];
int width = 3 * clustersX + 2 * gap; int cols = getRowsCols()[1];
int height = 3 * clustersY + 2 * gap; int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] dbg_img = new double [num_pars * 3][width*height]; double [][] dbg_img = new double [num_pars * 3][width*height];
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
titles3[3 * par + 0] = titles[par]+""; titles3[3 * par + 0] = titles[par]+"";
titles3[3 * par + 1] = titles[par]+"_delta"; titles3[3 * par + 1] = titles[par]+"_delta";
titles3[3 * par + 2] = titles[par]+"_diff"; titles3[3 * par + 2] = titles[par]+"_diff";
for (int mode = 0; mode < points_sample; mode++) { for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap); int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX); int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
...@@ -1624,8 +1634,8 @@ public class ExtrinsicAdjustment { ...@@ -1624,8 +1634,8 @@ public class ExtrinsicAdjustment {
dbg_img = new double [num_pars][width*height]; dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < points_sample; mode++) { for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap); int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX); int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
...@@ -1659,12 +1669,14 @@ public class ExtrinsicAdjustment { ...@@ -1659,12 +1669,14 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix int gap = 10; //pix
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
String [] titles = {"Y", "-fX", "Y+fx"}; String [] titles = {"Y", "-fX", "Y+fx"};
int width = 3 * clustersX + 2 * gap; int rows = getRowsCols()[0];
int height = 3 * clustersY + 2 * gap; int cols = getRowsCols()[1];
int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] dbg_img = new double [3][width*height]; double [][] dbg_img = new double [3][width*height];
for (int mode = 0; mode < points_sample; mode++) { for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap); int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX); int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
...@@ -1998,6 +2010,14 @@ public class ExtrinsicAdjustment { ...@@ -1998,6 +2010,14 @@ public class ExtrinsicAdjustment {
public int [] getRowsCols() {
int rows = (int) Math.round(Math.sqrt(points_sample));
int cols = points_sample/rows;
if (rows*cols < points_sample) {
cols++;
}
return new int [] {rows,cols};
}
private void dbgYminusFxWeight( private void dbgYminusFxWeight(
double [] fx, double [] fx,
...@@ -2009,12 +2029,14 @@ public class ExtrinsicAdjustment { ...@@ -2009,12 +2029,14 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix int gap = 10; //pix
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"}; String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"};
int width = 3 * clustersX + 2 * gap; int rows = getRowsCols()[0];
int height = 3 * clustersY + 2 * gap; int cols = getRowsCols()[1];
int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] dbg_img = new double [titles.length][width*height]; double [][] dbg_img = new double [titles.length][width*height];
for (int mode = 0; mode < points_sample; mode++) { for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap); int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX); int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
...@@ -2053,7 +2075,12 @@ public class ExtrinsicAdjustment { ...@@ -2053,7 +2075,12 @@ public class ExtrinsicAdjustment {
} }
} }
} else { } else {
dbg_img[0][pix] = Double.NaN; if (pix >= dbg_img[0].length) {
System.out.println("pix="+pix+" >= "+dbg_img[0].length);
System.out.println("pix="+pix+" >= "+dbg_img[0].length);
System.out.println("pix="+pix+" >= "+dbg_img[0].length);
}
dbg_img[0][pix] = Double.NaN; //Index 16240 out of bounds for length 16240
dbg_img[1][pix] = Double.NaN; dbg_img[1][pix] = Double.NaN;
dbg_img[2][pix] = Double.NaN; dbg_img[2][pix] = Double.NaN;
dbg_img[3][pix] = Double.NaN; dbg_img[3][pix] = Double.NaN;
...@@ -2078,15 +2105,17 @@ public class ExtrinsicAdjustment { ...@@ -2078,15 +2105,17 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix int gap = 10; //pix
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
String [] titles = {"meas", "correction", "diff"}; String [] titles = {"meas", "correction", "diff"};
int width = 3 * clustersX + 2 * gap; int rows = getRowsCols()[0];
int height = 3 * clustersY + 2 * gap; int cols = getRowsCols()[1];
int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] xy = getXYNondistorted(corr_vector, false); double [][] xy = getXYNondistorted(corr_vector, false);
double [][] dbg_img = new double [3][width*height]; double [][] dbg_img = new double [3][width*height];
double [][] moving_objects = new double [3][clusters]; double [][] moving_objects = new double [3][clusters];
for (int mode = 0; mode < 2 * num_sensors + 1; mode++) { for (int mode = 0; mode < 2 * num_sensors + 1; mode++) {
int x0 = (mode % 3) * (clustersX + gap); int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX); int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
...@@ -2142,6 +2171,7 @@ public class ExtrinsicAdjustment { ...@@ -2142,6 +2171,7 @@ public class ExtrinsicAdjustment {
// Mismatch mm = mismatch_list.get(indx); // Mismatch mm = mismatch_list.get(indx);
// double [] pXY = mm.getPXY(); // double [] pXY = mm.getPXY();
// will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters // will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters
// Now 2*num_sensors+1
double [][] deriv = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir double [][] deriv = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main, geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets, use_rig_offsets, // boolean use_rig_offsets,
...@@ -2156,7 +2186,7 @@ public class ExtrinsicAdjustment { ...@@ -2156,7 +2186,7 @@ public class ExtrinsicAdjustment {
/// int dbg_index = cluster; // dbg_index (pXY, dbg_decimate); /// int dbg_index = cluster; // dbg_index (pXY, dbg_decimate);
// convert to symmetrical coordinates // convert to symmetrical coordinates
// derivatives of each port coordinates (in pixels) for each of selected symmetric all parameters (sym0 is convergence for disparity) // derivatives of each port coordinates (in pixels) for each of selected symmetric all parameters (sym0 is convergence for disparity)
double [][] jt_partial = corr_vector.getJtPartial( double [][] jt_partial = corr_vector.getJtPartial( // extract common?
deriv, // double [][] port_coord_deriv, deriv, // double [][] port_coord_deriv,
par_mask); // boolean [] par_mask par_mask); // boolean [] par_mask
...@@ -2286,7 +2316,7 @@ public class ExtrinsicAdjustment { ...@@ -2286,7 +2316,7 @@ public class ExtrinsicAdjustment {
if (this.last_rms == null) { //first time, need to calculate all (vector is valid) if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points]; this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector); this.last_ymfx = getFx(corr_vector);
if (debug_level > -1) { // temporary if (debug_level > -2) { // temporary
dbgYminusFxWeight( dbgYminusFxWeight(
this.last_ymfx, this.last_ymfx,
this.weights, this.weights,
...@@ -2300,8 +2330,14 @@ public class ExtrinsicAdjustment { ...@@ -2300,8 +2330,14 @@ public class ExtrinsicAdjustment {
this.last_ymfx); // modifies this.last_ymfx (weights and subtracts fx) this.last_ymfx); // modifies this.last_ymfx (weights and subtracts fx)
this.initial_rms = this.last_rms.clone(); this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone(); this.good_or_bad_rms = this.last_rms.clone();
if (debug_level > -1) {
showDerivatives(0);
showDerivatives(1);
showDerivatives(2);
showDerivatives(3);
}
// TODO: Restore/implement // TODO: Restore/implement
if (debug_level > 3) { if (debug_level > 3) { // compares true jacobians and calculated with delta (same for all parameters)
dbgJacobians( dbgJacobians(
corr_vector, // CorrVector corr_vector, corr_vector, // CorrVector corr_vector,
1E-5, // double delta, 1E-5, // double delta,
...@@ -2409,4 +2445,101 @@ public class ExtrinsicAdjustment { ...@@ -2409,4 +2445,101 @@ public class ExtrinsicAdjustment {
} }
return rslt; return rslt;
} }
public void showDerivatives(int typ4) {
// typ == 0 -> DDND, 1 - XT
int typ = typ4 % 2;
boolean use_sym = typ4 > 1;
int gap = 10;
int clusters = clustersX * clustersY;
// int num_pars = getNumPars(); // only used
int num_pars = corr_vector.toArray().length; // all parameters
String [] titles = new String [num_pars]; //ea.getSymNames();
for (int i = 0; i < num_pars; i++) {
titles[i] = "S"+i;
}
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
Matrix from_sym = corr_vector.symmVectorsSet.from_sym;
double [] imu = corr_vector.getIMU(); //
int rows = getRowsCols()[0];
int cols = getRowsCols()[1];
int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] dbg_img = new double [num_pars][width*height];
// double [][][] derivs = new double [points_sample][][];
double [][][] derivs = new double [clusters][][];
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
// Mismatch mm = mismatch_list.get(indx);
// double [] pXY = mm.getPXY();
// will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters
// Now 2*num_sensors+1
if (typ == 0) {
derivs[cluster] = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir
this.geometryCorrection, // GeometryCorrection gc_main,
this.use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
this.pY_offset[cluster], // double [] py_offset, // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
imu, // double [] imu,
this.world_xyz[cluster], // double [] xyz, // world XYZ for ERS correction
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
} else {
derivs[cluster] = geometryCorrection.getPortsXYDerivativesNew( // USED in lwir
this.geometryCorrection, // GeometryCorrection gc_main,
this.use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
this.pY_offset[cluster], // double [] py_offset, // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
imu, // double [] imu,
this.world_xyz[cluster], // double [] xyz, // world XYZ for ERS correction
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
this.measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
}
// optionally convert to use sym vectors
if (use_sym) {
Matrix derivs_tarz = new Matrix(derivs[cluster]);
Matrix derivs_sym = derivs_tarz.times(from_sym);
derivs[cluster] = derivs_sym.getArray();
}
}
for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % cols) * (clustersX + gap);
int y0 = (mode / cols) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX);
int pix = x + y * width;
// int indx = (mode == 0) ? INDX_DIFF : (indx_dd0 + mode - 1);
if (typ == 0) {
double d = (derivs[cluster] == null)? Double.NaN : derivs[cluster][mode][par];
if (mode == 0) {
dbg_img[par][pix] = -d;
} else {
dbg_img[par][pix] = d;
}
} else {
if (mode == 0) {
dbg_img[par][pix] = Double.NaN; // skip
} else {
dbg_img[par][pix] = (derivs[cluster] == null)? Double.NaN : derivs[cluster][mode-1][par];
}
}
}
}
}
String title = ((typ == 0)?"dDND_dpar":"dXY_dpar") + (use_sym? "_sym":"_tarz");
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
width,
height,
true,
title, // +(update_disparity?"U":""),
(use_sym? titles: corr_vector.getCorrNames())); // titles);
}
} }
...@@ -2806,6 +2806,35 @@ public class GeometryCorrection { ...@@ -2806,6 +2806,35 @@ public class GeometryCorrection {
return jt; return jt;
} }
public double [][] getPortsXYDerivativesNew( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [] py_offset, // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
double [] imu, // may be null
double [] xyz, // world XYZ for ERS correction
double px,
double py,
double disparity)
{
// make sure initPrePostMatrices(true) already ran (in constructor). (true) means that minus sign is already incorporated
double [][] pXYNDderiv = new double [2*numSensors][]; // CorrVector.LENGTH][];
getPortsNonDistortedCoordinatesAndDerivativesNew( // USED in lwir
gc_main,
use_rig_offsets,
rots,
deriv_rots,
pXYNDderiv, // if not null, should be double[8][]
py_offset, // // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
imu,
xyz,
px,
py,
disparity);
return(new Matrix(pXYNDderiv)).getArray();
}
......
...@@ -4042,6 +4042,28 @@ public class ImageDttCPU { ...@@ -4042,6 +4042,28 @@ public class ImageDttCPU {
disparity_map[DISPARITY_INDEX_POLY+1][tIndex] = ds[0][1]; disparity_map[DISPARITY_INDEX_POLY+1][tIndex] = ds[0][1];
if (debugTile0) { if (debugTile0) {
lma2.printStats(ds,1); lma2.printStats(ds,1);
double [][] ddnd = lma2.getDdNd();
if (ddnd != null) {
double [][] dxy= new double [ddnd.length][2];
for (int i = 0; i < dxy.length; i++) {
dxy[i][0] = ddnd[i][0] * rXY[i][0] - ddnd[i][1] * rXY[i][1];
dxy[i][1] = ddnd[i][0] * rXY[i][1] + ddnd[i][1] * rXY[i][0];
}
System.out.print(" Port: ");
for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %2d ", i)); System.out.println();
System.out.print("Radial_in = [");
for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", ddnd[i][0])); System.out.println("]");
System.out.print("Tangent_CW = [");
for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", ddnd[i][1])); System.out.println("]");
System.out.print("X = [");
for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", dxy[i][0])); System.out.println("]");
System.out.print("Y = [");
for (int i = 0; i < dxy.length; i++) System.out.print(String.format(" %6.3f,", dxy[i][1])); System.out.println("]");
System.out.println();
}
} }
} }
} }
......
...@@ -6028,9 +6028,9 @@ public class QuadCLTCPU { ...@@ -6028,9 +6028,9 @@ public class QuadCLTCPU {
float [][] rgba = new float [num_slices][]; float [][] rgba = new float [num_slices][];
for (int i = 0; i < 3; i++) rgba[i] = new float [iclt_data[green_index].length]; for (int i = 0; i < 3; i++) rgba[i] = new float [iclt_data[green_index].length];
for (int i = 0; i < rbg_in[green_index].length; i++) { for (int i = 0; i < rbg_in[green_index].length; i++) {
if (i == 700) { // if (i == 700) {
System.out.println("linearStackToColor(): i="+i); // System.out.println("linearStackToColor(): i="+i);
} // }
float [] rgb = tc.getRGB(iclt_data[green_index][i]); float [] rgb = tc.getRGB(iclt_data[green_index][i]);
rgba[0][i] = rgb[0]; // red rgba[0][i] = rgb[0]; // red
rgba[1][i] = rgb[1]; // green rgba[1][i] = rgb[1]; // green
...@@ -6147,9 +6147,9 @@ public class QuadCLTCPU { ...@@ -6147,9 +6147,9 @@ public class QuadCLTCPU {
double [][] rgba = new double [num_slices][]; double [][] rgba = new double [num_slices][];
for (int i = 0; i < 3; i++) rgba[i] = new double [iclt_data[green_index].length]; for (int i = 0; i < 3; i++) rgba[i] = new double [iclt_data[green_index].length];
for (int i = 0; i < rbg_in[green_index].length; i++) { for (int i = 0; i < rbg_in[green_index].length; i++) {
if (i == 700) { // if (i == 700) {
System.out.println("linearStackToColor(): i="+i); // System.out.println("linearStackToColor(): i="+i);
} // }
double [] rgb = tc.getRGB(iclt_data[green_index][i]); double [] rgb = tc.getRGB(iclt_data[green_index][i]);
rgba[0][i] = rgb[0]; // red rgba[0][i] = rgb[0]; // red
rgba[1][i] = rgb[1]; // green rgba[1][i] = rgb[1]; // green
...@@ -8158,8 +8158,78 @@ public class QuadCLTCPU { ...@@ -8158,8 +8158,78 @@ public class QuadCLTCPU {
double inf_max, // = 1.0; double inf_max, // = 1.0;
final int threadsMax, // maximal number of threads to launch final int threadsMax, // maximal number of threads to launch
final boolean updateStatus, final boolean updateStatus,
final int debugLevel) {
return extrinsicsCLT(
clt_parameters,
null, // String dbg_path, // if not null - read extrinsics_bgnd_combo file instead of extrinsics_prepare
adjust_poly,
inf_min, // = -1.0;
inf_max, // = 1.0;
threadsMax, // maximal number of threads to launch
updateStatus,
debugLevel);
}
public boolean getPreparedExtrinsics(String path) {
String [] titles = {"bgnd_disp","bgnd_str","combo_disp","combo_str","bg_sel","bg_use","combo_use"};
int exp_slices = titles.length;
if (path == null) {return false;}
ImagePlus img_extrinsics_bgnd_combo = new ImagePlus(path);
ImageStack stack_extrinsics_bgnd_combo = img_extrinsics_bgnd_combo.getStack();
int nSlices=stack_extrinsics_bgnd_combo.getSize();
int width = img_extrinsics_bgnd_combo.getWidth();
int height = img_extrinsics_bgnd_combo.getHeight();
if (nSlices != exp_slices) {
throw new IllegalArgumentException ("getPreparedExtrinsics(): Expected "+exp_slices+" in "+path+", got "+nSlices);
}
double [][] data = new double [nSlices][width*height];
for (int slice = 0; slice < nSlices; slice ++) {
float [] pixels = (float []) stack_extrinsics_bgnd_combo.getPixels(slice+1);
for (int i = 0; i < pixels.length; i++) {
data[slice][i] = pixels[i];
}
}
boolean [] bg_sel = new boolean [width*height];
boolean [] bg_use = new boolean [width*height];
boolean [] combo_use = new boolean [width*height];
for (int i = 0; i < bg_sel.length; i++) {
bg_sel[i] = data[4][i] > 0; // NaN OK
bg_use[i] = data[5][i] > 0; // NaN OK
combo_use[i] = data[6][i] > 0; // NaN OK
}
CLTPass3d bg_scan = new CLTPass3d(tp, 0 );
CLTPass3d combo_scan = new CLTPass3d(tp, 0 );
int op = ImageDtt.setImgMask(0, 0xf);
op = ImageDtt.setPairMask(op,0xf);
op = ImageDtt.setForcedDisparity(op,true);
for (int ty = 0; ty < height; ty++) {
for (int tx = 0; tx < width; tx++) {
int indx = ty*width+tx;
bg_scan.tile_op[ty][tx] = bg_use[indx]? op: 0;
combo_scan.tile_op[ty][tx] = combo_use[indx]? op: 0;
// bg_scan.disparity[ty][tx] = bg_use[indx]? data[0][indx]: Double.NaN;
bg_scan.disparity[ty][tx] = bg_use[indx]? 0.0: Double.NaN;
combo_scan.disparity[ty][tx] = combo_use[indx]? data[2][indx]: Double.NaN;
}
}
tp.clt_3d_passes.add(bg_scan);
tp.clt_3d_passes.add(combo_scan);
return true;
}
public boolean extrinsicsCLT(
CLTParameters clt_parameters,
String dbg_path, // if not null - read extrinsics_bgnd_combo file instead of extrinsics_prepare
boolean adjust_poly,
double inf_min, // = -1.0;
double inf_max, // = 1.0;
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel) final int debugLevel)
{ {
boolean got_saved = getPreparedExtrinsics(dbg_path);
if (!got_saved) {
extrinsics_prepare( extrinsics_prepare(
clt_parameters, clt_parameters,
inf_min, // = -1.0; inf_min, // = -1.0;
...@@ -8167,9 +8237,9 @@ public class QuadCLTCPU { ...@@ -8167,9 +8237,9 @@ public class QuadCLTCPU {
threadsMax, // maximal number of threads to launch threadsMax, // maximal number of threads to launch
updateStatus, updateStatus,
debugLevel); debugLevel);
}
final boolean batch_mode = false; // clt_parameters.batch_run;
final boolean batch_mode = clt_parameters.batch_run;
int debugLevelInner = batch_mode ? -5: debugLevel; int debugLevelInner = batch_mode ? -5: debugLevel;
boolean update_disp_from_latest = clt_parameters.lym_update_disp ; // true; boolean update_disp_from_latest = clt_parameters.lym_update_disp ; // true;
int max_tries = clt_parameters.lym_iter; // 25; int max_tries = clt_parameters.lym_iter; // 25;
...@@ -8254,7 +8324,7 @@ public class QuadCLTCPU { ...@@ -8254,7 +8324,7 @@ public class QuadCLTCPU {
false, // boolean update_disparity, // re-measure disparity before measuring LY false, // boolean update_disparity, // re-measure disparity before measuring LY
threadsMax, // final int threadsMax, // maximal number of threads to launch threadsMax, // final int threadsMax, // maximal number of threads to launch
updateStatus, //final boolean updateStatus, updateStatus, //final boolean updateStatus,
1E-3, // double delta, 1E-2, // 1E-3, // double delta,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
} }
...@@ -12465,31 +12535,62 @@ public class QuadCLTCPU { ...@@ -12465,31 +12535,62 @@ public class QuadCLTCPU {
double delta, double delta,
final int debugLevel) final int debugLevel)
{ {
double [] parameter_scales = { // multiply delay for each parameter delta = 0.0003;
0.3, // 0.014793657667505566, // 00 10 /*double [] parameter_scales4 = { // multiply delay for each parameter
0.3, // 0.015484017460841183, // 01 10 0.3, // 0.014793657667505566, // 00 10 tilt0
0.3, // 0.02546712771769517, // 02 10 0.3, // 0.015484017460841183, // 01 10 tilt1
0.3, // 0.02071573747995167, // 03 10 0.3, // 0.02546712771769517, // 02 10 tilt2
0.3, // 0.026584237444512468, // 04 10
0.3, // 0.014168012698804967, // 05 10 0.3, // 0.02071573747995167, // 03 10 az0
2.0, // 1.8554483718240792E-4, // 06 0.3, // 0.026584237444512468, // 04 10 az1
0.3, //2.3170738149889717E-4, // 07 0.3, // 0.014168012698804967, // 05 10 az2
0.3, //3.713239026512266E-4, // 08
0.3, //2.544834643007531E-4, // 09 2.0, // 1.8554483718240792E-4,// 06 roll0
0.3, // 2.5535557646736286E-4, // 10 0.3, //2.3170738149889717E-4, // 07 roll1
0.3, // 1.98531249109261E-4, // 11 0.3, //3.713239026512266E-4, // 08 roll2
0.3, // 2.1802727086879284E-4, // 12 0.3, //2.544834643007531E-4, // 09 roll3
150, // 8.814346720176489E-1, // 5, // 13 10000x 0.3, // 2.5535557646736286E-4, // 10 zoom0
150, // 7.071297501674136E-1, // 5, // 14 10000x 0.3, // 1.98531249109261E-4, // 11 zoom1
150, // 1.306306793587865E-0, // 4, // 15 10000x 0.3, // 2.1802727086879284E-4, // 12 zoom2
300, // 2.8929916645453735E-0, // 4, // 16 10000x
300, // 2.943408022525927E-0, // 4, // 17 10000x 150, // 8.814346720176489E-1, // 5, // 13 10000x omega-tilt
500.0}; // 390.6185365641268}; //4}; // 18 100000x 150, // 7.071297501674136E-1, // 5, // 14 10000x omega az
150, // 1.306306793587865E-0, // 4, // 15 10000x omega roll
300, // 2.8929916645453735E-0, // 4, // 16 10000x vx
300, // 2.943408022525927E-0, // 4, // 17 10000x vy
500.0}; // 390.6185365641268}; //4}; // 18 100000x vz
*/
double scale_tl = 0.3;
double scale_az = 0.3;
double scale_rl0 = 2.0;
double scale_rl = 0.3;
double scale_zoom = 0.3;
double [] scales_imu = {
150, // 8.814346720176489E-1, // 5, // 13 10000x omega-tilt
150, // 7.071297501674136E-1, // 5, // 14 10000x omega az
150, // 1.306306793587865E-0, // 4, // 15 10000x omega roll
300, // 2.8929916645453735E-0, // 4, // 16 10000x vx
300, // 2.943408022525927E-0, // 4, // 17 10000x vy
500.0}; // 390.6185365641268}; //4}; // 18 100000x vz
// delta = 0.001; // should be 0.001 // delta = 0.001; // should be 0.001
boolean debug_img = false; boolean debug_img = false;
int debugLevelInner = -5; int debugLevelInner = -5;
CLTPass3d scan = tp.clt_3d_passes.get(scanIndex); CLTPass3d scan = tp.clt_3d_passes.get(scanIndex);
CorrVector corr_vector = geometryCorrection.getCorrVector().clone(); CorrVector corr_vector = geometryCorrection.getCorrVector().clone();
// String [] corr_names = corr_vector.getCorrNames();
int num_sensors=getNumSensors();
double [] parameter_scales = new double [corr_vector.getLength()];
for (int i = 0; i < num_sensors; i++) {
parameter_scales [corr_vector.getRollIndex()+ i] = (i > 0) ? scale_rl : scale_rl0;
if (i < num_sensors - 1) {
parameter_scales[corr_vector.getTiltIndex()+ i]=scale_tl;
parameter_scales[corr_vector.getAzimuthIndex()+i]=scale_az;
parameter_scales[corr_vector.getZoomIndex()+ i]=scale_zoom;
}
}
for (int i = 0; i < scales_imu.length; i++) {
parameter_scales[corr_vector.getIMUIndex()+ i] = scales_imu[i];
}
double [] curr_corr_arr = corr_vector.toArray(); double [] curr_corr_arr = corr_vector.toArray();
int clusters = ea.clustersX * ea.clustersY; int clusters = ea.clustersX * ea.clustersY;
int num_ly = ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); // scan.getLazyEyeData().length; int num_ly = ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); // scan.getLazyEyeData().length;
...@@ -12514,6 +12615,10 @@ public class QuadCLTCPU { ...@@ -12514,6 +12615,10 @@ public class QuadCLTCPU {
ly_initial, // double[][] data, ly_initial, // double[][] data,
"drv_reference");// String title); "drv_reference");// String title);
} }
String [] titles = corr_vector.getCorrNames(); // new String [num_pars]; //ea.getSymNames(); // why "S" here, while it is tarz???
// for (int i = 0; i < num_pars; i++) {
// titles[i] = "S"+i;
// }
for (int npar = 0; npar < num_pars; npar++) { for (int npar = 0; npar < num_pars; npar++) {
// perform asymmetric delta // perform asymmetric delta
...@@ -12524,7 +12629,8 @@ public class QuadCLTCPU { ...@@ -12524,7 +12629,8 @@ public class QuadCLTCPU {
corr_vectorp.incrementVector(corr_delta, 1.0); // 0.5 for p/m corr_vectorp.incrementVector(corr_delta, 1.0); // 0.5 for p/m
geometryCorrection.setCorrVector(corr_vectorp) ; geometryCorrection.setCorrVector(corr_vectorp) ;
double rdelta = 1.0/ par_inc[npar]; double rdelta = 1.0/ par_inc[npar];
System.out.println("S"+npar+" scale="+rdelta+"\n"+(geometryCorrection.getCorrVector().toString())); // System.out.println("S"+npar+" scale="+rdelta); // +"\n"+(geometryCorrection.getCorrVector().toString()));
System.out.println(npar+": "+ titles[npar]+", scale="+rdelta); // +"\n"+(geometryCorrection.getCorrVector().toString()));
gpuResetCorrVector(); gpuResetCorrVector();
if (update_disparity) { if (update_disparity) {
CLTMeasureCorr( // perform single pass according to prepared tiles operations and disparity CLTMeasureCorr( // perform single pass according to prepared tiles operations and disparity
...@@ -12585,12 +12691,12 @@ public class QuadCLTCPU { ...@@ -12585,12 +12691,12 @@ public class QuadCLTCPU {
} }
*/ */
int gap = 10; int gap = 10;
String [] titles = new String [num_pars]; //ea.getSymNames(); // int width = 3 * ea.clustersX + 2 * gap;
for (int i = 0; i < num_pars; i++) { // int height = 3 * ea.clustersY + 2 * gap;
titles[i] = "S"+i; int rows = ea.getRowsCols()[0];
} int cols = ea.getRowsCols()[1];
int width = 3 * ea.clustersX + 2 * gap; int width = cols * (ea.clustersX + gap) - gap;
int height = 3 * ea.clustersY + 2 * gap; int height = rows * (ea.clustersY + gap) - gap;
double [][] dbg_img = new double [num_pars][width*height]; double [][] dbg_img = new double [num_pars][width*height];
/* /*
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
...@@ -12617,8 +12723,8 @@ public class QuadCLTCPU { ...@@ -12617,8 +12723,8 @@ public class QuadCLTCPU {
dbg_img = new double [num_pars][width*height]; dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) { for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap); int x0 = (mode % cols) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap); int y0 = (mode / cols) * (ea.clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % ea.clustersX); int x = x0 + (cluster % ea.clustersX);
int y = y0 + (cluster / ea.clustersX); int y = y0 + (cluster / ea.clustersX);
...@@ -12643,8 +12749,8 @@ public class QuadCLTCPU { ...@@ -12643,8 +12749,8 @@ public class QuadCLTCPU {
dbg_img = new double [num_pars][width*height]; dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) { for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap); int x0 = (mode % cols) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap); int y0 = (mode / cols) * (ea.clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % ea.clustersX); int x = x0 + (cluster % ea.clustersX);
int y = y0 + (cluster / ea.clustersX); int y = y0 + (cluster / ea.clustersX);
......
...@@ -103,7 +103,7 @@ public class SymmVector { ...@@ -103,7 +103,7 @@ public class SymmVector {
full_type2, full_type2,
debug_level); debug_level);
rvs.xy = sv.exportXY(); rvs.xy = sv.exportXY();
rvs.rt = sv.exportRT(); rvs.rt = sv.exportRT(); // radius/tangential (out, cw, out, cw
rvs.dir_rt = sv.exportDirRT(); rvs.dir_rt = sv.exportDirRT();
rvs.rots = sv.exportRZ(false); // include common roll rvs.rots = sv.exportRZ(false); // include common roll
rvs.zooms = sv.exportRZ(true); // no common zoom rvs.zooms = sv.exportRZ(true); // no common zoom
...@@ -1153,7 +1153,8 @@ public class SymmVector { ...@@ -1153,7 +1153,8 @@ public class SymmVector {
xy2ta.set( i, 2* i + 1, TILT_SIGN); xy2ta.set( i, 2* i + 1, TILT_SIGN);
xy2ta.set(N-1+i, 2 *i + 0, AZIMUTH_SIGN); xy2ta.set(N-1+i, 2 *i + 0, AZIMUTH_SIGN);
} }
return xy2ta.times(sym2xy); Matrix sym2ta = xy2ta.times(sym2xy);
return sym2ta;
} }
/** /**
...@@ -1195,6 +1196,7 @@ public class SymmVector { ...@@ -1195,6 +1196,7 @@ public class SymmVector {
to_sym.setMatrix(i1,i2-1,i1,i2-1, sym2roll.inverse()); to_sym.setMatrix(i1,i2-1,i1,i2-1, sym2roll.inverse());
to_sym.setMatrix(i2,i3-1,i2,i3-1, sym2zoom.inverse()); to_sym.setMatrix(i2,i3-1,i2,i3-1, sym2zoom.inverse());
to_sym.setMatrix(i3,i4-1,i3,i4-1, sym2ers.inverse()); to_sym.setMatrix(i3,i4-1,i3,i4-1, sym2ers.inverse());
// Matrix dbg = from_sym.times(to_sym);
return new Matrix[] {from_sym,to_sym}; return new Matrix[] {from_sym,to_sym};
} }
//getColumnDimension //getColumnDimension
......
...@@ -9495,6 +9495,12 @@ if (debugLevel > -100) return true; // temporarily ! ...@@ -9495,6 +9495,12 @@ if (debugLevel > -100) return true; // temporarily !
System.out.println("Building basic DSI for the AUX camera image set "+quadCLT_main.image_name+ System.out.println("Building basic DSI for the AUX camera image set "+quadCLT_main.image_name+
" using main camera DSI, pass "+(num_adjust_aux+1)+" of "+adjust_aux); " using main camera DSI, pass "+(num_adjust_aux+1)+" of "+adjust_aux);
} }
String dbg_path = clt_parameters.lym_dbg_path; // /home/elphel/lwir16-proc/proc1/results_cuda/25/extrinsics_bgnd_combo.tif
if (dbg_path.length()==0) {
dbg_path = null;
}
// dbg_path = "/home/elphel/lwir16-proc/proc1/results_cuda/25/extrinsics_bgnd_combo.tif";
if (dbg_path == null) {
quadCLT_aux.preExpandCLTQuad3d( // returns ImagePlus, but it already should be saved/shown quadCLT_aux.preExpandCLTQuad3d( // returns ImagePlus, but it already should be saved/shown
imp_srcs_aux, // [srcChannel], // should have properties "name"(base for saving results), "channel","path" imp_srcs_aux, // [srcChannel], // should have properties "name"(base for saving results), "channel","path"
saturation_imp_aux, // boolean [][] saturation_imp, // (near) saturated pixels or null saturation_imp_aux, // boolean [][] saturation_imp, // (near) saturated pixels or null
...@@ -9513,23 +9519,25 @@ if (debugLevel > -100) return true; // temporarily ! ...@@ -9513,23 +9519,25 @@ if (debugLevel > -100) return true; // temporarily !
System.out.println("Adjusting AUX camera image set for "+quadCLT_aux.image_name+ System.out.println("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
", pass "+(num_adjust_aux+1)+" of "+adjust_aux); ", pass "+(num_adjust_aux+1)+" of "+adjust_aux);
} }
}
if (quadCLT_aux.ds_from_main == null) { if (quadCLT_aux.ds_from_main == null) {
System.out.println("BUG: quadCLT_aux.ds_from_main should be not null here!"); System.out.println("BUG: quadCLT_aux.ds_from_main should be not null here!");
double inf_min = -1.0; double inf_min = -1.0;
double inf_max = 1.0; double inf_max = 1.0;
if (num_adjust_aux >= (adjust_aux/2)) { if (num_adjust_aux >= (adjust_aux/2)) {
inf_min = -0.2; inf_min = -0.2;
inf_max = 0.05; inf_max = 0.2; // 0.05; Changed for LWIR16
} }
// adjust w/o main camera - maybe will be used in the future // adjust w/o main camera - maybe will be used in the future
boolean ok = quadCLT_aux.extrinsicsCLT( boolean ok = quadCLT_aux.extrinsicsCLT(
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters, clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
dbg_path,
false, // adjust_poly, false, // adjust_poly,
inf_min, // double inf_min, inf_min, // double inf_min,
inf_max, // double inf_max, inf_max, // double inf_max,
threadsMax, //final int threadsMax, // maximal number of threads to launch threadsMax, //final int threadsMax, // maximal number of threads to launch
updateStatus,// final boolean updateStatus, updateStatus,// final boolean updateStatus,
debugLevelInner); // final int debugLevel) 1); // debugLevelInner); // final int debugLevel)
if (!ok) break; if (!ok) break;
} else { } else {
boolean ok = quadCLT_aux.extrinsicsCLTfromGT( boolean ok = quadCLT_aux.extrinsicsCLTfromGT(
......
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