Skip to content

I
imagej-elphel
Commit 6293946d authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Debugging LY adjustment, tested semi-manual OK

parent 71ac4a10
Hide whitespace changes
Inline Side-by-side
Showing with 422 additions and 106 deletions
src/main/java/com/elphel/imagej/cameras/CLTParameters.java
View file @ 6293946d
......@@ -292,6 +292,8 @@ public class CLTParameters {
public double ly_norm_disp = 5.0; // Reduce weight of higher disparity tiles
// 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 boolean lym_update_disp = true; // Update target disparity after each step
public int lym_iter = 25; // Maximal number of iterations
......@@ -1188,6 +1190,9 @@ public class CLTParameters {
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+"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_iter", this.lym_iter+"");
properties.setProperty(prefix+"lym_change", this.lym_change +"");
......@@ -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_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_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_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"));
......@@ -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)");
gd.addNumericField("Reduce weight of higher disparity tiles", this.ly_norm_disp, 5,8,"");
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.addCheckbox ("Update target disparity after each step", this.lym_update_disp);
gd.addNumericField("Maximal number of iterations", this.lym_iter, 0);
......@@ -3833,6 +3841,8 @@ public class CLTParameters {
this.ly_disp_var_gt= gd.getNextNumber();
this.ly_disp_rvar_gt= gd.getNextNumber();
this.ly_norm_disp= gd.getNextNumber();
this.lym_dbg_path = gd.getNextString();
this.lym_overexp= gd.getNextNumber();
this.lym_update_disp= gd.getNextBoolean();
this.lym_iter= (int) gd.getNextNumber();
......
src/main/java/com/elphel/imagej/tileprocessor/CLTPass3d.java
View file @ 6293946d
......@@ -902,7 +902,9 @@ public class CLTPass3d{
*/
public void updateDisparity()
{
setTileOpDisparity(null, getDisparity(0));
if (disparity_map != null) {
setTileOpDisparity(null, getDisparity(0));
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/CorrVector.java
View file @ 6293946d
......@@ -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) {
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
int[] sub_pars = new int [par_list.size()];
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
......@@ -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) {
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);
......@@ -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) {
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);
......
src/main/java/com/elphel/imagej/tileprocessor/ExtrinsicAdjustment.java
View file @ 6293946d
......@@ -102,11 +102,11 @@ public class ExtrinsicAdjustment {
private CorrVector corr_vector = null;
private boolean [] par_mask = null;
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 [][] 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[][] world_xyz = null;
private double[][] world_xyz = null; // only set in solveCorr()
private double [] weight_window = null; // center area is more reliable
public final GeometryCorrection geometryCorrection;
......@@ -296,7 +296,13 @@ public class ExtrinsicAdjustment {
*/
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;
double [][] pixels = new double [titles.length][clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
......@@ -392,7 +398,7 @@ public class ExtrinsicAdjustment {
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
// 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,
true); // boolean set_dydisp)
......@@ -418,7 +424,7 @@ public class ExtrinsicAdjustment {
double [] dfe = null;
if (max_dfe > 0) {
dfe = distanceFromEdge (
dfe = distanceFromEdge ( // all 0? too little, non-continuous
force_disparity,
measured_dsxy,
min_dfe, // 1.75
......@@ -434,7 +440,7 @@ public class ExtrinsicAdjustment {
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,
min_num_forced/4, // 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,
......@@ -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_use);
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");
// disable parameters...all extrinsic
use_disparity = false;
......@@ -475,7 +481,7 @@ public class ExtrinsicAdjustment {
use_diff_rolls = false;
common_roll = 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");
// disable parameters: all extrinsic but disparity (S0)
use_aztilts = false;
......@@ -821,7 +827,7 @@ public class ExtrinsicAdjustment {
private boolean [] filterInfinity(
double [][] measured_dsxy,
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)
int min_in_half,
double min_infinity_abs,
......@@ -829,6 +835,9 @@ public class ExtrinsicAdjustment {
double min_infinity,
double max_infinity
) {
if (min_in_half < 1) {
min_in_half = 1;
}
int clusters = clustersX * clustersY;
double half_width = 0.5 * clustersX;
boolean [] true_infinity = new boolean[clusters];
......@@ -1584,17 +1593,18 @@ public class ExtrinsicAdjustment {
}
if (graphic) {
String [] titles3 = new String[num_pars * 3];
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
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 * 3][width*height];
for (int par = 0; par < num_pars; par++) {
titles3[3 * par + 0] = titles[par]+"";
titles3[3 * par + 1] = titles[par]+"_delta";
titles3[3 * par + 2] = titles[par]+"_diff";
for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
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);
......@@ -1624,8 +1634,8 @@ public class ExtrinsicAdjustment {
dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
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);
......@@ -1659,12 +1669,14 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix
int clusters = clustersX * clustersY;
String [] titles = {"Y", "-fX", "Y+fx"};
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
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 [3][width*height];
for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
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);
......@@ -1998,7 +2010,15 @@ 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(
double [] fx,
double [] weights,
......@@ -2009,12 +2029,14 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix
int clusters = clustersX * clustersY;
String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"};
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
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 [titles.length][width*height];
for (int mode = 0; mode < points_sample; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
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);
......@@ -2053,7 +2075,12 @@ public class ExtrinsicAdjustment {
}
}
} 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[2][pix] = Double.NaN;
dbg_img[3][pix] = Double.NaN;
......@@ -2078,15 +2105,17 @@ public class ExtrinsicAdjustment {
int gap = 10; //pix
int clusters = clustersX * clustersY;
String [] titles = {"meas", "correction", "diff"};
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
int rows = getRowsCols()[0];
int cols = getRowsCols()[1];
int width = cols * (clustersX + gap) - gap;
int height = rows * (clustersY + gap) - gap;
double [][] xy = getXYNondistorted(corr_vector, false);
double [][] dbg_img = new double [3][width*height];
double [][] moving_objects = new double [3][clusters];
for (int mode = 0; mode < 2 * num_sensors + 1; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
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);
......@@ -2142,6 +2171,7 @@ public class ExtrinsicAdjustment {
// 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
double [][] deriv = geometryCorrection.getPortsDDNDDerivativesNew( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
......@@ -2156,7 +2186,7 @@ public class ExtrinsicAdjustment {
/// int dbg_index = cluster; // dbg_index (pXY, dbg_decimate);
// convert to symmetrical coordinates
// 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,
par_mask); // boolean [] par_mask
......@@ -2286,7 +2316,7 @@ public class ExtrinsicAdjustment {
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_ymfx = getFx(corr_vector);
if (debug_level > -1) { // temporary
if (debug_level > -2) { // temporary
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
......@@ -2300,8 +2330,14 @@ public class ExtrinsicAdjustment {
this.last_ymfx); // modifies this.last_ymfx (weights and subtracts fx)
this.initial_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
if (debug_level > 3) {
if (debug_level > 3) { // compares true jacobians and calculated with delta (same for all parameters)
dbgJacobians(
corr_vector, // CorrVector corr_vector,
1E-5, // double delta,
......@@ -2409,4 +2445,101 @@ public class ExtrinsicAdjustment {
}
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);
}
}
src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java
View file @ 6293946d
......@@ -2806,7 +2806,36 @@ public class GeometryCorrection {
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();
}
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDttCPU.java
View file @ 6293946d
......@@ -4042,6 +4042,28 @@ public class ImageDttCPU {
disparity_map[DISPARITY_INDEX_POLY+1][tIndex] = ds[0][1];
if (debugTile0) {
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();
}
}
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 6293946d
......@@ -6028,9 +6028,9 @@ public class QuadCLTCPU {
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 < rbg_in[green_index].length; i++) {
if (i == 700) {
System.out.println("linearStackToColor(): i="+i);
}
// if (i == 700) {
// System.out.println("linearStackToColor(): i="+i);
// }
float [] rgb = tc.getRGB(iclt_data[green_index][i]);
rgba[0][i] = rgb[0]; // red
rgba[1][i] = rgb[1]; // green
......@@ -6147,9 +6147,9 @@ public class QuadCLTCPU {
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 < rbg_in[green_index].length; i++) {
if (i == 700) {
System.out.println("linearStackToColor(): i="+i);
}
// if (i == 700) {
// System.out.println("linearStackToColor(): i="+i);
// }
double [] rgb = tc.getRGB(iclt_data[green_index][i]);
rgba[0][i] = rgb[0]; // red
rgba[1][i] = rgb[1]; // green
......@@ -8158,18 +8158,88 @@ public class QuadCLTCPU {
double inf_max, // = 1.0;
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel)
{
extrinsics_prepare(
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)
{
boolean got_saved = getPreparedExtrinsics(dbg_path);
if (!got_saved) {
extrinsics_prepare(
clt_parameters,
inf_min, // = -1.0;
inf_max, // = 1.0;
threadsMax, // maximal number of threads to launch
updateStatus,
debugLevel);
}
final boolean batch_mode = clt_parameters.batch_run;
final boolean batch_mode = false; // clt_parameters.batch_run;
int debugLevelInner = batch_mode ? -5: debugLevel;
boolean update_disp_from_latest = clt_parameters.lym_update_disp ; // true;
int max_tries = clt_parameters.lym_iter; // 25;
......@@ -8254,7 +8324,7 @@ public class QuadCLTCPU {
false, // boolean update_disparity, // re-measure disparity before measuring LY
threadsMax, // final int threadsMax, // maximal number of threads to launch
updateStatus, //final boolean updateStatus,
1E-3, // double delta,
1E-2, // 1E-3, // double delta,
debugLevel); // final int debugLevel)
}
......@@ -12464,32 +12534,63 @@ public class QuadCLTCPU {
final boolean updateStatus,
double delta,
final int debugLevel)
{
double [] parameter_scales = { // multiply delay for each parameter
0.3, // 0.014793657667505566, // 00 10
0.3, // 0.015484017460841183, // 01 10
0.3, // 0.02546712771769517, // 02 10
0.3, // 0.02071573747995167, // 03 10
0.3, // 0.026584237444512468, // 04 10
0.3, // 0.014168012698804967, // 05 10
2.0, // 1.8554483718240792E-4, // 06
0.3, //2.3170738149889717E-4, // 07
0.3, //3.713239026512266E-4, // 08
0.3, //2.544834643007531E-4, // 09
0.3, // 2.5535557646736286E-4, // 10
0.3, // 1.98531249109261E-4, // 11
0.3, // 2.1802727086879284E-4, // 12
150, // 8.814346720176489E-1, // 5, // 13 10000x
150, // 7.071297501674136E-1, // 5, // 14 10000x
150, // 1.306306793587865E-0, // 4, // 15 10000x
300, // 2.8929916645453735E-0, // 4, // 16 10000x
300, // 2.943408022525927E-0, // 4, // 17 10000x
500.0}; // 390.6185365641268}; //4}; // 18 100000x
{
delta = 0.0003;
/*double [] parameter_scales4 = { // multiply delay for each parameter
0.3, // 0.014793657667505566, // 00 10 tilt0
0.3, // 0.015484017460841183, // 01 10 tilt1
0.3, // 0.02546712771769517, // 02 10 tilt2
0.3, // 0.02071573747995167, // 03 10 az0
0.3, // 0.026584237444512468, // 04 10 az1
0.3, // 0.014168012698804967, // 05 10 az2
2.0, // 1.8554483718240792E-4,// 06 roll0
0.3, //2.3170738149889717E-4, // 07 roll1
0.3, //3.713239026512266E-4, // 08 roll2
0.3, //2.544834643007531E-4, // 09 roll3
0.3, // 2.5535557646736286E-4, // 10 zoom0
0.3, // 1.98531249109261E-4, // 11 zoom1
0.3, // 2.1802727086879284E-4, // 12 zoom2
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
*/
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
boolean debug_img = false;
int debugLevelInner = -5;
CLTPass3d scan = tp.clt_3d_passes.get(scanIndex);
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();
int clusters = ea.clustersX * ea.clustersY;
int num_ly = ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); // scan.getLazyEyeData().length;
......@@ -12514,7 +12615,11 @@ public class QuadCLTCPU {
ly_initial, // double[][] data,
"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++) {
// perform asymmetric delta
double [] par_inc = new double [num_pars];
......@@ -12524,7 +12629,8 @@ public class QuadCLTCPU {
corr_vectorp.incrementVector(corr_delta, 1.0); // 0.5 for p/m
geometryCorrection.setCorrVector(corr_vectorp) ;
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();
if (update_disparity) {
CLTMeasureCorr( // perform single pass according to prepared tiles operations and disparity
......@@ -12585,12 +12691,12 @@ public class QuadCLTCPU {
}
*/
int gap = 10;
String [] titles = new String [num_pars]; //ea.getSymNames();
for (int i = 0; i < num_pars; i++) {
titles[i] = "S"+i;
}
int width = 3 * ea.clustersX + 2 * gap;
int height = 3 * ea.clustersY + 2 * gap;
// int width = 3 * ea.clustersX + 2 * gap;
// int height = 3 * ea.clustersY + 2 * gap;
int rows = ea.getRowsCols()[0];
int cols = ea.getRowsCols()[1];
int width = cols * (ea.clustersX + gap) - gap;
int height = rows * (ea.clustersY + gap) - gap;
double [][] dbg_img = new double [num_pars][width*height];
/*
for (int par = 0; par < num_pars; par++) {
......@@ -12617,8 +12723,8 @@ public class QuadCLTCPU {
dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap);
int x0 = (mode % cols) * (ea.clustersX + gap);
int y0 = (mode / cols) * (ea.clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % ea.clustersX);
int y = y0 + (cluster / ea.clustersX);
......@@ -12643,8 +12749,8 @@ public class QuadCLTCPU {
dbg_img = new double [num_pars][width*height];
for (int par = 0; par < num_pars; par++) {
for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap);
int x0 = (mode % cols) * (ea.clustersX + gap);
int y0 = (mode / cols) * (ea.clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % ea.clustersX);
int y = y0 + (cluster / ea.clustersX);
......
src/main/java/com/elphel/imagej/tileprocessor/SymmVector.java
View file @ 6293946d
......@@ -103,7 +103,7 @@ public class SymmVector {
full_type2,
debug_level);
rvs.xy = sv.exportXY();
rvs.rt = sv.exportRT();
rvs.rt = sv.exportRT(); // radius/tangential (out, cw, out, cw
rvs.dir_rt = sv.exportDirRT();
rvs.rots = sv.exportRZ(false); // include common roll
rvs.zooms = sv.exportRZ(true); // no common zoom
......@@ -1152,8 +1152,9 @@ public class SymmVector {
for (int i = 0; i < N - 1; i++) {
xy2ta.set( i, 2* i + 1, TILT_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 {
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(i3,i4-1,i3,i4-1, sym2ers.inverse());
// Matrix dbg = from_sym.times(to_sym);
return new Matrix[] {from_sym,to_sym};
}
//getColumnDimension
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 6293946d
......@@ -9495,41 +9495,49 @@ if (debugLevel > -100) return true; // temporarily !
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);
}
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"
saturation_imp_aux, // boolean [][] saturation_imp, // (near) saturated pixels or null
clt_parameters,
debayerParameters,
colorProcParameters_aux,
rgbParameters,
threadsMax, // maximal number of threads to launch
updateStatus,
debugLevelInner);
// adjust extrinsics here
System.out.println("Adjust AUX extrinsics here");
if (updateStatus) IJ.showStatus("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
", pass "+(num_adjust_aux+1)+" of "+adjust_aux);
if (debugLevel > -5) {
System.out.println("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
", 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
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
clt_parameters,
debayerParameters,
colorProcParameters_aux,
rgbParameters,
threadsMax, // maximal number of threads to launch
updateStatus,
debugLevelInner);
// adjust extrinsics here
System.out.println("Adjust AUX extrinsics here");
if (updateStatus) IJ.showStatus("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
", pass "+(num_adjust_aux+1)+" of "+adjust_aux);
if (debugLevel > -5) {
System.out.println("Adjusting AUX camera image set for "+quadCLT_aux.image_name+
", pass "+(num_adjust_aux+1)+" of "+adjust_aux);
}
}
if (quadCLT_aux.ds_from_main == null) {
System.out.println("BUG: quadCLT_aux.ds_from_main should be not null here!");
double inf_min = -1.0;
double inf_max = 1.0;
if (num_adjust_aux >= (adjust_aux/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
boolean ok = quadCLT_aux.extrinsicsCLT(
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
dbg_path,
false, // adjust_poly,
inf_min, // double inf_min,
inf_max, // double inf_max,
threadsMax, //final int threadsMax, // maximal number of threads to launch
updateStatus,// final boolean updateStatus,
debugLevelInner); // final int debugLevel)
1); // debugLevelInner); // final int debugLevel)
if (!ok) break;
} else {
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