Skip to content

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

Dual max LMA

parent db3deeef
Show whitespace changes
Inline Side-by-side
Showing with 1238 additions and 659 deletions
src/main/java/com/elphel/imagej/gpu/GPUTileProcessor.java
View file @ c3ee5b3f
......@@ -74,6 +74,10 @@ import jcuda.nvrtc.nvrtcProgram;
public class GPUTileProcessor {
public static boolean USE_DS_DP = false; // Use Dynamic Shared memory with Dynamic Parallelism (not implemented)
String LIBRARY_PATH = "/usr/local/cuda/targets/x86_64-linux/lib/libcudadevrt.a"; // linux
// Can be downloaded and twice extracted from
// https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-cudart-dev-11-2_11.2.152-1_amd64.deb
// First deb itself, then data.tar.xz, and it will have usr/local/cuda/targets/x86_64-linux/lib/libcudadevrt.a inside
// Found "cuda-cudart-dev" on https://ubuntu.pkgs.org/
static String GPU_RESOURCE_DIR = "kernels";
static String [] GPU_KERNEL_FILES = {"dtt8x8.cuh","TileProcessor.cuh"};
// "*" - generated defines, first index - separately compiled unit
......
src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java
View file @ c3ee5b3f
......@@ -99,11 +99,10 @@ public class Corr2dLMA {
private int [] par_map;
private double [] vector;
private ArrayList<Sample> samples = new ArrayList<Sample>();
// private ArrayList<Sample> norm_samples = new ArrayList<Sample>(); // to calculate
private double total_weight;
private double total_weight; // total weight of all samples
private int total_tiles;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization terms
private double pure_weight; // weight of samples only
private double pure_weight; // weight of samples only as a fraction of total weight
private double [] values;
// next values are only updated after success
......@@ -118,11 +117,9 @@ public class Corr2dLMA {
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 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 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_pairs_map; // for each camera index return used index ??
private int [][] used_pairs_map; // [tile][pair] -1 for unused pairs, >=0 for used ones
private boolean [] used_tiles;
......@@ -130,26 +127,22 @@ public class Corr2dLMA {
private final int transform_size;
private final double [][] corr_wnd;
private boolean [] used_cameras;
private boolean [][] used_pairs;
private int ncam_used; // number of used cameras
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 pre_last_cam; // index of the pre-last camera (special treatment for disparity correction)
private Matrix [][] m_disp;
// private Matrix [][][] m_pairs;
// 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 final int numMax; // number of maximums to process (normally 1, some now 2)
private Matrix mddnd; // Matrix to calculate 2 last corrections in disparity direction and 1 ortho from the first ones (normally 2+3=5)
// private boolean gaussian_mode = true;
private int gaussian_mode = 1; // 0 - parabola, 1 - gaussian, 2 - limited parabola, 4 - limited squared parabola
private int gaussian_mode = 3; // 0 - parabola, 1 - gaussian, 2 - limited parabola, 4 - limited squared parabola
private boolean lazy_eye; // calculate parameters/derivatives for the "lazy eye" parameters
private boolean [] lazy_eye_dir = null; // after first set will be boolean[2] {adjust_lazyeye_par, adjust_lazyeye_ortho}
private double [][] rXY;
private int [] dd_indices; //normally 5-long (2 * ncam -3), absolute parameter indices for dd_pre_last, dd_last and nd_last
......@@ -158,15 +151,11 @@ public class Corr2dLMA {
public int getNumIter() {
return iter;
}
// private int iter = 0; // to be able to read number of used iterations
public class Sample{ // USED in lwir
int tile; // tile in a cluster
int pair; // pair index
// int fcam; // first camera index
// int scam; // second camera index
int imax;
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)
double v; // correlation value at that point
......@@ -174,8 +163,7 @@ public class Corr2dLMA {
Sample (
int tile,
int pair,
// int fcam, // first camera index
// int scam, // second camera index
int imax,
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)
double v, // correlation value at that point
......@@ -183,8 +171,7 @@ public class Corr2dLMA {
{
this.tile = tile;
this.pair = pair;
// this.fcam = fcam;
// this.scam = scam;
this.imax = imax;
this.ix = x;
this.iy = y;
this.v = v;
......@@ -192,7 +179,6 @@ public class Corr2dLMA {
}
@Override
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, p=%d, x=%d, y=%d, v=%f, w=%f", tile, pair, ix, iy, v, w);
}
}
......@@ -200,22 +186,24 @@ public class Corr2dLMA {
public Corr2dLMA (
int numTiles,
int numMax,
Correlation2d correlation2d,
int ts, // null - use default table
double [][] corr_wnd, // may be null
double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
int gaussian_mode // 0 - parabola, 1 - Gaussian, 2 - limited parabola, 3 - limited squared parabola
) {
// [(-Disparity, A, B, C-A, G0,... G119, {-Disparity2, A2, B2, C2-A2, G0_2,... G119_2}], ddisp_index..., ndisp_index
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.gaussian_mode = gaussian_mode;
this.used_cams_map = new int[num_cams];
this.numMax = numMax;
this.tile_params = G0_INDEX + this.num_pairs;
this.numTiles = numTiles;
ddisp_index = this.numTiles * this.tile_params; // ;
ddisp_index = this.numTiles * this.tile_params * this.numMax; // ;
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
this.transform_size = ts;
......@@ -254,15 +242,14 @@ public class Corr2dLMA {
public void addSample( // x = 0, y=0 - center
int tile,
int pair,
int imax, // number of maximum (just a hint)
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,pair,x,y,v,w));
if ((w > 0) && !Double.isNaN(v)) samples.add(new Sample(tile, pair, imax, x, y, v, w));
}
public ArrayList<Sample> filterSamples(
double [][] disparity_strength) {
ArrayList<Sample> filtered_samples = new ArrayList<Sample>();
......@@ -379,6 +366,16 @@ public class Corr2dLMA {
return np;
}
public static Matrix [] getMatrices(double [][] am_disp, int num_cams) {
Matrix [] m_disp = new Matrix[num_cams];
for (int n = 0; n < num_cams; n++) {
double [][] am = {
{am_disp[n][0],am_disp[n][1]},
{am_disp[n][2],am_disp[n][3]}};
m_disp[n] = new Matrix(am);
}
return m_disp;
}
public void setMatrices(double [][] am_disp) {
......@@ -434,18 +431,41 @@ public class Corr2dLMA {
mddnd = A33.inverse().times(A35);
}
public void initDisparity ( // USED in lwir
double [][] disp_str // initial value of disparity
double [][] disp_str, // initial value of disparity
int nmax
) {
for (int nTile = 0; nTile < numTiles; nTile++) {
int offs = (nTile * numMax + nmax) * tile_params;
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 + offs] = -disp_str[nTile][0]; // disp0;
} else {
this.all_pars[DISP_INDEX + offs] = 0.0; // disp0; // null pointer
}
}
}
public void initDisparity ( // USED in lwir
double [][][] disp_str // initial value of disparity [max][tile]{disp, strength}
) {
for (int nmax = 0; nmax < numMax; nmax++) {
for (int nTile = 0; nTile < numTiles; nTile++) {
int offs = (nTile * numMax + nmax) * tile_params;
if ( (disp_str[nmax] != null) &&
(disp_str[nmax][nTile] != null) &&
(disp_str[nmax][nTile][1] > 0.0) &&
used_tiles[nTile]) {
this.all_pars[DISP_INDEX + offs] = -disp_str[nmax][nTile][0]; // disp0;
} else {
this.all_pars[DISP_INDEX + nTile*tile_params] = 0.0; // disp0; // null pointer
this.all_pars[DISP_INDEX + offs] = 0.0; // disp0; // null pointer
}
}
}
}
public boolean setInitialLYOffsets(
double [][] pair_centers,
double step_weight, // scale corrections
......@@ -454,7 +474,11 @@ public class Corr2dLMA {
if (pair_centers == null) {
return false;
}
// verify that LMA is for a single maximum
if (numMax != 1) {
System.out.println("Number of maximums is not 1, it is "+numMax);
return false; // not all cameras present
}
// Verify that all cameras are used
for (int ncam = 0; ncam <num_cams; ncam++) if (!used_cameras[ncam]){
return false; // not all cameras present
......@@ -559,16 +583,23 @@ public class Corr2dLMA {
}
public boolean initVector( // USED in lwir
boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
boolean adjust_lazyeye_ortho, // obsolete - make == adjust_lazyeye_par adjust disparity corrections orthogonal to disparities lma_adjust_ly1
double [][] disp_str, // initial value of disparity
double [][][] disp_str_all, // initial value of disparity [max][tile]{disp, strength}
double half_width, // A=1/(half_widh)^2 lma_half_width
double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
) {
if (adjust_disparities == null) {
adjust_disparities = new boolean[numMax];
Arrays.fill(adjust_disparities, true);
}
/// double [][] disp_str = disp_str_all[0]; //FIXME: ****************
adjust_lazyeye_ortho = adjust_lazyeye_par; // simplify relations for the calculated/dependent parameters
lazy_eye = adjust_lazyeye_par | adjust_lazyeye_ortho;
bad_tile = -1;
......@@ -625,23 +656,34 @@ public class Corr2dLMA {
this.all_pars = new double[num_all_pars];
this.par_mask = new boolean[num_all_pars];
total_tiles = 0;
total_tiles = 0; // now
// per-tile parameters
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[A_INDEX + nTile*tile_params] = 1.0/(half_width * half_width);
this.all_pars[B_INDEX + nTile*tile_params] = 0.0;
this.all_pars[CMA_INDEX + nTile*tile_params] = 0.0; // C-A
this.par_mask[DISP_INDEX + nTile*tile_params] = true;
this.par_mask[A_INDEX + nTile*tile_params] = adjust_width;
this.par_mask[B_INDEX + nTile*tile_params] = adjust_ellipse;
this.par_mask[CMA_INDEX + nTile*tile_params] = adjust_ellipse;
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]) {
int num_good_max = 0;
for (int nmax = 0; nmax < numMax; nmax++) {
if ((disp_str_all[nmax][nTile] != null) && (disp_str_all[nmax][nTile][1] > 0.0)){
num_good_max++;
}
}
if (num_good_max == numMax) { // only use tiles with all (both) max
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (nTile * numMax + nmax) * tile_params;
this.all_pars[DISP_INDEX + offs] = -disp_str_all[nmax][nTile][0]; // disp0;
this.all_pars[A_INDEX + offs] = 1.0/(half_width * half_width);
this.all_pars[B_INDEX + offs] = 0.0;
this.all_pars[CMA_INDEX + offs] = 0.0; // C-A
this.par_mask[DISP_INDEX + offs] = adjust_disparities[nmax];// true;
this.par_mask[A_INDEX + offs] = adjust_width;
this.par_mask[B_INDEX + offs] = adjust_ellipse;
this.par_mask[CMA_INDEX + offs] = adjust_ellipse;
for (int i = 0; i <num_pairs; i++) {
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.par_mask[G0_INDEX + i + offs] = used_pairs[nTile][i] & adjust_scales;
this.all_pars[G0_INDEX + i + offs] = Double.NaN; // will be assigned later for used - should be for all !
}
}
total_tiles++;
}
}
// common for all tiles parameters
for (int i = 0; i <num_cams; i++) {
this.all_pars[ddisp_index + i] = 0.0; // C-A
......@@ -663,13 +705,14 @@ public class Corr2dLMA {
double sw = 0;
total_weight = 0.0;
// use per-tile, per maximum maximal measured value as initial amplitude
for (int i = 0; i < np; i++) {
Sample s = samples.get(i);
weights[i] = s.w;
total_weight += s.w;
values[i] = s.v;
sw += weights[i];
int indx = G0_INDEX + s.pair + s.tile * tile_params;
int indx = G0_INDEX + s.pair + (s.tile * numMax + s.imax) * tile_params;
double d = s.v;
if (this.corr_wnd !=null) {
d /= this.corr_wnd[s.iy][s.ix];
......@@ -707,6 +750,223 @@ public class Corr2dLMA {
return true;
}
/**
* Initialize parameters values before calling setParMask() that will copy adjustable parameters to a vector
* @param disp_str_all initial value of disparity [max][tile]{disp, strength}
* @param half_width initial half-width of a maximum A=1/(half_widh)^2
* @return OK/failure
*/
public boolean preparePars(
double [][][] disp_str_all,
double half_width
) {
bad_tile = -1;
used_cameras = new boolean[num_cams];
used_pairs = new boolean[numTiles][num_pairs];
// 0-weight values and NaN-s should be filtered on input!
used_pairs_map = new int [numTiles][num_pairs];
for (int t = 0; t < numTiles; t++) {
Arrays.fill(used_pairs_map[t], -1);
}
used_tiles = new boolean[numTiles];
for (Sample s:samples) { // ignore zero-weight samples
int pair = s.pair;
int [] fscam = correlation2d.getPair(pair);
used_cameras[fscam[0]]=true;
used_cameras[fscam[1]]=true;
used_tiles[s.tile] = true;
used_pairs[s.tile][pair]=true; // throws < 0 - wrong pair, f==s
}
ncam_used = 0;
npairs =new int [numTiles]; // pairs in each tile
for (int i = 0; i < num_cams; i++) {
used_cams_map[i] = ncam_used;
if (used_cameras[i]) {
ncam_used++;
}
}
used_cams_rmap = new int [ncam_used];
ncam_used = 0;
for (int i = 0; i < num_cams; i++) {
if (used_cameras[i]) {
used_cams_rmap[ncam_used++] = i;
}
}
if (ncam_used < 2) {
return false;
}
last_cam = (ncam_used >= 1)? used_cams_rmap[ncam_used - 1] :-1;
pre_last_cam = (ncam_used >= 2)? used_cams_rmap[ncam_used - 2] :-1;
setOffsetMatrices();
for (int nTile = 0; nTile < numTiles; nTile++) {
int [] upmam = new int[num_pairs];
for (int i = 0; i < num_pairs; i++) {
upmam[i] = npairs[nTile];
if (used_pairs[nTile][i]) npairs[nTile]++;
}
for (int pair = 0; pair < num_pairs; pair++) {
int npair = upmam[pair];
if (used_pairs[nTile][pair]) used_pairs_map[nTile][pair] = npair;
}
}
this.all_pars = new double[num_all_pars];
this.par_mask = new boolean[num_all_pars];
total_tiles = 0; // now
// per-tile parameters
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]) {
int num_good_max = 0;
for (int nmax = 0; nmax < numMax; nmax++) {
if ((disp_str_all[nmax][nTile] != null) && (disp_str_all[nmax][nTile][1] > 0.0)){
num_good_max++;
}
}
if (num_good_max == numMax) { // only use tiles with all (both) max
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (nTile * numMax + nmax) * tile_params;
this.all_pars[DISP_INDEX + offs] = -disp_str_all[nmax][nTile][0]; // disp0;
this.all_pars[A_INDEX + offs] = 1.0/(half_width * half_width);
this.all_pars[B_INDEX + offs] = 0.0;
this.all_pars[CMA_INDEX + offs] = 0.0; // C-A
for (int i = 0; i <num_pairs; i++) {
this.all_pars[G0_INDEX + i + offs] = Double.NaN; // will be assigned later for used - should be for all !
}
}
total_tiles++;
} else {
used_tiles[nTile] = false; /// Was not here before 02/13/2022
}
}
// common for all tiles parameters
for (int i = 0; i <num_cams; i++) {
this.all_pars[ddisp_index + i] = 0.0; // C-A
this.all_pars[ndisp_index + i] = 0.0; // C-A
}
int np = samples.size();
weights = new double [np + 2 * num_cams]; // npairs];
values = new double [np + 2 * num_cams]; // npairs];
total_weight = 0.0;
// use per-tile, per maximum maximal measured value as initial amplitude
for (int i = 0; i < np; i++) {
Sample s = samples.get(i);
if (used_tiles[s.tile]) { // normally should always be true
weights[i] = s.w;
total_weight += s.w;
values[i] = s.v;
int indx = G0_INDEX + s.pair + (s.tile * numMax + s.imax) * tile_params;
double d = s.v;
if (this.corr_wnd !=null) {
d /= this.corr_wnd[s.iy][s.ix];
}
if (!(d <= this.all_pars[indx])) this.all_pars[indx] = d; // to include Double.isNaN()
}
}
for (int i = 0; i < num_cams; i++) {
values [np + i] = 0.0;
values [np + num_cams + i] = 0.0;
}
// Normalize w/o LY
if (total_weight != 0.0) {
double kw = 1.0/total_weight;
for (int i = 0; i < weights.length; i++) weights[i] *= kw;
pure_weight = 1.0; // *= kw; // it is now fraction (0..1.0), and weights are normalized
}
return true;
}
/**
* Set/modify parameters mask. May be called after preparePars () or after updateFromVector() if LMA was ran
* @param adjust_disparities null to adjust all (1 or 2) disparities or a boolean array of per maximum
* individual disparity adjusts
* @param adjust_width adjust correlation maximum width
* @param adjust_scales adjust per-pair amplitude
* @param adjust_ellipse adjust per-pair maximum shape as an ellipse
* @param adjust_lazyeye_par adjust lazy eye parallel to disparity
* @param adjust_lazyeye_ortho adjust lazy eye orthogonal to disparity
* @param cost_lazyeye_par cost of lazy eye parallel to disparity
* @param cost_lazyeye_odtho cost of lazy eye orthogonal to disparity
* @return OK/failure
*/
public boolean setParMask( // USED in lwir
boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
boolean adjust_lazyeye_ortho, // obsolete - make == adjust_lazyeye_par adjust disparity corrections orthogonal to disparities lma_adjust_ly1
double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
) {
if (adjust_disparities == null) {
adjust_disparities = new boolean[numMax];
Arrays.fill(adjust_disparities, true);
}
total_tiles = 0; // now
// per-tile parameters
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]) {
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (nTile * numMax + nmax) * tile_params;
this.par_mask[DISP_INDEX + offs] = adjust_disparities[nmax];// true;
this.par_mask[A_INDEX + offs] = adjust_width;
this.par_mask[B_INDEX + offs] = adjust_ellipse;
this.par_mask[CMA_INDEX + offs] = adjust_ellipse;
for (int i = 0; i <num_pairs; i++) {
this.par_mask[G0_INDEX + i + offs] = used_pairs[nTile][i] & adjust_scales;
}
}
total_tiles++;
}
// common for all tiles parameters
for (int i = 0; i <num_cams; i++) {
this.par_mask[ddisp_index + i] = used_cameras[i] & adjust_lazyeye_par & (i != last_cam) & (i != pre_last_cam);
this.par_mask[ndisp_index + i] = used_cameras[i] & adjust_lazyeye_ortho & (i != last_cam);
}
int np = samples.size();
// First time always, next only if adjust_lazyeye_par or adjust_lazyeye_ortho is changed
if ((lazy_eye_dir == null) || (lazy_eye_dir[0] != adjust_lazyeye_par)|| (lazy_eye_dir[1] != adjust_lazyeye_ortho)) {
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 + num_cams + i] = (used_cameras[i] & adjust_lazyeye_ortho)? (cost_lazyeye_odtho * numTiles) : 0.0; // ndisp - including last_camera
}
double sw = total_weight;
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];
}
if (sw != 0.0) {
double kw_pure = total_weight / pure_weight/ sw;
double kw = 1.0/sw;
for (int i = 0; i < np; i++) weights[i] *= kw_pure;
for (int i = np; i < weights.length; i++) weights[i] *= kw;
pure_weight = total_weight * kw; // *= kw; // it is now fraction (0..1.0), and weights are normalized
}
}
par_map = new int [par_mask.length];
int par_indx = 0;
for (int i = 0; i < par_mask.length; i++) {
if (par_mask[i]) par_map[i] = par_indx++;
else par_map[i] = -1;
}
dd_indices = lazy_eye? new int [2 * ncam_used -3] : null;
if (dd_indices != null) {
int pi = 0;
for (int i = 0; i < num_cams; i++) {
if (par_map[ddisp_index + i] >=0) dd_indices[pi++] = par_map[ddisp_index + i];
}
for (int i = 0; i < num_cams; i++) {
if (par_map[ndisp_index + i] >=0) dd_indices[pi++] = par_map[ndisp_index + i];
}
}
toVector();
return true;
}
public void initMatrices() { // should be called after initVector and after setMatrices
......@@ -738,7 +998,7 @@ public class Corr2dLMA {
}
/**
* Calculate initial disparity by polynomial approximation. Only works with a single tile now
* Calculate initial disparity by polynomial approximation. Only works with a single tile now and a single maximum
* Note: combo correlation scale is one pixel for 1 pixel disparity for diameter cameras, while standard
* (pre-shift) disparity refers to a quad-camera (square) pixel offset. So the returned disparity should be divided by sqrt(2)
* @param corr_wnd_inv_limited window (same as for finding convex) to boost gain in peripheral areas, but not the very marginal ones
......@@ -763,7 +1023,7 @@ public class Corr2dLMA {
} else {
bv = s.v / corr_wnd[s.iy][s.ix];
}
bv /=this.all_pars[G0_INDEX + s.pair + s.tile * tile_params];
bv /=this.all_pars[G0_INDEX + s.pair + s.tile * tile_params]; // FIXME: numMax
//corr_wnd
int indx = 2 * ns;
mdata[indx ][0] = new double [2];
......@@ -904,51 +1164,55 @@ public class Corr2dLMA {
if (vector == null) return null;
double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [][][][][] xp_yp = new double[numMax][numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
double [][] AT = new double [numMax][numTiles]; // av[A_INDEX];
double [][] BT = new double [numMax][numTiles]; // av[B_INDEX];
double [][] CT = new double [numMax][numTiles]; // A + av[CMA_INDEX];
for (int nmax = 0; nmax < numMax; nmax++) {
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
int offs = (nTile * numMax + nmax) * tile_params;
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+ offs]+ av[ddisp_index + i], av[ndisp_index + i]};
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 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[nmax][nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
}
}
AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
AT[nmax][nTile] = av[A_INDEX + offs];
BT[nmax][nTile] = av[B_INDEX + offs];
CT[nmax][nTile] = AT[nmax][nTile] + av[CMA_INDEX + offs];
}
}
int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd
for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns);
// int pair = pindx[s.fcam][s.scam]; // all pairs, not 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 B = BT[s.tile];
double C = CT[s.tile];
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (s.tile * numMax + nmax) * tile_params;
double A = AT[nmax][s.tile];
double B = BT[nmax][s.tile];
double C = CT[nmax][s.tile];
double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Gp = av[G0_INDEX + pair + offs];
double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp;
// 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 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 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 xmxp = s.ix - xp_yp[nmax][s.tile][fs[0]][fs[1]][0]; // TODO - change format of xp_yp
double ymyp = s.iy - xp_yp[nmax][s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp;
double d = Wp*(1.0 - (A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
fx[ns] = d * Gp;
fx[ns] += d * Gp;
if (Double.isNaN(fx[ns])) {
System.out.println("fx["+ns+"]="+fx[ns]);
}
......@@ -956,46 +1220,45 @@ public class Corr2dLMA {
System.out.print("");
}
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 *
// ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
// (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
if (par_map[DISP_INDEX + offs] >= 0) {
jt[par_map[DISP_INDEX + offs]][ns] = 2 * WGp *
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 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] = -WGp*(xmxp2 + ymyp2);
if (par_map[A_INDEX + offs] >= 0) {
jt[par_map[A_INDEX + offs]][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[B_INDEX + offs] >= 0) {
jt[par_map[B_INDEX + offs]][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;
if (par_map[CMA_INDEX + offs] >= 0) {
jt[par_map[CMA_INDEX + offs]][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 (par_map[G0_INDEX + p + offs] >= 0) {
jt[par_map[G0_INDEX + p + offs]][ns] = (p== pair)? d : 0.0; // (par_mask[G0_INDEX + pair])? d;
}
}
if (lazy_eye) {
if (nmax == 0) { // only zero during first pass, then accumulate only
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[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
// if ((s.fcam == pre_last_cam)) {
// if ((s.fcam == pre_last_cam)) {
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGp *
( (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));
// } 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 *
( (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));
}
// if ((s.fcam == last_cam)) {
// if ((s.fcam == last_cam)) {
if ((fs[0] == last_cam)) {
dd_deriv[1] = 2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
......@@ -1004,7 +1267,7 @@ public class Corr2dLMA {
( (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));
// } else if ((s.scam == last_cam)) {
// } else if ((s.scam == last_cam)) {
} else if ((fs[1] == last_cam)) {
dd_deriv[1] = -2 * WGp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][last_cam].get(0, 0)+
......@@ -1041,9 +1304,10 @@ public class Corr2dLMA {
for (int ddn = 0; ddn < 3; ddn++) if (dd_deriv[ddn] != 0.0) {
for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][ns] += dd_deriv[ddn] * mddnd.get(ddn, i);
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
}
}
}
......@@ -1081,46 +1345,50 @@ public class Corr2dLMA {
if (vector == null) return null;
double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [][][][][] xp_yp = new double[numMax][numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
double [][] AT = new double [numMax][numTiles]; // av[A_INDEX];
double [][] BT = new double [numMax][numTiles]; // av[B_INDEX];
double [][] CT = new double [numMax][numTiles]; // A + av[CMA_INDEX];
for (int nmax = 0; nmax < numMax; nmax++) {
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
int offs = (nTile * numMax + nmax) * tile_params;
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+ offs]+ av[ddisp_index + i], av[ndisp_index + i]};
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 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[nmax][nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
}
}
AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
AT[nmax][nTile] = av[A_INDEX + offs];
BT[nmax][nTile] = av[B_INDEX + offs];
CT[nmax][nTile] = AT[nmax][nTile] + av[CMA_INDEX + offs];
}
}
int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd
for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns);
// int pair = pindx[s.fcam][s.scam]; // all pairs, not 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 B = BT[s.tile];
double C = CT[s.tile];
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (s.tile * numMax + nmax) * tile_params;
double A = AT[nmax][s.tile];
double B = BT[nmax][s.tile];
double C = CT[nmax][s.tile];
double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Gp = av[G0_INDEX + pair + offs];
double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp;
// 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 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 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 xmxp = s.ix - xp_yp[nmax][s.tile][fs[0]][fs[1]][0]; // TODO - change format of xp_yp
double ymyp = s.iy - xp_yp[nmax][s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp;
......@@ -1129,7 +1397,7 @@ public class Corr2dLMA {
double lim_negative = (d < 0)? 0.0 : 1.0;
d *= lim_negative;
fx[ns] = d * Gp;
fx[ns] += d * Gp;
if (Double.isNaN(fx[ns])) {
System.out.println("fx["+ns+"]="+fx[ns]);
}
......@@ -1137,33 +1405,35 @@ public class Corr2dLMA {
System.out.print("");
}
if (jt != null) {
if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = lim_negative* 2 * WGp *
// ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
// (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
if (par_map[DISP_INDEX + offs] >= 0) {
jt[par_map[DISP_INDEX + offs]][ns] = lim_negative* 2 * WGp *
// ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
// (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 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] = -WGp * (xmxp2 + ymyp2) * lim_negative;
if (par_map[A_INDEX + offs] >= 0) {
jt[par_map[A_INDEX + offs]][ns] = -WGp * (xmxp2 + ymyp2) * lim_negative;
}
if (par_map[B_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[B_INDEX + s.tile*tile_params]][ns] = -WGp * 2 * xmxp_ymyp * lim_negative;
if (par_map[B_INDEX + offs] >= 0) {
jt[par_map[B_INDEX + offs]][ns] = -WGp * 2 * xmxp_ymyp * lim_negative;
}
if (par_map[CMA_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[CMA_INDEX + s.tile*tile_params]][ns] = -WGp * ymyp2 * lim_negative;
if (par_map[CMA_INDEX + offs] >= 0) {
jt[par_map[CMA_INDEX + offs]][ns] = -WGp * ymyp2 * lim_negative;
}
// 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
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 (par_map[G0_INDEX + p + offs] >= 0) {
jt[par_map[G0_INDEX + p + offs]][ns] = (p== pair)? d : 0.0; // (par_mask[G0_INDEX + pair])? d;
}
}
if (lazy_eye) {
if (nmax == 0) { // only zero during first pass, then accumulate only // ****
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[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
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGp * lim_negative *
......@@ -1218,9 +1488,10 @@ public class Corr2dLMA {
for (int ddn = 0; ddn < 3; ddn++) if (dd_deriv[ddn] != 0.0) {
for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][ns] += dd_deriv[ddn] * mddnd.get(ddn, i); // already includes lim_negative *
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
}
}
}
......@@ -1258,25 +1529,28 @@ public class Corr2dLMA {
if (vector == null) return null;
double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [][][][][] xp_yp = new double[numMax][numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
double [][] AT = new double [numMax][numTiles]; // av[A_INDEX];
double [][] BT = new double [numMax][numTiles]; // av[B_INDEX];
double [][] CT = new double [numMax][numTiles]; // A + av[CMA_INDEX];
for (int nmax = 0; nmax < numMax; nmax++) {
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
int offs = (nTile * numMax + nmax) * tile_params;
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+ offs]+ av[ddisp_index + i], av[ndisp_index + i]};
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 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[nmax][nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
}
}
AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
AT[nmax][nTile] = av[A_INDEX + offs];
BT[nmax][nTile] = av[B_INDEX + offs];
CT[nmax][nTile] = AT[nmax][nTile] + av[CMA_INDEX + offs];
}
}
int num_samples = samples.size();
......@@ -1285,16 +1559,20 @@ public class Corr2dLMA {
for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns);
int pair = s.pair; // all pairs, not just used?
// if (s.pair == 78) {
// System.out.println("getFxJt_parabola_squared() s.pair="+s.pair);
// }
int [] fs = correlation2d.getPair(pair);
double A = AT[s.tile];
double B = BT[s.tile];
double C = CT[s.tile];
double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Wp = corr_wnd[s.ix][s.iy]; // *********
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (s.tile * numMax + nmax) * tile_params;
double A = AT[nmax][s.tile];
double B = BT[nmax][s.tile];
double C = CT[nmax][s.tile];
double Gp = av[G0_INDEX + pair + offs];
double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp;
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 xmxp = s.ix - xp_yp[nmax][s.tile][fs[0]][fs[1]][0]; // TODO - change format of xp_yp
double ymyp = s.iy - xp_yp[nmax][s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp;
......@@ -1303,7 +1581,7 @@ public class Corr2dLMA {
double lim_negative = (d < 0)? 0.0 : 1.0;
d *= lim_negative;
fx[ns] = d * d * Gp;
fx[ns] += d * d * Gp;
// d(d^2)/dp = 2*d *dd/dp
......@@ -1316,30 +1594,32 @@ public class Corr2dLMA {
System.out.print("");
}
if (jt != null) {
if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = lim_negative_2d* 2 * WGp *
if (par_map[DISP_INDEX + offs] >= 0) {
jt[par_map[DISP_INDEX + offs]][ns] = lim_negative_2d* 2 * WGp *
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 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] = -WGp * (xmxp2 + ymyp2) * lim_negative_2d;
if (par_map[A_INDEX + offs] >= 0) {
jt[par_map[A_INDEX + offs]][ns] = -WGp * (xmxp2 + ymyp2) * lim_negative_2d;
}
if (par_map[B_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[B_INDEX + s.tile*tile_params]][ns] = -WGp * 2 * xmxp_ymyp * lim_negative_2d;
if (par_map[B_INDEX + offs] >= 0) {
jt[par_map[B_INDEX + offs]][ns] = -WGp * 2 * xmxp_ymyp * lim_negative_2d;
}
if (par_map[CMA_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[CMA_INDEX + s.tile*tile_params]][ns] = -WGp * ymyp2 * lim_negative_2d;
if (par_map[CMA_INDEX + offs] >= 0) {
jt[par_map[CMA_INDEX + offs]][ns] = -WGp * ymyp2 * lim_negative_2d;
}
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 * d) : 0.0; // (par_mask[G0_INDEX + pair])? d;
if (par_map[G0_INDEX + p + offs] >= 0) {
jt[par_map[G0_INDEX + p + offs]][ns] = (p== pair)? (d * d) : 0.0; // (par_mask[G0_INDEX + pair])? d;
}
}
if (lazy_eye) {
if (nmax == 0) { // only zero during first pass, then accumulate only // ****
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[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
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGp * lim_negative_2d *
......@@ -1394,14 +1674,15 @@ public class Corr2dLMA {
for (int ddn = 0; ddn < 3; ddn++) if (dd_deriv[ddn] != 0.0) {
for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][ns] += dd_deriv[ddn] * mddnd.get(ddn, i); // already includes lim_negative *
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
}
}
}
} // for (int nmax = 0; nmax < numMax; nmax++) {// ****
} // for (int ns = 0; ns < num_samples; ns++) {
if (lazy_eye) { // do not include lim_negative *
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];
......@@ -1428,60 +1709,61 @@ public class Corr2dLMA {
return fx;
}
private double [] getFxJt_gaussian( // USED in lwir
double [] vector,
double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
if (vector == null) return null;
double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [][][][][] xp_yp = new double[numMax][numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
double [][] AT = new double [numMax][numTiles]; // av[A_INDEX];
double [][] BT = new double [numMax][numTiles]; // av[B_INDEX];
double [][] CT = new double [numMax][numTiles]; // A + av[CMA_INDEX];
for (int nmax = 0; nmax < numMax; nmax++) {
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
int offs = (nTile * numMax + nmax) * tile_params;
for (int ncam = 0; ncam < num_cams; ncam++) if (used_cameras[ncam]) {
double [] add_dnd = {av[DISP_INDEX+ nTile * tile_params]+ av[ddisp_index + ncam], av[ndisp_index + ncam]};
double [] add_dnd = {av[DISP_INDEX+ offs]+ av[ddisp_index + ncam], av[ndisp_index + ncam]};
xcam_ycam[nTile][ncam] = m_disp[nTile][ncam].times(new Matrix(add_dnd,2));
}
for (int f = 0; f < num_cams; f++) if (used_cameras[f]) {
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[nmax][nTile][f][s] =xcam_ycam[nTile][f].minus(xcam_ycam[nTile][s]).plus(xc_yc).getColumnPackedCopy();
}
}
AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
AT[nmax][nTile] = av[A_INDEX + offs];
BT[nmax][nTile] = av[B_INDEX + offs];
CT[nmax][nTile] = AT[nmax][nTile] + av[CMA_INDEX + offs];
}
}
int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd
for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns);
// int pair = pindx[s.fcam][s.scam]; // all pairs, not 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 B = BT[s.tile];
double C = CT[s.tile];
double Gp = av[G0_INDEX + pair + s.tile * tile_params];
for (int nmax = 0; nmax < numMax; nmax++) {
int offs = (s.tile * numMax + nmax) * tile_params;
double A = AT[nmax][s.tile];
double B = BT[nmax][s.tile];
double C = CT[nmax][s.tile];
double Gp = av[G0_INDEX + pair + offs];
double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp;
double xmxp = s.ix - xp_yp[s.tile][fs[0]][fs[1]][0];
double ymyp = s.iy - xp_yp[s.tile][fs[0]][fs[1]][1];
double xmxp = s.ix - xp_yp[nmax][s.tile][fs[0]][fs[1]][0];
double ymyp = s.iy - xp_yp[nmax][s.tile][fs[0]][fs[1]][1];
double xmxp2 = xmxp * xmxp;
double ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp;
//// double comm = Wp*(1.0 - (A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
//// double comm = Wp*(1.0 - (A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
double exp = Math.exp(-(A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
// if ((exp > 1000.0) || (exp < 0.0000001)) {
// if ((exp > 1000.0) || (exp < 0.0000001)) {
if (exp > 1000.0) {
// System.out.println("Unreasonable exp = "+exp);
// System.out.println("Unreasonable exp = "+exp);
removeBadTile(s.tile, fs[0], fs[1]);
return null; // should re-start LMA
}
......@@ -1494,30 +1776,35 @@ public class Corr2dLMA {
double comm = exp * Wp;
double WGpexp = WGp*exp;
fx[ns] = comm * Gp;
fx[ns] += comm * Gp;
if (Double.isNaN(fx[ns])) {
System.out.println("fx["+ns+"]="+fx[ns]);
}
if (s.tile > 0) {
System.out.print("");
}
if (jt != null) { // Need derivatives too, no just Fx
if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = 2 * WGpexp *
// all those before lazy_eye for different nmax do not overlap, so OK to "=", not "+="
if (par_map[DISP_INDEX + offs] >= 0) jt[par_map[DISP_INDEX + offs]][ns] = 2 * WGpexp *
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 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[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[A_INDEX + offs] >= 0) jt[par_map[A_INDEX + offs]][ns] = -WGpexp*(xmxp2 + ymyp2);
if (par_map[B_INDEX + offs] >= 0) jt[par_map[B_INDEX + offs]][ns] = -WGpexp* 2 * xmxp_ymyp;
if (par_map[CMA_INDEX + offs] >= 0) jt[par_map[CMA_INDEX + offs]][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
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 + offs] >= 0) jt[par_map[G0_INDEX + p + offs]][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)
if (lazy_eye) {
if (nmax == 0) { // only zero during first pass, then accumulate only
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[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
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGpexp *
......@@ -1563,7 +1850,6 @@ public class Corr2dLMA {
(B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 1));
}
if (par_map[ndisp_index + fs[1]] >= 0) {
jt[par_map[ndisp_index + fs[1]]][ns] -= 2 * WGpexp *
( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 1));
......@@ -1574,15 +1860,197 @@ public class Corr2dLMA {
for (int ddn = 0; ddn < 3; ddn++) if (dd_deriv[ddn] != 0.0) {
for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][ns] += dd_deriv[ddn] * mddnd.get(ddn, i);
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_gaussian(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
}
}
}
} // for (int nmax = 0; nmax < numMax; nmax++) {
} // for (int ns = 0; ns < num_samples; ns++)
if (lazy_eye) {
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 + num_cams + n] = av[ndisp_index + n];
}
// and derivatives
if (jt != null) {
for (int i = 0; i < num_cams; i++) {
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;
}
if ((i != last_cam) && (par_map[ndisp_index + i] >= 0)) {
jt[par_map[ndisp_index + i] ][num_samples + num_cams + i] = 1.0;
}
}
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 + last_cam] = mddnd.get(1, i);
jt[dd_indices[i]][num_samples + num_cams + last_cam] = mddnd.get(2, i);
}
}
}
return fx;
}
@Deprecated
private double [] getFxJt_parabola_lim_nodual( // USED in lwir
double [] vector,
double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
if (vector == null) return null;
double [] av = fromVector(vector);
Matrix [][] xcam_ycam = new Matrix[numTiles][num_cams];
double [][][][] xp_yp = new double[numTiles][num_cams][num_cams][];
double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
Matrix xc_yc = new Matrix(axc_yc, 2);
double [] AT = new double [numTiles]; // av[A_INDEX];
double [] BT = new double [numTiles]; // av[B_INDEX];
double [] CT = new double [numTiles]; // A + av[CMA_INDEX];
for (int nTile = 0; nTile < numTiles; nTile++) if (used_tiles[nTile]){
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]};
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 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();
}
}
AT[nTile] = av[A_INDEX + nTile * tile_params];
BT[nTile] = av[B_INDEX + nTile * tile_params];
CT[nTile] = AT[nTile] + av[CMA_INDEX + nTile * tile_params];
}
int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * num_cams];
//corr_wnd
for (int ns = 0; ns < num_samples; ns++) {
Sample s = samples.get(ns);
// 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 B = BT[s.tile];
double C = CT[s.tile];
double Gp = av[G0_INDEX + pair + s.tile * tile_params];
double Wp = corr_wnd[s.ix][s.iy];
double WGp = Wp * Gp;
// 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 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 ymyp2 = ymyp * ymyp;
double xmxp_ymyp = xmxp * ymyp;
double d = Wp*(1.0 - (A*xmxp2 + 2 * B * xmxp_ymyp + C * ymyp2));
double lim_negative = (d < 0)? 0.0 : 1.0;
d *= lim_negative;
fx[ns] = d * Gp;
if (Double.isNaN(fx[ns])) {
System.out.println("fx["+ns+"]="+fx[ns]);
}
if (s.tile > 0) {
System.out.print("");
}
if (jt != null) {
if (par_map[DISP_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[DISP_INDEX + s.tile*tile_params]][ns] = lim_negative* 2 * WGp *
// ((A * xmxp + B * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(0, 0)+
// (B * xmxp + C * ymyp) * m_pairs[s.tile][s.fcam][s.scam].get(1, 0));
((A * xmxp + B * ymyp) * m_pairs[s.tile][s.pair].get(0, 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] = -WGp * (xmxp2 + ymyp2) * lim_negative;
}
if (par_map[B_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[B_INDEX + s.tile*tile_params]][ns] = -WGp * 2 * xmxp_ymyp * lim_negative;
}
if (par_map[CMA_INDEX + s.tile*tile_params] >= 0) {
jt[par_map[CMA_INDEX + s.tile*tile_params]][ns] = -WGp * ymyp2 * lim_negative;
}
// 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) {
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[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
if ((fs[0] == pre_last_cam)) {
dd_deriv[0] = 2 * WGp * lim_negative *
( (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));
} else if ((fs[1] == pre_last_cam)) {
dd_deriv[0] = -2 * WGp * lim_negative *
( (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));
}
if ((fs[0] == last_cam)) {
dd_deriv[1] = 2 * WGp * lim_negative *
( (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));
dd_deriv[2] = 2 * WGp * lim_negative *
( (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));
} else if ((fs[1] == last_cam)) {
dd_deriv[1] = -2 * WGp * lim_negative *
( (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));
dd_deriv[2] = -2 * WGp * lim_negative *
( (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));
}
// now accumulate derivatives:
// first calculate contributions of the dd, nd directly:
if (par_map[ddisp_index + fs[0]] >= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + fs[0]]][ns] += 2 * WGp * lim_negative *
((A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 0));
}
if (par_map[ddisp_index + fs[1]]>= 0){ // par_map[ddisp_index + last_cam] always <0
jt[par_map[ddisp_index + fs[1]]][ns] -= 2 * WGp * lim_negative *
((A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 0)+
(B * xmxp + C * ymyp) * m_disp[s.tile][fs[1]].get(1, 0));
}
if (par_map[ndisp_index + fs[0]] >=0){
jt[par_map[ndisp_index + fs[0]]][ns] += 2 * WGp * lim_negative *
( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[0]].get(0, 1)+
(B * xmxp + C * ymyp) * m_disp[s.tile][fs[0]].get(1, 1));
}
if (par_map[ndisp_index + fs[1]] >= 0) {
jt[par_map[ndisp_index + fs[1]]][ns] -= 2 * WGp * lim_negative *
( (A * xmxp + B * ymyp) * m_disp[s.tile][fs[1]].get(0, 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])
//// private int [] dd_indices; //normally 5-long (2 * ncam -3), absolute parameter indices for dd_pre_last, dd_last and nd_last
for (int ddn = 0; ddn < 3; ddn++) if (dd_deriv[ddn] != 0.0) {
for (int i = 0; i < dd_indices.length; i++) {
jt[dd_indices[i]][ns] += dd_deriv[ddn] * mddnd.get(ddn, i); // already includes lim_negative *
// if (Double.isNaN(jt[dd_indices[i]][ns])){
// System.out.println("getFxJt_gaussian(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// System.out.println("getFxJt_parabola(): jt[dd_indices["+i+"]]["+ns+"] == NaN, dd_indices["+i+"]="+dd_indices[i]);
// }
}
}
}
}
}
if (lazy_eye) {
if (lazy_eye) { // do not include lim_negative *
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 + num_cams + n] = av[ndisp_index + n];
......@@ -1605,7 +2073,6 @@ public class Corr2dLMA {
}
}
}
return fx;
}
......@@ -1635,28 +2102,7 @@ public class Corr2dLMA {
return xy_offsets;
}
/**
* Calculate offsets for multiple disparities, for each defined pair x,y expected offset, assuming only disparity, not lazy eye
* @param corrs - pairs 2D correlations (each in scanline order) - just to determine null/non-null
* @param disparity_strengths - expected {disparity, strength} pairs (e.g. from CM)
* @param disp_dist - per camera disparity matrix as a 1d (linescan order))
* @return array of per-disparity, per pair x,y expected center offset in 2D correlations or nulls for undefined pairs (or null if already set)
*/
public double [][][] getPairsOffsets(
double [][] corrs,
boolean [] pair_mask,
double [][] disparity_strengths,
double [][] disp_dist){ //
double [][][] pairs_offsets = new double [disparity_strengths.length][][];
for (int i = 0; i < pairs_offsets.length; i++) {
pairs_offsets[i] = getPairsOffsets(
corrs, // double [][] corrs,
pair_mask, // boolean [] pair_mask,
disparity_strengths[i][0], // double disparity,
disp_dist); // double [][] disp_dist)
}
return pairs_offsets;
}
public void printParams() { // not used in lwir
// to make sure it is updated
......@@ -1666,10 +2112,12 @@ public class Corr2dLMA {
if (np >= ndisp_index) parname = PAR_NAME_CORRNDISP + (np - ndisp_index);
else if (np >= ddisp_index) parname = PAR_NAME_CORRDISP + (np - ddisp_index);
else {
int ntile = np / tile_params;
int ntile_max = np / tile_params;
int ntile = ntile_max / numMax;
int nmax = ntile_max % numMax;
int anpr = np % tile_params;
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile;
else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX);
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+nmax+"-"+ntile;
else parname = PAR_NAME_SCALE +nmax+"-"+ntile + ":"+ (anpr - G0_INDEX);
}
System.out.println(String.format("%2d%1s %22s %f",
......@@ -1754,8 +2202,8 @@ public class Corr2dLMA {
if (i < samples.size()) {
s = samples.get(i);
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=%3d fcam=%2d scam=%2d tile=%d",
i, s.ix, s.iy, s.v, fx_pos, s.w, s.pair, fs[0], fs[1], s.tile));
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f fx=%9.6f w=%9.7f pair=%3d fcam=%2d scam=%2d tile=%d mx=%d",
i, s.ix, s.iy, s.v, fx_pos, s.w, s.pair, fs[0], fs[1], s.tile, s.imax));
}
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]));
......@@ -1765,8 +2213,8 @@ public class Corr2dLMA {
int ns =0;
for (Sample s:samples){
int [] fs = correlation2d.getPair(s.pair);
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f w=%9.7f pair= %3d fcam=%2d scam=%2d tile=%d",
ns++, s.ix, s.iy, s.v, s.w, s.pair, fs[0], fs[1], s.tile));
System.out.println(String.format("%3d: x=%2d y=%2d v=%9.6f w=%9.7f pair= %3d fcam=%2d scam=%2d tile=%d mx=%d",
ns++, s.ix, s.iy, s.v, s.w, s.pair, fs[0], fs[1], s.tile, s.imax));
}
}
}
......@@ -1782,8 +2230,9 @@ public class Corr2dLMA {
return all_pars;
}
/*
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;
for (int i = 0; i < num_pairs; i++) {
sum_amp += all_pars[G0_INDEX + i + tile_params * nTile]; // group_weights is normalized
......@@ -1794,6 +2243,26 @@ public class Corr2dLMA {
if (sum_amp > 1.25 * max_amp) sum_amp = max_amp;
double [] ds = {disparity, sum_amp};
return ds;
}*/
public double [] getDisparityStrength(int nTile) { // USED in lwir
return getDisparityStrength(nTile, 0);
}
public double [] getDisparityStrength(int nTile, int nMax) { // USED in lwir
int offs = (nTile * numMax + nMax) * tile_params;
double disparity = -all_pars[DISP_INDEX + offs];
double sum_amp = 0.0;
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 + offs]; // group_weights is normalized
}
// protect from weird fitting results
double max_amp = 0.0;
for (Sample s: samples) if ((s.v > max_amp) && (s.tile == nTile)) max_amp = s.v;
if (sum_amp > 1.25 * max_amp) sum_amp = max_amp;
double [] ds = {disparity, sum_amp};
return ds;
}
public double [][] getDisparityStrength() { // USED in lwir
......@@ -1807,9 +2276,19 @@ public class Corr2dLMA {
public double [] getDisparityStrengthABC(int nTile) {// width = 1/sqrt(all_pars[A_INDEX])
double [] ds = getDisparityStrength(nTile);
return getDisparityStrengthABC(nTile, 0);
}
public double [] getDisparityStrengthABC(int nTile, int nMax) {// width = 1/sqrt(all_pars[A_INDEX])
double [] ds = getDisparityStrength(nTile, nMax);
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
int offs = (nTile * numMax + nMax) * tile_params;
double [] dsw = {
ds[0],
ds[1],
all_pars[A_INDEX + offs],
all_pars[B_INDEX + offs],
all_pars[CMA_INDEX+ offs]}; // asymmetry
return dsw;
}
......@@ -1881,7 +2360,7 @@ public class Corr2dLMA {
double [] max_diff = new double [num_pars];
// delta = 0.001;
// FIXME numMax
double [][] jt = new double [num_pars][num_points];
double [][] jt_delta = new double [num_pars][num_points];
double [] fx = getFxJt( vector,jt);
......@@ -1894,10 +2373,12 @@ public class Corr2dLMA {
if (anp >= ndisp_index) parname = PAR_NAME_CORRNDISP + (anp - ndisp_index);
else if (anp >= ddisp_index) parname = PAR_NAME_CORRDISP + (anp - ddisp_index);
else {
int ntile = anp / tile_params;
int ntile_max = anp / tile_params;
int ntile = ntile_max / numMax;
int nmax = ntile_max % numMax;
int anpr = anp % tile_params;
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+"-"+ntile;
else parname = PAR_NAME_SCALE +"-"+ntile + ":"+ (anpr - G0_INDEX);
if (anpr < G0_INDEX) parname = PAR_NAMES[anpr]+nmax+"-"+ntile;
else parname = PAR_NAME_SCALE +nmax+"-"+ntile + ":"+ (anpr - G0_INDEX);
}
System.out.print(String.format("| %16s ", parname));
}
......@@ -2017,57 +2498,30 @@ public class Corr2dLMA {
return rslt;
}
public double [][] lmaDisparityStrength1( // restored from git
public double [][] lmaDisparityStrength(
double lmas_min_amp, // minimal ratio of minimal pair correlation amplitude to maximal pair correlation amplitude
double lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
double lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
double lma_min_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
double lma_min_max_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
double lma_min_min_ac, // minimal of A and C coefficients minimum (measures sharpest point)
double lma_max_area, // maximal half-area (if > 0.0)
double lma_str_scale, // convert lma-generated strength to match previous ones - scale
double lma_str_offset // convert lma-generated strength to match previous ones - add to result
){
double [][] ds = new double[numTiles][2];
double [] rms = getRmsTile();
double [][] maxmin_amp = getMaxMinAmpTile();
double [][] abc = getABCTile();
for (int tile = 0; tile < numTiles; tile++) {
ds[tile][0] = Double.NaN;
if (Double.isNaN(maxmin_amp[tile][0])) {
continue;
}
double avg = 0.5*(maxmin_amp[tile][0]+maxmin_amp[tile][1]);
double rrms = rms[tile]/avg;
if (((lma_max_rel_rms > 0.0) && (rrms > lma_max_rel_rms)) ||
(Math.max(abc[tile][0], abc[tile][2]) < lma_min_ac)) {
continue;
}
if (lma_max_area > 0) {
if ((abc[tile][0] > 0.0) && (abc[tile][2] > 0.0)) {
double area = 1.0/abc[tile][0] + 1.0/abc[tile][2]; // area of a maximum
if (area > lma_max_area) {
continue; // too wide maximum
}
} else {
continue; // not a maximum
}
return lmaDisparityStrengths(
lmas_min_amp, // minimal ratio of minimal pair correlation amplitude to maximal pair correlation amplitude
lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
lma_min_max_ac, // minimal of A and C coefficients maximum (measures sharpest point/line)
lma_min_min_ac, // minimal of A and C coefficients minimum (measures sharpest point)
lma_max_area, // maximal half-area (if > 0.0)
lma_str_scale, // convert lma-generated strength to match previous ones - scale
lma_str_offset // convert lma-generated strength to match previous ones - add to result
)[0];
}
double strength = Math.sqrt(avg/rrms);
double disparity = -all_pars[DISP_INDEX + tile*tile_params];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue;
}
ds[tile][0] = disparity;
ds[tile][1] = (strength * lma_str_scale) + lma_str_offset;
}
return ds;
}
public double [][] lmaDisparityStrength(
public double [][][] lmaDisparityStrengths(
double lmas_min_amp, // minimal ratio of minimal pair correlation amplitude to maximal pair correlation amplitude
double lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
double lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
......@@ -2077,12 +2531,14 @@ public class Corr2dLMA {
double lma_str_scale, // convert lma-generated strength to match previous ones - scale
double lma_str_offset // convert lma-generated strength to match previous ones - add to result
){
double [][] ds = new double[numTiles][3];
double [][][] ds = new double[numMax][numTiles][3];
double [] rms = getRmsTile();
double [][] maxmin_amp = getMaxMinAmpTile();
double [][] abc = getABCTile();
for (int nmax = 0; nmax < numMax; nmax++) {
double [][] maxmin_amp = getMaxMinAmpTile(nmax); // nmax
double [][] abc = getABCTile(nmax); // nmax
for (int tile = 0; tile < numTiles; tile++) {
ds[tile][0] = Double.NaN + 0;
int offs = (tile * numMax + nmax) * tile_params;
ds[nmax][tile][0] = Double.NaN + 0;
if (Double.isNaN(maxmin_amp[tile][0])) {
continue;
}
......@@ -2096,13 +2552,13 @@ public class Corr2dLMA {
double rrms = rms[tile]/avg;
if (((lma_max_rel_rms > 0.0) && (rrms > lma_max_rel_rms)) ||
(Math.max(abc[tile][0], abc[tile][2]) < lma_min_max_ac)
// || (Math.min(abc[tile][0], abc[tile][2]) < lma_min_min_ac)
// || (Math.min(abc[tile][0], abc[tile][2]) < lma_min_min_ac)
) {
continue;
}
if (lma_max_area > 0) {
if ((abc[tile][0] > 0.0) && (abc[tile][2] > 0.0)) {
// double area_old = 1.0/abc[tile][0] + 1.0/abc[tile][2]; // area of a maximum
// double area_old = 1.0/abc[tile][0] + 1.0/abc[tile][2]; // area of a maximum
double area = 1.0/Math.sqrt(abc[tile][0] * abc[tile][2]);
if (area > lma_max_area) {
continue; // too wide maximum
......@@ -2114,7 +2570,7 @@ public class Corr2dLMA {
}
double strength = Math.sqrt(avg/rrms);
double disparity = -all_pars[DISP_INDEX + tile*tile_params];
double disparity = -all_pars[DISP_INDEX + offs];
if ((strength < lma_min_strength) || Double.isNaN(disparity)) {
continue;
}
......@@ -2123,9 +2579,10 @@ public class Corr2dLMA {
continue;
}
strength = Math.sqrt(strength * Math.sqrt(ac)); // / area ); // new strength
ds[tile][0] = disparity;
ds[tile][1] = (strength * lma_str_scale) + lma_str_offset;
ds[tile][2] = strength; // as is
ds[nmax][tile][0] = disparity;
ds[nmax][tile][1] = (strength * lma_str_scale) + lma_str_offset;
ds[nmax][tile][2] = strength; // as is
}
}
return ds;
}
......@@ -2143,6 +2600,10 @@ public class Corr2dLMA {
double [] rms = getRmsTile();
double [][] maxmin_amp = getMaxMinAmpTile();
double [][] abc = getABCTile();
if (numMax != 1) {
System.out.println("lmaDisparityStrengthLY(): numMax = "+numMax+", while only numMax==1 is supported");
return null;
}
for (int tile = 0; tile < numTiles; tile++) {
ds[tile][0] = Double.NaN;
if (Double.isNaN(maxmin_amp[tile][0])) {
......@@ -2191,6 +2652,10 @@ public class Corr2dLMA {
double lma_str_scale, // convert lma-generated strength to match previous ones - scale
double lma_str_offset // convert lma-generated strength to match previous ones - add to result
){
if (numMax != 1) {
System.out.println("lmaGetExtendedStats(): numMax = "+numMax+", while only numMax==1 is supported");
return null;
}
// double [][] ds = new double[numTiles][2];
double [][] ext_stats = new double[numTiles][11];
double [] rms = getRmsTile();
......@@ -2325,17 +2790,21 @@ public class Corr2dLMA {
* @return array [tile][{amax, amin}]
*/
public double [][] getMaxMinAmpTile(){
return getMaxMinAmpTile(0);
}
public double [][] getMaxMinAmpTile(int nmax){
double [][] maxmins = new double[numTiles][2];
for (int tile = 0; tile < numTiles; tile++) {
int offs = (tile * numMax + nmax) * tile_params;
maxmins[tile][0] = Double.NaN;
maxmins[tile][1] = Double.NaN;
for (int i = 0; i <num_pairs; i++) {
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
maxmins[tile][0] = all_pars[G0_INDEX + i + tile*tile_params];
if (par_mask[G0_INDEX + i + offs]) {
if (!(all_pars[G0_INDEX + i + offs] <= maxmins[tile][0])) { // NaN will trigger true
maxmins[tile][0] = all_pars[G0_INDEX + i + offs];
}
if (!(all_pars[G0_INDEX + i + tile*tile_params] >= maxmins[tile][1])) {
maxmins[tile][1] = all_pars[G0_INDEX + i + tile*tile_params];
if (!(all_pars[G0_INDEX + i + offs] >= maxmins[tile][1])) {
maxmins[tile][1] = all_pars[G0_INDEX + i + offs];
}
}
}
......@@ -2375,7 +2844,7 @@ public class Corr2dLMA {
return maxmins;
}
public double [][] getABCTile(){
/*public double [][] getABCTile(){
double [][] abc = new double[numTiles][3];
for (int tile = 0; tile < numTiles; tile++) {
abc[tile][0] = all_pars[A_INDEX+ tile * tile_params];
......@@ -2383,9 +2852,25 @@ public class Corr2dLMA {
abc[tile][2] = abc[tile][0] + all_pars[CMA_INDEX+ tile * tile_params];
}
return abc;
}*/
public double [][] getABCTile(int nmax){
double [][] abc = new double[numTiles][3];
for (int tile = 0; tile < numTiles; tile++) {
int offs = (tile * numMax + nmax) * tile_params;
abc[tile][0] = all_pars[A_INDEX+ offs];
abc[tile][1] = all_pars[B_INDEX+ offs];
abc[tile][2] = abc[tile][0] + all_pars[CMA_INDEX+ offs];
}
return abc;
}
public double [][] getABCTile(){
return getABCTile(0);
}
public double [] getJtWdiff( // not used in lwir
@Deprecated
public static double [] getJtWdiff( // not used in lwir
double [] wdiff,
double [][] jt){
int num_pars = jt.length;
......
src/main/java/com/elphel/imagej/tileprocessor/Correlation2d.java
View file @ c3ee5b3f
......@@ -3518,6 +3518,7 @@ public class Correlation2d {
}
Corr2dLMA lma = new Corr2dLMA(
corrs.length,
1, // int numMax,
this, // Correlation2d correlation2d,
transform_size,
corr_wnd,
......@@ -3620,6 +3621,7 @@ public class Correlation2d {
lma.addSample( // x = 0, y=0 - center
ntile, // tile
npair,
0, // int imax, // number of maximum (just a hint)
// 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
......@@ -3660,12 +3662,13 @@ public class Correlation2d {
boolean lmaSuccess = false;
while (!lmaSuccess) {
boolean OK = lma.initVector( // USED in lwir
null, // boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
imgdtt_params.lma_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
imgdtt_params.lma_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
imgdtt_params.lma_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
imgdtt_params.lma_adjust_ly1, // boolean adjust_lazyeye_ortho, // adjust disparity corrections orthogonal to disparities lma_adjust_ly1
disp_str, // xcenter_str, // double [][] disp_str, // initial value of disparity/strength/?
new double[][][] {disp_str}, // xcenter,
imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
imgdtt_params.lma_cost_wy, // double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
imgdtt_params.lma_cost_wxy // double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
......@@ -3746,6 +3749,7 @@ public class Correlation2d {
} else { // have to restart LMA initialization
lma = new Corr2dLMA(
corrs.length,
1, // int numMax,
this, // Correlation2d correlation2d,
transform_size,
corr_wnd,
......@@ -3755,12 +3759,14 @@ public class Correlation2d {
lma.setSamples(samples); // restore samples
}
lma.initVector( // USED in lwir
null, // boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
imgdtt_params.lma_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
imgdtt_params.lma_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
imgdtt_params.lma_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
true, // imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
true, // imgdtt_params.lma_adjust_ly1, // boolean adjust_lazyeye_ortho, // adjust disparity corrections orthogonal to disparities lma_adjust_ly1
ds, // disp_str, // xcenter_str, // double [][] disp_str, // initial value of disparity/strength/?
// ds, // disp_str, // xcenter_str, // double [][] disp_str, // initial value of disparity/strength/?
new double[][][] {ds}, // xcenter,
imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
imgdtt_params.lma_cost_wy, // double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
imgdtt_params.lma_cost_wxy // double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
......@@ -3959,6 +3965,7 @@ public class Correlation2d {
int corr_size = 2 * transform_size - 1;
Corr2dLMA lma = new Corr2dLMA(
1,
1, // int numMax,
this, // Correlation2d correlation2d,
transform_size,
corr_wnd,
......@@ -4118,6 +4125,7 @@ public class Correlation2d {
lma.addSample( // x = 0, y=0 - center
0, // tile
npair,
0, // int imax, // number of maximum (just a hint)
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)
v, // double v, // correlation value at that point
......@@ -4193,12 +4201,13 @@ public class Correlation2d {
}
lma.initVector(
null, // boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
imgdtt_params.lmas_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
imgdtt_params.lmas_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
imgdtt_params.lmas_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
(adjust_ly ? imgdtt_params.lma_adjust_wxy : false), //imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
(adjust_ly ? imgdtt_params.lma_adjust_ly1: false), // imgdtt_params.lma_adjust_ly1, // boolean adjust_lazyeye_ortho, // adjust disparity corrections orthogonal to disparities lma_adjust_ly1
disp_str2_scaled, // xcenter,
new double[][][] {disp_str2_scaled}, // xcenter,
imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
(adjust_ly ? imgdtt_params.lma_cost_wy : 0.0), // imgdtt_params.lma_cost_wy, // double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
(adjust_ly ? imgdtt_params.lma_cost_wxy : 0.0) //imgdtt_params.lma_cost_wxy // double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
......@@ -4262,7 +4271,8 @@ public class Correlation2d {
if (disp != null) {
disp_str2[0] = disp;
lma.initDisparity( // USED in lwir null pointer
disp_str2); // double [][] disp_str // initial value of disparity
disp_str2, // double [][] disp_str // initial value of disparity
0); // int nmax); //
if (debug_level > 1) {
System.out.println("Input data:");
......@@ -4413,15 +4423,7 @@ public class Correlation2d {
if (imgdtt_params.lma_sigma > 0) gb = new DoubleGaussianBlur();
int center = transform_size - 1;
int corr_size = 2 * transform_size - 1;
Corr2dLMA lma = new Corr2dLMA(
1,
this, // Correlation2d correlation2d,
transform_size,
corr_wnd,
rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.lmas_gaussian //boolean gaussian_mode
);
double [][][] pair_offsets0 = lma.getPairsOffsets(
double [][][] pair_offsets0 = getPairsOffsets(
corrs, // double [][] corrs,
pair_mask, // boolean [] pair_mask,
disp_str_dual, // double [][] disparity_strengths,
......@@ -4429,7 +4431,7 @@ public class Correlation2d {
double [][][] own_masks0 = new double [pair_offsets0.length][][];
double [][][] lma_corr_weights0 = getLmaWeights(
imgdtt_params, // ImageDttParameters imgdtt_params,
lma, // Corr2dLMA lma,
// lma, // Corr2dLMA lma,
imgdtt_params.bimax_lpf_neib, // double lpf_neib, // if >0, add ortho neibs (corners - squared)
imgdtt_params.bimax_notch_pwr, // double notch_pwr, // = 4.00;
imgdtt_params.bimax_adv_power, // double adv_power, // reduce weight from overlap with adversarial maximum
......@@ -4572,6 +4574,16 @@ public class Correlation2d {
if (num_used_pairs < imgdtt_params.bimax_min_num_pairs) {
return null;
}
Corr2dLMA lma = new Corr2dLMA(
1,
lma_corr_weights.length, // int numMax,
this, // Correlation2d correlation2d,
transform_size,
corr_wnd,
rXY, //double [][] rXY, // non-distorted X,Y offset per nominal pixel of disparity
imgdtt_params.lmas_gaussian //boolean gaussian_mode
);
for (int npair = 0; npair < pair_mask.length; npair++) if ((corrs[npair] != null) && (used_pairs[npair])){
for (int nmax = 0; nmax < lma_corr_weights.length; nmax++) { // use same blurred version for all max-es
for (int i = 1; i < lma_corr_weights[nmax][npair].length; i++) if (lma_corr_weights[nmax][npair][i] > 0.0) {
......@@ -4585,6 +4597,7 @@ public class Correlation2d {
lma.addSample( // x = 0, y=0 - center
0, // tile
npair,
nmax, // int imax, // number of maximum (just a hint)
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)
v, // double v, // correlation value at that point
......@@ -4631,41 +4644,55 @@ public class Correlation2d {
corr_size,
corr_size,
true,
"samples_weights"+"_x"+tileX+"_y"+tileY,
"samples_weights"+"_x"+tileX+"_y"+tileY+"_nmx"+nmax,
getCorrTitles());
}
}
}
// initially will use just first (selected) maximum, both - whenLMA will be modified to support 3 maximums.
double [][] disp_str2 = {disp_str_all[0]}; // temporary // will be calculated/set later
double [][][] disp_str2 = new double[disp_str_all.length][1][];// = {disp_str_all[0]}; // temporary // will be calculated/set later
for (int i = 0; i <disp_str_all.length; i++) if (disp_str_all[i] != null){
disp_str2[i][0] = disp_str_all[i];
}
// double [][] disp_str2 = {disp_str_all[0]}; // temporary // will be calculated/set later
boolean lmaSuccess = false;
int num_lma_retries = 0;
while (!lmaSuccess) {
num_lma_retries ++; // debug
lma.initVector(
// int num_lma_retries = 0;
// When running LMA - first do not touch disparity?
// int lma_pass = imgdtt_params.bimax_dual_pass? 0 : 1; // pass0 - w/o disparity, pass 1 - with
boolean [] adjust_disparities = new boolean [disp_str_all.length]; // all false;
boolean needprep = true; //t npass = 0;
for (int npass = (imgdtt_params.bimax_dual_pass? 00 : 1); npass < 2; npass++) { // may break while (!lmaSuccess) {
// num_lma_retries ++; // debug
if (needprep) {
lma.preparePars(
disp_str2, // double [][][] disp_str_all, initial value of disparity [max][tile]{disp, strength}
imgdtt_params.lma_half_width); // double half_width, // A=1/(half_widh)^2 lma_half_width
needprep = false;
lma.setMatrices (disp_dist);
lma.initMatrices (); // should be called after initVector and after setMatrices
lma.initDisparity(disp_str2); // initial value of disparity [max][tile]{disp, strength}
} else {
lma.updateFromVector();
}
if (npass > 0) {
adjust_disparities = null;
}
lma.setParMask( // USED in lwir
adjust_disparities, // null, // boolean [] adjust_disparities, // null - adjust all, otherwise - per maximum
imgdtt_params.lmas_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
imgdtt_params.lmas_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
imgdtt_params.lmas_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
false, // (adjust_ly ? imgdtt_params.lma_adjust_wxy : false), //imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye_par, // adjust disparity corrections parallel to disparities lma_adjust_wxy
false, // (adjust_ly ? imgdtt_params.lma_adjust_ly1: false), // imgdtt_params.lma_adjust_ly1, // boolean adjust_lazyeye_ortho, // adjust disparity corrections orthogonal to disparities lma_adjust_ly1
disp_str2, // xcenter,
imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
0.0, // (adjust_ly ? imgdtt_params.lma_cost_wy : 0.0), // imgdtt_params.lma_cost_wy, // double cost_lazyeye_par, // cost for each of the non-zero disparity corrections lma_cost_wy
0.0 // (adjust_ly ? imgdtt_params.lma_cost_wxy : 0.0) //imgdtt_params.lma_cost_wxy // double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
);
lma.setMatrices(disp_dist);
lma.initMatrices(); // should be called after initVector and after setMatrices
lma.initDisparity( // USED in lwir null pointer
disp_str2); // double [][] disp_str // initial value of disparity
0.0); // (adjust_ly ? imgdtt_params.lma_cost_wxy : 0.0) //imgdtt_params.lma_cost_wxy // double cost_lazyeye_odtho // cost for each of the non-zero ortho disparity corrections lma_cost_wxy
if (debug_level > 1) {
if (debug_level > 0) { // 1) {
System.out.println("Input data:");
lma.printInputDataFx(false);
lma.printParams();
}
lmaSuccess = lma.runLma(
imgdtt_params.lmas_lambda_initial, // double lambda, // 0.1
......@@ -4679,14 +4706,12 @@ public class Correlation2d {
if (debug_level > -2) {
System.out.println("Found bad tile/pair during single (probably wrong initial maximum - try around preliminary? "+lma.getBadTile());
}
} else {
break;
}
}
if (lmaSuccess) {
lma.updateFromVector();
double [][] dispStr = lma.lmaDisparityStrength( //TODO: add parameter to filter out negative minimums ?
double [][][] dispStrs = lma.lmaDisparityStrengths( //TODO: add parameter to filter out negative minimums ?
imgdtt_params.lmas_min_amp, // minimal ratio of minimal pair correlation amplitude to maximal pair correlation amplitude
imgdtt_params.lmas_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lmas_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
......@@ -4696,20 +4721,25 @@ public class Correlation2d {
imgdtt_params.lma_str_scale, // convert lma-generated strength to match previous ones - scale
imgdtt_params.lma_str_offset // convert lma-generated strength to match previous ones - add to result
);
if (dispStr[0][1] <= 0) {
for (int nmax = 0; nmax < dispStrs.length; nmax++) if (dispStrs[nmax][0][1] <= 0) {
lmaSuccess = false;
break;
}
if (!lmaSuccess) {
if (debug_lma_tile != null) {
debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
// debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
debug_lma_tile[4] = lma.getNumIter();
}
} else {
if (debug_level > -2) {
System.out.println(String.format("LMA disparity=%8.5f, str=%8.5f",
dispStr[0][0],dispStr[0][1]));
for (int nmax = 0; nmax < dispStrs.length; nmax++) {
System.out.println(String.format("LMA max-%d: disparity=%8.5f, str=%8.5f",
nmax,dispStrs[nmax][0][0],dispStrs[nmax][0][1]));
}
}
double [] rms = lma.getRMS();
if (debug_lma_tile != null) {
debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
// debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
debug_lma_tile[4] = lma.getNumIter();
debug_lma_tile[5] = rms[1]; // pure rms
}
......@@ -4747,7 +4777,7 @@ public class Correlation2d {
}
} else if (debug_lma_tile != null) {
debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
// debug_lma_tile[3] = num_lma_retries; // number of wasted attempts
}
return lmaSuccess? lma: null;
}
......@@ -4777,10 +4807,61 @@ public class Correlation2d {
}
}
/**
* Calculate offsets for multiple disparities, for each defined pair x,y expected offset, assuming only disparity, not lazy eye
* @param corrs - pairs 2D correlations (each in scanline order) - just to determine null/non-null
* @param disparity_strengths - expected {disparity, strength} pairs (e.g. from CM)
* @param disp_dist - per camera disparity matrix as a 1d (linescan order))
* @return array of per-disparity, per pair x,y expected center offset in 2D correlations or nulls for undefined pairs (or null if already set)
*/
public double [][][] getPairsOffsets(
double [][] corrs,
boolean [] pair_mask,
double [][] disparity_strengths,
double [][] disp_dist){ //
double [][][] pairs_offsets = new double [disparity_strengths.length][][];
for (int i = 0; i < pairs_offsets.length; i++) {
pairs_offsets[i] = getPairsOffsets(
corrs, // double [][] corrs,
pair_mask, // boolean [] pair_mask,
disparity_strengths[i][0], // double disparity,
disp_dist); // double [][] disp_dist)
}
return pairs_offsets;
}
/**
* Calculate each defined pair x,y expected offset, assuming only disparity, not lazy eye
* @param corrs - pairs 2D correlations (each in scanline order) - just to determine null/non-null
* @param disparity - expected disparity (e.g. from CM)
* @param disp_dist - per camera disparity matrix as a 1d (linescan order))
* @return per pair x,y expected center offset in 2D correlations or nulls for undefined pairs (or null if already set)
*/
public double [][] getPairsOffsets(
double [][] corrs,
boolean [] pair_mask,
double disparity,
double [][] disp_dist){ //
double [][] xy_offsets = new double [corrs.length][];
Matrix [] m_disp = Corr2dLMA.getMatrices(
disp_dist, // double [][] am_disp,
getNumSensors()); // int num_cams)
for (int pair = 0; pair < xy_offsets.length; pair++) if ((pair < getNumPairs()) && (corrs[pair] != null) && ((pair_mask == null) || pair_mask[pair])){ // OK to calculate for each
int [] fscam = getPair(pair); // returns [first_cam, second_cam]
Matrix mdd_dnd = new Matrix(new double[] {-disparity, 0.0},2);
Matrix xcam_ycam_f = m_disp[fscam[0]].times(mdd_dnd);
Matrix xcam_ycam_s = m_disp[fscam[1]].times(mdd_dnd);
xy_offsets[pair] = xcam_ycam_f.minus(xcam_ycam_s).getColumnPackedCopy();
}
return xy_offsets;
}
public double[][][] getLmaWeights(
ImageDttParameters imgdtt_params,
Corr2dLMA lma,
// Corr2dLMA lma,
double lpf_neib, // if >0, add ortho neibs (corners - squared)
double notch_pwr, // = 4.00;
double adv_power, // reduce weight from overlap with adversarial maximum
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDtt.java
View file @ c3ee5b3f
......@@ -2629,9 +2629,9 @@ public class ImageDtt extends ImageDttCPU {
}
}
if (debugTile1) {
correlation2d.corrLMA2DualMax( // null pointer
Corr2dLMA lma_dual = correlation2d.corrLMA2DualMax( // null pointer
imgdtt_params, // ImageDttParameters imgdtt_params,
1, // int combine_mode, // 0 - both, 1 - strongest, 2 - nearest to zero, 3 - FG, 4 - BG
0, // 3, // 0, // 1, // int combine_mode, // 0 - both, 1 - strongest, 2 - nearest to zero, 3 - FG, 4 - BG
// imgdtt_params.lmas_LY_single, // false, // boolean adjust_ly, // adjust Lazy Eye
corr_wnd, // double [][] corr_wnd, // correlation window to save on re-calculation of the window
corr_wnd_inv_limited, // corr_wnd_limited, // correlation window, limited not to be smaller than threshold - used for finding max/convex areas (or null)
......@@ -2647,6 +2647,8 @@ public class ImageDtt extends ImageDttCPU {
(debugTile0 ? 1: -2), // int debug_level,
tileX, // int tileX, // just for debug output
tileY );
System.out.println("clt_process_tl_correlations() corrLMA2DualMax() done, lma_dual="+
((lma_dual== null)? "null": " not null"));
}
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDttParameters.java
View file @ c3ee5b3f
......@@ -84,6 +84,7 @@ public class ImageDttParameters {
public int bimax_rad_convex_search = 2; // how far from predicted to search for maximums
public int bimax_min_num_samples = 4; // minimal number of samples per pair per maximum
public int bimax_min_num_pairs = 8; // minimal number of used pairs
public boolean bimax_dual_pass = true; // First pass - do not adjust disparity
//lmamask_
public boolean lmamask_dbg = false; // show LMA images, exit after single BG
......@@ -183,7 +184,7 @@ public class ImageDttParameters {
public double lma_disp_range = 5.0; // disparity range to combine in one cluster (to mitigate ERS
// LMA single parameters
// public boolean lmas_gaussian = false; // model correlation maximum as a Gaussian (false - as a parabola)
public int lmas_gaussian = 0; // 0 - parabola, 1 - Gaussian, 2 - limited parabola, 3 - limited squared parabola
public int lmas_gaussian = 3; // 0 - parabola, 1 - Gaussian, 2 - limited parabola, 3 - limited squared parabola
public boolean lmas_adjust_wm = true; // used in new for width
public boolean lmas_adjust_wy = true; // adjust non-circular
public boolean lmas_adjust_ag = true; // adjust gains gains
......@@ -212,7 +213,7 @@ public class ImageDttParameters {
public int lma_gaussian = 0; // 0 - parabola, 1 - Gaussian, 2 - limited parabola, 3 - limited squared parabola
public boolean lma_second = true; // re-run LMA after removing weak/failed tiles
// public boolean lma_second_gaussian = false; // re-run after removing weal/failed in Gaussian mode
public int lma_second_gaussian = 0; // false; // re-run after removing weal/failed in Gaussian mode
public int lma_second_gaussian = 0; // false; // re-run after removing weak/failed in Gaussian mode
public boolean lma_adjust_wm = true; // used in new for width
public boolean lma_adjust_wy = true; // false; // used in new for ellipse
public boolean lma_adjust_wxy = true; // used in new for lazy eye adjust parallel-to-disparity correction
......@@ -472,6 +473,8 @@ public class ImageDttParameters {
"Minimal number of samples per pair per maximum to use this pair by LMA (each of the maximums used)");
gd.addNumericField("Minimal number of used pairs", this.bimax_min_num_pairs, 0, 3, "",
"Do not use LMA if total number of used pairs is lower");
gd.addCheckbox ("Dual-pass LMA", this.bimax_dual_pass,
"First adjust other parameters (keeping disparities), then add disparities");
gd.addMessage("LMA samples filter based on estimated disparity");
gd.addCheckbox ("Debug LMA", this.lmamask_dbg,
......@@ -888,6 +891,7 @@ public class ImageDttParameters {
this.bimax_rad_convex_search= (int) gd.getNextNumber();
this.bimax_min_num_samples= (int) gd.getNextNumber();
this.bimax_min_num_pairs= (int) gd.getNextNumber();
this.bimax_dual_pass = gd.getNextBoolean();
this.lmamask_dbg = gd.getNextBoolean();
this.lmamask_en = gd.getNextBoolean();
......@@ -1114,6 +1118,7 @@ public class ImageDttParameters {
properties.setProperty(prefix+"bimax_rad_convex_search", this.bimax_rad_convex_search +"");
properties.setProperty(prefix+"bimax_min_num_samples", this.bimax_min_num_samples +"");
properties.setProperty(prefix+"bimax_min_num_pairs", this.bimax_min_num_pairs +"");
properties.setProperty(prefix+"bimax_dual_pass", this.bimax_dual_pass +"");
properties.setProperty(prefix+"lmamask_dbg", this.lmamask_dbg +"");
properties.setProperty(prefix+"lmamask_en", this.lmamask_en +"");
......@@ -1344,6 +1349,7 @@ public class ImageDttParameters {
if (properties.getProperty(prefix+"bimax_rad_convex_search")!=null) this.bimax_rad_convex_search=Integer.parseInt(properties.getProperty(prefix+"bimax_rad_convex_search"));
if (properties.getProperty(prefix+"bimax_min_num_samples")!=null) this.bimax_min_num_samples=Integer.parseInt(properties.getProperty(prefix+"bimax_min_num_samples"));
if (properties.getProperty(prefix+"bimax_min_num_pairs")!=null) this.bimax_min_num_pairs=Integer.parseInt(properties.getProperty(prefix+"bimax_min_num_pairs"));
if (properties.getProperty(prefix+"bimax_dual_pass")!=null) this.bimax_dual_pass=Boolean.parseBoolean(properties.getProperty(prefix+"bimax_dual_pass"));
if (properties.getProperty(prefix+"lmamask_dbg")!=null) this.lmamask_dbg=Boolean.parseBoolean(properties.getProperty(prefix+"lmamask_dbg"));
if (properties.getProperty(prefix+"lmamask_en")!=null) this.lmamask_en=Boolean.parseBoolean(properties.getProperty(prefix+"lmamask_en"));
......@@ -1591,6 +1597,7 @@ public class ImageDttParameters {
idp.bimax_rad_convex_search= this.bimax_rad_convex_search;
idp.bimax_min_num_samples= this.bimax_min_num_samples;
idp.bimax_min_num_pairs= this.bimax_min_num_pairs;
idp.bimax_dual_pass= this.bimax_dual_pass;
idp.lmamask_dbg= this.lmamask_dbg;
idp.lmamask_en= this.lmamask_en;
......
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