Skip to content

I
imagej-elphel
Commit 1a329010 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Updating Corr2dLMA to support variable number of image sensors

parent e6a1d91b
Hide whitespace changes
Inline Side-by-side
Showing with 421 additions and 289 deletions
src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java
View file @ 1a329010
...@@ -75,20 +75,24 @@ import Jama.Matrix; ...@@ -75,20 +75,24 @@ import Jama.Matrix;
public class Corr2dLMA { public class Corr2dLMA {
final static int NUM_CAMS = 4; // not all have to be used, so it is maximal number of cameras /// final static int NUM_CAMS = 4; // not all have to be used, so it is maximal number of cameras
final static int NUM_PAIRS = NUM_CAMS* (NUM_CAMS -1)/2; // number of possible pairs /// final static int NUM_PAIRS = NUM_CAMS* (NUM_CAMS -1)/2; // number of possible pairs
final static int DISP_INDEX = 0; // common/average disparity final static int DISP_INDEX = 0; // common/average disparity
final static int A_INDEX = 1; // A*(x-x0)^2 final static int A_INDEX = 1; // A*(x-x0)^2
final static int B_INDEX = 2; // 2*B*(x-x0)*(y-y0) final static int B_INDEX = 2; // 2*B*(x-x0)*(y-y0)
final static int CMA_INDEX = 3; // C*(y-y0)^2, encode C-A final static int CMA_INDEX = 3; // C*(y-y0)^2, encode C-A
final static int G0_INDEX = 4; // scale of correlation pair, final static int G0_INDEX = 4; // scale of correlation pair,
final static int TILE_PARAMS = G0_INDEX + NUM_PAIRS; // number of tile-individual parameters /// final static int TILE_PARAMS = G0_INDEX + NUM_PAIRS; // number of tile-individual parameters
final static String [] PAR_NAMES = {"DISP","A","B","C-A"}; final static String [] PAR_NAMES = {"DISP","A","B","C-A"};
final static String PAR_NAME_SCALE = "SCALE"; final static String PAR_NAME_SCALE = "SCALE";
final static String PAR_NAME_CORRDISP = "CORR-DISP"; final static String PAR_NAME_CORRDISP = "CORR-DISP";
final static String PAR_NAME_CORRNDISP = "CORR-NDISP"; final static String PAR_NAME_CORRNDISP = "CORR-NDISP";
final Correlation2d correlation2d;
final int num_cams;
final int num_pairs;
final int tile_params;
double [] all_pars; double [] all_pars;
private boolean [] par_mask; private boolean [] par_mask;
private int [] par_map; private int [] par_map;
...@@ -111,7 +115,9 @@ public class Corr2dLMA { ...@@ -111,7 +115,9 @@ public class Corr2dLMA {
private int ddisp_index; // = G0_INDEX + NUM_PAIRS; // disparity offset per camera (at least 1 should be disabled) private int ddisp_index; // = G0_INDEX + NUM_PAIRS; // disparity offset per camera (at least 1 should be disabled)
private int ndisp_index; // = ddisp_index + NUM_CAMS; // disparity offset per camera - none should be disable private int ndisp_index; // = ddisp_index + NUM_CAMS; // disparity offset per camera - none should be disable
private int num_all_pars; // = ndisp_index+ NUM_CAMS; // maximal number of parameters private int num_all_pars; // = ndisp_index+ NUM_CAMS; // maximal number of parameters
private int [] used_cams_map = new int[NUM_CAMS]; // for each camera index return used index ??? // private int [] used_cams_map = new int[NUM_CAMS]; // for each camera index return used index ???
private final int [] used_cams_map; // = new int[NUM_CAMS]; // for each camera index return used index ???
private int [] used_cams_rmap; // variable-length list of used cameras numbers private int [] used_cams_rmap; // variable-length list of used cameras numbers
private int [][][] used_pairs_map; // for each camera index return used index ?? private int [][][] used_pairs_map; // for each camera index return used index ??
private boolean [] used_tiles; private boolean [] used_tiles;
...@@ -119,14 +125,20 @@ public class Corr2dLMA { ...@@ -119,14 +125,20 @@ public class Corr2dLMA {
private final int transform_size; private final int transform_size;
private final double [][] corr_wnd; private final double [][] corr_wnd;
private boolean [] used_cameras; private boolean [] used_cameras;
private Matrix [][] m_disp; private int ncam_used; // number of used cameras
private int ncam; // number of used cameras
private int [] npairs; // number of used pairs per tile private int [] npairs; // number of used pairs per tile
private int last_cam; // index of the last camera (special treatment for disparity correction) private int last_cam; // index of the last camera (special treatment for disparity correction)
private int pre_last_cam; // index of the pre-last camera (special treatment for disparity correction) private int pre_last_cam; // index of the pre-last camera (special treatment for disparity correction)
private Matrix [][][] m_pairs; private Matrix [][] m_disp;
private Matrix [][][] m_pairs_inv; // inverted m_pairs to calculate x,y -> dd,nd for initial disparity calculation // private Matrix [][][] m_pairs;
private final int [][] pindx = new int [NUM_CAMS][NUM_CAMS]; // private Matrix [][][] m_pairs_inv; // inverted m_pairs to calculate x,y -> dd,nd for initial disparity calculation
private Matrix [][] m_pairs;
private Matrix [][] m_pairs_inv; // inverted m_pairs to calculate x,y -> dd,nd for initial disparity calculation
// private final int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
// private final int [][] pindx; // = new int [NUM_CAMS][NUM_CAMS];
private int numTiles = 1; private int numTiles = 1;
private Matrix mddnd; // Matrix to calculate 2 last corrections in disparity direction and 1 ortho from the first ones (normally 2+3=5) private Matrix mddnd; // Matrix to calculate 2 last corrections in disparity direction and 1 ortho from the first ones (normally 2+3=5)
...@@ -140,24 +152,28 @@ public class Corr2dLMA { ...@@ -140,24 +152,28 @@ public class Corr2dLMA {
public class Sample{ // USED in lwir public class Sample{ // USED in lwir
int tile; // tile in a cluster int tile; // tile in a cluster
int fcam; // first camera index int pair; // pair index
int scam; // second camera index // int fcam; // first camera index
// int scam; // second camera index
int ix; // x coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1) int ix; // x coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1)
int iy; // y coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1) int iy; // y coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1)
double v; // correlation value at that point double v; // correlation value at that point
double w; // weight double w; // weight
Sample ( Sample (
int tile, int tile,
int fcam, // first camera index int pair,
int scam, // second camera index // int fcam, // first camera index
// int scam, // second camera index
int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
int y, // coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1) int y, // coordinate in 2D correlation (0.. 2*transform_size-2, center: (transform_size-1)
double v, // correlation value at that point double v, // correlation value at that point
double w) double w)
{ {
this.tile = tile; this.tile = tile;
this.fcam = fcam; this.pair = pair;
this.scam = scam; // this.fcam = fcam;
// this.scam = scam;
this.ix = x; this.ix = x;
this.iy = y; this.iy = y;
this.v = v; this.v = v;
...@@ -165,31 +181,45 @@ public class Corr2dLMA { ...@@ -165,31 +181,45 @@ public class Corr2dLMA {
} }
@Override @Override
public String toString() { public String toString() {
return String.format("tile=%d, f=%d, s=%d, x=%d, y=%d, v=%f, w=%f", tile, fcam, scam, ix, iy, v, w); // return String.format("tile=%d, f=%d, s=%d, x=%d, y=%d, v=%f, w=%f", tile, fcam, scam, ix, iy, v, w);
return String.format("tile=%d, p=%d, x=%d, y=%d, v=%f, w=%f", tile, pair, ix, iy, v, w);
} }
} }
public Corr2dLMA ( public Corr2dLMA (
int numTiles, int numTiles,
Correlation2d correlation2d,
int ts, // null - use default table int ts, // null - use default table
double [][] corr_wnd, // may be null double [][] corr_wnd, // may be null
double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
boolean gaussian_mode boolean gaussian_mode
) { ) {
this.correlation2d = correlation2d;
this.num_cams = correlation2d.getNumSensors();
this.num_pairs = correlation2d.getNumPairs();
this.tile_params = G0_INDEX + this.num_pairs;
this.rXY = rXY; this.rXY = rXY;
this.gaussian_mode = gaussian_mode; this.gaussian_mode = gaussian_mode;
for (int f = 0; f < NUM_CAMS; f++) { this.used_cams_map = new int[num_cams];
/*
this.pindx = new int [num_cams][num_cams];
for (int f = 0; f < num_cams; f++) {
pindx[f][f]=-1; pindx[f][f]=-1;
for (int s = f+1; s < NUM_CAMS; s++) { for (int s = f+1; s < num_cams; s++) {
pindx[f][s] = getPairIndex(f,s); pindx[f][s] = getPairIndex(f,s);
pindx[s][f] = pindx[f][s]; pindx[s][f] = pindx[f][s];
} }
} }
*/
this.numTiles = numTiles; this.numTiles = numTiles;
ddisp_index = this.numTiles * TILE_PARAMS; ddisp_index = this.numTiles * this.tile_params; // ;
ndisp_index = ddisp_index + NUM_CAMS; // disparity offset per camera - none should be disable ndisp_index = ddisp_index + num_cams; // disparity offset per camera - none should be disable
num_all_pars = ndisp_index+ NUM_CAMS; // maximal number of parameters num_all_pars = ndisp_index+ num_cams; // maximal number of parameters
// boolean sq = true; // false; // boolean sq = true; // false;
this.transform_size = ts; this.transform_size = ts;
...@@ -234,7 +264,7 @@ public class Corr2dLMA { ...@@ -234,7 +264,7 @@ public class Corr2dLMA {
// public double[][] getCorrWnd() { // public double[][] getCorrWnd() {
// return this.corr_wnd; // return this.corr_wnd;
// } // }
/*
public void addSample( // x = 0, y=0 - center public void addSample( // x = 0, y=0 - center
int tile, int tile,
int fcam, // first camera index int fcam, // first camera index
...@@ -245,7 +275,22 @@ public class Corr2dLMA { ...@@ -245,7 +275,22 @@ public class Corr2dLMA {
double w) { // sample weight double w) { // sample weight
if ((w > 0) && !Double.isNaN(v)) samples.add(new Sample(tile,fcam,scam,x,y,v,w)); if ((w > 0) && !Double.isNaN(v)) samples.add(new Sample(tile,fcam,scam,x,y,v,w));
} }
*/
public void addSample( // x = 0, y=0 - center
int tile,
int pair,
// int fcam, // first camera index
// int scam, // second camera index
int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
int y, // y coordinate (0 - disparity axis)
double v, // correlation value at that point
double w) { // sample weight
// if ((w > 0) && !Double.isNaN(v)) samples.add(new Sample(tile,fcam,scam,x,y,v,w));
if ((w > 0) && !Double.isNaN(v)) samples.add(new Sample(tile,pair,x,y,v,w));
}
public ArrayList<Sample> filterSamples( public ArrayList<Sample> filterSamples(
double [][] disparity_strength) { double [][] disparity_strength) {
ArrayList<Sample> filtered_samples = new ArrayList<Sample>(); ArrayList<Sample> filtered_samples = new ArrayList<Sample>();
...@@ -318,38 +363,40 @@ public class Corr2dLMA { ...@@ -318,38 +363,40 @@ public class Corr2dLMA {
if (mode == 0) d = s.v; if (mode == 0) d = s.v;
else if (mode == 1) d = s.w; else if (mode == 1) d = s.w;
else if (mode == 2) d = fx[ns]; else if (mode == 2) d = fx[ns];
int np = comb_map[s.fcam][s.scam]; //////////////////// // int np = comb_map[s.fcam][s.scam]; ////////////////////
int np = s.pair; ////////////////////
rslt[s.tile][np][s.iy*size + s.ix] = d; rslt[s.tile][np][s.iy*size + s.ix] = d;
} }
return rslt; return rslt;
} }
@Deprecated
public String [] dbgGetSliceTiles() { public String [] dbgGetSliceTiles() {
int [][] comb_map = getCombMap(); int [][] comb_map = getCombMap();
int np = comb_map[0][0]; int np = comb_map[0][0];
comb_map[0][0] = -1; comb_map[0][0] = -1;
String [] srslt = new String [np]; String [] srslt = new String [np];
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
if (comb_map[f][s] >= 0) { if (comb_map[f][s] >= 0) {
srslt[comb_map[f][s]] = ""+f+"->"+s; srslt[comb_map[f][s]] = ""+f+"->"+s;
} }
} }
return srslt; return srslt;
} }
@Deprecated
public int [][] getCombMap(){ public int [][] getCombMap(){
boolean [][] comb_pairs = new boolean[NUM_CAMS][NUM_CAMS]; boolean [][] comb_pairs = new boolean[num_cams][num_cams];
for (int t = 0; t < numTiles; t++) { for (int t = 0; t < numTiles; t++) {
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
comb_pairs[f][s] |= used_pairs_map[t][f][s] >= 0; comb_pairs[f][s] |= used_pairs_map[t][f][s] >= 0;
} }
} }
int np = 0; int np = 0;
int [][] comb_map = new int [NUM_CAMS][NUM_CAMS]; int [][] comb_map = new int [num_cams][num_cams];
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
if (comb_pairs[f][s]) comb_map[f][s] = np++; if (comb_pairs[f][s]) comb_map[f][s] = np++;
else comb_map[f][s] = -1; else comb_map[f][s] = -1;
} }
...@@ -359,19 +406,21 @@ public class Corr2dLMA { ...@@ -359,19 +406,21 @@ public class Corr2dLMA {
} }
@Deprecated
public int getPairIndex(int f, int s) { public int getPairIndex(int f, int s) {
if (f > s) { if (f > s) {
int t = f; int t = f;
f = s; f = s;
s = t; s = t;
} }
return (NUM_CAMS * f) - (f + 1)*f/2 - f - 1 + s ; // return n*i - i*(i+1)//2 - i + j -1 return (num_cams * f) - (f + 1)*f/2 - f - 1 + s ; // return n*i - i*(i+1)//2 - i + j -1
} }
public void setMatrices(double [][] am_disp) { public void setMatrices(double [][] am_disp) {
m_disp = new Matrix[1][NUM_CAMS]; m_disp = new Matrix[1][num_cams];
for (int n = 0; n < NUM_CAMS; n++) { for (int n = 0; n < num_cams; n++) {
double [][] am = { double [][] am = {
{am_disp[n][0],am_disp[n][1]}, {am_disp[n][0],am_disp[n][1]},
{am_disp[n][2],am_disp[n][3]}}; {am_disp[n][2],am_disp[n][3]}};
...@@ -380,9 +429,9 @@ public class Corr2dLMA { ...@@ -380,9 +429,9 @@ public class Corr2dLMA {
} }
public void setMatrices(double [][][] am_disp) { public void setMatrices(double [][][] am_disp) {
m_disp = new Matrix[am_disp.length][NUM_CAMS]; m_disp = new Matrix[am_disp.length][num_cams];
for (int nt = 0; nt < numTiles; nt++) if (used_tiles[nt]){ for (int nt = 0; nt < numTiles; nt++) if (used_tiles[nt]){
for (int n = 0; n < NUM_CAMS; n++) { for (int n = 0; n < num_cams; n++) {
double [][] am = { double [][] am = {
{am_disp[nt][n][0], am_disp[nt][n][1]}, {am_disp[nt][n][0], am_disp[nt][n][1]},
{am_disp[nt][n][2], am_disp[nt][n][3]}}; {am_disp[nt][n][2], am_disp[nt][n][3]}};
...@@ -404,16 +453,16 @@ public class Corr2dLMA { ...@@ -404,16 +453,16 @@ public class Corr2dLMA {
{rXY[pre_last_cam][1], rXY[last_cam][1], rXY[last_cam][0]}, // sum(y) = 0 {rXY[pre_last_cam][1], rXY[last_cam][1], rXY[last_cam][0]}, // sum(y) = 0
{1.0, 1.0, 0.0 }}; // sum(dd) = 0 {1.0, 1.0, 0.0 }}; // sum(dd) = 0
A33 = new Matrix(aA33); A33 = new Matrix(aA33);
double [][] aA35 = new double [3][2 * ncam-3]; double [][] aA35 = new double [3][2 * ncam_used-3];
for (int i = 0; i < (ncam-2); i++) { // coefficients for dd[i] for (int i = 0; i < (ncam_used-2); i++) { // coefficients for dd[i]
int n = used_cams_rmap[i]; int n = used_cams_rmap[i];
aA35[0][i] = -rXY[n][0]; aA35[0][i] = -rXY[n][0];
aA35[1][i] = -rXY[n][1]; aA35[1][i] = -rXY[n][1];
aA35[2][i] = -1.0; aA35[2][i] = -1.0;
} }
for (int i = 0; i < (ncam-1); i++) { // coefficients for nd[i] for (int i = 0; i < (ncam_used-1); i++) { // coefficients for nd[i]
int n = used_cams_rmap[i]; int n = used_cams_rmap[i];
int i1 = ncam - 2 + i; int i1 = ncam_used - 2 + i;
aA35[0][i1] = rXY[n][1]; aA35[0][i1] = rXY[n][1];
aA35[1][i1] = -rXY[n][0]; aA35[1][i1] = -rXY[n][0];
// aA35[2][i] = 0.0; // already 0.0 // aA35[2][i] = 0.0; // already 0.0
...@@ -427,9 +476,9 @@ public class Corr2dLMA { ...@@ -427,9 +476,9 @@ public class Corr2dLMA {
) { ) {
for (int nTile = 0; nTile < numTiles; nTile++) { for (int nTile = 0; nTile < numTiles; nTile++) {
if ((disp_str[nTile] != null) && (disp_str[nTile][1] > 0.0) && used_tiles[nTile]) { if ((disp_str[nTile] != null) && (disp_str[nTile][1] > 0.0) && used_tiles[nTile]) {
this.all_pars[DISP_INDEX + nTile*TILE_PARAMS] = -disp_str[nTile][0]; // disp0; this.all_pars[DISP_INDEX + nTile*tile_params] = -disp_str[nTile][0]; // disp0;
} else { } else {
this.all_pars[DISP_INDEX + nTile*TILE_PARAMS] = 0.0; // disp0; this.all_pars[DISP_INDEX + nTile*tile_params] = 0.0; // disp0;
} }
} }
} }
...@@ -448,58 +497,69 @@ public class Corr2dLMA { ...@@ -448,58 +497,69 @@ public class Corr2dLMA {
adjust_lazyeye_ortho = adjust_lazyeye_par; // simplify relations for the calculated/dependent parameters adjust_lazyeye_ortho = adjust_lazyeye_par; // simplify relations for the calculated/dependent parameters
lazy_eye = adjust_lazyeye_par | adjust_lazyeye_ortho; lazy_eye = adjust_lazyeye_par | adjust_lazyeye_ortho;
bad_tile = -1; bad_tile = -1;
used_pairs_map = new int [numTiles][NUM_CAMS][NUM_CAMS]; used_pairs_map = new int [numTiles][num_cams][num_cams];
used_cameras = new boolean[NUM_CAMS]; used_cameras = new boolean[num_cams];
boolean [][] used_pairs = new boolean[numTiles][NUM_PAIRS]; boolean [][] used_pairs = new boolean[numTiles][num_pairs];
// 0-weight values and NaN-s should be filtered on input! // 0-weight values and NaN-s should be filtered on input!
for (int t = 0; t < numTiles; t++) for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int t = 0; t < numTiles; t++) for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
used_pairs_map[t][f][s] = -1; used_pairs_map[t][f][s] = -1;
} }
boolean [][][] used_pairs_dir = new boolean [numTiles][NUM_CAMS][NUM_CAMS]; boolean [][][] used_pairs_dir = new boolean [numTiles][num_cams][num_cams];
used_tiles = new boolean[numTiles]; used_tiles = new boolean[numTiles];
for (Sample s:samples) { // ignore zero-weight samples for (Sample s:samples) { // ignore zero-weight samples
used_cameras[s.fcam]=true; int pair = s.pair;
used_cameras[s.scam]=true; int [] fscam = correlation2d.getPair(pair);
used_cameras[fscam[0]]=true;
used_cameras[fscam[1]]=true;
used_tiles[s.tile] = true; used_tiles[s.tile] = true;
used_pairs[s.tile][pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s used_pairs[s.tile][pair]=true; // throws < 0 - wrong pair, f==s
used_pairs_dir[s.tile][s.fcam][s.scam] = true; // used_pairs_dir[s.tile][s.fcam][s.scam] = true;
} used_pairs_dir[s.tile][fscam[0]][fscam[1]] = true;
ncam = 0; }
npairs =new int [numTiles]; ncam_used = 0;
for (int i = 0; i < NUM_CAMS; i++) { npairs =new int [numTiles]; // pairs in each tile
used_cams_map[i] = ncam; for (int i = 0; i < num_cams; i++) {
used_cams_map[i] = ncam_used;
if (used_cameras[i]) { if (used_cameras[i]) {
ncam++; ncam_used++;
} }
} }
used_cams_rmap = new int [ncam]; used_cams_rmap = new int [ncam_used];
ncam = 0; ncam_used = 0;
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
if (used_cameras[i]) { if (used_cameras[i]) {
used_cams_rmap[ncam++] = i; used_cams_rmap[ncam_used++] = i;
} }
} }
if (ncam < 2) { if (ncam_used < 2) {
return false; return false;
} }
last_cam = (ncam > 1)? used_cams_rmap[ncam - 1] :-1; last_cam = (ncam_used > 1)? used_cams_rmap[ncam_used - 1] :-1;
pre_last_cam = (ncam > 2)? used_cams_rmap[ncam - 2] :-1; pre_last_cam = (ncam_used > 2)? used_cams_rmap[ncam_used - 2] :-1;
setOffsetMatrices(); setOffsetMatrices();
for (int nTile = 0; nTile < numTiles; nTile++) { for (int nTile = 0; nTile < numTiles; nTile++) {
int [] upmam = new int[NUM_PAIRS]; int [] upmam = new int[num_pairs];
for (int i = 0; i < NUM_PAIRS; i++) { for (int i = 0; i < num_pairs; i++) {
upmam[i] = npairs[nTile]; upmam[i] = npairs[nTile];
if (used_pairs[nTile][i]) npairs[nTile]++; if (used_pairs[nTile][i]) npairs[nTile]++;
} }
for (int f = 0; f < NUM_CAMS; f++) { /*
for (int s = f+1; s < NUM_CAMS; s++) { for (int f = 0; f < num_cams; f++) {
for (int s = f+1; s < num_cams; s++) {
int npair = upmam[pindx[f][s]]; int npair = upmam[pindx[f][s]];
if (used_pairs_dir[nTile][f][s]) used_pairs_map[nTile][f][s] = npair; // either or, can not be f,s and s,f pairs if (used_pairs_dir[nTile][f][s]) used_pairs_map[nTile][f][s] = npair; // either or, can not be f,s and s,f pairs
else if (used_pairs_dir[nTile][s][f]) used_pairs_map[nTile][s][f] = npair; else if (used_pairs_dir[nTile][s][f]) used_pairs_map[nTile][s][f] = npair;
} }
} }
*/
for (int pair = 0; pair < num_pairs; pair++) {
int npair = upmam[pair];
int [] fs = correlation2d.getPair(pair);
if (used_pairs_dir[nTile][fs[0]][fs[1]]) used_pairs_map[nTile][fs[0]][fs[1]] = npair; // either or, can not be f,s and s,f pairs
else if (used_pairs_dir[nTile][fs[1]][fs[0]]) used_pairs_map[nTile][fs[1]][fs[0]] = npair;
}
} }
this.all_pars = new double[num_all_pars]; this.all_pars = new double[num_all_pars];
...@@ -507,22 +567,22 @@ public class Corr2dLMA { ...@@ -507,22 +567,22 @@ public class Corr2dLMA {
total_tiles = 0; total_tiles = 0;
// per-tile parameters // per-tile parameters
for (int nTile = 0; nTile < numTiles; nTile++) if ((disp_str[nTile] != null) && (disp_str[nTile][1] > 0.0) && used_tiles[nTile]){ for (int nTile = 0; nTile < numTiles; nTile++) if ((disp_str[nTile] != null) && (disp_str[nTile][1] > 0.0) && used_tiles[nTile]){
this.all_pars[DISP_INDEX + nTile*TILE_PARAMS] = -disp_str[nTile][0]; // disp0; this.all_pars[DISP_INDEX + nTile*tile_params] = -disp_str[nTile][0]; // disp0;
this.all_pars[A_INDEX + nTile*TILE_PARAMS] = 1.0/(half_width * half_width); this.all_pars[A_INDEX + nTile*tile_params] = 1.0/(half_width * half_width);
this.all_pars[B_INDEX + nTile*TILE_PARAMS] = 0.0; this.all_pars[B_INDEX + nTile*tile_params] = 0.0;
this.all_pars[CMA_INDEX + nTile*TILE_PARAMS] = 0.0; // C-A this.all_pars[CMA_INDEX + nTile*tile_params] = 0.0; // C-A
this.par_mask[DISP_INDEX + nTile*TILE_PARAMS] = true; this.par_mask[DISP_INDEX + nTile*tile_params] = true;
this.par_mask[A_INDEX + nTile*TILE_PARAMS] = adjust_width; this.par_mask[A_INDEX + nTile*tile_params] = adjust_width;
this.par_mask[B_INDEX + nTile*TILE_PARAMS] = adjust_ellipse; this.par_mask[B_INDEX + nTile*tile_params] = adjust_ellipse;
this.par_mask[CMA_INDEX + nTile*TILE_PARAMS] = adjust_ellipse; this.par_mask[CMA_INDEX + nTile*tile_params] = adjust_ellipse;
for (int i = 0; i <NUM_PAIRS; i++) { for (int i = 0; i <num_pairs; i++) {
this.par_mask[G0_INDEX + i + nTile*TILE_PARAMS] = used_pairs[nTile][i] & adjust_scales; this.par_mask[G0_INDEX + i + nTile*tile_params] = used_pairs[nTile][i] & adjust_scales;
this.all_pars[G0_INDEX + i + nTile*TILE_PARAMS] = Double.NaN; // will be assigned later for used - should be for all ! this.all_pars[G0_INDEX + i + nTile*tile_params] = Double.NaN; // will be assigned later for used - should be for all !
} }
total_tiles++; total_tiles++;
} }
// common for all tiles parameters // common for all tiles parameters
for (int i = 0; i <NUM_CAMS; i++) { for (int i = 0; i <num_cams; i++) {
this.all_pars[ddisp_index + i] = 0.0; // C-A this.all_pars[ddisp_index + i] = 0.0; // C-A
this.par_mask[ddisp_index + i] = used_cameras[i] & adjust_lazyeye_par & (i != last_cam) & (i != pre_last_cam); this.par_mask[ddisp_index + i] = used_cameras[i] & adjust_lazyeye_par & (i != last_cam) & (i != pre_last_cam);
this.all_pars[ndisp_index + i] = 0.0; // C-A this.all_pars[ndisp_index + i] = 0.0; // C-A
...@@ -530,13 +590,13 @@ public class Corr2dLMA { ...@@ -530,13 +590,13 @@ public class Corr2dLMA {
} }
int np = samples.size(); int np = samples.size();
weights = new double [np + 2 * NUM_CAMS]; // npairs]; weights = new double [np + 2 * num_cams]; // npairs];
values = new double [np + 2 * NUM_CAMS]; // npairs]; values = new double [np + 2 * num_cams]; // npairs];
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
weights[np + i] = (used_cameras[i] & adjust_lazyeye_par)? (cost_lazyeye_par * numTiles) : 0.0; // ddisp - including last_cameras weights[np + i] = (used_cameras[i] & adjust_lazyeye_par)? (cost_lazyeye_par * numTiles) : 0.0; // ddisp - including last_cameras
weights[np + NUM_CAMS + i] = (used_cameras[i] & adjust_lazyeye_ortho)? (cost_lazyeye_odtho * numTiles) : 0.0; // ndisp - including last_camera weights[np + num_cams + i] = (used_cameras[i] & adjust_lazyeye_ortho)? (cost_lazyeye_odtho * numTiles) : 0.0; // ndisp - including last_camera
values [np + i] = 0.0; values [np + i] = 0.0;
values [np + NUM_CAMS + i] = 0.0; values [np + num_cams + i] = 0.0;
} }
double sw = 0; double sw = 0;
...@@ -548,7 +608,8 @@ public class Corr2dLMA { ...@@ -548,7 +608,8 @@ public class Corr2dLMA {
total_weight += s.w; total_weight += s.w;
values[i] = s.v; values[i] = s.v;
sw += weights[i]; sw += weights[i];
int indx = G0_INDEX + pindx[s.fcam][s.scam] + s.tile * TILE_PARAMS; // int indx = G0_INDEX + pindx[s.fcam][s.scam] + s.tile * tile_params;
int indx = G0_INDEX + s.pair + s.tile * tile_params;
double d = s.v; double d = s.v;
if (this.corr_wnd !=null) { if (this.corr_wnd !=null) {
d /= this.corr_wnd[s.iy][s.ix]; d /= this.corr_wnd[s.iy][s.ix];
...@@ -557,7 +618,7 @@ public class Corr2dLMA { ...@@ -557,7 +618,7 @@ public class Corr2dLMA {
} }
pure_weight = sw; pure_weight = sw;
for (int i = 0; i < 2 * NUM_CAMS; i++) { // weight of the regularization terms (twice number of cameras, some may be disabled by a mask) for (int i = 0; i < 2 * num_cams; i++) { // weight of the regularization terms (twice number of cameras, some may be disabled by a mask)
sw += weights[np + i]; sw += weights[np + i];
} }
if (sw != 0.0) { if (sw != 0.0) {
...@@ -572,13 +633,13 @@ public class Corr2dLMA { ...@@ -572,13 +633,13 @@ public class Corr2dLMA {
if (par_mask[i]) par_map[i] = par_indx++; if (par_mask[i]) par_map[i] = par_indx++;
else par_map[i] = -1; else par_map[i] = -1;
} }
dd_indices = lazy_eye? new int [2 * ncam -3] : null; dd_indices = lazy_eye? new int [2 * ncam_used -3] : null;
if (dd_indices != null) { if (dd_indices != null) {
int pi = 0; int pi = 0;
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
if (par_map[ddisp_index + i] >=0) dd_indices[pi++] = par_map[ddisp_index + i]; if (par_map[ddisp_index + i] >=0) dd_indices[pi++] = par_map[ddisp_index + i];
} }
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
if (par_map[ndisp_index + i] >=0) dd_indices[pi++] = par_map[ndisp_index + i]; if (par_map[ndisp_index + i] >=0) dd_indices[pi++] = par_map[ndisp_index + i];
} }
} }
...@@ -589,15 +650,16 @@ public class Corr2dLMA { ...@@ -589,15 +650,16 @@ public class Corr2dLMA {
public void initMatrices() { // should be called after initVector and after setMatrices public void initMatrices() { // should be called after initVector and after setMatrices
m_pairs = new Matrix[used_pairs_map.length][NUM_CAMS][NUM_CAMS]; // m_pairs = new Matrix[used_pairs_map.length][num_cams][num_cams];
m_pairs_inv = new Matrix[used_pairs_map.length][NUM_CAMS][NUM_CAMS]; // m_pairs_inv = new Matrix[used_pairs_map.length][num_cams][num_cams];
m_pairs = new Matrix[used_pairs_map.length][num_pairs];
m_pairs_inv = new Matrix[used_pairs_map.length][num_pairs];
for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){ for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int npair = 0; npair < num_pairs; npair++) {
m_pairs[nTile][f][s] = null; int [] fs = correlation2d.getPair(npair); // TODO: change used_pairs_map?
// m_pairs_last[f][s] = null; if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
if (used_pairs_map[nTile][f][s] >= 0) { m_pairs[nTile][npair] = m_disp[nTile][fs[0]].minus(m_disp[nTile][fs[1]]);
m_pairs[nTile][f][s] = m_disp[nTile][f].minus(m_disp[nTile][s]); m_pairs_inv[nTile][npair] = m_pairs[nTile][npair].inverse();
m_pairs_inv[nTile][f][s] = m_pairs[nTile][f][s].inverse();
} }
} }
} }
...@@ -605,13 +667,14 @@ public class Corr2dLMA { ...@@ -605,13 +667,14 @@ public class Corr2dLMA {
public void initInvertMatrices() { // should be called after initMatrices only if m_pairs_inv are needed public void initInvertMatrices() { // should be called after initMatrices only if m_pairs_inv are needed
m_pairs_inv = new Matrix[used_pairs_map.length][NUM_CAMS][NUM_CAMS]; // m_pairs_inv = new Matrix[used_pairs_map.length][num_cams][num_cams];
m_pairs_inv = new Matrix[used_pairs_map.length][num_pairs];
for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){ for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) { for (int npair = 0; npair < num_pairs; npair++) {
m_pairs_inv[nTile][f][s] = null; int [] fs = correlation2d.getPair(npair); // TODO: change used_pairs_map?
if (used_pairs_map[nTile][f][s] >= 0) { if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
m_pairs_inv[nTile][f][s] = m_pairs[nTile][f][s].inverse(); m_pairs_inv[nTile][npair] = m_pairs[nTile][npair].inverse();
} }
} }
} }
...@@ -641,7 +704,8 @@ public class Corr2dLMA { ...@@ -641,7 +704,8 @@ public class Corr2dLMA {
} else { } else {
bv = s.v / corr_wnd[s.iy][s.ix]; bv = s.v / corr_wnd[s.iy][s.ix];
} }
bv /=this.all_pars[G0_INDEX + pindx[s.fcam][s.scam] + s.tile * TILE_PARAMS]; // bv /=this.all_pars[G0_INDEX + pindx[s.fcam][s.scam] + s.tile * tile_params];
bv /=this.all_pars[G0_INDEX + s.pair + s.tile * tile_params];
//corr_wnd //corr_wnd
int indx = 2 * ns; int indx = 2 * ns;
mdata[indx ][0] = new double [2]; mdata[indx ][0] = new double [2];
...@@ -653,7 +717,8 @@ public class Corr2dLMA { ...@@ -653,7 +717,8 @@ public class Corr2dLMA {
double [] aXY = {s.ix - center, s.iy - center}; double [] aXY = {s.ix - center, s.iy - center};
Matrix mXY = new Matrix(aXY,2); Matrix mXY = new Matrix(aXY,2);
Matrix mDDND = m_pairs_inv[s.tile][s.fcam][s.scam].times(mXY); // Matrix mDDND = m_pairs_inv[s.tile][s.fcam][s.scam].times(mXY);
Matrix mDDND = m_pairs_inv[s.tile][s.pair].times(mXY);
mdata[indx ][0][0] = mDDND.get(0, 0); // dd mdata[indx ][0][0] = mDDND.get(0, 0); // dd
mdata[indx+1][0][0] = mdata[indx ][0][0]; mdata[indx+1][0][0] = mdata[indx ][0][0];
...@@ -701,9 +766,9 @@ public class Corr2dLMA { ...@@ -701,9 +766,9 @@ public class Corr2dLMA {
double scale, double scale,
int irad, int irad,
double sigma){ double sigma){
double [][] dbg_img = new double [NUM_CAMS*NUM_CAMS+1][]; double [][] dbg_img = new double [num_cams*num_cams+1][];
double [][] dbg_weights = new double [NUM_CAMS*NUM_CAMS+1][]; double [][] dbg_weights = new double [num_cams*num_cams+1][];
String [] titles = new String [NUM_CAMS*NUM_CAMS+1]; String [] titles = new String [num_cams*num_cams+1];
int size = 2 * irad+1; int size = 2 * irad+1;
int nSamples = samples.size(); int nSamples = samples.size();
DoubleGaussianBlur gb = new DoubleGaussianBlur(); DoubleGaussianBlur gb = new DoubleGaussianBlur();
...@@ -714,11 +779,13 @@ public class Corr2dLMA { ...@@ -714,11 +779,13 @@ public class Corr2dLMA {
for (int ns = 0; ns < nSamples; ns++) { for (int ns = 0; ns < nSamples; ns++) {
int indx = 2 * ns; int indx = 2 * ns;
Sample s = samples.get(ns); Sample s = samples.get(ns);
int np = s.fcam* NUM_CAMS + s.scam +1; int np = s.pair; // .fcam* num_cams + s.scam +1;
int [] fs = correlation2d.getPair(np);
if (dbg_img[np] == null) { if (dbg_img[np] == null) {
dbg_img[np] = new double [size*size]; dbg_img[np] = new double [size*size];
dbg_weights[np] = new double [size*size]; dbg_weights[np] = new double [size*size];
titles[np]=String.format("%d->%d", s.fcam,s.scam); // titles[np]=String.format("%d->%d", s.fcam,s.scam);
titles[np]=String.format("%d->%d", fs[0], fs[1]);
} }
double d = mdata[indx][1][0]; double d = mdata[indx][1][0];
double w = mdata[indx][2][0]; double w = mdata[indx][2][0];
...@@ -776,43 +843,47 @@ public class Corr2dLMA { ...@@ -776,43 +843,47 @@ public class Corr2dLMA {
double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
if (vector == null) return null; if (vector == null) return null;
double [] av = fromVector(vector); double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][NUM_CAMS]; Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][NUM_CAMS][NUM_CAMS][]; double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0}; double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2); Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX]; double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX]; double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX]; double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){ for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
for (int i = 0; i < NUM_CAMS; i++) if (used_cameras[i]) { for (int i = 0; i < num_cams; i++) if (used_cameras[i]) {
double [] add_dnd = {av[DISP_INDEX+ nTile * TILE_PARAMS]+ av[ddisp_index + i], av[ndisp_index + i]}; double [] add_dnd = {av[DISP_INDEX+ nTile * tile_params]+ av[ddisp_index + i], av[ndisp_index + i]};
xcam_ycam[nTile][i] = m_disp[nTile][i].times(new Matrix(add_dnd,2)); xcam_ycam[nTile][i] = m_disp[nTile][i].times(new Matrix(add_dnd,2));
} }
for (int f = 0; f < NUM_CAMS; f++) if (used_cameras[f]) { for (int f = 0; f < num_cams; f++) if (used_cameras[f]) {
for (int s = 0; s < NUM_CAMS; s++) if (used_cameras[s]) { for (int s = 0; s < num_cams; s++) if (used_cameras[s]) {
xp_yp[nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy(); xp_yp[nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
} }
} }
AT[nTile] = av[A_INDEX + nTile * TILE_PARAMS]; AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * TILE_PARAMS]; BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * TILE_PARAMS]; CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
} }
int num_samples = samples.size(); int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * NUM_CAMS]; double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd //corr_wnd
for (int ns = 0; ns < num_samples; ns++) { for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns); Sample s = samples.get(ns);
int pair = pindx[s.fcam][s.scam]; // all pairs, noit just used? // int pair = pindx[s.fcam][s.scam]; // all pairs, noit just used?
int pair = s.pair; // all pairs, noit just used?
int [] fs = correlation2d.getPair(pair);
double A = AT[s.tile]; double A = AT[s.tile];
double B = BT[s.tile]; double B = BT[s.tile];
double C = CT[s.tile]; double C = CT[s.tile];
double Gp = av[G0_INDEX + pair + s.tile * TILE_PARAMS]; double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Wp = corr_wnd[s.ix][s.iy]; double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp; double WGp = Wp * Gp;
double xmxp = s.ix - xp_yp[s.tile][s.fcam][s.scam][0]; // double xmxp = s.ix - xp_yp[s.tile][s.fcam][s.scam][0];
double ymyp = s.iy - xp_yp[s.tile][s.fcam][s.scam][1]; // double ymyp = s.iy - xp_yp[s.tile][s.fcam][s.scam][1];
double xmxp = s.ix - xp_yp[s.tile][fs[0]][fs[1]][0]; // TODO - change format of xp_yp
double ymyp = s.iy - xp_yp[s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp; double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp; double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp; double xmxp_ymyp = xmxp * ymyp;
...@@ -825,31 +896,47 @@ public class Corr2dLMA { ...@@ -825,31 +896,47 @@ public class Corr2dLMA {
System.out.print(""); System.out.print("");
} }
if (jt != null) { if (jt != null) {
if (par_map[DISP_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[DISP_INDEX + s.tile*TILE_PARAMS]][ns] = 2 * WGp * if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) {
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+ jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = 2 * WGp *
(B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0)); // ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
if (par_map[A_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[A_INDEX + s.tile*TILE_PARAMS]][ns] = -WGp*(xmxp2 + ymyp2); // (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
if (par_map[B_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[B_INDEX + s.tile*TILE_PARAMS]][ns] = -WGp* 2 * xmxp_ymyp; ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 0)+
if (par_map[CMA_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[CMA_INDEX + s.tile*TILE_PARAMS]][ns] = -WGp* ymyp2; (B * xmxp + C * ymyp) * m_pairs[s.tile][s.pair].get(1, 0));
for (int p = 0; p < npairs[s.tile]; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0 }
if (par_map[G0_INDEX + p + s.tile*TILE_PARAMS] >= 0) jt[par_map[G0_INDEX + p + s.tile*TILE_PARAMS]][ns] = (p== pair)? d : 0.0; // (par_mask[G0_INDEX + pair])? d; if (par_map[A_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[A_INDEX + s.tile*tile_params]][ns] = -WGp*(xmxp2 + ymyp2);
}
if (par_map[B_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[B_INDEX + s.tile*tile_params]][ns] = -WGp* 2 * xmxp_ymyp;
}
if (par_map[CMA_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[CMA_INDEX + s.tile*tile_params]][ns] = -WGp* ymyp2;
}
// for (int p = 0; p < npairs[s.tile]; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0
for (int p = 0; p < num_pairs; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0
if (par_map[G0_INDEX + p + s.tile*tile_params] >= 0) {
jt[par_map[G0_INDEX + p + s.tile*tile_params]][ns] = (p== pair)? d : 0.0; // (par_mask[G0_INDEX + pair])? d;
}
} }
if (lazy_eye) { if (lazy_eye) {
for (int f = 0; f < NUM_CAMS; f++) { // -1 for the last_cam and pre_last_cam for (int f = 0; f < num_cams; f++) { // -1 for the last_cam and pre_last_cam
if (par_map[ddisp_index + f] >= 0) jt[par_map[ddisp_index + f]][ns] = 0.0; if (par_map[ddisp_index + f] >= 0) jt[par_map[ddisp_index + f]][ns] = 0.0;
if (par_map[ndisp_index + f] >= 0) jt[par_map[ndisp_index + f]][ns] = 0.0; if (par_map[ndisp_index + f] >= 0) jt[par_map[ndisp_index + f]][ns] = 0.0;
} }
double [] dd_deriv = new double[3]; // derivatives by dependent dd_pre_lars, dd_last and nd_last (calculated on demand) with sign according to first/second in a pair double [] dd_deriv = new double[3]; // derivatives by dependent dd_pre_lars, dd_last and nd_last (calculated on demand) with sign according to first/second in a pair
if ((s.fcam == pre_last_cam)) { // if ((s.fcam == pre_last_cam)) {
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGp * dd_deriv[0] = 2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0));
} else if ((s.scam == pre_last_cam)) { // } else if ((s.scam == pre_last_cam)) {
} else if ((fs[1] == pre_last_cam)) {
dd_deriv[0] = -2 * WGp * dd_deriv[0] = -2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0));
} }
if ((s.fcam == last_cam)) { // if ((s.fcam == last_cam)) {
if ((fs[0] == last_cam)) {
dd_deriv[1] = 2 * WGp * dd_deriv[1] = 2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0));
...@@ -857,7 +944,8 @@ public class Corr2dLMA { ...@@ -857,7 +944,8 @@ public class Corr2dLMA {
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 1));
} else if ((s.scam == last_cam)) { // } else if ((s.scam == last_cam)) {
} else if ((fs[1] == last_cam)) {
dd_deriv[1] = -2 * WGp * dd_deriv[1] = -2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0));
...@@ -867,26 +955,26 @@ public class Corr2dLMA { ...@@ -867,26 +955,26 @@ public class Corr2dLMA {
} }
// now accumulate derivatives: // now accumulate derivatives:
// first calculate contributions of the dd, nd directly: // first calculate contributions of the dd, nd directly:
if (par_map[ddisp_index + s.fcam] >= 0){ // par_map[ddisp_index + last_cam] always <0 if (par_map[ddisp_index + fs[0]] >= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + s.fcam]][ns] += 2 * WGp * jt[par_map[ddisp_index + fs[0]]][ns] += 2 * WGp *
((A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 0)+ ((A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 0));
} }
if (par_map[ddisp_index + s.scam]>= 0){ // par_map[ddisp_index + last_cam] always <0 if (par_map[ddisp_index + fs[1]]>= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + s.scam]][ns] -= 2 * WGp * jt[par_map[ddisp_index + fs[1]]][ns] -= 2 * WGp *
((A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 0)+ ((A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 0));
} }
if (par_map[ndisp_index + s.fcam] >=0){ if (par_map[ndisp_index + fs[0]] >=0){
jt[par_map[ndisp_index + s.fcam]][ns] += 2 * WGp * jt[par_map[ndisp_index + fs[0]]][ns] += 2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 1));
} }
if (par_map[ndisp_index + s.scam] >= 0) { if (par_map[ndisp_index + fs[1]] >= 0) {
jt[par_map[ndisp_index + s.scam]][ns] -= 2 * WGp * jt[par_map[ndisp_index + fs[1]]][ns] -= 2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 1));
} }
// now calculate indirect ones through derivatives by dd_pre_last (dd_deriv[0]), dd_last (dd_deriv[1]) and nd_last (dd_deriv[2]) // now calculate indirect ones through derivatives by dd_pre_last (dd_deriv[0]), dd_last (dd_deriv[1]) and nd_last (dd_deriv[2])
//// private int [] dd_indices; //normally 5-long (2 * ncam -3), absolute parameter indices for dd_pre_last, dd_last and nd_last //// private int [] dd_indices; //normally 5-long (2 * ncam -3), absolute parameter indices for dd_pre_last, dd_last and nd_last
...@@ -902,25 +990,25 @@ public class Corr2dLMA { ...@@ -902,25 +990,25 @@ public class Corr2dLMA {
} }
} }
if (lazy_eye) { if (lazy_eye) {
for (int n = 0; n < NUM_CAMS; n++) { // av[ddisp_index +last_cam] and other 2 are already populated for (int n = 0; n < num_cams; n++) { // av[ddisp_index +last_cam] and other 2 are already populated
fx[num_samples + n] = av[ddisp_index + n]; fx[num_samples + n] = av[ddisp_index + n];
fx[num_samples + NUM_CAMS + n] = av[ndisp_index + n]; fx[num_samples + num_cams + n] = av[ndisp_index + n];
} }
// and derivatives // and derivatives
if (jt != null) { if (jt != null) {
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
if ((i != last_cam) && (i != pre_last_cam) && (par_map[ddisp_index + i] >= 0)) { if ((i != last_cam) && (i != pre_last_cam) && (par_map[ddisp_index + i] >= 0)) {
jt[par_map[ddisp_index + i]][num_samples + i] = 1.0; jt[par_map[ddisp_index + i]][num_samples + i] = 1.0;
} }
if ((i != last_cam) && (par_map[ndisp_index + i] >= 0)) { if ((i != last_cam) && (par_map[ndisp_index + i] >= 0)) {
jt[par_map[ndisp_index + i] ][num_samples + NUM_CAMS + i] = 1.0; jt[par_map[ndisp_index + i] ][num_samples + num_cams + i] = 1.0;
} }
} }
for (int i = 0; i < dd_indices.length; i++) { for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][num_samples + pre_last_cam] = mddnd.get(0, i); jt[dd_indices[i]][num_samples + pre_last_cam] = mddnd.get(0, i);
jt[dd_indices[i]][num_samples + last_cam] = mddnd.get(1, i); jt[dd_indices[i]][num_samples + last_cam] = mddnd.get(1, i);
jt[dd_indices[i]][num_samples + NUM_CAMS + last_cam] = mddnd.get(2, i); jt[dd_indices[i]][num_samples + num_cams + last_cam] = mddnd.get(2, i);
} }
} }
} }
...@@ -932,43 +1020,45 @@ public class Corr2dLMA { ...@@ -932,43 +1020,45 @@ public class Corr2dLMA {
double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
if (vector == null) return null; if (vector == null) return null;
double [] av = fromVector(vector); double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][NUM_CAMS]; Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][NUM_CAMS][NUM_CAMS][]; double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0}; double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2); Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX]; double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX]; double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX]; double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){ for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
for (int i = 0; i < NUM_CAMS; i++) if (used_cameras[i]) { for (int i = 0; i < num_cams; i++) if (used_cameras[i]) {
double [] add_dnd = {av[DISP_INDEX+ nTile * TILE_PARAMS]+ av[ddisp_index + i], av[ndisp_index + i]}; double [] add_dnd = {av[DISP_INDEX+ nTile * tile_params]+ av[ddisp_index + i], av[ndisp_index + i]};
xcam_ycam[nTile][i] = m_disp[nTile][i].times(new Matrix(add_dnd,2)); xcam_ycam[nTile][i] = m_disp[nTile][i].times(new Matrix(add_dnd,2));
} }
for (int f = 0; f < NUM_CAMS; f++) if (used_cameras[f]) { for (int f = 0; f < num_cams; f++) if (used_cameras[f]) {
for (int s = 0; s < NUM_CAMS; s++) if (used_cameras[s]) { for (int s = 0; s < num_cams; s++) if (used_cameras[s]) {
xp_yp[nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy(); xp_yp[nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
} }
} }
AT[nTile] = av[A_INDEX + nTile * TILE_PARAMS]; AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * TILE_PARAMS]; BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * TILE_PARAMS]; CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
} }
int num_samples = samples.size(); int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * NUM_CAMS]; double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd //corr_wnd
for (int ns = 0; ns < num_samples; ns++) { for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns); Sample s = samples.get(ns);
int pair = pindx[s.fcam][s.scam]; // all pairs, noit just used? // int pair = pindx[s.fcam][s.scam]; // all pairs, not just used?
int pair = s.pair; // all pairs, not just used?
int [] fs = correlation2d.getPair(pair);
double A = AT[s.tile]; double A = AT[s.tile];
double B = BT[s.tile]; double B = BT[s.tile];
double C = CT[s.tile]; double C = CT[s.tile];
double Gp = av[G0_INDEX + pair + s.tile * TILE_PARAMS]; double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Wp = corr_wnd[s.ix][s.iy]; double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp; double WGp = Wp * Gp;
double xmxp = s.ix - xp_yp[s.tile][s.fcam][s.scam][0]; double xmxp = s.ix - xp_yp[s.tile][fs[0]][fs[1]][0];
double ymyp = s.iy - xp_yp[s.tile][s.fcam][s.scam][1]; double ymyp = s.iy - xp_yp[s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp; double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp; double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp; double xmxp_ymyp = xmxp * ymyp;
...@@ -977,12 +1067,12 @@ public class Corr2dLMA { ...@@ -977,12 +1067,12 @@ public class Corr2dLMA {
// if ((exp > 1000.0) || (exp < 0.0000001)) { // if ((exp > 1000.0) || (exp < 0.0000001)) {
if (exp > 1000.0) { if (exp > 1000.0) {
// System.out.println("Unreasonable exp = "+exp); // System.out.println("Unreasonable exp = "+exp);
removeBadTile(s.tile, s.fcam, s.scam); removeBadTile(s.tile, fs[0], fs[1]);
return null; // should re-start LMA return null; // should re-start LMA
} }
if (Double.isInfinite(exp)) { if (Double.isInfinite(exp)) {
removeBadTile(s.tile, s.fcam, s.scam); removeBadTile(s.tile, fs[0], fs[1]);
return null; // should re-start LMA return null; // should re-start LMA
} }
...@@ -997,34 +1087,34 @@ public class Corr2dLMA { ...@@ -997,34 +1087,34 @@ public class Corr2dLMA {
System.out.print(""); System.out.print("");
} }
if (jt != null) { if (jt != null) {
if (par_map[DISP_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[DISP_INDEX + s.tile*TILE_PARAMS]][ns] = 2 * WGpexp * if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = 2 * WGpexp *
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+ ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 0)+
(B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0)); (B * xmxp + C * ymyp) * m_pairs[s.tile][s.pair].get(1, 0));
if (par_map[A_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[A_INDEX + s.tile*TILE_PARAMS]][ns] = -WGpexp*(xmxp2 + ymyp2); if (par_map[A_INDEX + s.tile*tile_params] >= 0) jt[par_map[A_INDEX + s.tile*tile_params]][ns] = -WGpexp*(xmxp2 + ymyp2);
if (par_map[B_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[B_INDEX + s.tile*TILE_PARAMS]][ns] = -WGpexp* 2 * xmxp_ymyp; if (par_map[B_INDEX + s.tile*tile_params] >= 0) jt[par_map[B_INDEX + s.tile*tile_params]][ns] = -WGpexp* 2 * xmxp_ymyp;
if (par_map[CMA_INDEX + s.tile*TILE_PARAMS] >= 0) jt[par_map[CMA_INDEX + s.tile*TILE_PARAMS]][ns] = -WGpexp* ymyp2; if (par_map[CMA_INDEX + s.tile*tile_params] >= 0) jt[par_map[CMA_INDEX + s.tile*tile_params]][ns] = -WGpexp* ymyp2;
for (int p = 0; p < npairs[s.tile]; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0 for (int p = 0; p < npairs[s.tile]; p++) { // par_mask[G0_INDEX + p] as all pairs either used, or not - then npairs == 0
if (par_map[G0_INDEX + p + s.tile*TILE_PARAMS] >= 0) jt[par_map[G0_INDEX + p + s.tile*TILE_PARAMS]][ns] = (p== pair)? comm : 0.0; // (par_mask[G0_INDEX + pair])? d; if (par_map[G0_INDEX + p + s.tile*tile_params] >= 0) jt[par_map[G0_INDEX + p + s.tile*tile_params]][ns] = (p== pair)? comm : 0.0; // (par_mask[G0_INDEX + pair])? d;
} }
// process ddisp (last camera not used, is equal to minus sum of others to make a sum == 0) // process ddisp (last camera not used, is equal to minus sum of others to make a sum == 0)
if (lazy_eye) { if (lazy_eye) {
for (int f = 0; f < NUM_CAMS; f++) if (par_map[ddisp_index + f] >= 0) { // -1 for the last_cam for (int f = 0; f < num_cams; f++) if (par_map[ddisp_index + f] >= 0) { // -1 for the last_cam
jt[par_map[ddisp_index + f]][ns] = 0.0; jt[par_map[ddisp_index + f]][ns] = 0.0;
jt[par_map[ndisp_index + f]][ns] = 0.0; jt[par_map[ndisp_index + f]][ns] = 0.0;
} }
double [] dd_deriv = new double[3]; // derivatives by dependent dd_pre_lars, dd_last and nd_last (calculated on demand) with sign according to first/second in a pair double [] dd_deriv = new double[3]; // derivatives by dependent dd_pre_lars, dd_last and nd_last (calculated on demand) with sign according to first/second in a pair
if ((s.fcam == pre_last_cam)) { if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGpexp * dd_deriv[0] = 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0));
} else if ((s.scam == pre_last_cam)) { } else if ((fs[1] == pre_last_cam)) {
dd_deriv[0] = -2 * WGpexp * dd_deriv[0] = -2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][pre_last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][pre_last_cam].get(1, 0));
} }
if ((s.fcam == last_cam)) { if ((fs[0] == last_cam)) {
dd_deriv[1] = 2 * WGpexp * dd_deriv[1] = 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0));
...@@ -1032,7 +1122,7 @@ public class Corr2dLMA { ...@@ -1032,7 +1122,7 @@ public class Corr2dLMA {
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 1));
} else if ((s.scam == last_cam)) { } else if ((fs[1] == last_cam)) {
dd_deriv[1] = -2 * WGpexp * dd_deriv[1] = -2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][last_cam].get(1, 0));
...@@ -1042,26 +1132,26 @@ public class Corr2dLMA { ...@@ -1042,26 +1132,26 @@ public class Corr2dLMA {
} }
// now accumulate derivatives: // now accumulate derivatives:
// first calculate contributions of the dd, nd directly: // first calculate contributions of the dd, nd directly:
if (par_map[ddisp_index + s.fcam] >= 0){ // par_map[ddisp_index + last_cam] always <0 if (par_map[ddisp_index + fs[0]] >= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + s.fcam]][ns] += 2 * WGpexp * jt[par_map[ddisp_index + fs[0]]][ns] += 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 0));
} }
if (par_map[ddisp_index + s.scam]>= 0){ // par_map[ddisp_index + last_cam] always <0 if (par_map[ddisp_index + fs[1]]>= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + s.scam]][ns] -= 2 * WGpexp * jt[par_map[ddisp_index + fs[1]]][ns] -= 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 0)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 0)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 0));
} }
if (par_map[ndisp_index + s.fcam] >=0){ if (par_map[ndisp_index + fs[0]] >=0){
jt[par_map[ndisp_index + s.fcam]][ns] += 2 * WGpexp * jt[par_map[ndisp_index + fs[0]]][ns] += 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.fcam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.fcam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 1));
} }
if (par_map[ndisp_index + s.scam] >= 0) { if (par_map[ndisp_index + fs[1]] >= 0) {
jt[par_map[ndisp_index + s.scam]][ns] -= 2 * WGpexp * jt[par_map[ndisp_index + fs[1]]][ns] -= 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][s.scam].get(0, 1)+ ( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][s.scam].get(1, 1)); (B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 1));
} }
// now calculate indirect ones through derivatives by dd_pre_last (dd_deriv[0]), dd_last (dd_deriv[1]) and nd_last (dd_deriv[2]) // now calculate indirect ones through derivatives by dd_pre_last (dd_deriv[0]), dd_last (dd_deriv[1]) and nd_last (dd_deriv[2])
...@@ -1078,25 +1168,25 @@ public class Corr2dLMA { ...@@ -1078,25 +1168,25 @@ public class Corr2dLMA {
} }
} }
if (lazy_eye) { if (lazy_eye) {
for (int n = 0; n < NUM_CAMS; n++) { // av[ddisp_index +last_cam] and other 2 are already populated for (int n = 0; n < num_cams; n++) { // av[ddisp_index +last_cam] and other 2 are already populated
fx[num_samples + n] = av[ddisp_index + n]; fx[num_samples + n] = av[ddisp_index + n];
fx[num_samples + NUM_CAMS + n] = av[ndisp_index + n]; fx[num_samples + num_cams + n] = av[ndisp_index + n];
} }
// and derivatives // and derivatives
if (jt != null) { if (jt != null) {
for (int i = 0; i < NUM_CAMS; i++) { for (int i = 0; i < num_cams; i++) {
if ((i != last_cam) && (i != pre_last_cam) && (par_map[ddisp_index + i] >= 0)) { if ((i != last_cam) && (i != pre_last_cam) && (par_map[ddisp_index + i] >= 0)) {
jt[par_map[ddisp_index + i]][num_samples + i] = 1.0; jt[par_map[ddisp_index + i]][num_samples + i] = 1.0;
} }
if ((i != last_cam) && (par_map[ndisp_index + i] >= 0)) { if ((i != last_cam) && (par_map[ndisp_index + i] >= 0)) {
jt[par_map[ndisp_index + i] ][num_samples + NUM_CAMS + i] = 1.0; jt[par_map[ndisp_index + i] ][num_samples + num_cams + i] = 1.0;
} }
} }
for (int i = 0; i < dd_indices.length; i++) { for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][num_samples + pre_last_cam] = mddnd.get(0, i); jt[dd_indices[i]][num_samples + pre_last_cam] = mddnd.get(0, i);
jt[dd_indices[i]][num_samples + last_cam] = mddnd.get(1, i); jt[dd_indices[i]][num_samples + last_cam] = mddnd.get(1, i);
jt[dd_indices[i]][num_samples + NUM_CAMS + last_cam] = mddnd.get(2, i); jt[dd_indices[i]][num_samples + num_cams + last_cam] = mddnd.get(2, i);
} }
} }
} }
...@@ -1113,8 +1203,8 @@ public class Corr2dLMA { ...@@ -1113,8 +1203,8 @@ public class Corr2dLMA {
if (np >= ndisp_index) parname = PAR_NAME_CORRNDISP + (np - ndisp_index); if (np >= ndisp_index) parname = PAR_NAME_CORRNDISP + (np - ndisp_index);
else if (np >= ddisp_index) parname = PAR_NAME_CORRDISP + (np - ddisp_index); else if (np >= ddisp_index) parname = PAR_NAME_CORRDISP + (np - ddisp_index);
else { else {
int ntile = np / TILE_PARAMS; int ntile = np / tile_params;
int anpr = np % TILE_PARAMS; int anpr = np % tile_params;
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile; if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile;
else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX); else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX);
} }
...@@ -1202,7 +1292,9 @@ public class Corr2dLMA { ...@@ -1202,7 +1292,9 @@ public class Corr2dLMA {
double fx_pos = fx[i]; double fx_pos = fx[i];
if (i < samples.size()) { if (i < samples.size()) {
s = samples.get(i); s = samples.get(i);
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f fx=%9.6f w=%9.7f fcam=%1d scam=%1d tile=%d", i, s.ix, s.iy, s.v, fx_pos, s.w, s.fcam, s.scam, s.tile)); int [] fs = correlation2d.getPair(s.pair);
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f fx=%9.6f w=%9.7f pair=%2d scam=%2d tile=%d",
i, s.ix, s.iy, s.v, fx_pos, s.w, fs[0], fs[1], s.tile));
} }
else { else {
System.out.println(String.format("%3d: %2s %2s v=%9.6f fx=%9.6f w=%9.7f", i, "-", "-", this.values[i], fx_pos, this.weights[i])); System.out.println(String.format("%3d: %2s %2s v=%9.6f fx=%9.6f w=%9.7f", i, "-", "-", this.values[i], fx_pos, this.weights[i]));
...@@ -1211,7 +1303,9 @@ public class Corr2dLMA { ...@@ -1211,7 +1303,9 @@ public class Corr2dLMA {
} else { } else {
int ns =0; int ns =0;
for (Sample s:samples){ for (Sample s:samples){
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f w=%9.7f fcam=%1d scam=%1d tile=%d", ns++, s.ix, s.iy, s.v, s.w, s.fcam, s.scam, s.tile)); int [] fs = correlation2d.getPair(s.pair);
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f w=%9.7f fcam=%1d scam=%1d tile=%d",
ns++, s.ix, s.iy, s.v, s.w, fs[0], fs[1], s.tile));
} }
} }
} }
...@@ -1228,10 +1322,10 @@ public class Corr2dLMA { ...@@ -1228,10 +1322,10 @@ public class Corr2dLMA {
} }
public double [] getDisparityStrength(int nTile) { // USED in lwir public double [] getDisparityStrength(int nTile) { // USED in lwir
double disparity = -all_pars[DISP_INDEX + nTile*TILE_PARAMS]; double disparity = -all_pars[DISP_INDEX + nTile*tile_params];
double sum_amp = 0.0; double sum_amp = 0.0;
for (int i = 0; i < NUM_PAIRS; i++) { for (int i = 0; i < num_pairs; i++) {
sum_amp += all_pars[G0_INDEX + i + TILE_PARAMS * nTile]; // group_weights is normalized sum_amp += all_pars[G0_INDEX + i + tile_params * nTile]; // group_weights is normalized
} }
// protect from weird fitting results // protect from weird fitting results
double max_amp = 0.0; double max_amp = 0.0;
...@@ -1254,12 +1348,12 @@ public class Corr2dLMA { ...@@ -1254,12 +1348,12 @@ public class Corr2dLMA {
public double [] getDisparityStrengthABC(int nTile) {// width = 1/sqrt(all_pars[A_INDEX]) public double [] getDisparityStrengthABC(int nTile) {// width = 1/sqrt(all_pars[A_INDEX])
double [] ds = getDisparityStrength(nTile); double [] ds = getDisparityStrength(nTile);
if (ds == null) return null; if (ds == null) return null;
double [] dsw = {ds[0], ds[1], all_pars[A_INDEX + TILE_PARAMS * nTile], all_pars[B_INDEX+ TILE_PARAMS * nTile],all_pars[CMA_INDEX+ TILE_PARAMS * nTile]}; // asymmetry double [] dsw = {ds[0], ds[1], all_pars[A_INDEX + tile_params * nTile], all_pars[B_INDEX+ tile_params * nTile],all_pars[CMA_INDEX+ tile_params * nTile]}; // asymmetry
return dsw; return dsw;
} }
public double [] getLazyEye() { public double [] getLazyEye() {
double [] rslt = new double [2 * NUM_CAMS]; double [] rslt = new double [2 * num_cams];
for (int i = 0; i < rslt.length; i++) { for (int i = 0; i < rslt.length; i++) {
rslt[i] = all_pars[ddisp_index + i]; rslt[i] = all_pars[ddisp_index + i];
} }
...@@ -1280,12 +1374,12 @@ public class Corr2dLMA { ...@@ -1280,12 +1374,12 @@ public class Corr2dLMA {
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) all_pars[i] = vector[np++]; for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) all_pars[i] = vector[np++];
// just for reporting // just for reporting
double [] a5 = new double [2 * ncam-3]; double [] a5 = new double [2 * ncam_used-3];
for (int i = 0; i < (ncam - 2); i++) { for (int i = 0; i < (ncam_used - 2); i++) {
a5[i] = all_pars[ddisp_index + used_cams_rmap[i]]; a5[i] = all_pars[ddisp_index + used_cams_rmap[i]];
} }
for (int i = 0; i < (ncam - 1); i++) { for (int i = 0; i < (ncam_used - 1); i++) {
a5[ncam - 2 + i] = all_pars[ndisp_index + used_cams_rmap[i]]; a5[ncam_used - 2 + i] = all_pars[ndisp_index + used_cams_rmap[i]];
} }
Matrix m5 = new Matrix(a5,a5.length); // single column, normally 5 rows Matrix m5 = new Matrix(a5,a5.length); // single column, normally 5 rows
Matrix m3 = mddnd.times(m5); Matrix m3 = mddnd.times(m5);
...@@ -1302,12 +1396,12 @@ public class Corr2dLMA { ...@@ -1302,12 +1396,12 @@ public class Corr2dLMA {
int np = 0; int np = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) ap[i] = vector[np++]; for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) ap[i] = vector[np++];
// Fill in missing values (2 last dd-s, 1 last nd) // Fill in missing values (2 last dd-s, 1 last nd)
double [] a5 = new double [2 * ncam-3]; double [] a5 = new double [2 * ncam_used-3];
for (int i = 0; i < (ncam - 2); i++) { for (int i = 0; i < (ncam_used - 2); i++) {
a5[i] = ap[ddisp_index + used_cams_rmap[i]]; a5[i] = ap[ddisp_index + used_cams_rmap[i]];
} }
for (int i = 0; i < (ncam - 1); i++) { for (int i = 0; i < (ncam_used - 1); i++) {
a5[ncam - 2 + i] = ap[ndisp_index + used_cams_rmap[i]]; a5[ncam_used - 2 + i] = ap[ndisp_index + used_cams_rmap[i]];
} }
Matrix m5 = new Matrix(a5,a5.length); // single column, normally 5 rows Matrix m5 = new Matrix(a5,a5.length); // single column, normally 5 rows
Matrix m3 = mddnd.times(m5); Matrix m3 = mddnd.times(m5);
...@@ -1340,8 +1434,8 @@ public class Corr2dLMA { ...@@ -1340,8 +1434,8 @@ public class Corr2dLMA {
if (anp >= ndisp_index) parname = PAR_NAME_CORRNDISP + (anp - ndisp_index); if (anp >= ndisp_index) parname = PAR_NAME_CORRNDISP + (anp - ndisp_index);
else if (anp >= ddisp_index) parname = PAR_NAME_CORRDISP + (anp - ddisp_index); else if (anp >= ddisp_index) parname = PAR_NAME_CORRDISP + (anp - ddisp_index);
else { else {
int ntile = anp / TILE_PARAMS; int ntile = anp / tile_params;
int anpr = anp % TILE_PARAMS; int anpr = anp % tile_params;
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile; if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile;
else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX); else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX);
} }
...@@ -1351,7 +1445,9 @@ public class Corr2dLMA { ...@@ -1351,7 +1445,9 @@ public class Corr2dLMA {
int npair0 = -1; int npair0 = -1;
for (int i = 0; i < num_points; i++) { for (int i = 0; i < num_points; i++) {
if (i < samples.size()) { if (i < samples.size()) {
int npair = used_pairs_map[samples.get(i).tile][samples.get(i).fcam][samples.get(i).scam]; int [] fs = correlation2d.getPair(samples.get(i).pair);
// int npair = used_pairs_map[samples.get(i).tile][samples.get(i).fcam][samples.get(i).scam];
int npair = used_pairs_map[samples.get(i).tile][fs[0]][fs[1]];
if (npair !=npair0) { if (npair !=npair0) {
if (npair0 >=0) System.out.println(); if (npair0 >=0) System.out.println();
npair0 = npair; npair0 = npair;
...@@ -1498,7 +1594,7 @@ public class Corr2dLMA { ...@@ -1498,7 +1594,7 @@ public class Corr2dLMA {
} }
double strength = Math.sqrt(avg/rrms); double strength = Math.sqrt(avg/rrms);
double disparity = -all_pars[DISP_INDEX + tile*TILE_PARAMS]; double disparity = -all_pars[DISP_INDEX + tile*tile_params];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) { if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue; continue;
} }
...@@ -1550,7 +1646,7 @@ public class Corr2dLMA { ...@@ -1550,7 +1646,7 @@ public class Corr2dLMA {
} }
double strength = Math.sqrt(avg/rrms); double strength = Math.sqrt(avg/rrms);
double disparity = -all_pars[DISP_INDEX + tile*TILE_PARAMS]; double disparity = -all_pars[DISP_INDEX + tile*tile_params];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) { if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue; continue;
} }
...@@ -1600,7 +1696,7 @@ public class Corr2dLMA { ...@@ -1600,7 +1696,7 @@ public class Corr2dLMA {
} }
double strength = Math.sqrt(avg/rrms); double strength = Math.sqrt(avg/rrms);
double disparity = -all_pars[DISP_INDEX + tile*TILE_PARAMS]; double disparity = -all_pars[DISP_INDEX + tile*tile_params];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) { if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue; continue;
} }
...@@ -1668,13 +1764,13 @@ public class Corr2dLMA { ...@@ -1668,13 +1764,13 @@ public class Corr2dLMA {
double strength = Math.sqrt(avg/rrms); double strength = Math.sqrt(avg/rrms);
ext_stats[tile][9] = (strength * lma_str_scale) + lma_str_offset; ext_stats[tile][9] = (strength * lma_str_scale) + lma_str_offset;
ext_stats[tile][3] = Math.sqrt(strength / area ); // new strength ext_stats[tile][3] = Math.sqrt(strength / area ); // new strength
double disparity = -all_pars[DISP_INDEX + tile*TILE_PARAMS]; double disparity = -all_pars[DISP_INDEX + tile*tile_params];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) { if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
if (ext_stats[tile][10] == 0) ext_stats[tile][10] = 5; if (ext_stats[tile][10] == 0) ext_stats[tile][10] = 5;
continue; continue;
} }
ext_stats[tile][8] = -all_pars[DISP_INDEX + tile*TILE_PARAMS]; ext_stats[tile][8] = -all_pars[DISP_INDEX + tile*tile_params];
if (ext_stats[tile][10] == 0) ext_stats[tile][10] = 6; if (ext_stats[tile][10] == 0) ext_stats[tile][10] = 6;
} }
return ext_stats; return ext_stats;
...@@ -1682,8 +1778,8 @@ public class Corr2dLMA { ...@@ -1682,8 +1778,8 @@ public class Corr2dLMA {
// ext_stats[tile][3] = Math.max(abc[tile][0], abc[tile][2])-lma_min_ac; // ext_stats[tile][3] = Math.max(abc[tile][0], abc[tile][2])-lma_min_ac;
public double [][] getDdNd(){ // this.all_pars should be current public double [][] getDdNd(){ // this.all_pars should be current
double [][] ddnd = new double [NUM_CAMS][2]; double [][] ddnd = new double [num_cams][2];
for (int nc = 0; nc < NUM_CAMS; nc++) { for (int nc = 0; nc < num_cams; nc++) {
ddnd[nc][0] = all_pars[ddisp_index + nc]; ddnd[nc][0] = all_pars[ddisp_index + nc];
ddnd[nc][1] = all_pars[ndisp_index + nc]; ddnd[nc][1] = all_pars[ndisp_index + nc];
} }
...@@ -1704,7 +1800,7 @@ public class Corr2dLMA { ...@@ -1704,7 +1800,7 @@ public class Corr2dLMA {
public void getDdNd(double [][] ddnd){ // this.all_pars should be current public void getDdNd(double [][] ddnd){ // this.all_pars should be current
if (ddnd != null) { if (ddnd != null) {
for (int nc = 0; nc < NUM_CAMS; nc++) { for (int nc = 0; nc < num_cams; nc++) {
ddnd[nc] = new double [2]; ddnd[nc] = new double [2];
ddnd[nc][0] = all_pars[ddisp_index + nc]; ddnd[nc][0] = all_pars[ddisp_index + nc];
ddnd[nc][1] = all_pars[ndisp_index + nc]; ddnd[nc][1] = all_pars[ndisp_index + nc];
...@@ -1760,13 +1856,13 @@ public class Corr2dLMA { ...@@ -1760,13 +1856,13 @@ public class Corr2dLMA {
for (int tile = 0; tile < numTiles; tile++) { for (int tile = 0; tile < numTiles; tile++) {
maxmins[tile][0] = Double.NaN; maxmins[tile][0] = Double.NaN;
maxmins[tile][1] = Double.NaN; maxmins[tile][1] = Double.NaN;
for (int i = 0; i <NUM_PAIRS; i++) { for (int i = 0; i <num_pairs; i++) {
if (par_mask[G0_INDEX + i + tile * TILE_PARAMS]) { if (par_mask[G0_INDEX + i + tile * tile_params]) {
if (!(all_pars[G0_INDEX + i + tile*TILE_PARAMS] <= maxmins[tile][0])) { // NaN will trigger true if (!(all_pars[G0_INDEX + i + tile*tile_params] <= maxmins[tile][0])) { // NaN will trigger true
maxmins[tile][0] = all_pars[G0_INDEX + i + tile*TILE_PARAMS]; maxmins[tile][0] = all_pars[G0_INDEX + i + tile*tile_params];
} }
if (!(all_pars[G0_INDEX + i + tile*TILE_PARAMS] >= maxmins[tile][1])) { if (!(all_pars[G0_INDEX + i + tile*tile_params] >= maxmins[tile][1])) {
maxmins[tile][1] = all_pars[G0_INDEX + i + tile*TILE_PARAMS]; maxmins[tile][1] = all_pars[G0_INDEX + i + tile*tile_params];
} }
} }
} }
...@@ -1778,12 +1874,13 @@ public class Corr2dLMA { ...@@ -1778,12 +1874,13 @@ public class Corr2dLMA {
* @return array [tile][{vmax, vmin}] * @return array [tile][{vmax, vmin}]
*/ */
public double [][] getMaxMinValTile(){ public double [][] getMaxMinValTile(){
double [][] tile_pair_max = new double[numTiles][NUM_PAIRS]; double [][] tile_pair_max = new double[numTiles][num_pairs];
int num_samples = samples.size(); int num_samples = samples.size();
for (int i = 0; i < num_samples; i++) { for (int i = 0; i < num_samples; i++) {
Sample s = samples.get(i); Sample s = samples.get(i);
int tile = s.tile; int tile = s.tile;
int pair = pindx[s.fcam][s.scam]; // int pair = pindx[s.fcam][s.scam];
int pair = s.pair;
if (s.v > tile_pair_max[tile][pair]) tile_pair_max[tile][pair] = s.v; if (s.v > tile_pair_max[tile][pair]) tile_pair_max[tile][pair] = s.v;
} }
...@@ -1791,7 +1888,7 @@ public class Corr2dLMA { ...@@ -1791,7 +1888,7 @@ public class Corr2dLMA {
for (int tile = 0; tile < numTiles; tile++) { for (int tile = 0; tile < numTiles; tile++) {
maxmins[tile][0] = Double.NaN; maxmins[tile][0] = Double.NaN;
maxmins[tile][1] = Double.NaN; maxmins[tile][1] = Double.NaN;
for (int i = 0; i <NUM_PAIRS; i++) { for (int i = 0; i <num_pairs; i++) {
if (tile_pair_max[tile][i] > 0) { if (tile_pair_max[tile][i] > 0) {
if (!(tile_pair_max[tile][i] <= maxmins[tile][0])) { // NaN will trigger true if (!(tile_pair_max[tile][i] <= maxmins[tile][0])) { // NaN will trigger true
maxmins[tile][0] = tile_pair_max[tile][i]; maxmins[tile][0] = tile_pair_max[tile][i];
...@@ -1808,9 +1905,9 @@ public class Corr2dLMA { ...@@ -1808,9 +1905,9 @@ public class Corr2dLMA {
public double [][] getABCTile(){ public double [][] getABCTile(){
double [][] abc = new double[numTiles][3]; double [][] abc = new double[numTiles][3];
for (int tile = 0; tile < numTiles; tile++) { for (int tile = 0; tile < numTiles; tile++) {
abc[tile][0] = (par_mask[A_INDEX+ tile * TILE_PARAMS])? all_pars[A_INDEX+ tile * TILE_PARAMS] :Double.NaN; abc[tile][0] = (par_mask[A_INDEX+ tile * tile_params])? all_pars[A_INDEX+ tile * tile_params] :Double.NaN;
abc[tile][1] = (par_mask[B_INDEX+ tile * TILE_PARAMS])? all_pars[B_INDEX+ tile * TILE_PARAMS] :Double.NaN; abc[tile][1] = (par_mask[B_INDEX+ tile * tile_params])? all_pars[B_INDEX+ tile * tile_params] :Double.NaN;
abc[tile][2] = abc[tile][0] + ( (par_mask[CMA_INDEX+ tile * TILE_PARAMS])? all_pars[CMA_INDEX+ tile * TILE_PARAMS] :Double.NaN); abc[tile][2] = abc[tile][0] + ( (par_mask[CMA_INDEX+ tile * tile_params])? all_pars[CMA_INDEX+ tile * tile_params] :Double.NaN);
} }
return abc; return abc;
} }
......
src/main/java/com/elphel/imagej/tileprocessor/Correlation2d.java
View file @ 1a329010
...@@ -278,6 +278,10 @@ public class Correlation2d { ...@@ -278,6 +278,10 @@ public class Correlation2d {
corr_pairs = sel.clone(); corr_pairs = sel.clone();
} }
public int getNumSensors() {
return this.numSensors;
}
public void setCorrPairsFilter(double [][][][][][] clt_data, int tileY, int tileX ) { // these pairs will be correlated public void setCorrPairsFilter(double [][][][][][] clt_data, int tileY, int tileX ) { // these pairs will be correlated
corr_pairs_filter = new boolean [pair_start_end.length]; corr_pairs_filter = new boolean [pair_start_end.length];
boolean [] en = new boolean [numSensors]; boolean [] en = new boolean [numSensors];
...@@ -322,6 +326,20 @@ public class Correlation2d { ...@@ -322,6 +326,20 @@ public class Correlation2d {
return cor_titles_combo; return cor_titles_combo;
} }
public static boolean [] longToArray(long sel_bits, int num_pairs) {
boolean [] sel = new boolean [num_pairs];
for (int i = 0; i < sel.length;i++) {
sel[i] = (sel_bits & 1) > 0;
sel_bits >>= 1;
}
return sel;
}
public boolean [] longToArray(long sel_bits) {
return longToArray (sel_bits, pair_start_end.length);
}
/** /**
* Add 2D correlation for a pair to the combined correlation tile, applying rotation/scaling * Add 2D correlation for a pair to the combined correlation tile, applying rotation/scaling
* @param accum_tile tile for accumulation in line-scan order, same dimension as during generateResample() * @param accum_tile tile for accumulation in line-scan order, same dimension as during generateResample()
...@@ -1621,10 +1639,10 @@ public class Correlation2d { ...@@ -1621,10 +1639,10 @@ public class Correlation2d {
* (1 - largest, 2 - half, 4 - quarter) * (1 - largest, 2 - half, 4 - quarter)
* @return single square correlation array, same dimension as the input (now 15x15) * @return single square correlation array, same dimension as the input (now 15x15)
*/ */
@Deprecated
public double [] combineCompatiblePairs(// USED in lwir public double [] combineCompatiblePairs(// USED in lwir
double [][] correlations, double [][] correlations,
int pairs_mask, boolean [] pairs_mask,
boolean diagonal, boolean diagonal,
int baseline_scale int baseline_scale
) { ) {
...@@ -1632,7 +1650,9 @@ public class Correlation2d { ...@@ -1632,7 +1650,9 @@ public class Correlation2d {
double [] combo = new double [width * width]; double [] combo = new double [width * width];
int number_combined = 0; int number_combined = 0;
// find diagonal/ortho and scale that determine compatible correlations // find diagonal/ortho and scale that determine compatible correlations
for (int npair = 0; npair < PAIRS.length; npair++) if ((((pairs_mask >> npair) & 1) != 0 ) && (correlations[npair]!=null) && // if (pairs_mask[npair] && (correlations[npair]!=null)){
// for (int npair = 0; npair < PAIRS.length; npair++) if ((((pairs_mask >> npair) & 1) != 0 ) && (correlations[npair]!=null) &&
for (int npair = 0; npair < PAIRS.length; npair++) if (pairs_mask[npair] && (correlations[npair]!=null) &&
(isDiagonalPair(npair) == diagonal) && (PAIRS[npair][3] == baseline_scale)){ (isDiagonalPair(npair) == diagonal) && (PAIRS[npair][3] == baseline_scale)){
if (isHorizontalPair(npair) || isDiagonalMainPair(npair)) { if (isHorizontalPair(npair) || isDiagonalMainPair(npair)) {
for (int i = 0; i < combo.length; i++) combo[i]+= correlations[npair][i]; for (int i = 0; i < combo.length; i++) combo[i]+= correlations[npair][i];
...@@ -1662,9 +1682,10 @@ public class Correlation2d { ...@@ -1662,9 +1682,10 @@ public class Correlation2d {
* (1 - largest, 2 - half, 4 - quarter) * (1 - largest, 2 - half, 4 - quarter)
* @return {number of compatible pairs among the selection, index of the base pair} * @return {number of compatible pairs among the selection, index of the base pair}
*/ */
@Deprecated
public int [] getNumberBaseOfCompatiblePairs(// USED in lwir public int [] getNumberBaseOfCompatiblePairs(// USED in lwir
double [][] correlations, double [][] correlations,
int pairs_mask, boolean [] pairs_mask,
boolean diagonal, boolean diagonal,
int baseline_scale int baseline_scale
) { ) {
...@@ -1674,7 +1695,8 @@ public class Correlation2d { ...@@ -1674,7 +1695,8 @@ public class Correlation2d {
for (int npair = 0; npair < PAIRS.length; npair++) { for (int npair = 0; npair < PAIRS.length; npair++) {
if ((isDiagonalPair(npair) == diagonal) && (PAIRS[npair][3] == baseline_scale)){ if ((isDiagonalPair(npair) == diagonal) && (PAIRS[npair][3] == baseline_scale)){
if (base_pair < 0) base_pair = npair; if (base_pair < 0) base_pair = npair;
if ((((pairs_mask >> npair) & 1) != 0 ) && (correlations[npair]!=null)){ // if ((((pairs_mask >> npair) & 1) != 0 ) && (correlations[npair]!=null)){
if (pairs_mask[npair] && (correlations[npair]!=null)){
number_combined++; number_combined++;
} }
} }
...@@ -2795,7 +2817,7 @@ public class Correlation2d { ...@@ -2795,7 +2817,7 @@ public class Correlation2d {
Correlations2dLMA lma=corrLMA( Correlations2dLMA lma=corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs, corrs, // double [][] corrs,
this_mask, // int pair_mask, // which pairs to process longToArray(this_mask), //this_mask, // int pair_mask, // which pairs to process
true, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation true, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
xcenter, // double xcenter, // preliminary center x in pixels for largest baseline xcenter, // double xcenter, // preliminary center x in pixels for largest baseline
vasw_pwr, // double vasw_pwr, // value as weight to this power, vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -2946,7 +2968,7 @@ public class Correlation2d { ...@@ -2946,7 +2968,7 @@ public class Correlation2d {
Correlations2dLMA lma=corrLMA( Correlations2dLMA lma=corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs, corrs, // double [][] corrs,
this_mask, // int pair_mask, // which pairs to process longToArray(this_mask), // this_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
xcenter, // double xcenter, // preliminary center x in pixels for largest baseline xcenter, // double xcenter, // preliminary center x in pixels for largest baseline
vasw_pwr, // double vasw_pwr, // value as weight to this power, vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -3141,7 +3163,7 @@ public class Correlation2d { ...@@ -3141,7 +3163,7 @@ public class Correlation2d {
double [][][] corrs, // per tile, per pair, 2 correlation in line-scan order double [][][] corrs, // per tile, per pair, 2 correlation in line-scan order
double [][][] disp_dist, // per tile, per camera disparity matrix as a 1d (linescan order) double [][][] disp_dist, // per tile, per camera disparity matrix as a 1d (linescan order)
double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
int pair_mask, // which pairs to process boolean [] pair_mask, // which pairs to process
double[][] disp_str, // -preliminary center x in pixels for largest baseline double[][] disp_str, // -preliminary center x in pixels for largest baseline
double vasw_pwr, // value as weight to this power, double vasw_pwr, // value as weight to this power,
int debug_level, int debug_level,
...@@ -3165,6 +3187,7 @@ public class Correlation2d { ...@@ -3165,6 +3187,7 @@ public class Correlation2d {
} }
Corr2dLMA lma = new Corr2dLMA( Corr2dLMA lma = new Corr2dLMA(
corrs.length, corrs.length,
this, // Correlation2d correlation2d,
transform_size, transform_size,
corr_wnd, corr_wnd,
rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
...@@ -3194,8 +3217,8 @@ public class Correlation2d { ...@@ -3194,8 +3217,8 @@ public class Correlation2d {
((disp_str == null) || ((disp_str[ntile] != null) && (disp_str[ntile][1] > 0.0)))){ ((disp_str == null) || ((disp_str[ntile] != null) && (disp_str[ntile][1] > 0.0)))){
double[][] corr = new double[corrs[ntile].length][]; double[][] corr = new double[corrs[ntile].length][];
double [][] filtWeight = new double [corrs[ntile].length][]; double [][] filtWeight = new double [corrs[ntile].length][];
// blur_max[ntile] = new double [corrs[ntile].length]; // for (int npair = 0; npair < corrs[ntile].length; npair++) if ((corrs[ntile][npair] != null) && (((pair_mask >> npair) & 1) !=0)){
for (int npair = 0; npair < corrs[ntile].length; npair++) if ((corrs[ntile][npair] != null) && (((pair_mask >> npair) & 1) !=0)){ for (int npair = 0; npair < corrs[ntile].length; npair++) if ((corrs[ntile][npair] != null) && (pair_mask[npair])){
corr[npair] = corrs[ntile][npair].clone(); corr[npair] = corrs[ntile][npair].clone();
if (corr_wnd_inv_limited != null) { if (corr_wnd_inv_limited != null) {
for (int i = 0; i < corr.length; i++) { for (int i = 0; i < corr.length; i++) {
...@@ -3253,8 +3276,8 @@ public class Correlation2d { ...@@ -3253,8 +3276,8 @@ public class Correlation2d {
// numpairs // numpairs
if (numpairs >= imgdtt_params.cnvx_min_pairs) { if (numpairs >= imgdtt_params.cnvx_min_pairs) {
for (int npair = 0; npair < corrs[ntile].length; npair++) if (filtWeight[npair] != null){ for (int npair = 0; npair < corrs[ntile].length; npair++) if (filtWeight[npair] != null){
int fcam = PAIRS[npair][0]; // int fcam = PAIRS[npair][0];
int scam = PAIRS[npair][1]; // int scam = PAIRS[npair][1];
for (int i = 1; i < filtWeight[npair].length; i++) if (filtWeight[npair][i] > 0.0) { for (int i = 1; i < filtWeight[npair].length; i++) if (filtWeight[npair][i] > 0.0) {
int ix = i % corr_size; // >=0 int ix = i % corr_size; // >=0
int iy = i / corr_size; // >=0 int iy = i / corr_size; // >=0
...@@ -3265,8 +3288,9 @@ public class Correlation2d { ...@@ -3265,8 +3288,9 @@ public class Correlation2d {
} }
lma.addSample( // x = 0, y=0 - center lma.addSample( // x = 0, y=0 - center
ntile, // tile ntile, // tile
fcam, // int fcam, // first camera index npair,
scam, // int scam, // second camera index // fcam, // int fcam, // first camera index
// scam, // int scam, // second camera index
ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
iy, // int y, // y coordinate (0 - disparity axis) iy, // int y, // y coordinate (0 - disparity axis)
v, // double v, // correlation value at that point v, // double v, // correlation value at that point
...@@ -3390,6 +3414,7 @@ public class Correlation2d { ...@@ -3390,6 +3414,7 @@ public class Correlation2d {
} else { // have to restart LMA initialization } else { // have to restart LMA initialization
lma = new Corr2dLMA( lma = new Corr2dLMA(
corrs.length, corrs.length,
this, // Correlation2d correlation2d,
transform_size, transform_size,
corr_wnd, corr_wnd,
rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
...@@ -3540,7 +3565,7 @@ public class Correlation2d { ...@@ -3540,7 +3565,7 @@ public class Correlation2d {
double [][] corrs, double [][] corrs,
double [][] disp_dist, // per camera disparity matrix as a 1d (linescan order) double [][] disp_dist, // per camera disparity matrix as a 1d (linescan order)
double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
int pair_mask, // which pairs to process boolean [] pair_mask, // which pairs to process
double[] disp_str, // -preliminary center x in pixels for largest baseline double[] disp_str, // -preliminary center x in pixels for largest baseline
double[] poly_ds, // null or pair of disparity/strength double[] poly_ds, // null or pair of disparity/strength
double vasw_pwr, // value as weight to this power, double vasw_pwr, // value as weight to this power,
...@@ -3564,6 +3589,7 @@ public class Correlation2d { ...@@ -3564,6 +3589,7 @@ public class Correlation2d {
int corr_size = 2 * transform_size - 1; int corr_size = 2 * transform_size - 1;
Corr2dLMA lma = new Corr2dLMA( Corr2dLMA lma = new Corr2dLMA(
1, 1,
this, // Correlation2d correlation2d,
transform_size, transform_size,
corr_wnd, corr_wnd,
rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
...@@ -3584,7 +3610,9 @@ public class Correlation2d { ...@@ -3584,7 +3610,9 @@ public class Correlation2d {
} }
for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (((pair_mask >> npair) & 1) !=0)){ // for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (((pair_mask >> npair) & 1) !=0)){
for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (pair_mask[npair])){
// double[] corr = corrs[npair].clone(); // double[] corr = corrs[npair].clone();
double [] corr_blur = corrs[npair].clone(); double [] corr_blur = corrs[npair].clone();
if (corr_wnd_inv_limited != null) { if (corr_wnd_inv_limited != null) {
...@@ -3620,8 +3648,8 @@ public class Correlation2d { ...@@ -3620,8 +3648,8 @@ public class Correlation2d {
// Normalize weight for each pair to compensate for different number of convex samples? // Normalize weight for each pair to compensate for different number of convex samples?
int fcam = PAIRS[npair][0]; // int fcam = PAIRS[npair][0];
int scam = PAIRS[npair][1]; // int scam = PAIRS[npair][1];
for (int i = 1; i < filtWeight.length; i++) if (filtWeight[i] > 0.0) { for (int i = 1; i < filtWeight.length; i++) if (filtWeight[i] > 0.0) {
int ix = i % corr_size; // >=0 int ix = i % corr_size; // >=0
int iy = i / corr_size; // >=0 int iy = i / corr_size; // >=0
...@@ -3633,8 +3661,9 @@ public class Correlation2d { ...@@ -3633,8 +3661,9 @@ public class Correlation2d {
// if (v > blur_max[npair]) blur_max[npair] = v; // if (v > blur_max[npair]) blur_max[npair] = v;
lma.addSample( // x = 0, y=0 - center lma.addSample( // x = 0, y=0 - center
0, // tile 0, // tile
fcam, // int fcam, // first camera index npair,
scam, // int scam, // second camera index // fcam, // int fcam, // first camera index
// scam, // int scam, // second camera index
ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
iy, // int y, // y coordinate (0 - disparity axis) iy, // int y, // y coordinate (0 - disparity axis)
v, // double v, // correlation value at that point v, // double v, // correlation value at that point
...@@ -3816,7 +3845,8 @@ public class Correlation2d { ...@@ -3816,7 +3845,8 @@ public class Correlation2d {
public Correlations2dLMA corrLMA( // USED in lwir public Correlations2dLMA corrLMA( // USED in lwir
ImageDttParameters imgdtt_params, ImageDttParameters imgdtt_params,
double [][] corrs, double [][] corrs,
int pair_mask, // which pairs to process // int pair_mask, // which pairs to process
boolean [] pair_mask, // which pairs to process
boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
double xcenter, // preliminary center x in pixels for largest baseline double xcenter, // preliminary center x in pixels for largest baseline
double vasw_pwr, // value as weight to this power, double vasw_pwr, // value as weight to this power,
...@@ -3967,7 +3997,7 @@ public class Correlation2d { ...@@ -3967,7 +3997,7 @@ public class Correlation2d {
ImageDttParameters imgdtt_params, ImageDttParameters imgdtt_params,
double [][] disparity_distortions, // {d_disp/dx, d_ndisp/dx, d_disp/dy, d_ndisp/dy} for each camera double [][] disparity_distortions, // {d_disp/dx, d_ndisp/dx, d_disp/dy, d_ndisp/dy} for each camera
double [][] corrs, double [][] corrs,
int pair_mask, // which pairs to process boolean[] pair_mask, // which pairs to process
boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
double xcenter, // preliminary center x in pixels for largest baseline double xcenter, // preliminary center x in pixels for largest baseline
double vasw_pwr, // value as weight to this power, double vasw_pwr, // value as weight to this power,
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDtt.java
View file @ 1a329010
...@@ -2591,7 +2591,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -2591,7 +2591,7 @@ public class ImageDtt extends ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs, fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(0x11), // 0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -2602,7 +2602,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -2602,7 +2602,7 @@ public class ImageDtt extends ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs, corrs, // double [][] corrs,
used_pairs, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(used_pairs), // used_pairs, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -3253,7 +3253,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3253,7 +3253,7 @@ public class ImageDtt extends ImageDttCPU {
corrs, // double [][] corrs, corrs, // double [][] corrs,
tile_disp_dist, tile_disp_dist,
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process 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 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 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, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -3571,7 +3571,7 @@ public class ImageDtt extends ImageDttCPU { ...@@ -3571,7 +3571,7 @@ public class ImageDtt extends ImageDttCPU {
corrs, // double [][] corrs, corrs, // double [][] corrs,
tile_disp_dist, tile_disp_dist,
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process 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 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 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, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDttCPU.java
View file @ 1a329010
...@@ -2266,7 +2266,7 @@ public class ImageDttCPU { ...@@ -2266,7 +2266,7 @@ public class ImageDttCPU {
corrs[cTile], // double [][] corrs, corrs[cTile], // double [][] corrs,
disp_dist[cTile], disp_dist[cTile],
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process 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 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 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, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -2322,7 +2322,7 @@ public class ImageDttCPU { ...@@ -2322,7 +2322,7 @@ public class ImageDttCPU {
corrs, // [tIndex], // double [][] corrs, corrs, // [tIndex], // double [][] corrs,
disp_dist, // [tIndex], disp_dist, // [tIndex],
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process 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 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, 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 clust_lma_debug_level + 0, // 2, // int debug_level, // for a single cluster
...@@ -3193,7 +3193,7 @@ public class ImageDttCPU { ...@@ -3193,7 +3193,7 @@ public class ImageDttCPU {
} }
// calculate all selected pairs correlations // calculate all selected pairs correlations
int all_pairs = imgdtt_params.dbg_pair_mask; //TODO: use tile tasks int all_pairs = imgdtt_params.dbg_pair_mask; //TODO: use tile tasks
double [][] corr_tiles = correlation2d.correlateCompositeFD( double [][] corr_tiles = correlation2d.correlateCompositeFD(
clt_data, // double [][][][][][] clt_data, clt_data, // double [][][][][][] clt_data,
tileX, // int tileX, tileX, // int tileX,
...@@ -3348,7 +3348,6 @@ public class ImageDttCPU { ...@@ -3348,7 +3348,6 @@ public class ImageDttCPU {
} }
*/ */
} }
/*
// 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 // 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 (corr_stat != null) { if (corr_stat != null) {
// skipping DISPARITY_VARIATIONS_INDEX - it was not used // skipping DISPARITY_VARIATIONS_INDEX - it was not used
...@@ -3359,19 +3358,20 @@ public class ImageDttCPU { ...@@ -3359,19 +3358,20 @@ public class ImageDttCPU {
} }
// debug new LMA correlations // debug new LMA correlations
if (debugTile) { if (debugTile0) { // should be debugTile
System.out.println("Will run new LMA for tileX="+tileX+", tileY="+tileY); System.out.println("Will run new LMA for tileX="+tileX+", tileY="+tileY);
double [] poly_disp = {Double.NaN, 0.0}; double [] poly_disp = {Double.NaN, 0.0};
Corr2dLMA lma2 = corr2d.corrLMA2Single( Corr2dLMA lma2 = correlation2d.corrLMA2Single(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
false, // boolean adjust_ly, // adjust Lazy Eye false, // boolean adjust_ly, // adjust Lazy Eye
corr_wnd, // double [][] corr_wnd, // correlation window to save on re-calculation of the window 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) 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, // double [][] corrs, corr_tiles, // corrs, // double [][] corrs,
disp_dist, disp_dist,
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process // all that are not null in corr_tiles
correlation2d.selectAll(), // longToArray(imgdtt_params.dbg_pair_mask), // int pair_mask, // which pairs to process
disp_str, //corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline disp_str, //corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
poly_disp, // double[] poly_ds, // null or pair of disparity/strength 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, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -3392,6 +3392,8 @@ public class ImageDttCPU { ...@@ -3392,6 +3392,8 @@ public class ImageDttCPU {
lma2.printStats(ds,1); lma2.printStats(ds,1);
} }
} }
/*
// disparity_map[DISPARITY_INDEX_CM + 1][tIndex] = // y not available here // disparity_map[DISPARITY_INDEX_CM + 1][tIndex] = // y not available here
// calculate/fill out hor and vert // calculate/fill out hor and vert
...@@ -3605,7 +3607,9 @@ public class ImageDttCPU { ...@@ -3605,7 +3607,9 @@ public class ImageDttCPU {
} }
} }
} }
*/
} // end of if (corr_stat != null) } // end of if (corr_stat != null)
/*
if (corr_mode == 0) extra_disparity = disparity_map[DISPARITY_INDEX_INT][tIndex]; if (corr_mode == 0) extra_disparity = disparity_map[DISPARITY_INDEX_INT][tIndex];
else if (corr_mode == 1) extra_disparity = disparity_map[DISPARITY_INDEX_CM][tIndex]; else if (corr_mode == 1) extra_disparity = disparity_map[DISPARITY_INDEX_CM][tIndex];
else if (corr_mode == 2) extra_disparity = disparity_map[DISPARITY_INDEX_POLY][tIndex]; else if (corr_mode == 2) extra_disparity = disparity_map[DISPARITY_INDEX_POLY][tIndex];
...@@ -6973,12 +6977,13 @@ public class ImageDttCPU { ...@@ -6973,12 +6977,13 @@ public class ImageDttCPU {
// create LMA instance, calculate LMA composite argmax // create LMA instance, calculate LMA composite argmax
// Create 2 groups: ortho & diag // Create 2 groups: ortho & diag
Correlations2dLMA lma; Correlations2dLMA lma;
int num_pairs = (quad * (quad-1)) / 2;
if (imgdtt_params.pcorr_use) { // new group phase correlation if (imgdtt_params.pcorr_use) { // new group phase correlation
double [][] fake_corrs = {corrs[6],null,null,null,corrs[7],null}; double [][] fake_corrs = {corrs[6],null,null,null,corrs[7],null};
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs, fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(0x11), // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -6989,7 +6994,7 @@ public class ImageDttCPU { ...@@ -6989,7 +6994,7 @@ public class ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs, corrs, // double [][] corrs,
used_pairs, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(used_pairs), // used_pairs, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -9822,7 +9827,7 @@ public class ImageDttCPU { ...@@ -9822,7 +9827,7 @@ public class ImageDttCPU {
Correlations2dLMA lma = corr2d.corrLMA( Correlations2dLMA lma = corr2d.corrLMA(
clt_parameters.img_dtt, // ImageDttParameters clt_parameters.img_dtt, clt_parameters.img_dtt, // ImageDttParameters clt_parameters.img_dtt,
corrs, // double [][] corrs, corrs, // double [][] corrs,
clt_parameters.img_dtt.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(clt_parameters.img_dtt.dbg_pair_mask), // clt_parameters.img_dtt.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
clt_parameters.img_dtt.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, clt_parameters.img_dtt.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -13391,7 +13396,7 @@ public class ImageDttCPU { ...@@ -13391,7 +13396,7 @@ public class ImageDttCPU {
corrs, // double [][] corrs, corrs, // double [][] corrs,
disp_dist, disp_dist,
rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity rXY, // double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(imgdtt_params.dbg_pair_mask), // imgdtt_params.dbg_pair_mask // int pair_mask, // which pairs to process
disp_str, //corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline disp_str, //corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
poly_disp, // double[] poly_ds, // null or pair of disparity/strength 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, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -13427,7 +13432,7 @@ public class ImageDttCPU { ...@@ -13427,7 +13432,7 @@ public class ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs, fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(0x11), // 0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -13440,7 +13445,7 @@ public class ImageDttCPU { ...@@ -13440,7 +13445,7 @@ public class ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs, fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(0x11), // 0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
...@@ -13453,7 +13458,7 @@ public class ImageDttCPU { ...@@ -13453,7 +13458,7 @@ public class ImageDttCPU {
lma = corr2d.corrLMA( lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params, imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs, corrs, // double [][] corrs,
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process corr2d.longToArray(imgdtt_params.dbg_pair_mask), // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power, imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
......
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