Skip to content

I
imagej-elphel
Commit 624db0c0 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Updating ExtrinsicAdjustment for variable number of sensors

parent 31555875
Hide whitespace changes
Inline Side-by-side
Showing with 1136 additions and 188 deletions
src/main/java/com/elphel/imagej/tileprocessor/ExtrinsicAdjustment.java
View file @ 624db0c0
...@@ -30,22 +30,20 @@ import Jama.Matrix; ...@@ -30,22 +30,20 @@ import Jama.Matrix;
*/ */
public class ExtrinsicAdjustment { public class ExtrinsicAdjustment {
/*
static final int NUM_SENSORS = 4; static final int NUM_SENSORS = 4;
static final int INDX_DISP = 0; // composite static final int INDX_DISP = 0; // composite
static final int INDX_STRENGTH = 1; static final int INDX_STRENGTH = 1;
static final int INDX_X0 = 2; static final int INDX_X0 = 2; // 2 * NUM_SENSORS long
static final int INDX_TARGET = 10; // target disparity static final int INDX_TARGET = 10; // target disparity
static final int INDX_DIFF = 11; // differential disparity (now w/o magic composite =target + diff) static final int INDX_DIFF = 11; // differential disparity (now w/o magic composite =target + diff)
static final int INDX_DYDDISP0 =12; // derivative of pixel y over disparity (for ERS) static final int INDX_DYDDISP0 =12; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
static final int INDX_PX = 16; static final int INDX_PX = 16; // 2 ?
static final int INDX_DD0 = 18; static final int INDX_DD0 = 18; // NUM_SENSORS long
static final int INDX_ND0 = 22; static final int INDX_ND0 = 22; // NUM_SENSORS long
static final int INDX_PYDIST = 26; // average pY to calculate ERS time difference static final int INDX_PYDIST = 26; // average pY to calculate ERS time difference
static final int INDX_LENGTH = 26 + 4; static final int INDX_LENGTH = 26 + 4; // total length
static final int POINTS_SAMPLE = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd static final int POINTS_SAMPLE = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
static final String [] DATA_TITLES = { static final String [] DATA_TITLES = {
"Disparity", "Strength", "Disparity", "Strength",
...@@ -56,7 +54,41 @@ public class ExtrinsicAdjustment { ...@@ -56,7 +54,41 @@ public class ExtrinsicAdjustment {
"DD-0", "DD-1","DD-2","DD-3", "DD-0", "DD-1","DD-2","DD-3",
"ND-0", "ND-1","ND-2","ND-3", "ND-0", "ND-1","ND-2","ND-3",
"pY0", "pY1", "pY2", "pY3"}; "pY0", "pY1", "pY2", "pY3"};
*/
// changing values to consolidate fixed values (not dependent on the number of sensors to use static)
// static final int NUM_SENSORS = 4;
static final int INDX_DISP = 0; // composite
static final int INDX_STRENGTH = 1;
static final int INDX_TARGET = 2; // target disparity
static final int INDX_DIFF = 3; // differential disparity (now w/o magic composite =target + diff)
static final int INDX_PX = 4; // 4,5
static final int INDX_X0 = 6; // 2 * NUM_SENSORS long
// keep static for above, replace non-static
// static final int INDX_DYDDISP0 =14; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
// static final int INDX_DD0 = 18; // NUM_SENSORS long
// static final int INDX_ND0 = 22; // NUM_SENSORS long
// static final int INDX_PYDIST = 26; // average pY to calculate ERS time difference
// static final int INDX_LENGTH = 26 + 4; // total length
// static final int POINTS_SAMPLE = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
final int indx_dyddisp0; //INDX_X0 + 2 * num_sensors =14; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
final int indx_dd0; // = 18; // NUM_SENSORS long
final int indx_nd0; // = 22; // NUM_SENSORS long
final int indx_pydist; // = 26; // average pY to calculate ERS time difference
final int indx_length; // = 26 + 4; // total length
final int points_sample; // = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
final String [] data_titles;
/*
static final String [] DATA_TITLES = {
"Disparity", "Strength",
"DX-0","DY-0","DX-1","DY-1","DX-2","DY-2","DX-3","DY-3",
"Target Disparity","Diff. Disparity",
"dY_dD-0","dY_dD-1","dY_dD-2","dY_dD-3",
"pX","pY",
"DD-0", "DD-1","DD-2","DD-3",
"ND-0", "ND-1","ND-2","ND-3",
"pY0", "pY1", "pY2", "pY3"};
*/
// next values are only updated after success // next values are only updated after success
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
...@@ -77,11 +109,13 @@ public class ExtrinsicAdjustment { ...@@ -77,11 +109,13 @@ public class ExtrinsicAdjustment {
private double[][] world_xyz = null; private double[][] world_xyz = null;
private double [] weight_window = null; // center area is more reliable private double [] weight_window = null; // center area is more reliable
public GeometryCorrection geometryCorrection = null; public final GeometryCorrection geometryCorrection;
public int clusterSize; public int clusterSize;
public int clustersX; public int clustersX;
public int clustersY; public int clustersY;
public final int num_sensors;
public double dbg_delta = 0; // 1.0E-5; // if not 0 - use delta instead of the derivatives in getJacobianTransposed public double dbg_delta = 0; // 1.0E-5; // if not 0 - use delta instead of the derivatives in getJacobianTransposed
public double [] getOldNewRMS() { public double [] getOldNewRMS() {
...@@ -99,28 +133,90 @@ public class ExtrinsicAdjustment { ...@@ -99,28 +133,90 @@ public class ExtrinsicAdjustment {
return on_rms; return on_rms;
} }
/*
final int indx_dyddisp0; //INDX_X0 + 2 * num_sensors =14; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
final int indx_dd0; // = 18; // NUM_SENSORS long
final int indx_nd0; // = 22; // NUM_SENSORS long
final int indx_pydist; // = 26; // average pY to calculate ERS time difference
final int indx_length; // = 26 + 4; // total length
final int points_sample; // = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
// static final int INDX_DYDDISP0 =14; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
// static final int INDX_DD0 = 18; // NUM_SENSORS long
// static final int INDX_ND0 = 22; // NUM_SENSORS long
// static final int INDX_PYDIST = 26; // average pY to calculate ERS time difference
// static final int INDX_LENGTH = 26 + 4; // total length
static final int POINTS_SAMPLE = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
*/
public static int get_INDX_DYDDISP0(int num_sensors) {return INDX_X0 + 2 * num_sensors;}
public static int get_INDX_DD0 (int num_sensors) {return INDX_X0 + 3 * num_sensors;}
public static int get_INDX_ND0 (int num_sensors) {return INDX_X0 + 4 * num_sensors;}
public static int get_INDX_PYDIST (int num_sensors) {return INDX_X0 + 5 * num_sensors;}
public static int get_INDX_LENGTH (int num_sensors) {return INDX_X0 + 6 * num_sensors;}
public static int get_POINTS_SAMPLE(int num_sensors) {return 2 * num_sensors + 1;}
public ExtrinsicAdjustment ( public ExtrinsicAdjustment (
GeometryCorrection gc, GeometryCorrection gc,
int clusterSize, int clusterSize,
int clustersX, int clustersX,
int clustersY) { int clustersY) {
geometryCorrection = gc; this.geometryCorrection = gc;
this.num_sensors = gc.getNumSensors();
this.clusterSize = clusterSize; this.clusterSize = clusterSize;
this.clustersX = clustersX; this.clustersX = clustersX;
this.clustersY = clustersY; this.clustersY = clustersY;
indx_dyddisp0 = get_INDX_DYDDISP0(num_sensors); // = 14; // derivative of pixel y over disparity (for ERS) NUM_SENSORS long
indx_dd0 = get_INDX_DD0 (num_sensors); // = 18; // NUM_SENSORS long
indx_nd0 = get_INDX_ND0 (num_sensors); // = 22; // NUM_SENSORS long
indx_pydist = get_INDX_PYDIST (num_sensors); // = 26; // average pY to calculate ERS time difference
indx_length = get_INDX_LENGTH (num_sensors); // = 26 + 4; // total length
points_sample = get_POINTS_SAMPLE(num_sensors); // = 2 * NUM_SENSORS +1; //points per sample residual disparity, 4*dd, *nd
data_titles = new String[indx_length];
String [] titles_static = {"Disparity", "Strength",
"Target Disparity","Diff. Disparity",
"pX","pY"};
int indx = 0;
for (int i = 0; i < titles_static.length; i++) {
data_titles[indx++] = titles_static[i];
}
for (int i = 0; i < num_sensors; i++) {
data_titles[indx++] = "DX-"+i;
data_titles[indx++] = "DY-"+i;
}
for (int i = 0; i < num_sensors; i++) {
data_titles[indx++] = "dY_dD-"+i;
}
for (int i = 0; i < num_sensors; i++) {
data_titles[indx++] = "DD-"+i;
}
for (int i = 0; i < num_sensors; i++) {
data_titles[indx++] = "ND-"+i;
}
for (int i = 0; i < num_sensors; i++) {
data_titles[indx++] = "pY"+i;
}
} }
public void showInput(double[][] data, String title) { public void showInput(double[][] data, String title) {
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double [][] pixels = new double [ExtrinsicAdjustment.INDX_LENGTH+4][clusters]; // double [][] pixels = new double [ExtrinsicAdjustment.INDX_LENGTH+4][clusters];
String [] titles = new String[ExtrinsicAdjustment.INDX_LENGTH+4]; // String [] titles = new String[ExtrinsicAdjustment.INDX_LENGTH+4];
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++) { // for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++) {
titles[i] = ExtrinsicAdjustment.DATA_TITLES[i]; double [][] pixels = new double [indx_length + 4][clusters];
} String [] titles = new String[indx_length + 4];
titles[ExtrinsicAdjustment.INDX_LENGTH+0]="Force_disparity"; for (int i = 0; i < indx_length; i++) {
titles[ExtrinsicAdjustment.INDX_LENGTH+1]="dx-sum"; // titles[i] = ExtrinsicAdjustment.DATA_TITLES[i];
titles[ExtrinsicAdjustment.INDX_LENGTH+2]="dy_sum"; titles[i] = data_titles[i];
titles[ExtrinsicAdjustment.INDX_LENGTH+3]="dd_sum"; }
// titles[ExtrinsicAdjustment.INDX_LENGTH+0]="Force_disparity";
// titles[ExtrinsicAdjustment.INDX_LENGTH+1]="dx-sum";
// titles[ExtrinsicAdjustment.INDX_LENGTH+2]="dy_sum";
// titles[ExtrinsicAdjustment.INDX_LENGTH+3]="dd_sum";
titles[indx_length+0]="Force_disparity";
titles[indx_length+1]="dx-sum";
titles[indx_length+2]="dy_sum";
titles[indx_length+3]="dd_sum";
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
if (data[cluster] != null) { if (data[cluster] != null) {
...@@ -128,9 +224,12 @@ public class ExtrinsicAdjustment { ...@@ -128,9 +224,12 @@ public class ExtrinsicAdjustment {
pixels[c][cluster] = data[cluster][c]; pixels[c][cluster] = data[cluster][c];
} }
for (int i = 0;i <4; i++) { for (int i = 0;i <4; i++) {
pixels[ExtrinsicAdjustment.INDX_LENGTH+1][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_X0 + 2 * i]; // pixels[ExtrinsicAdjustment.INDX_LENGTH+1][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_X0 + 2 * i];
pixels[ExtrinsicAdjustment.INDX_LENGTH+2][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_X0 + 2 * i + 1]; // pixels[ExtrinsicAdjustment.INDX_LENGTH+2][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_X0 + 2 * i + 1];
pixels[ExtrinsicAdjustment.INDX_LENGTH+3][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_DD0 + i]; // pixels[ExtrinsicAdjustment.INDX_LENGTH+3][cluster] += 0.25 * data[cluster][ExtrinsicAdjustment.INDX_DD0 + i];
pixels[indx_length+1][cluster] += 0.25 * data[cluster][INDX_X0 + 2 * i];
pixels[indx_length+2][cluster] += 0.25 * data[cluster][INDX_X0 + 2 * i + 1];
pixels[indx_length+3][cluster] += 0.25 * data[cluster][indx_dd0 + i];
} }
} else { } else {
for (int c = 0; c < pixels.length; c++) { for (int c = 0; c < pixels.length; c++) {
...@@ -139,7 +238,8 @@ public class ExtrinsicAdjustment { ...@@ -139,7 +238,8 @@ public class ExtrinsicAdjustment {
} }
} }
if (force_disparity!=null) { if (force_disparity!=null) {
pixels[ExtrinsicAdjustment.INDX_LENGTH][cluster] = force_disparity[cluster]?1.0:0.0; // pixels[ExtrinsicAdjustment.INDX_LENGTH][cluster] = force_disparity[cluster]?1.0:0.0;
pixels[indx_length][cluster] = force_disparity[cluster]?1.0:0.0;
} }
} }
(new ShowDoubleFloatArrays()).showArrays( (new ShowDoubleFloatArrays()).showArrays(
...@@ -150,7 +250,7 @@ public class ExtrinsicAdjustment { ...@@ -150,7 +250,7 @@ public class ExtrinsicAdjustment {
title, title,
titles); //ExtrinsicAdjustment.DATA_TITLES); titles); //ExtrinsicAdjustment.DATA_TITLES);
} }
/*
public static void showLYInput( public static void showLYInput(
double[][] data, double[][] data,
String title, String title,
...@@ -161,6 +261,8 @@ public class ExtrinsicAdjustment { ...@@ -161,6 +261,8 @@ public class ExtrinsicAdjustment {
String [] titles = new String[ExtrinsicAdjustment.INDX_LENGTH+3]; String [] titles = new String[ExtrinsicAdjustment.INDX_LENGTH+3];
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++) { for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++) {
titles[i] = ExtrinsicAdjustment.DATA_TITLES[i]; titles[i] = ExtrinsicAdjustment.DATA_TITLES[i];
// titles[i] = data_titles[i];
} }
titles[ExtrinsicAdjustment.INDX_LENGTH+0]="dx-sum"; titles[ExtrinsicAdjustment.INDX_LENGTH+0]="dx-sum";
titles[ExtrinsicAdjustment.INDX_LENGTH+1]="dy_sum"; titles[ExtrinsicAdjustment.INDX_LENGTH+1]="dy_sum";
...@@ -191,7 +293,7 @@ public class ExtrinsicAdjustment { ...@@ -191,7 +293,7 @@ public class ExtrinsicAdjustment {
title, title,
titles); //ExtrinsicAdjustment.DATA_TITLES); titles); //ExtrinsicAdjustment.DATA_TITLES);
} }
*/
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"}; String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"};
...@@ -483,18 +585,15 @@ public class ExtrinsicAdjustment { ...@@ -483,18 +585,15 @@ public class ExtrinsicAdjustment {
} }
if (measured_dsxy[cluster] != null) { if (measured_dsxy[cluster] != null) {
if (set_dydisp) { if (set_dydisp) {
/// dy_ddisparity[cluster] = new double[NUM_SENSORS]; pY_offset [cluster] = new double[num_sensors];
pY_offset [cluster] = new double[NUM_SENSORS];
double lines_avg = 0; double lines_avg = 0;
for (int i = 0; i < NUM_SENSORS; i++) { for (int i = 0; i < num_sensors; i++) {
/// dy_ddisparity[cluster][i] = measured_dsxy[cluster][INDX_DYDDISP0 + i];
// find average time // find average time
// pY_offset[cluster][i] = measured_dsxy[cluster][INDX_PYDIST + i] - py_time0[i]; pY_offset[cluster][i] = measured_dsxy[cluster][indx_pydist + i] - top_woi[i]; // time (scanlines) since frame start
pY_offset[cluster][i] = measured_dsxy[cluster][INDX_PYDIST + i] - top_woi[i]; // time (scanlines) since frame start
lines_avg += pY_offset[cluster][i]; lines_avg += pY_offset[cluster][i];
} }
lines_avg /= NUM_SENSORS; lines_avg /= num_sensors;
for (int i = 0; i < NUM_SENSORS; i++) { for (int i = 0; i < num_sensors; i++) {
pY_offset[cluster][i] -= lines_avg; pY_offset[cluster][i] -= lines_avg;
} }
...@@ -540,7 +639,7 @@ public class ExtrinsicAdjustment { ...@@ -540,7 +639,7 @@ public class ExtrinsicAdjustment {
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double rms = 0, rms_pure = 0; double rms = 0, rms_pure = 0;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
int indx0 = POINTS_SAMPLE * cluster + 0; int indx0 = points_sample * cluster + 0;
// force_disparity - compensate measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF], // force_disparity - compensate measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF],
// false - keep (so force fx==0 // false - keep (so force fx==0
double d = - fx[indx0]; double d = - fx[indx0];
...@@ -549,13 +648,15 @@ public class ExtrinsicAdjustment { ...@@ -549,13 +648,15 @@ public class ExtrinsicAdjustment {
} }
fx[indx0] = this.weights[indx0] * d; fx[indx0] = this.weights[indx0] * d;
rms += fx[indx0]*d; // sum of weights rms += fx[indx0]*d; // sum of weights
for (int cam = 0; cam < NUM_SENSORS; cam++) { for (int cam = 0; cam < num_sensors; cam++) {
int indx = indx0 + cam + 1; int indx = indx0 + cam + 1;
d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + cam] - fx[indx]); // d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + cam] - fx[indx]);
d = (-measured_dsxy[cluster][indx_dd0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d; fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights rms += fx[indx] * d; // sum of weights
indx = indx0 + cam + 5; // nd indx = indx0 + cam + 5; // nd
d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + cam] - fx[indx]); // d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + cam] - fx[indx]);
d = (-measured_dsxy[cluster][indx_nd0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d; fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights rms += fx[indx] * d; // sum of weights
} }
...@@ -566,7 +667,7 @@ public class ExtrinsicAdjustment { ...@@ -566,7 +667,7 @@ public class ExtrinsicAdjustment {
double [] rslt = {rms, rms_pure}; double [] rslt = {rms, rms_pure};
return rslt; return rslt;
} }
/*
@Deprecated @Deprecated
private double [] getWeightWindowOld(double marg_fraction) { // 0.0 - no margins, 1.0 - pure cosine private double [] getWeightWindowOld(double marg_fraction) { // 0.0 - no margins, 1.0 - pure cosine
double mf_hor = marg_fraction; double mf_hor = marg_fraction;
...@@ -610,8 +711,8 @@ public class ExtrinsicAdjustment { ...@@ -610,8 +711,8 @@ public class ExtrinsicAdjustment {
} }
} }
return w; return w;
} }
*/
private double [] getWeightWindow( private double [] getWeightWindow(
double marg_fraction, // 0.0 - no margins, 1.0 - pure cosine double marg_fraction, // 0.0 - no margins, 1.0 - pure cosine
...@@ -827,8 +928,8 @@ public class ExtrinsicAdjustment { ...@@ -827,8 +928,8 @@ public class ExtrinsicAdjustment {
double swf = 0.0; double swf = 0.0;
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
int indx = cluster * POINTS_SAMPLE + i; int indx = cluster * points_sample + i;
sw += weights[indx]; sw += weights[indx];
if (fg[cluster]) { if (fg[cluster]) {
swf += weights[indx]; swf += weights[indx];
...@@ -843,8 +944,8 @@ public class ExtrinsicAdjustment { ...@@ -843,8 +944,8 @@ public class ExtrinsicAdjustment {
double k_fg = fg_boost_fraction/swf; double k_fg = fg_boost_fraction/swf;
double k_bg = (1.0 - fg_boost_fraction)/(1.0 - swf); double k_bg = (1.0 - fg_boost_fraction)/(1.0 - swf);
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
int indx = cluster * POINTS_SAMPLE + i; int indx = cluster * points_sample + i;
weights[indx] *= fg[cluster] ? k_fg : k_bg; weights[indx] *= fg[cluster] ? k_fg : k_bg;
} }
} }
...@@ -863,12 +964,12 @@ public class ExtrinsicAdjustment { ...@@ -863,12 +964,12 @@ public class ExtrinsicAdjustment {
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double disparity = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]; // INDX_DISP]; double disparity = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]; // INDX_DISP];
if (disparity <= max_disparity) { if (disparity <= max_disparity) {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
sw_near += weights[cluster * POINTS_SAMPLE + i]; sw_near += weights[cluster * points_sample + i];
} }
} }
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
sw += weights[cluster * POINTS_SAMPLE + i]; sw += weights[cluster * points_sample + i];
} }
} }
...@@ -877,12 +978,12 @@ public class ExtrinsicAdjustment { ...@@ -877,12 +978,12 @@ public class ExtrinsicAdjustment {
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double disparity = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]; // INDX_DISP]; double disparity = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]; // INDX_DISP];
if (disparity <= max_disparity) { if (disparity <= max_disparity) {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] *= wboost; weights[cluster * points_sample + i] *= wboost;
} }
} else { } else {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = 0; weights[cluster * points_sample + i] = 0;
} }
} }
} }
...@@ -947,12 +1048,12 @@ public class ExtrinsicAdjustment { ...@@ -947,12 +1048,12 @@ public class ExtrinsicAdjustment {
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
if (!prohibit[cluster]) { if (!prohibit[cluster]) {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
sw_en += weights[cluster * POINTS_SAMPLE + i]; sw_en += weights[cluster * points_sample + i];
} }
} }
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
sw += weights[cluster * POINTS_SAMPLE + i]; sw += weights[cluster * points_sample + i];
} }
} }
...@@ -960,12 +1061,12 @@ public class ExtrinsicAdjustment { ...@@ -960,12 +1061,12 @@ public class ExtrinsicAdjustment {
double wboost = sw/sw_en; double wboost = sw/sw_en;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
if (!prohibit[cluster]) { if (!prohibit[cluster]) {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] *= wboost; weights[cluster * points_sample + i] *= wboost;
} }
} else { } else {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = 0; weights[cluster * points_sample + i] = 0;
} }
} }
} }
...@@ -980,11 +1081,11 @@ public class ExtrinsicAdjustment { ...@@ -980,11 +1081,11 @@ public class ExtrinsicAdjustment {
boolean [] filtered_infinity, boolean [] filtered_infinity,
int min_num_forced, // if number of forced samples exceeds this, zero out weights of non-forced int min_num_forced, // if number of forced samples exceeds this, zero out weights of non-forced
double weight_disparity, // now 0.0 - 1.0 fraction of disparity in all samples double weight_disparity, // now 0.0 - 1.0 fraction of disparity in all samples
double weight_lazyeye) // relative weight of disparity to 1/POINTS_SAMPLE double weight_lazyeye) // relative weight of disparity to 1/points_sample
{ {
// weight_disparity =.5; // FIXME: Fix! // weight_disparity =.5; // FIXME: Fix!
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double [] weights = new double [clusters * POINTS_SAMPLE]; double [] weights = new double [clusters * points_sample];
boolean [] disable = new boolean [clusters]; boolean [] disable = new boolean [clusters];
if (filtered_infinity != null) { if (filtered_infinity != null) {
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
...@@ -1013,12 +1114,12 @@ public class ExtrinsicAdjustment { ...@@ -1013,12 +1114,12 @@ public class ExtrinsicAdjustment {
if (use_forced) { if (use_forced) {
// disparity weight non-zero only for infinity, non-disparity - equal scale for all // disparity weight non-zero only for infinity, non-disparity - equal scale for all
k_inf_disp = weight_disparity / swf; k_inf_disp = weight_disparity / swf;
k_inf_ly = (1.0 - weight_disparity) / sw / (POINTS_SAMPLE - 1); k_inf_ly = (1.0 - weight_disparity) / sw / (points_sample - 1);
k_other_disp = 0.0; k_other_disp = 0.0;
k_other_ly = k_inf_ly; k_other_ly = k_inf_ly;
} else { } else {
k_other_disp = weight_lazyeye / sw / POINTS_SAMPLE; k_other_disp = weight_lazyeye / sw / points_sample;
k_other_ly = (1.0 - weight_lazyeye/POINTS_SAMPLE) / (POINTS_SAMPLE - 1) / sw; k_other_ly = (1.0 - weight_lazyeye/points_sample) / (points_sample - 1) / sw;
k_inf_disp = k_other_disp; k_inf_disp = k_other_disp;
k_inf_ly = k_other_ly; k_inf_ly = k_other_ly;
} }
...@@ -1026,14 +1127,14 @@ public class ExtrinsicAdjustment { ...@@ -1026,14 +1127,14 @@ public class ExtrinsicAdjustment {
for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){ for (int cluster = 0; cluster < clusters; cluster++) if ((measured_dsxy[cluster] != null) && !disable[cluster]){
double s = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH] * weight_window[cluster]; // cluster weight double s = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH] * weight_window[cluster]; // cluster weight
if ((force_disparity != null) && force_disparity[cluster]) { if ((force_disparity != null) && force_disparity[cluster]) {
weights[cluster * POINTS_SAMPLE + 0] = s * k_inf_disp; weights[cluster * points_sample + 0] = s * k_inf_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = s * k_inf_ly; weights[cluster * points_sample + i] = s * k_inf_ly;
} }
} else { } else {
weights[cluster * POINTS_SAMPLE + 0] = s * k_other_disp; weights[cluster * points_sample + 0] = s * k_other_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = s * k_other_ly; weights[cluster * points_sample + i] = s * k_other_ly;
} }
} }
} }
...@@ -1100,7 +1201,7 @@ public class ExtrinsicAdjustment { ...@@ -1100,7 +1201,7 @@ public class ExtrinsicAdjustment {
double max_disparity_use) // do not process near objects to avoid ERS double max_disparity_use) // do not process near objects to avoid ERS
{ {
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double [] weights = new double [clusters * POINTS_SAMPLE]; double [] weights = new double [clusters * points_sample];
double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly
boolean [] disable = new boolean [clusters]; boolean [] disable = new boolean [clusters];
if (filtered_infinity != null) { if (filtered_infinity != null) {
...@@ -1141,13 +1242,13 @@ public class ExtrinsicAdjustment { ...@@ -1141,13 +1242,13 @@ public class ExtrinsicAdjustment {
} }
if (use_forced) { if (use_forced) {
k_inf_disp = weight_infinity / swf; k_inf_disp = weight_infinity / swf;
k_inf_ly = weight_infinity / swf / (POINTS_SAMPLE - 1); k_inf_ly = weight_infinity / swf / (points_sample - 1);
k_other_disp = (1.0 - weight_infinity) / (sw - swf); k_other_disp = (1.0 - weight_infinity) / (sw - swf);
k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (POINTS_SAMPLE - 1); k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (points_sample - 1);
} else { } else {
k_other_disp = 1.0 / sw; k_other_disp = 1.0 / sw;
k_other_ly = 1.0 / sw / (POINTS_SAMPLE - 1); k_other_ly = 1.0 / sw / (points_sample - 1);
k_inf_disp = k_other_disp; k_inf_disp = k_other_disp;
k_inf_ly = k_other_ly; k_inf_ly = k_other_ly;
} }
...@@ -1156,14 +1257,14 @@ public class ExtrinsicAdjustment { ...@@ -1156,14 +1257,14 @@ public class ExtrinsicAdjustment {
double sd = strength[0][cluster]; double sd = strength[0][cluster];
double sly =strength[1][cluster]; double sly =strength[1][cluster];
if ((force_disparity != null) && force_disparity[cluster]) { if ((force_disparity != null) && force_disparity[cluster]) {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_inf_disp; weights[cluster * points_sample + 0] = sd * k_inf_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_inf_ly; weights[cluster * points_sample + i] = sly * k_inf_ly;
} }
} else { } else {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_other_disp; weights[cluster * points_sample + 0] = sd * k_other_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_other_ly; weights[cluster * points_sample + i] = sly * k_other_ly;
} }
} }
} }
...@@ -1186,7 +1287,7 @@ public class ExtrinsicAdjustment { ...@@ -1186,7 +1287,7 @@ public class ExtrinsicAdjustment {
{ {
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
double half_width = 0.5 * clustersX; double half_width = 0.5 * clustersX;
double [] weights = new double [clusters * POINTS_SAMPLE]; double [] weights = new double [clusters * points_sample];
double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly double [][] strength = new double [2][clusters]; // 0 - for disparity, 1 - for ly
boolean [] disable = new boolean [clusters]; boolean [] disable = new boolean [clusters];
if (filtered_infinity != null) { if (filtered_infinity != null) {
...@@ -1233,13 +1334,13 @@ public class ExtrinsicAdjustment { ...@@ -1233,13 +1334,13 @@ public class ExtrinsicAdjustment {
} }
if (use_forced) { if (use_forced) {
k_inf_disp = weight_infinity / swf; k_inf_disp = weight_infinity / swf;
k_inf_ly = weight_infinity / swf / (POINTS_SAMPLE - 1); k_inf_ly = weight_infinity / swf / (points_sample - 1);
k_other_disp = (1.0 - weight_infinity) / (sw - swf); k_other_disp = (1.0 - weight_infinity) / (sw - swf);
k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (POINTS_SAMPLE - 1); k_other_ly = (1.0 - weight_infinity) / (sw - swf) / (points_sample - 1);
} else { } else {
k_other_disp = 1.0 / sw; k_other_disp = 1.0 / sw;
k_other_ly = 1.0 / sw / (POINTS_SAMPLE - 1); k_other_ly = 1.0 / sw / (points_sample - 1);
k_inf_disp = k_other_disp; k_inf_disp = k_other_disp;
k_inf_ly = k_other_ly; k_inf_ly = k_other_ly;
} }
...@@ -1248,14 +1349,14 @@ public class ExtrinsicAdjustment { ...@@ -1248,14 +1349,14 @@ public class ExtrinsicAdjustment {
double sd = strength[0][cluster]; double sd = strength[0][cluster];
double sly =strength[1][cluster]; double sly =strength[1][cluster];
if ((force_disparity != null) && force_disparity[cluster]) { if ((force_disparity != null) && force_disparity[cluster]) {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_inf_disp; weights[cluster * points_sample + 0] = sd * k_inf_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_inf_ly; weights[cluster * points_sample + i] = sly * k_inf_ly;
} }
} else { } else {
weights[cluster * POINTS_SAMPLE + 0] = sd * k_other_disp; weights[cluster * points_sample + 0] = sd * k_other_disp;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] = sly * k_other_ly; weights[cluster * points_sample + i] = sly * k_other_ly;
} }
} }
} }
...@@ -1268,10 +1369,10 @@ public class ExtrinsicAdjustment { ...@@ -1268,10 +1369,10 @@ public class ExtrinsicAdjustment {
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){ if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){
double x = cluster % clustersX; double x = cluster % clustersX;
double wd = weights[cluster * POINTS_SAMPLE + 0]; double wd = weights[cluster * points_sample + 0];
double wly = 0; double wly = 0;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
wly += weights[cluster * POINTS_SAMPLE + i]; wly += weights[cluster * points_sample + i];
} }
if (x >= half_width) { if (x >= half_width) {
swdr += wd; swdr += wd;
...@@ -1290,14 +1391,14 @@ public class ExtrinsicAdjustment { ...@@ -1290,14 +1391,14 @@ public class ExtrinsicAdjustment {
if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){ if ((measured_dsxy[cluster] != null) && force_disparity[cluster] && !disable[cluster]){
double x = cluster % clustersX; double x = cluster % clustersX;
if (x >= half_width) { if (x >= half_width) {
weights[cluster * POINTS_SAMPLE + 0] *= kwdr; weights[cluster * points_sample + 0] *= kwdr;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] *= kwlyr; weights[cluster * points_sample + i] *= kwlyr;
} }
} else { } else {
weights[cluster * POINTS_SAMPLE + 0] *= kwdl; weights[cluster * points_sample + 0] *= kwdl;
for (int i = 1; i < POINTS_SAMPLE; i++) { for (int i = 1; i < points_sample; i++) {
weights[cluster * POINTS_SAMPLE + i] *= kwlyl; weights[cluster * points_sample + i] *= kwlyl;
} }
} }
} }
...@@ -1353,7 +1454,7 @@ public class ExtrinsicAdjustment { ...@@ -1353,7 +1454,7 @@ public class ExtrinsicAdjustment {
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices(); Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i) double [] imu = corr_vector.getIMU(); // i)
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE]; double [] y_minus_fx = new double [clusters * points_sample];
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivativesNew( // USED in lwir double [] ddnd = geometryCorrection.getPortsDDNDAndDerivativesNew( // USED in lwir
...@@ -1372,11 +1473,11 @@ public class ExtrinsicAdjustment { ...@@ -1372,11 +1473,11 @@ public class ExtrinsicAdjustment {
//arraycopy(Object src, int srcPos, Object dest, int destPos, int length) //arraycopy(Object src, int srcPos, Object dest, int destPos, int length)
// System.arraycopy(src_pixels, 0, dst_pixels, 0, src_pixels.length); /* for the borders closer to 1/2 kernel size*/ // System.arraycopy(src_pixels, 0, dst_pixels, 0, src_pixels.length); /* for the borders closer to 1/2 kernel size*/
ddnd[0] = ddnd[0]; // ? ddnd[0] = ddnd[0]; // ?
for (int i = 0; i < NUM_SENSORS; i++) { for (int i = 0; i < num_sensors; i++) {
ddnd[i + 1] = ddnd[i + 1]; ddnd[i + 1] = ddnd[i + 1];
ddnd[i + 5] = ddnd[i + 5]; ddnd[i + 5] = ddnd[i + 5];
} }
System.arraycopy(ddnd, 0, y_minus_fx, cluster*POINTS_SAMPLE, POINTS_SAMPLE); System.arraycopy(ddnd, 0, y_minus_fx, cluster*points_sample, points_sample);
} }
} }
return y_minus_fx; return y_minus_fx;
...@@ -1387,7 +1488,7 @@ public class ExtrinsicAdjustment { ...@@ -1387,7 +1488,7 @@ public class ExtrinsicAdjustment {
double delta){ double delta){
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
int num_pars = getNumPars(); int num_pars = getNumPars();
double [][] jt = new double [num_pars][clusters * POINTS_SAMPLE ]; double [][] jt = new double [num_pars][clusters * points_sample ];
double rdelta = 1.0/delta; double rdelta = 1.0/delta;
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
double [] pars = new double[num_pars]; double [] pars = new double[num_pars];
...@@ -1451,18 +1552,18 @@ public class ExtrinsicAdjustment { ...@@ -1451,18 +1552,18 @@ public class ExtrinsicAdjustment {
double [][] jt = getJacobianTransposed(corr_vector); double [][] jt = getJacobianTransposed(corr_vector);
double [][] jt_delta = getJacobianTransposed(corr_vector, delta); double [][] jt_delta = getJacobianTransposed(corr_vector, delta);
double tot_error = 0; double tot_error = 0;
double [][] err = new double [num_pars][POINTS_SAMPLE]; double [][] err = new double [num_pars][points_sample];
double [] err_par = new double [num_pars]; double [] err_par = new double [num_pars];
for (int par = 0; par < num_pars; par++) { for (int par = 0; par < num_pars; par++) {
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){ for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
int indx = cluster * POINTS_SAMPLE+ i; int indx = cluster * points_sample+ i;
if (Math.abs(jt[par][indx] - jt_delta[par][indx]) > err[par][i]) { if (Math.abs(jt[par][indx] - jt_delta[par][indx]) > err[par][i]) {
err[par][i] = Math.abs(jt[par][indx] - jt_delta[par][indx]); err[par][i] = Math.abs(jt[par][indx] - jt_delta[par][indx]);
} }
} }
} }
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
if (err[par][i] > err_par[par]) { if (err[par][i] > err_par[par]) {
err_par[par] = err[par][i]; err_par[par] = err[par][i];
} }
...@@ -1491,14 +1592,14 @@ public class ExtrinsicAdjustment { ...@@ -1491,14 +1592,14 @@ public class ExtrinsicAdjustment {
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 % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / 3) * (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);
int pix = x + y * width; int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode; int indx = cluster * points_sample + mode;
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
dbg_img[3 * par + 0][pix] = jt[par][indx]; dbg_img[3 * par + 0][pix] = jt[par][indx];
...@@ -1522,14 +1623,14 @@ public class ExtrinsicAdjustment { ...@@ -1522,14 +1623,14 @@ 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 % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / 3) * (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);
int pix = x + y * width; int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode; int indx = cluster * points_sample + mode;
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
dbg_img[par][pix] = jt[par][indx]; dbg_img[par][pix] = jt[par][indx];
...@@ -1561,23 +1662,25 @@ public class ExtrinsicAdjustment { ...@@ -1561,23 +1662,25 @@ public class ExtrinsicAdjustment {
int width = 3 * clustersX + 2 * gap; int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap; int height = 3 * clustersY + 2 * 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 % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / 3) * (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);
int pix = x + y * width; int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode; int indx = cluster * points_sample + mode;
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
if (mode ==0) { if (mode ==0) {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF]; dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
dbg_img[1][pix] = -fx[indx]; dbg_img[1][pix] = -fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx]; dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
} else { } else {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1]; // dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1];
dbg_img[0][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1];
dbg_img[1][pix] = -fx[indx]; dbg_img[1][pix] = -fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]; // dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
} }
} else { } else {
dbg_img[ 0][pix] = Double.NaN; dbg_img[ 0][pix] = Double.NaN;
...@@ -1729,9 +1832,10 @@ public class ExtrinsicAdjustment { ...@@ -1729,9 +1832,10 @@ public class ExtrinsicAdjustment {
double [][] jt = getJacobianTransposed(corr_vector); double [][] jt = getJacobianTransposed(corr_vector);
for (int cluster = 0; cluster < clusters; cluster++) if ( measured_dsxy[cluster] != null) { for (int cluster = 0; cluster < clusters; cluster++) if ( measured_dsxy[cluster] != null) {
ers_tilt_az[cluster] = new double [2]; ers_tilt_az[cluster] = new double [2];
for (int i = 0; i < (POINTS_SAMPLE-1); i++) { for (int i = 0; i < (points_sample-1); i++) {
int indx = cluster * POINTS_SAMPLE + i + 1; // skipping disparity int indx = cluster * points_sample + i + 1; // skipping disparity
double d = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + i]; // double d = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + i];
double d = measured_dsxy[cluster][indx_dd0 + i];
if (fx != null) { if (fx != null) {
d += fx[indx]; d += fx[indx];
} }
...@@ -1748,8 +1852,8 @@ public class ExtrinsicAdjustment { ...@@ -1748,8 +1852,8 @@ public class ExtrinsicAdjustment {
double [] cluster_weights = new double [clusters]; double [] cluster_weights = new double [clusters];
double sw = 0.0; double sw = 0.0;
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
for (int i = 0; i < POINTS_SAMPLE; i++) { for (int i = 0; i < points_sample; i++) {
cluster_weights[cluster] += this.weights[cluster * POINTS_SAMPLE + i]; cluster_weights[cluster] += this.weights[cluster * points_sample + i];
sw += cluster_weights[cluster]; sw += cluster_weights[cluster];
} }
} }
...@@ -1908,14 +2012,14 @@ public class ExtrinsicAdjustment { ...@@ -1908,14 +2012,14 @@ public class ExtrinsicAdjustment {
int width = 3 * clustersX + 2 * gap; int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap; int height = 3 * clustersY + 2 * 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 % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / 3) * (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);
int pix = x + y * width; int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode; int indx = cluster * points_sample + mode;
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
if (mode ==0) { if (mode ==0) {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF]; dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
...@@ -1931,13 +2035,17 @@ public class ExtrinsicAdjustment { ...@@ -1931,13 +2035,17 @@ public class ExtrinsicAdjustment {
dbg_img[5][pix] = Double.NaN; dbg_img[5][pix] = Double.NaN;
} }
} else { } else {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1]; // dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1];
dbg_img[0][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1];
dbg_img[1][pix] = -fx[indx]; dbg_img[1][pix] = -fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]; // dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
if (weights[indx] > 0.0) { if (weights[indx] > 0.0) {
dbg_img[3][pix] = weights[indx]*clusters; dbg_img[3][pix] = weights[indx]*clusters;
dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]); // dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]);
dbg_img[5][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx]; // dbg_img[5][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
dbg_img[4][pix] = weights[indx]*clusters*(measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]);
dbg_img[5][pix] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
} else { } else {
dbg_img[3][pix] = Double.NaN; dbg_img[3][pix] = Double.NaN;
dbg_img[4][pix] = Double.NaN; dbg_img[4][pix] = Double.NaN;
...@@ -1976,7 +2084,7 @@ public class ExtrinsicAdjustment { ...@@ -1976,7 +2084,7 @@ public class ExtrinsicAdjustment {
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 % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap); int y0 = (mode / 3) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
...@@ -1984,7 +2092,7 @@ public class ExtrinsicAdjustment { ...@@ -1984,7 +2092,7 @@ public class ExtrinsicAdjustment {
int y = y0 + (cluster / clustersX); int y = y0 + (cluster / clustersX);
int pix = x + y * width; int pix = x + y * width;
if (measured_dsxy[cluster] != null){ if (measured_dsxy[cluster] != null){
if (mode < (2 * NUM_SENSORS)) { if (mode < (2 * num_sensors)) {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode]; dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode];
dbg_img[1][pix] = xy[cluster][mode] - x0y0[cluster][mode]; dbg_img[1][pix] = xy[cluster][mode] - x0y0[cluster][mode];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode] - (xy[cluster][mode] - x0y0[cluster][mode]); dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode] - (xy[cluster][mode] - x0y0[cluster][mode]);
...@@ -1992,9 +2100,9 @@ public class ExtrinsicAdjustment { ...@@ -1992,9 +2100,9 @@ public class ExtrinsicAdjustment {
moving_objects[1][cluster] += dbg_img[1][pix]*dbg_img[0][pix]; moving_objects[1][cluster] += dbg_img[1][pix]*dbg_img[0][pix];
moving_objects[2][cluster] += dbg_img[2][pix]*dbg_img[0][pix]; moving_objects[2][cluster] += dbg_img[2][pix]*dbg_img[0][pix];
} else { } else {
dbg_img[0][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * NUM_SENSORS)); dbg_img[0][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * num_sensors));
dbg_img[1][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * NUM_SENSORS)); dbg_img[1][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * num_sensors));
dbg_img[2][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * NUM_SENSORS)); dbg_img[2][pix] = Math.sqrt(moving_objects[0][cluster]/(2 * num_sensors));
} }
} else { } else {
dbg_img[ 0][pix] = Double.NaN; dbg_img[ 0][pix] = Double.NaN;
...@@ -2026,7 +2134,7 @@ public class ExtrinsicAdjustment { ...@@ -2026,7 +2134,7 @@ public class ExtrinsicAdjustment {
} }
int clusters = clustersX * clustersY; int clusters = clustersX * clustersY;
int num_pars = getNumPars(); int num_pars = getNumPars();
double [][] jt = new double [num_pars][clusters * POINTS_SAMPLE ]; double [][] jt = new double [num_pars][clusters * points_sample ];
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices(); Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i) double [] imu = corr_vector.getIMU(); // i)
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDtt.java
View file @ 624db0c0
...@@ -3334,7 +3334,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3334,7 +3334,7 @@ public class ImageDtt extends ImageDttCPU {
dbg_img[18][nclust] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr; dbg_img[18][nclust] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr;
} }
if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) { if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) {
lazy_eye_data[nclust] = new double [ExtrinsicAdjustment.INDX_LENGTH]; lazy_eye_data[nclust] = new double [ExtrinsicAdjustment.get_INDX_LENGTH(numSensors)];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_STRENGTH] = strengh_k * lma_ds[0][1] * num_in_cluster[clustY][clustX]; lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_STRENGTH] = strengh_k * lma_ds[0][1] * num_in_cluster[clustY][clustX];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_DISP] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr; lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_DISP] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr;
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_TARGET] = (disparity_array_center[clustY][clustX] + disparity_corr); lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_TARGET] = (disparity_array_center[clustY][clustX] + disparity_corr);
...@@ -3342,21 +3342,21 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3342,21 +3342,21 @@ public class ImageDtt extends ImageDttCPU {
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_PX + 0] = pxpy[clustY][clustX][0]; lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_PX + 0] = pxpy[clustY][clustX][0];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_PX + 1] = pxpy[clustY][clustX][1]; lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_PX + 1] = pxpy[clustY][clustX][1];
for (int cam = 0; cam < quad; cam++) { for (int cam = 0; cam < quad; cam++) {
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] = tile_disp_dist[cam][2]; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] = tile_disp_dist[cam][2];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_PYDIST + cam] = clust_pY [clustY][clustX][cam]; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] = clust_pY [clustY][clustX][cam];
} }
for (int cam = 0; cam < ddnd.length; cam++) { for (int cam = 0; cam < ddnd.length; cam++) {
if (ddnd[cam] != null) { //convert to x,y from dd/nd if (ddnd[cam] != null) { //convert to x,y from dd/nd
lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1]; lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0]; lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_DD0 + cam] = ddnd[cam][0]; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = ddnd[cam][0];
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_ND0 + cam] = ddnd[cam][1]; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = ddnd[cam][1];
} else { } else {
lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN; lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN; lazy_eye_data[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_DD0 + cam] = Double.NaN; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = Double.NaN;
lazy_eye_data[nclust][ExtrinsicAdjustment.INDX_ND0 + cam] = Double.NaN; lazy_eye_data[nclust][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = Double.NaN;
} }
} }
} }
...@@ -3652,7 +3652,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3652,7 +3652,7 @@ public class ImageDtt extends ImageDttCPU {
} }
if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) { if ((lma_ds[0] != null) && (lma_ds[0][1]> 0.0)) {
lazy_eye_data_final[nclust] = new double [ExtrinsicAdjustment.INDX_LENGTH]; lazy_eye_data_final[nclust] = new double [ExtrinsicAdjustment.get_INDX_LENGTH(numSensors)];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_STRENGTH] = strengh_k * lma_ds[0][1] * num_in_cluster_final[clustY][clustX]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_STRENGTH] = strengh_k * lma_ds[0][1] * num_in_cluster_final[clustY][clustX];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_DISP] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr; lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_DISP] = lma_ds[0][0] + disparity_array_center[clustY][clustX] + disparity_corr;
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_TARGET] = disparity_array_center[clustY][clustX] + disparity_corr; lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_TARGET] = disparity_array_center[clustY][clustX] + disparity_corr;
...@@ -3660,20 +3660,20 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3660,20 +3660,20 @@ public class ImageDtt extends ImageDttCPU {
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_PX + 0] = pxpy_super[clustY][clustX][0]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_PX + 0] = pxpy_super[clustY][clustX][0];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_PX + 1] = pxpy_super[clustY][clustX][1]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_PX + 1] = pxpy_super[clustY][clustX][1];
for (int cam = 0; cam < quad; cam++) { for (int cam = 0; cam < quad; cam++) {
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] = tile_disp_dist[cam][2]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] = tile_disp_dist[cam][2];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_PYDIST + cam] = clust_pY_super [clustY][clustX][cam]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] = clust_pY_super [clustY][clustX][cam];
} }
for (int cam = 0; cam < ddnd.length; cam++) { for (int cam = 0; cam < ddnd.length; cam++) {
if (ddnd[cam] != null) { //convert to x,y from dd/nd if (ddnd[cam] != null) { //convert to x,y from dd/nd
lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1]; lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0]; lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_DD0 + cam] = ddnd[cam][0]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = ddnd[cam][0];
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_ND0 + cam] = ddnd[cam][1]; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = ddnd[cam][1];
} else { } else {
lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN; lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN; lazy_eye_data_final[nclust][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_DD0 + cam] = Double.NaN; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = Double.NaN;
lazy_eye_data_final[nclust][ExtrinsicAdjustment.INDX_ND0 + cam] = Double.NaN; lazy_eye_data_final[nclust][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = Double.NaN;
} }
} }
} }
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDttCPU.java
View file @ 624db0c0
...@@ -1638,6 +1638,7 @@ public class ImageDttCPU { ...@@ -1638,6 +1638,7 @@ public class ImageDttCPU {
} }
// removing macro and FPGA modes // removing macro and FPGA modes
@Deprecated
public double[][] cltMeasureLazyEye ( // returns d,s lazy eye parameters public double[][] cltMeasureLazyEye ( // returns d,s lazy eye parameters
final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
final int [][] tile_op, // [tilesY][tilesX] - what to do - 0 - nothing for this tile final int [][] tile_op, // [tilesY][tilesX] - what to do - 0 - nothing for this tile
...@@ -2342,7 +2343,7 @@ public class ImageDttCPU { ...@@ -2342,7 +2343,7 @@ public class ImageDttCPU {
double [][] extra_stats = lma2.getTileStats(); double [][] extra_stats = lma2.getTileStats();
// final double [][] lazy_eye_data = new double [clustersY*clustersX][]; // final double [][] lazy_eye_data = new double [clustersY*clustersX][];
// calculate average disparity per cluster using a sum of the disparity_array and the result of the LMA // calculate average disparity per cluster using a sum of the disparity_array and the result of the LMA
lazy_eye_data[nCluster] = new double [ExtrinsicAdjustment.INDX_LENGTH]; lazy_eye_data[nCluster] = new double [ExtrinsicAdjustment.get_INDX_LENGTH(numSensors)];
double sum_w = 0; double sum_w = 0;
for (int cTileY = 0; cTileY < tileStep; cTileY++) { for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ; tileY = clustY * tileStep + cTileY ;
...@@ -2360,8 +2361,8 @@ public class ImageDttCPU { ...@@ -2360,8 +2361,8 @@ public class ImageDttCPU {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] += pxpy[cTile][0] * w; lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] += pxpy[cTile][0] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] += pxpy[cTile][1] * w; lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] += pxpy[cTile][1] * w;
for (int cam = 0; cam < numSensors; cam++) { for (int cam = 0; cam < numSensors; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] += disp_dist[cTile][cam][2] * w; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] += disp_dist[cTile][cam][2] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PYDIST + cam] += centersXY[cTile][cam][1] * w; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] += centersXY[cTile][cam][1] * w;
} }
sum_w += w; sum_w += w;
} }
...@@ -2377,21 +2378,21 @@ public class ImageDttCPU { ...@@ -2377,21 +2378,21 @@ public class ImageDttCPU {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] /= sum_w; lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] /= sum_w; lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] /= sum_w;
for (int cam = 0; cam < numSensors; cam++) { for (int cam = 0; cam < numSensors; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] /= sum_w; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PYDIST + cam] /= sum_w; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] /= sum_w;
} }
for (int cam = 0; cam < ddnd.length; cam++) { for (int cam = 0; cam < ddnd.length; cam++) {
if (ddnd[cam] != null) { //convert to x,y from dd/nd if (ddnd[cam] != null) { //convert to x,y from dd/nd
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1]; lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0]; lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = ddnd[cam][0]; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = ddnd[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = ddnd[cam][1]; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = ddnd[cam][1];
} else { } else {
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN; lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN; lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = Double.NaN; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = Double.NaN; lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = Double.NaN;
} }
} }
} else { } else {
...@@ -2494,29 +2495,855 @@ public class ImageDttCPU { ...@@ -2494,29 +2495,855 @@ public class ImageDttCPU {
return lazy_eye_data; // clt_data; return lazy_eye_data; // clt_data;
} }
public double [][][][][][] clt_aberrations_quad_corr( // 08/28/21 version public double[][] cltMeasureLazyEye ( // returns d,s lazy eye parameters
final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
final int macro_scale, // to correlate tile data instead of the pixel data: 1 - pixels, 8 - tiles
final int [][] tile_op, // [tilesY][tilesX] - what to do - 0 - nothing for this tile final int [][] tile_op, // [tilesY][tilesX] - what to do - 0 - nothing for this tile
final double [][] disparity_array, // [tilesY][tilesX] - individual per-tile expected disparity final double [][] disparity_array, // [tilesY][tilesX] - individual per-tile expected disparity
final double [][][] image_data, // first index - number of image in a quad final double [][][] image_data, // first index - number of image in a quad
final boolean [][] saturation_imp, // (near) saturated pixels or null final boolean [][] saturation_imp, // (near) saturated pixels or null
// correlation results // correlation results - final and partial
// final double [][][][] clt_corr_out, // sparse (by the first index) [type][tilesY][tilesX][(2*transform_size-1)*(2*transform_size-1)] or null
final double [][][][] clt_corr_out, // sparse (by the first index) [type][tilesY][tilesX][(2*transform_size-1)*(2*transform_size-1)] or null
final double [][][][] clt_combo_out, // sparse (by the first index) [type][tilesY][tilesX][(combo_tile_size] or null
final double [][][][] clt_combo_dbg, // generate sparse partial rotated/scaled pairs
// final double [][][][] clt_corr_combo_in, // [type][tilesY][tilesX][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
// [type][tilesY][tilesX] should be set by caller
// types: 0 - selected correlation (product+offset), 1 - sum
// final double [][][][][] clt_corr_partial,// [tilesY][tilesX][quad]color][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
// [tilesY][tilesX] should be set by caller
// When clt_mismatch is non-zero, no far objects extraction will be attempted // When clt_mismatch is non-zero, no far objects extraction will be attempted
// clt_mismatch is used in older code, not supported in GPU - there is cltMeasureLazyEye for that purpose // 3 * numSensors
// final double [][] clt_mismatch, // [12][tilesY * tilesX] // ***** transpose unapplied ***** ?. null - do not calculate final double [][] clt_mismatch, // [3 * numSensors][tilesY * tilesX] // ***** transpose unapplied ***** ?. null - do not calculate
// values in the "main" directions have disparity (*_CM) subtracted, in the perpendicular - as is // values in the "main" directions have disparity (*_CM) subtracted, in the perpendicular - as is
final double [][] disparity_map, // [8][tilesY][tilesX], only [6][] is needed on input or null - do not calculate
// last 2 - contrast, avg/ "geometric average)
final int width,
final double corr_fat_zero, // add to denominator to modify phase correlation (same units as data1, data2). <0 - pure sum
final double corr_red,
final double corr_blue,
final double corr_sigma,
final double min_corr, // 0.02; // minimal correlation value to consider valid
final GeometryCorrection geometryCorrection,
final GeometryCorrection geometryCorrection_main, // if not null correct this camera (aux) to the coordinates of the main
final double [][][][][][] clt_kernels, // [channel_in_quad][color][tileY][tileX][band][pixel] , size should match image (have 1 tile around)
final int kernel_step,
final int window_type,
final double [][] shiftXY, // [port]{shiftX,shiftY}
final double disparity_corr, // disparity at infinity
final double shiftX, // shift image horizontally (positive - right) - just for testing
final double shiftY, // shift image vertically (positive - down)
final int tileStep, // process tileStep x tileStep cluster of tiles when adjusting lazy eye parameters
final int mcorr_comb_width, // combined correlation tile width
final int mcorr_comb_height, // combined correlation tile full height
final int mcorr_comb_offset, // combined correlation tile height offset: 0 - centered (-height/2 to height/2), height/2 - only positive (0 to height)
final double mcorr_comb_disp, // Combined tile per-pixel disparity for baseline == side of a square
final int debug_tileX,
final int debug_tileY,
final int threadsMax, // maximal number of threads to launch
final int globalDebugLevel)
{
final boolean debug_distort= (globalDebugLevel >0); // .false; // true;
final double [][] debug_offsets = new double[imgdtt_params.lma_dbg_offset.length][2];
for (int i = 0; i < debug_offsets.length; i++) for (int j = 0; j < debug_offsets[i].length; j++) {
debug_offsets[i][j] = imgdtt_params.lma_dbg_offset[i][j]*imgdtt_params.lma_dbg_scale;
}
final int numcol = 3; // number of colors // keep the same, just do not use [0] and [1], [2] - green
final int height=image_data[0][0].length/width;
final int tilesX=width/transform_size;
final int tilesY=height/transform_size;
final int clustersX= (tilesX + tileStep - 1) / tileStep;
final int clustersY= (tilesY + tileStep - 1) / tileStep;
final double [][] lazy_eye_data = new double [clustersY*clustersX][];
final int nClustersInChn=clustersX * clustersY;
final int clustSize = tileStep*tileStep;
final int debug_clustX = debug_tileX / tileStep;
final int debug_clustY = debug_tileY / tileStep;
///tileStep
final double [][][][][][] clt_data = new double[numSensors][numcol][tilesY][tilesX][][];
final Thread[] threads = newThreadArray(threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final double [] col_weights= new double [numcol]; // colors are RBG
final double [][] dbg_distort = debug_distort? (new double [4*numSensors][tilesX*tilesY]) : null;
final double [][] corr_wnd = Corr2dLMA.getCorrWnd(
transform_size,
imgdtt_params.lma_wnd);
final double [] corr_wnd_inv_limited = (imgdtt_params.lma_min_wnd <= 1.0)? new double [corr_wnd.length * corr_wnd[0].length]: null;
if (corr_wnd_inv_limited != null) {
double inv_pwr = imgdtt_params.lma_wnd_pwr - (imgdtt_params.lma_wnd - 1.0); // compensate for lma_wnd
for (int i = imgdtt_params.lma_hard_marg; i < (corr_wnd.length - imgdtt_params.lma_hard_marg); i++) {
for (int j = imgdtt_params.lma_hard_marg; j < (corr_wnd.length - imgdtt_params.lma_hard_marg); j++) {
corr_wnd_inv_limited[i * (corr_wnd.length) + j] = 1.0/Math.max(Math.pow(corr_wnd[i][j],
inv_pwr),
imgdtt_params.lma_min_wnd);
}
}
}
if (clt_mismatch != null){
for (int i = 0; i<clt_mismatch.length;i++){
clt_mismatch[i] = new double [tilesY*tilesX]; // will use only "center of mass" centers
for (int j = 0; j < clt_mismatch[i].length; j++) {
clt_mismatch[i][j] = Double.NaN;
}
}
}
// keep for now for mono, find out what do they mean for macro mode
if (isMonochrome()) {
col_weights[2] = 1.0;// green color/mono
col_weights[0] = 0;
col_weights[1] = 0;
} else {
col_weights[2] = 1.0/(1.0 + corr_red + corr_blue); // green color
col_weights[0] = corr_red * col_weights[2];
col_weights[1] = corr_blue * col_weights[2];
}
final int corr_size = transform_size * 2 -1;
if ((globalDebugLevel > -10) && (disparity_corr != 0.0)){
System.out.println(String.format("Using manual infinity disparity correction of %8.5f pixels",disparity_corr));
}
final double [] enh_ortho_scale = new double [corr_size];
for (int i = 0; i < corr_size; i++){
if ((i < (transform_size - imgdtt_params.getEnhOrthoWidth(isAux()))) || (i > (transform_size - 2 + imgdtt_params.getEnhOrthoWidth(isAux())))) {
enh_ortho_scale[i] = 1.0;
} else {
enh_ortho_scale[i] = imgdtt_params.getEnhOrthoScale(isAux());
}
if (i == (transform_size-1)) enh_ortho_scale[i] = 0.0 ; // hardwired 0 in the center
enh_ortho_scale[i] *= Math.sin(Math.PI*(i+1.0)/(2*transform_size));
}
if (globalDebugLevel > 1){
System.out.println("getEnhOrthoWidth(isAux())="+ imgdtt_params.getEnhOrthoWidth(isAux())+" getEnhOrthoScale(isAux())="+ imgdtt_params.getEnhOrthoScale(isAux()));
for (int i = 0; i < corr_size; i++){
System.out.println(" enh_ortho_scale["+i+"]="+ enh_ortho_scale[i]);
}
}
// Create window to select center correlation strip using
// ortho_height - full width of non-zero elements
// ortho_eff_height - effective height (ration of the weighted column sum to the center value)
int wcenter = transform_size - 1;
final double [] ortho_weights = new double [corr_size]; // [15]
for (int i = 0; i < corr_size; i++){
if ((i >= wcenter - imgdtt_params.ortho_height/2) && (i <= wcenter + imgdtt_params.ortho_height/2)) {
double dx = 1.0*(i-wcenter)/(imgdtt_params.ortho_height/2 + 1);
ortho_weights[i] = 0.5*(1.0+Math.cos(Math.PI*dx))/imgdtt_params.ortho_eff_height;
}
}
if (globalDebugLevel > 0){
System.out.println("ortho_height="+ imgdtt_params.ortho_height+" ortho_eff_height="+ imgdtt_params.ortho_eff_height);
for (int i = 0; i < corr_size; i++){
System.out.println(" ortho_weights["+i+"]="+ ortho_weights[i]);
}
}
if (globalDebugLevel > 0) {
System.out.println("clt_aberrations_quad_corr(): width="+width+" height="+height+" transform_size="+transform_size+
" debug_tileX="+debug_tileX+" debug_tileY="+debug_tileY+" globalDebugLevel="+globalDebugLevel);
}
final double [] filter = doubleGetCltLpfFd(corr_sigma);
// prepare disparity maps and weights
if (globalDebugLevel > 0){
System.out.println("corr_fat_zero= "+corr_fat_zero);
System.out.println("disparity_array[0][0]= "+disparity_array[0][0]);
}
// add optional initialization of debug layers here
if (disparity_map != null){
for (int i = 0; i<disparity_map.length;i++){
if (i == OVEREXPOSED) {
if (saturation_imp!= null) {
disparity_map[i] = new double [tilesY*tilesX];
}
} else if (isSliceBit(i)) { // includes diffs?
disparity_map[i] = new double [tilesY*tilesX];
}
}
}
DttRad2 dtt = new DttRad2(transform_size);
dtt.set_window(window_type);
final double [] lt_window = dtt.getWin2d(); // [256]
final double [] lt_window2 = new double [lt_window.length]; // squared
for (int i = 0; i < lt_window.length; i++) lt_window2[i] = lt_window[i] * lt_window[i];
if (globalDebugLevel > 1) {
ShowDoubleFloatArrays sdfa_instance = new ShowDoubleFloatArrays(); // just for debugging?
sdfa_instance.showArrays(lt_window, 2*transform_size, 2*transform_size, "lt_window");
}
Matrix [] corr_rots_aux = null;
Matrix [][] deriv_rots_aux = null;
if (geometryCorrection_main != null) {
corr_rots_aux = geometryCorrection.getCorrVector().getRotMatrices(geometryCorrection.getRotMatrix(true));
deriv_rots_aux = geometryCorrection.getCorrVector().getRotDeriveMatrices();
}
final boolean use_main = corr_rots_aux != null;
final Matrix [] corr_rots = use_main ? corr_rots_aux : geometryCorrection.getCorrVector().getRotMatrices(); // get array of per-sensor rotation matrices
final Matrix [][] deriv_rots = use_main ? deriv_rots_aux : geometryCorrection.getCorrVector().getRotDeriveMatrices();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
DttRad2 dtt = new DttRad2(transform_size);
dtt.set_window(window_type);
int tileY,tileX, clustX, clustY, cTile, tIndex; // , chn;
// showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
double centerX; // center of aberration-corrected (common model) tile, X
double centerY; //
double [][] fract_shiftsXY = new double[numSensors][];
Correlation2d corr2d = new Correlation2d(
numSensors,
imgdtt_params, // ImageDttParameters imgdtt_params,
transform_size, // int transform_size,
2.0, // double wndx_scale, // (wndy scale is always 1.0)
isMonochrome(), // boolean monochrome,
(globalDebugLevel > -1)); // boolean debug)
corr2d.createOrtoNotch(
imgdtt_params.getEnhOrthoWidth(isAux()), // double getEnhOrthoWidth(isAux()),
imgdtt_params.getEnhOrthoScale(isAux()), //double getEnhOrthoScale(isAux()),
(imgdtt_params.lma_debug_level > 1)); // boolean debug);
double [][] rXY;
if (use_main) {
rXY = geometryCorrection.getRXY(true); // boolean use_rig_offsets,
} else {
rXY = geometryCorrection.getRXY(false); // boolean use_rig_offsets,
}
// for (int nTile = ai.getAndIncrement(); nTile < nTilesInChn; nTile = ai.getAndIncrement()) {
for (int nCluster = ai.getAndIncrement(); nCluster < nClustersInChn; nCluster = ai.getAndIncrement()) {
clustY = nCluster / clustersX;
clustX = nCluster % clustersX;
double [][][] centersXY = new double [clustSize][][];
double [][][] disp_dist = new double[clustSize][numSensors][]; // used to correct 3D correlations
double [][][] corrs = new double [clustSize][][];
double [][] corr_stat = new double [clustSize][];
double [] strength = new double [clustSize];
double [][] disp_str = new double [clustSize][];
double [][] pxpy = new double [clustSize][2];
boolean debugCluster = (clustX == debug_clustX) && (clustY == debug_clustY);
if (debugCluster) {
System.out.println("debugCluster");
}
int clust_lma_debug_level = debugCluster? imgdtt_params.lma_debug_level : -5;
// filter only tiles with similar disparity to enable lazy eye for the ERS.
int num_good_tiles = 0;
while (true) {
// num_good_tiles = 0; // FIXME: Was missing - uncomment?
int mnTx = -1, mnTy = -1, mxTx = -1, mxTy = -1;
double mn = Double.NaN;
double mx = Double.NaN;
double avg= 0.0;
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
for (int cTileX = 0; cTileX < tileStep; cTileX++) {
tileX = clustX * tileStep + cTileX ;
if ((tileX < tilesX) && (tile_op[tileY][tileX] != 0)) {
double d = disparity_array [tileY][tileX];
avg += d;
if (!(d <= mx)) {
mx = d;
mxTx = tileX;
mxTy = tileY;
}
if (!(d >= mn)) {
mn = d;
mnTx = tileX;
mnTy = tileY;
}
num_good_tiles++;
}
}
}
}
if (num_good_tiles ==0) {
break;
}
if ((mx-mn) <= imgdtt_params.lma_disp_range ) {
break;
}
avg /= num_good_tiles;
if ((mx-avg) > (avg-mn)) {
tile_op[mxTy][mxTx] = 0;
} else {
tile_op[mnTy][mnTx] = 0;
}
//imgdtt_params.lma_disp_range
}
if (num_good_tiles == 0) {
// fill out empty ...
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
for (int cTileX = 0; cTileX < tileStep; cTileX++) {
tileX = clustX * tileStep + cTileX ;
if (tileX < tilesX) {
cTile = cTileY * tileStep + cTileX;
tIndex = tileY * tilesX + tileX;
// int nTile = tileY * tilesX + tileX; // how is it different from tIndex?
if (clt_mismatch != null) {
for (int cam = 0; cam < numSensors; cam++) {
clt_mismatch[3*cam + 0][tIndex] = Double.NaN;
clt_mismatch[3*cam + 1][tIndex] = Double.NaN;
}
}
}
}
}
}
continue;
}
// num_good_tiles in a cluster > 0
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
for (int cTileX = 0; cTileX < tileStep; cTileX++) {
tileX = clustX * tileStep + cTileX ;
if (tileX < tilesX) {
cTile = cTileY * tileStep + cTileX;
tIndex = tileY * tilesX + tileX;
int nTile = tileY * tilesX + tileX; // how is it different from tIndex?
if (tile_op[tileY][tileX] == 0) {
disp_str[cTile] = null;
continue; // nothing to do for this tile
}
boolean debugTile =(tileX == debug_tileX) && (tileY == debug_tileY);
final int [] overexp_all = (saturation_imp != null) ? ( new int [2]): null;
// Moved from inside chn loop
centerX = tileX * transform_size + transform_size/2 - shiftX;
centerY = tileY * transform_size + transform_size/2 - shiftY;
// TODO: move port coordinates out of color channel loop
// double [][] centersXY;
// double [][] disp_dist = new double[quad][]; // used to correct 3D correlations
if ( (disparity_array == null) ||
(disparity_array[tileY] == null) ||
(Double.isNaN(disparity_array[tileY][tileX]))) {
System.out.println("Bug with disparity_array !!!");
continue; // nothing to do for this tile
}
if ((globalDebugLevel > -2) && debugTile) {
System.out.println ("Calculating offsets");
}
if (use_main) { // this is AUX camera that uses main coordinates // not used in lwir
centersXY[cTile] = geometryCorrection.getPortsCoordinatesAndDerivatives(
geometryCorrection_main, // GeometryCorrection gc_main,
true, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] pXYderiv, // if not null, should be double[8][]
disp_dist[cTile], // used to correct 3D correlations
centerX,
centerY,
disparity_array[tileY][tileX] + disparity_corr); // _aux); // + disparity_corr);
} else { // used in lwir
centersXY[cTile] = geometryCorrection.getPortsCoordinatesAndDerivatives(
geometryCorrection, // GeometryCorrection gc_main,
false, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] pXYderiv, // if not null, should be double[8][]
disp_dist[cTile], // used to correct 3D correlations
centerX,
centerY,
disparity_array[tileY][tileX] + disparity_corr);
}
pxpy[cTile][0] = centerX;
pxpy[cTile][1] = centerY;
if ((globalDebugLevel > 0) || debug_distort ||
(debugTile && (globalDebugLevel > -2))) {
for (int i = 0; i < numSensors; i++) {
System.out.println("clt_aberrations_quad_corr(): tileX="+tileX+", tileY="+tileY+
" centerX="+centerX+" centerY="+centerY+" disparity="+disparity_array[tileY][tileX]+
" centersXY["+cTile+"]["+i+"][0]="+centersXY[cTile][i][0]+" centersXY["+cTile+"]["+i+"][1]="+centersXY[cTile][i][1]);
}
}
if (debug_offsets != null) {
double [][] debug_offsets_xy = new double [debug_offsets.length][2];
for (int i = 0; i < debug_offsets.length; i++) {
debug_offsets_xy[i][0] = disp_dist[cTile][i][0] * debug_offsets[i][0] + disp_dist[cTile][i][1] * debug_offsets[i][1];
debug_offsets_xy[i][1] = disp_dist[cTile][i][2] * debug_offsets[i][0] + disp_dist[cTile][i][3] * debug_offsets[i][1];
}
for (int i = 0; i < debug_offsets.length; i++) {
centersXY[cTile][i][0] += debug_offsets_xy[i][0];
centersXY[cTile][i][1] += debug_offsets_xy[i][1];
}
if ((debug_distort && debugCluster) || debugTile) {
for (int i = 0; i < numSensors; i++) {
System.out.println(String.format("%d: {%8.3f, %8.3f}",i,debug_offsets_xy[i][0],debug_offsets_xy[i][1]));
}
for (int i = 0; i < numSensors; i++) {
System.out.println("Corrected clt_aberrations_quad_corr(): tileX="+tileX+", tileY="+tileY+
" centerX="+centerX+" centerY="+centerY+" disparity="+disparity_array[tileY][tileX]+
" centersXY["+cTile+"]["+i+"][0]="+centersXY[cTile][i][0]+" centersXY["+cTile+"]["+i+"][1]="+centersXY[cTile][i][1]);
}
}
}
if ((globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY)) { // before correction
System.out.print(disparity_array[tileY][tileX]+"\t"+
centersXY[cTile][0][0]+"\t"+centersXY[cTile][0][1]+"\t"+
centersXY[cTile][1][0]+"\t"+centersXY[cTile][1][1]+"\t"+
centersXY[cTile][2][0]+"\t"+centersXY[cTile][2][1]+"\t"+
centersXY[cTile][3][0]+"\t"+centersXY[cTile][3][1]+"\t");
}
for (int ip = 0; ip < centersXY[cTile].length; ip++){
centersXY[cTile][ip][0] -= shiftXY[ip][0];
centersXY[cTile][ip][1] -= shiftXY[ip][1];
}
// save disparity distortions for visualization:
if (dbg_distort != null) {
for (int cam = 0; cam <numSensors; cam++) {
dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[cTile][cam][0];
dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[cTile][cam][1];
dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[cTile][cam][2];
dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[cTile][cam][3];
}
}
for (int ncol = 0; ncol <numcol; ncol++) {
if (!isMonochrome() || (ncol == MONO_CHN)) { // in monochrome mode skip all non-mono (green) channels // used in lwir (5 of 6 branches)
if ((globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY) && (ncol == 2)) {
System.out.println("\nUsing "+"PIXEL"+" mode, centerX="+centerX+", centerY="+centerY);
System.out.println(disparity_array[tileY][tileX]+"\t"+
centersXY[cTile][0][0]+"\t"+centersXY[cTile][0][1]+"\t"+
centersXY[cTile][1][0]+"\t"+centersXY[cTile][1][1]+"\t"+
centersXY[cTile][2][0]+"\t"+centersXY[cTile][2][1]+"\t"+
centersXY[cTile][3][0]+"\t"+centersXY[cTile][3][1]+"\t");
}
for (int i = 0; i < numSensors; i++) {
clt_data[i][ncol][tileY][tileX] = new double [4][];
// Extract image tiles and kernels, correct aberrations, return (ut do not apply) fractional shifts
fract_shiftsXY[i] = extract_correct_tile( // return a pair of residual offsets
image_data[i],
width, // image width
((clt_kernels == null) ? null : clt_kernels[i]), // [color][tileY][tileX][band][pixel]
clt_data[i][ncol][tileY][tileX], //double [][] clt_tile, // should be double [4][];
kernel_step,
// transform_size,
dtt,
ncol,
centersXY[cTile][i][0], // centerX, // center of aberration-corrected (common model) tile, X
centersXY[cTile][i][1], // centerY, //
((!FPGA_COMPARE_DATA && (globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY) && (ncol == 2) && (i==0)) ? (globalDebugLevel + 0) : 0), // external tile compare
false, // no_deconvolution,
false, // ); // transpose);
((saturation_imp != null) ? saturation_imp[i] : null), //final boolean [][] saturation_imp, // (near) saturated pixels or null
((saturation_imp != null) ? overexp_all: null)); // final double [] overexposed)
} // for (int i = 0; i < quad; i++)
if ((globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY) && (ncol == 2)) {
System.out.println();
}
if ((globalDebugLevel > 0) && (debug_tileX == tileX) && (debug_tileY == tileY) && (ncol == 2) && !FPGA_COMPARE_DATA) {
ShowDoubleFloatArrays sdfa_instance = new ShowDoubleFloatArrays(); // just for debugging?
String [] titles = {"CC0","SC0","CS0","SS0","CC1","SC1","CS1","SS1","CC2","SC2","CS2","SS2","CC3","SC3","CS3","SS3"};
double [][] dbg_tile = new double [16][];
for (int i = 0; i < 16; i++) dbg_tile[i]=clt_data[i>>2][ncol][tileY][tileX][i & 3];
sdfa_instance.showArrays(dbg_tile, transform_size, transform_size, true, "pre-shifted_x"+tileX+"_y"+tileY, titles);
}
if ((globalDebugLevel > 0) && (tileX >= debug_tileX - 2) && (tileX <= debug_tileX + 2) &&
(tileY >= debug_tileY - 2) && (tileY <= debug_tileY+2)) {
for (int i = 0; i < numSensors; i++) {
System.out.println("clt_aberrations_quad(): color="+ncol+", tileX="+tileX+", tileY="+tileY+
" fract_shiftsXY["+i+"][0]="+fract_shiftsXY[i][0]+" fract_shiftsXY["+i+"][1]="+fract_shiftsXY[i][1]);
}
}
// apply residual shift
for (int i = 0; i < numSensors; i++) {
fract_shift( // fractional shift in transform domain. Currently uses sin/cos - change to tables with 2? rotations
clt_data[i][ncol][tileY][tileX], // double [][] clt_tile,
// transform_size,
fract_shiftsXY[i][0], // double shiftX,
fract_shiftsXY[i][1], // double shiftY,
// (globalDebugLevel > 0) && (tileX == debug_tileX) && (tileY == debug_tileY)); // external tile compare
((globalDebugLevel > 1) &&
((ncol==0) || isMonochrome()) &&
(tileX >= debug_tileX - 2) && (tileX <= debug_tileX + 2) &&
(tileY >= debug_tileY - 2) && (tileY <= debug_tileY+2)));
}
if ((globalDebugLevel > 0) && (debug_tileX == tileX) && (debug_tileY == tileY)) {
ShowDoubleFloatArrays sdfa_instance = new ShowDoubleFloatArrays(); // just for debugging?
String [] titles = {"CC0","SC0","CS0","SS0","CC1","SC1","CS1","SS1","CC2","SC2","CS2","SS2","CC3","SC3","CS3","SS3"};
double [][] dbg_tile = new double [16][];
for (int i = 0; i < 16; i++) dbg_tile[i]=clt_data[i>>2][ncol][tileY][tileX][i & 3];
sdfa_instance.showArrays(dbg_tile, transform_size, transform_size, true, "shifted_x"+tileX+"_y"+tileY+"-z", titles);
}
} else { // if (!isMonochrome() || (chn == MONO_CHN) || macro_mode) { // in monochrome mode skip all non-mono (green) channels
for (int i = 0; i < numSensors; i++) { // used in lwir
clt_data[i][ncol] = null; // erase unused clt_data
}
}
}// end of for (int chn = 0; chn <numcol; chn++)
// used in lwir
int tile_lma_debug_level = ((tileX == debug_tileX) && (tileY == debug_tileY))? imgdtt_params.lma_debug_level : -1;
// all color channels are done here
if (disparity_map != null){
for (int i = 0; i < disparity_map.length; i++) {
if (disparity_map[i] != null) disparity_map[i][nTile] = (
(i == DISPARITY_STRENGTH_INDEX) ||
(i == DISPARITY_INDEX_HOR_STRENGTH) ||
(i == DISPARITY_INDEX_VERT_STRENGTH)) ? 0.0 : Double.NaN; // once and for all
}
// calculate overexposed fraction
if (saturation_imp != null){
disparity_map[OVEREXPOSED][nTile] = (1.0 * overexp_all[0]) / overexp_all[1];
}
}
// calculate all selected pairs correlations
int all_pairs = imgdtt_params.dbg_pair_mask; //TODO: use tile tasks
corrs[cTile] = corr2d.correlateCompositeFD( // now works with nulls for some clt_data colors
clt_data, // double [][][][][][] clt_data,
tileX, // int tileX,
tileY, // int tileY,
all_pairs, // int pairs_mask,
filter, // double [] lpf,
getScaleStrengths(), // double scale_value, // scale correlation value
col_weights, // double [] col_weights,
corr_fat_zero); // double fat_zero)
// calculate interpolated "strips" to match different scales and orientations (ortho/diagonal) on the
// fine (0.5 pix) grid. ortho for scale == 1 provide even/even samples (1/4 of all), diagonal ones -
// checkerboard pattern
double [][] strips = corr2d.scaleRotateInterpoateCorrelations(
corrs[cTile], // double [][] correlations,
all_pairs, // int pairs_mask,
imgdtt_params.corr_strip_hight, //); // int hwidth);
(tile_lma_debug_level > 0) ? all_pairs:0); // debugMax);
// Combine strips for selected pairs. Now using only for all available pairs.
// Other combinations are used only if requested (clt_corr_partial != null)
double [] strip_combo = corr2d.combineInterpolatedCorrelations(
strips, // double [][] strips,
all_pairs, // int pairs_mask,
imgdtt_params.corr_offset, // double offset);
imgdtt_params.twice_diagonal); // boolean twice_diagonal)
// calculate CM maximums for all mixed channels
// First get integer correlation center, relative to the center
int [] ixy = corr2d.getMaxXYInt( // find integer pair or null if below threshold
strip_combo, // double [] data,
true, // boolean axis_only,
imgdtt_params.min_corr, // double minMax, // minimal value to consider (at integer location, not interpolated)
tile_lma_debug_level > 0); // boolean debug);
// double [] corr_stat = null;
// if integer argmax was strong enough, calculate CM argmax
// will not fill out DISPARITY_INDEX_INT+1, DISPARITY_INDEX_CM+1, DISPARITY_INDEX_POLY+1
// use clt_mismatch for that
// double strength = 0.0;
// double disparity = 0.0;
if (ixy != null) {
strength[cTile] = strip_combo[ixy[0]+transform_size-1]; // strength at integer max on axis
if (disparity_map != null){
disparity_map[DISPARITY_INDEX_INT][tIndex] = -ixy[0];
disparity_map[DISPARITY_STRENGTH_INDEX][tIndex] = strength[cTile];
if (Double.isNaN(disparity_map[DISPARITY_STRENGTH_INDEX][tIndex])) {
System.out.println("BUG: 1. disparity_map[DISPARITY_STRENGTH_INDEX]["+tIndex+"] should not be NaN");
}
}
corr_stat[cTile] = corr2d.getMaxXCm( // get fractional center as a "center of mass" inside circle/square from the integer max
strip_combo, // double [] data, // [data_size * data_size]
ixy[0], // int ixcenter, // integer center x
// corr_wndy, // double [] window_y, // (half) window function in y-direction(perpendicular to disparity: for row0 ==1
// corr_wndx, // double [] window_x, // half of a window function in x (disparity) direction
(tile_lma_debug_level > 0)); // boolean debug);
}
// proceed only if CM correlation result is non-null // for compatibility with old code we need it to run regardless of the strength of the normal correlation
if (disparity_map != null){
if (corr_stat[cTile] != null) {
disparity_map[DISPARITY_INDEX_CM][tIndex] = -corr_stat[cTile][0]; // disp_str[cTile][0]; // disparity is negative X
disparity_map[DISPARITY_INDEX_INT+1][tIndex] = -corr_stat[cTile][0]/.85 + disparity_array[tileY][tileX] + disparity_corr; // disp_str[cTile][0]; // disparity is negative X
}
// hack: reuse/overwrite for target disparity
disparity_map[DISPARITY_INDEX_INT][tIndex] = disparity_array[tileY][tileX] + disparity_corr;
}
disp_str[cTile] = new double[2];
if (tile_lma_debug_level > 0) {
System.out.println("Will run getMaxXSOrtho( ) for tileX="+tileX+", tileY="+tileY);
}
// debug new LMA correlations
int tdl = debugCluster ? tile_lma_debug_level : -3;
if (true) { // debugCluster1) {
if (debugCluster && (globalDebugLevel > -1)) { // -2)) {
System.out.println("Will run new LMA for tileX="+tileX+", tileY="+tileY);
}
double [] poly_disp = {Double.NaN, 0.0};
Corr2dLMA lma2 = corr2d.corrLMA2Single(
imgdtt_params, // ImageDttParameters imgdtt_params,
false, // boolean adjust_ly, // adjust Lazy Eye
corr_wnd, // double [][] corr_wnd, // correlation window to save on re-calculation of the window
corr_wnd_inv_limited, // corr_wnd_limited, // correlation window, limited not to be smaller than threshold - used for finding max/convex areas (or null)
corrs[cTile], // double [][] corrs,
disp_dist[cTile],
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
corr2d.longToArray(imgdtt_params.dbg_pair_mask), // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
null, // disp_str[cTile], //corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
poly_disp, // double[] poly_ds, // null or pair of disparity/strength
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tdl, // tile_lma_debug_level, //+2, // int debug_level,
tileX, // int tileX, // just for debug output
tileY); // int tileY
disp_str[cTile] = null;
if (disparity_map != null){
disparity_map[DISPARITY_INDEX_HOR][tIndex] = poly_disp[0];
disparity_map[DISPARITY_INDEX_HOR_STRENGTH][tIndex] = poly_disp[1];
}
if (lma2 != null) {
disp_str[cTile] = lma2.lmaDisparityStrength(
imgdtt_params.lmas_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lmas_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
imgdtt_params.lmas_min_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
imgdtt_params.lmas_min_min_ac, // minimal of A and C coefficients minimum (measures sharpest point)
imgdtt_params.lmas_max_area, // double lma_max_area, // maximal half-area (if > 0.0)
imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
imgdtt_params.lma_str_offset // convert lma-generated strength to match previous ones - add to result
)[0];
if (tile_lma_debug_level > 0) {
double [][] ds_dbg = {disp_str[cTile]};
lma2.printStats(ds_dbg,1);
}
if (disparity_map != null){
if (disp_str[cTile] != null) {
disparity_map[DISPARITY_INDEX_POLY][tIndex] = disp_str[cTile][0];
disparity_map[DISPARITY_INDEX_POLY+1][tIndex] = disp_str[cTile][1];
}
}
}
}
//} // end of if (corr_stat != null)
// } // if (disparity_map != null){ // not null - calculate correlations
// only debug is left
}
}
}
} //for (int cTileY = 0; cTileY < tileStep; cTileY++) {
// above - scanned each tile in a cluster
if (true) { //debugCluster1) {
if (globalDebugLevel > 0) {
System.out.println("Will run new LMA for clustX="+clustX+", clustY="+clustY);
}
Corr2dLMA lma2 = corr2d.corrLMA2Multi(
imgdtt_params, // ImageDttParameters imgdtt_params,
tileStep, // int clust_width,
corr_wnd, // double [][] corr_wnd, // correlation window to save on re-calculation of the window
corr_wnd_inv_limited, // corr_wnd_inv_limited, // correlation window, limited not to be smaller than threshold - used for finding max/convex areas (or null)
corrs, // [tIndex], // double [][] corrs,
disp_dist, // [tIndex],
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
corr2d.longToArray(imgdtt_params.dbg_pair_mask), // imgdtt_params.dbg_pair_mask // int pair_mask, // which pairs to process
disp_str, // corr_stat, // double[][] xcenter_str, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
clust_lma_debug_level + 0, // 2, // int debug_level, // for a single cluster
clustX, // int tileX, // just for debug output
clustY ); // int tileY
if (lma2 != null) {
double [][] ddnd = lma2.getDdNd();
double [] stats = lma2.getStats(num_good_tiles);
double [][] lma_ds = lma2.lmaDisparityStrength(
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
imgdtt_params.lma_min_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
imgdtt_params.lma_min_min_ac, // minimal of A and C coefficients minimum (measures sharpest point)
imgdtt_params.lma_max_area, //double lma_max_area, // maximal half-area (if > 0.0)
1.0, // imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
0.0); // imgdtt_params.lma_str_offset); // convert lma-generated strength to match previous ones - add to result
double [][] extra_stats = lma2.getTileStats();
// final double [][] lazy_eye_data = new double [clustersY*clustersX][];
// calculate average disparity per cluster using a sum of the disparity_array and the result of the LMA
lazy_eye_data[nCluster] = new double [ExtrinsicAdjustment.get_INDX_LENGTH(numSensors)];
double sum_w = 0;
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
for (int cTileX = 0; cTileX < tileStep; cTileX++) {
tileX = clustX * tileStep + cTileX ;
if (tileX < tilesX) {
cTile = cTileY * tileStep + cTileX;
tIndex = tileY * tilesX + tileX;
if ((lma_ds[cTile] != null) && (lma_ds[cTile][1]> 0.0)) {
double w = lma_ds[cTile][1];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP] += (lma_ds[cTile][0] + disparity_array[tileY][tileX] + disparity_corr) * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] += (disparity_array[tileY][tileX] + disparity_corr) * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF] += lma_ds[cTile][0] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] += pxpy[cTile][0] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] += pxpy[cTile][1] * w;
for (int cam = 0; cam < numSensors; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] += disp_dist[cTile][cam][2] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] += centersXY[cTile][cam][1] * w;
}
sum_w += w;
}
}
}
}
}
if (sum_w > 0.0) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_STRENGTH] = stats[0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] /= sum_w;
for (int cam = 0; cam < numSensors; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DYDDISP0(numSensors) + cam] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_PYDIST(numSensors) + cam] /= sum_w;
}
for (int cam = 0; cam < ddnd.length; cam++) {
if (ddnd[cam] != null) { //convert to x,y from dd/nd
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = ddnd[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = ddnd[cam][1];
} else {
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_DD0(numSensors) + cam] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.get_INDX_ND0(numSensors) + cam] = Double.NaN;
}
}
} else {
lazy_eye_data[nCluster] = null;
}
// just for debugging, can be removed
if (disparity_map != null){
double [][] lma2_ds = lma2.lmaDisparityStrength(
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
imgdtt_params.lma_min_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
imgdtt_params.lma_min_min_ac, // minimal of A and C coefficients minimum (measures sharpest point)
imgdtt_params.lma_max_area, //double lma_max_area, // maximal half-area (if > 0.0)
imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
imgdtt_params.lma_str_offset); // convert lma-generated strength to match previous ones - add to result
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
for (int cTileX = 0; cTileX < tileStep; cTileX++) {
tileX = clustX * tileStep + cTileX ;
if (tileX < tilesX) {
cTile = cTileY * tileStep + cTileX;
tIndex = tileY * tilesX + tileX;
// int nTile = tileY * tilesX + tileX; // how is it different from tIndex?
for (int cam = 0; cam < ddnd.length; cam++) {
if ((clt_mismatch != null) && (ddnd[cam] != null)) {
if (imgdtt_params.lma_diff_xy) {
clt_mismatch[3*cam + 0][tIndex] =
ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
clt_mismatch[3*cam + 1][tIndex] =
ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
} else {
clt_mismatch[3*cam + 0][tIndex] = ddnd[cam][0];
clt_mismatch[3*cam + 1][tIndex] = ddnd[cam][1];
}
}
if (stats != null) {
disparity_map[IMG_DIFF0_INDEX+0][tIndex] = stats[0];
disparity_map[IMG_DIFF0_INDEX+1][tIndex] = stats[1];
disparity_map[IMG_DIFF0_INDEX+2][tIndex] = stats[2];
// disparity_map[IMG_DIFF0_INDEX+3][tIndex] = stats[3];
}
if ((lma2_ds != null) && ((lma2_ds[cTile] != null))) {
// composite new disparity
disparity_map[DISPARITY_INDEX_VERT][tIndex] = lma2_ds[cTile][0]+ disparity_array[tileY][tileX] + disparity_corr;
disparity_map[DISPARITY_INDEX_VERT_STRENGTH][tIndex] = lma2_ds[cTile][1];
if (clt_mismatch != null) {
clt_mismatch[3*0 + 2][tIndex] =
(lma2_ds[cTile][1] - imgdtt_params.lma_str_offset)/imgdtt_params.lma_str_scale - imgdtt_params.lma_min_strength;
}
}
}
if (extra_stats != null) {
if (extra_stats[cTile] != null) {
disparity_map[DISPARITY_INDEX_CM+1][tIndex] = extra_stats[cTile][0];
disparity_map[DISPARITY_VARIATIONS_INDEX][tIndex] = extra_stats[cTile][2];
disparity_map[OVEREXPOSED][tIndex] = extra_stats[cTile][3];
if (clt_mismatch != null) {
clt_mismatch[3*1 + 2][tIndex] = extra_stats[cTile][0];
clt_mismatch[3*2 + 2][tIndex] = extra_stats[cTile][2];
clt_mismatch[3*3 + 2][tIndex] = extra_stats[cTile][3];
}
} else {
disparity_map[DISPARITY_INDEX_CM+1][tIndex] = Double.NaN;
disparity_map[DISPARITY_VARIATIONS_INDEX][tIndex] = Double.NaN;
disparity_map[OVEREXPOSED][tIndex] = Double.NaN;
if (clt_mismatch != null) {
clt_mismatch[3*1 + 2][tIndex] = Double.NaN;
clt_mismatch[3*2 + 2][tIndex] = Double.NaN;
clt_mismatch[3*3 + 2][tIndex] = Double.NaN;
}
}
}
}
}
}
}
}
/**/
}
}
}
}
};
}
startAndJoin(threads);
// final double [][] dbg_distort = debug_distort? (new double [4*quad][tilesX*tilesY]) : null;
if ((dbg_distort != null) &&(globalDebugLevel >=0)) {
(new ShowDoubleFloatArrays()).showArrays(dbg_distort, tilesX, tilesY, true, "disparity_distortions"); // , dbg_titles);
}
/*
if (dbg_ports_coords != null) {
(new showDoubleFloatArrays()).showArrays(dbg_ports_coords, tilesX, tilesY, true, "ports_coordinates", dbg_titles);
}
*/
return lazy_eye_data; // clt_data;
}
public double [][][][][][] clt_aberrations_quad_corr( // 08/28/21 version
final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
final int macro_scale, // to correlate tile data instead of the pixel data: 1 - pixels, 8 - tiles
final int [][] tile_op, // [tilesY][tilesX] - what to do - 0 - nothing for this tile
final double [][] disparity_array, // [tilesY][tilesX] - individual per-tile expected disparity
final double [][][] image_data, // first index - number of image in a quad
final boolean [][] saturation_imp, // (near) saturated pixels or null
// correlation results
final double [][][][] clt_corr_out, // sparse (by the first index) [type][tilesY][tilesX][(2*transform_size-1)*(2*transform_size-1)] or null
final double [][][][] clt_combo_out, // sparse (by the first index) [type][tilesY][tilesX][(combo_tile_size] or null
final double [][][][] clt_combo_dbg, // generate sparse partial rotated/scaled pairs
// When clt_mismatch is non-zero, no far objects extraction will be attempted
// clt_mismatch is used in older code, not supported in GPU - there is cltMeasureLazyEye for that purpose
final double [][] disparity_map, // [8][tilesY][tilesX], only [6][] is needed on input or null - do not calculate final double [][] disparity_map, // [8][tilesY][tilesX], only [6][] is needed on input or null - do not calculate
// last 2 - contrast, avg/ "geometric average) // last 2 - contrast, avg/ "geometric average)
final double [][][][] texture_tiles, // [tilesY][tilesX]["RGBA".length()][]; null - will skip images combining final double [][][][] texture_tiles, // [tilesY][tilesX]["RGBA".length()][]; null - will skip images combining
...@@ -3393,8 +4220,8 @@ public class ImageDttCPU { ...@@ -3393,8 +4220,8 @@ public class ImageDttCPU {
imgdtt_params.lma_str_offset // convert lma-generated strength to match previous ones - add to result imgdtt_params.lma_str_offset // convert lma-generated strength to match previous ones - add to result
); );
if (ds != null) { // always true if (ds != null) { // always true
disparity_map[DISPARITY_INDEX_POLY][tIndex] = ds[0][0]; disparity_map[DISPARITY_INDEX_POLY][tIndex] = ds[0][0];
disparity_map[DISPARITY_STRENGTH_INDEX][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);
} }
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ 624db0c0
...@@ -5078,7 +5078,14 @@ if (debugLevel < -100) { ...@@ -5078,7 +5078,14 @@ if (debugLevel < -100) {
clt_parameters.tileY, // final int debug_tileY, clt_parameters.tileY, // final int debug_tileY,
threadsMax, threadsMax,
debugLevel); debugLevel);
double [][] dbg_img = new double[ExtrinsicAdjustment.DATA_TITLES.length][lazy_eye_data_dbg.length]; ExtrinsicAdjustment ea_dbg = new ExtrinsicAdjustment (
geometryCorrection, // GeometryCorrection gc,
1, // int clusterSize,
tilesX, // int clustersX,
tilesY); // int clustersY)
// double [][] dbg_img = new double[ExtrinsicAdjustment.DATA_TITLES.length][lazy_eye_data_dbg.length];
double [][] dbg_img = new double[ea_dbg.data_titles.length][lazy_eye_data_dbg.length];
for (int i = 0; i < dbg_img.length; i++) { for (int i = 0; i < dbg_img.length; i++) {
Arrays.fill(dbg_img[i], Double.NaN); Arrays.fill(dbg_img[i], Double.NaN);
} }
...@@ -5094,7 +5101,7 @@ if (debugLevel < -100) { ...@@ -5094,7 +5101,7 @@ if (debugLevel < -100) {
tp.getTilesY(), tp.getTilesY(),
true, true,
"LY-everytile", "LY-everytile",
ExtrinsicAdjustment.DATA_TITLES); ea_dbg.data_titles); // ExtrinsicAdjustment.DATA_TITLES);
} }
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 624db0c0
...@@ -5945,7 +5945,7 @@ public class QuadCLTCPU { ...@@ -5945,7 +5945,7 @@ public class QuadCLTCPU {
double [][] dsxy = new double[clustersX * clustersY][]; double [][] dsxy = new double[clustersX * clustersY][];
ImagePlus imp_sel = WindowManager.getCurrentImage(); ImagePlus imp_sel = WindowManager.getCurrentImage();
if ((imp_sel == null) || (imp_sel.getStackSize() != ExtrinsicAdjustment.INDX_LENGTH)) { if ((imp_sel == null) || (imp_sel.getStackSize() != ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()))) {
System.out.println("No image / wrong image selected, bailing out"); System.out.println("No image / wrong image selected, bailing out");
return null; return null;
} else { } else {
...@@ -5958,7 +5958,7 @@ public class QuadCLTCPU { ...@@ -5958,7 +5958,7 @@ public class QuadCLTCPU {
return null; return null;
} }
ImageStack stack_sel = imp_sel.getStack(); ImageStack stack_sel = imp_sel.getStack();
float [][] fpixels = new float [ExtrinsicAdjustment.INDX_LENGTH][]; float [][] fpixels = new float [ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors())][];
for (int i = 0; i < fpixels.length; i++) { for (int i = 0; i < fpixels.length; i++) {
fpixels[i] = (float[]) stack_sel.getPixels(i+1); fpixels[i] = (float[]) stack_sel.getPixels(i+1);
} }
...@@ -6247,10 +6247,16 @@ public class QuadCLTCPU { ...@@ -6247,10 +6247,16 @@ public class QuadCLTCPU {
if (lazy_eye_data != null) { if (lazy_eye_data != null) {
int clustersX= (tilesX + clt_parameters.tileStep - 1) / clt_parameters.tileStep; int clustersX= (tilesX + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
int clustersY= (tilesY + clt_parameters.tileStep - 1) / clt_parameters.tileStep; int clustersY= (tilesY + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
double [][] dbg_cluster = new double [ExtrinsicAdjustment.INDX_LENGTH][clustersY * clustersX]; ExtrinsicAdjustment ea_dbg = new ExtrinsicAdjustment (
geometryCorrection, // GeometryCorrection gc,
clt_parameters.tileStep, // int clusterSize,
clustersX, // int clustersX,
clustersY); // int clustersY)
double [][] dbg_cluster = new double [ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors())][clustersY * clustersX];
for (int n = 0; n < lazy_eye_data.length; n++) { for (int n = 0; n < lazy_eye_data.length; n++) {
if ((lazy_eye_data[n] != null) && (lazy_eye_data[n][ExtrinsicAdjustment.INDX_STRENGTH] >= clt_parameters.img_dtt.lma_diff_minw)) { if ((lazy_eye_data[n] != null) && (lazy_eye_data[n][ExtrinsicAdjustment.INDX_STRENGTH] >= clt_parameters.img_dtt.lma_diff_minw)) {
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++ ) { for (int i = 0; i < ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); i++ ) {
if (i == ExtrinsicAdjustment.INDX_STRENGTH) { if (i == ExtrinsicAdjustment.INDX_STRENGTH) {
dbg_cluster[i][n] = lazy_eye_data[n][i] - clt_parameters.img_dtt.lma_diff_minw; // strength dbg_cluster[i][n] = lazy_eye_data[n][i] - clt_parameters.img_dtt.lma_diff_minw; // strength
} else { } else {
...@@ -6258,7 +6264,7 @@ public class QuadCLTCPU { ...@@ -6258,7 +6264,7 @@ public class QuadCLTCPU {
} }
} }
} else { } else {
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++ ) { for (int i = 0; i < ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); i++ ) {
if (i == ExtrinsicAdjustment.INDX_STRENGTH) { if (i == ExtrinsicAdjustment.INDX_STRENGTH) {
dbg_cluster[i][n] = 0.0; // strength dbg_cluster[i][n] = 0.0; // strength
} else { } else {
...@@ -6274,7 +6280,7 @@ public class QuadCLTCPU { ...@@ -6274,7 +6280,7 @@ public class QuadCLTCPU {
clustersY, clustersY,
true, true,
name+sAux()+"-CLT_MISMATCH-D"+clt_parameters.disparity+"_"+clt_parameters.tileStep+"x"+clt_parameters.tileStep, name+sAux()+"-CLT_MISMATCH-D"+clt_parameters.disparity+"_"+clt_parameters.tileStep+"x"+clt_parameters.tileStep,
ExtrinsicAdjustment.DATA_TITLES); ea_dbg.data_titles); // ExtrinsicAdjustment.DATA_TITLES);
if (disparity_map != null){ if (disparity_map != null){
int target_index = ImageDtt.DISPARITY_INDEX_INT; int target_index = ImageDtt.DISPARITY_INDEX_INT;
...@@ -13084,7 +13090,7 @@ public class QuadCLTCPU { ...@@ -13084,7 +13090,7 @@ public class QuadCLTCPU {
CorrVector corr_vector = geometryCorrection.getCorrVector().clone(); CorrVector corr_vector = geometryCorrection.getCorrVector().clone();
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.INDX_LENGTH; // scan.getLazyEyeData().length; int num_ly = ExtrinsicAdjustment.get_INDX_LENGTH(getNumSensors()); // scan.getLazyEyeData().length;
int num_pars = curr_corr_arr.length; int num_pars = curr_corr_arr.length;
double [][][] ly_diff = new double [num_pars][num_ly][clusters]; double [][][] ly_diff = new double [num_pars][num_ly][clusters];
for (int np = 0; np < num_pars; np++) { for (int np = 0; np < num_pars; np++) {
...@@ -13208,14 +13214,14 @@ public class QuadCLTCPU { ...@@ -13208,14 +13214,14 @@ 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.POINTS_SAMPLE; mode++) { for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap); int x0 = (mode % 3) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap); int y0 = (mode / 3) * (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);
int pix = x + y * width; int pix = x + y * width;
int indx = (mode == 0) ? ExtrinsicAdjustment.INDX_DIFF : (ExtrinsicAdjustment.INDX_DD0 + mode - 1); int indx = (mode == 0) ? ExtrinsicAdjustment.INDX_DIFF : (ExtrinsicAdjustment.get_INDX_DD0(getNumSensors()) + mode - 1);
if (mode == 0) { if (mode == 0) {
dbg_img[par][pix] = -ly_diff[par][indx][cluster]; dbg_img[par][pix] = -ly_diff[par][indx][cluster];
} else { } else {
...@@ -13234,7 +13240,7 @@ public class QuadCLTCPU { ...@@ -13234,7 +13240,7 @@ 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.POINTS_SAMPLE; mode++) { for (int mode = 0; mode < ExtrinsicAdjustment.get_POINTS_SAMPLE(getNumSensors()); mode++) {
int x0 = (mode % 3) * (ea.clustersX + gap); int x0 = (mode % 3) * (ea.clustersX + gap);
int y0 = (mode / 3) * (ea.clustersY + gap); int y0 = (mode / 3) * (ea.clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) { for (int cluster = 0; cluster < clusters; cluster++) {
......
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