Commit b5dc98c9 authored by Andrey Filippov's avatar Andrey Filippov

Tested LMA2+LY for one tile

parent 1a329010
......@@ -64,6 +64,7 @@ Fx=A(i)*W(x,y)*(pa*(x-x0j)^2+2*pb*(x-x0j)*(y-y0j)+pc*(y-y0j)^2)
*
*/
import java.util.ArrayList;
import java.util.Arrays;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
......@@ -119,7 +120,9 @@ public class Corr2dLMA {
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; // 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;
private final int transform_size;
......@@ -334,13 +337,12 @@ public class Corr2dLMA {
}
public double [][][] dbgGetSamples(double [][] ds, int mode){
int [][] comb_map = getCombMap();
int numPairs = comb_map[0][0];
comb_map[0][0] = -1;
// int [][] comb_map = getCombMap();
// int numPairs = comb_map[0][0];
// comb_map[0][0] = -1;
int numPairs = getNumAllTilesUsedPairs();
int size = 2* transform_size -1;
int size2 = size*size;
double [][][] rslt = new double [numTiles][numPairs][size2];
for (int nTile = 0; nTile < numTiles; nTile++) {
for (int np = 0; np < numPairs; np++) {
......@@ -364,27 +366,29 @@ public class Corr2dLMA {
else if (mode == 1) d = s.w;
else if (mode == 2) d = fx[ns];
// int np = comb_map[s.fcam][s.scam]; ////////////////////
int np = s.pair; ////////////////////
rslt[s.tile][np][s.iy*size + s.ix] = d;
// int np = s.pair; ////////////////////
int inp = used_pairs_map[s.tile][s.pair];
rslt[s.tile][inp][s.iy*size + s.ix] = d;
}
return rslt;
}
@Deprecated
public String [] dbgGetSliceTiles() {
int [][] comb_map = getCombMap();
int np = comb_map[0][0];
comb_map[0][0] = -1;
public String [] dbgGetSliceTitles() {
int [] comb_map = getCombMap();
int np = getNumAllTilesUsedPairs(); // comb_map[0][0];
// comb_map[0][0] = -1;
String [] srslt = new String [np];
for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
if (comb_map[f][s] >= 0) {
srslt[comb_map[f][s]] = ""+f+"->"+s;
for (int npair = 0; npair < comb_map.length; npair++) {
if (comb_map[npair] >= 0) {
int [] fs = correlation2d.getPair(npair);
srslt[comb_map[npair]] = ""+fs[0]+"->"+fs[1];
}
}
return srslt;
}
/*
@Deprecated
public int [][] getCombMap(){
boolean [][] comb_pairs = new boolean[num_cams][num_cams];
......@@ -404,8 +408,36 @@ public class Corr2dLMA {
return comb_map;
}
*/
public int [] getCombMap(){
int []comb_map = new int [num_pairs];
Arrays.fill(comb_map, -1);
int np = 0;
for (int npair = 0; npair < num_pairs; npair++) {
for (int t = 0; t < numTiles; t++) {
if (used_pairs_map[t][npair] >= 0) {
comb_map[npair] = np++;
break;
}
}
}
return comb_map;
}
public int getNumAllTilesUsedPairs() {
int np = 0;
for (int npair = 0; npair < num_pairs; npair++) {
for (int t = 0; t < numTiles; t++) {
if (used_pairs_map[t][npair] >= 0) {
np++;
break;
}
}
}
return np;
}
@Deprecated
public int getPairIndex(int f, int s) {
if (f > s) {
......@@ -497,14 +529,18 @@ public class Corr2dLMA {
adjust_lazyeye_ortho = adjust_lazyeye_par; // simplify relations for the calculated/dependent parameters
lazy_eye = adjust_lazyeye_par | adjust_lazyeye_ortho;
bad_tile = -1;
used_pairs_map = new int [numTiles][num_cams][num_cams];
// used_pairs_map = new int [numTiles][num_cams][num_cams];
used_cameras = new boolean[num_cams];
boolean [][] used_pairs = new boolean[numTiles][num_pairs];
// 0-weight values and NaN-s should be filtered on input!
for (int t = 0; t < numTiles; t++) for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
used_pairs_map[t][f][s] = -1;
// for (int t = 0; t < numTiles; t++) for (int f = 0; f < num_cams; f++) for (int s = 0; s < num_cams; s++) {
// used_pairs_map[t][f][s] = -1;
// }
used_pairs_map = new int [numTiles][num_pairs];
for (int t = 0; t < numTiles; t++) {
Arrays.fill(used_pairs_map[t], -1);
}
boolean [][][] used_pairs_dir = new boolean [numTiles][num_cams][num_cams];
// boolean [][][] used_pairs_dir = new boolean [numTiles][num_cams][num_cams];
used_tiles = new boolean[numTiles];
for (Sample s:samples) { // ignore zero-weight samples
int pair = s.pair;
......@@ -514,7 +550,7 @@ public class Corr2dLMA {
used_tiles[s.tile] = true;
used_pairs[s.tile][pair]=true; // throws < 0 - wrong pair, f==s
// used_pairs_dir[s.tile][s.fcam][s.scam] = true;
used_pairs_dir[s.tile][fscam[0]][fscam[1]] = true;
// used_pairs_dir[s.tile][fscam[0]][fscam[1]] = true;
}
ncam_used = 0;
npairs =new int [numTiles]; // pairs in each tile
......@@ -556,9 +592,12 @@ public class Corr2dLMA {
*/
for (int pair = 0; pair < num_pairs; pair++) {
int npair = upmam[pair];
int [] fs = correlation2d.getPair(pair);
if (used_pairs_dir[nTile][fs[0]][fs[1]]) used_pairs_map[nTile][fs[0]][fs[1]] = npair; // either or, can not be f,s and s,f pairs
else if (used_pairs_dir[nTile][fs[1]][fs[0]]) used_pairs_map[nTile][fs[1]][fs[0]] = npair;
// int [] fs = correlation2d.getPair(pair);
// if (used_pairs_dir[nTile][fs[0]][fs[1]]) used_pairs_map[nTile][fs[0]][fs[1]] = npair; // either or, can not be f,s and s,f pairs
// else if (used_pairs_dir[nTile][fs[1]][fs[0]]) used_pairs_map[nTile][fs[1]][fs[0]] = npair;
// if (used_pairs_dir[nTile][fs[0]][fs[1]]) used_pairs_map[nTile][pair] = npair; // either or, can not be f,s and s,f pairs
// else if (used_pairs_dir[nTile][fs[1]][fs[0]]) used_pairs_map[nTile][pair] = npair;
if (used_pairs[nTile][pair]) used_pairs_map[nTile][pair] = npair;
}
}
......@@ -657,7 +696,8 @@ public class Corr2dLMA {
for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){
for (int npair = 0; npair < num_pairs; npair++) {
int [] fs = correlation2d.getPair(npair); // TODO: change used_pairs_map?
if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
// if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
if (used_pairs_map[nTile][npair] >= 0) {
m_pairs[nTile][npair] = m_disp[nTile][fs[0]].minus(m_disp[nTile][fs[1]]);
m_pairs_inv[nTile][npair] = m_pairs[nTile][npair].inverse();
}
......@@ -673,7 +713,8 @@ public class Corr2dLMA {
for (int nTile = 0; nTile < used_pairs_map.length; nTile++) if (used_tiles[nTile]){
for (int npair = 0; npair < num_pairs; npair++) {
int [] fs = correlation2d.getPair(npair); // TODO: change used_pairs_map?
if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
// if (used_pairs_map[nTile][fs[0]][fs[1]] >= 0) {
if (used_pairs_map[nTile][npair] >= 0) {
m_pairs_inv[nTile][npair] = m_pairs[nTile][npair].inverse();
}
}
......@@ -1445,9 +1486,10 @@ public class Corr2dLMA {
int npair0 = -1;
for (int i = 0; i < num_points; i++) {
if (i < samples.size()) {
int [] fs = correlation2d.getPair(samples.get(i).pair);
// int [] fs = correlation2d.getPair(samples.get(i).pair);
// int npair = used_pairs_map[samples.get(i).tile][samples.get(i).fcam][samples.get(i).scam];
int npair = used_pairs_map[samples.get(i).tile][fs[0]][fs[1]];
// int npair = used_pairs_map[samples.get(i).tile][fs[0]][fs[1]];
int npair = used_pairs_map[samples.get(i).tile][samples.get(i).pair];
if (npair !=npair0) {
if (npair0 >=0) System.out.println();
npair0 = npair;
......
......@@ -3496,10 +3496,10 @@ public class Correlation2d {
}
}
// ds[tile]{disparity, strength}
if (debug_graphic && lmaSuccess) {
String [] sliceTitles = lma.dbgGetSliceTiles();
String [] sliceTitles = lma.dbgGetSliceTitles();
if (corrs.length == 1) {
(new ShowDoubleFloatArrays()).showArrays(
lma.dbgGetSamples(ds, 0)[0],
......@@ -3550,7 +3550,7 @@ public class Correlation2d {
dbg_out_width,
dbg_out_height,
true,
"weights"+"_x"+tileX+"_y"+tileY, sliceTitles);
"weights_late"+"_x"+tileX+"_y"+tileY, sliceTitles);
}
}
......@@ -3605,7 +3605,8 @@ public class Correlation2d {
corr_size,
corr_size,
true,
"corr_pairs"+"_x"+tileX+"_y"+tileY);
"corr_pairs"+"_x"+tileX+"_y"+tileY,
getCorrTitles());
}
......@@ -3678,14 +3679,16 @@ public class Correlation2d {
corr_size,
corr_size,
true,
"corr_blurred"+"_x"+tileX+"_y"+tileY);
"corr_blurred"+"_x"+tileX+"_y"+tileY,
getCorrTitles());
(new ShowDoubleFloatArrays()).showArrays(
dbg_weights,
corr_size,
corr_size,
true,
"corr_weights"+"_x"+tileX+"_y"+tileY);
"corr_weights"+"_x"+tileX+"_y"+tileY,
getCorrTitles());
}
// double [][] disp_str = {{xcenter, 1.0}}; // temporary
......@@ -3813,8 +3816,8 @@ public class Correlation2d {
}
// double [][] ds = null;
if (debug_graphic && lmaSuccess) {
String [] sliceTitles = lma.dbgGetSliceTiles();
if (corrs.length == 1) { // only for single-tile cluster
String [] sliceTitles = lma.dbgGetSliceTitles();
// if (corrs.length == 1) { // only for single-tile cluster (here it is always single, and corrs is double [][], not double [][][]
(new ShowDoubleFloatArrays()).showArrays(
lma.dbgGetSamples(null,0)[0],
corr_size,
......@@ -3832,8 +3835,8 @@ public class Correlation2d {
corr_size,
corr_size,
true,
"corr_weights"+"_x"+tileX+"_y"+tileY, sliceTitles);
}
"corr_weights_late"+"_x"+tileX+"_y"+tileY, sliceTitles);
// }
}
}
......
......@@ -3392,222 +3392,6 @@ public class ImageDttCPU {
lma2.printStats(ds,1);
}
}
/*
// disparity_map[DISPARITY_INDEX_CM + 1][tIndex] = // y not available here
// calculate/fill out hor and vert
// convert to multi-baseline combining results from several integer scales
// see if strength is enough to proceed with LMA/poly (otherwise keep disp/strength
if (disp_str[1] > imgdtt_params.min_poly_strength) {
// create LMA instance, calculate LMA composite argmax
// Create 2 groups: ortho & diag
Correlations2dLMA lma;
if (imgdtt_params.pcorr_use && !imgdtt_params.pcorr_use_hv) { // incorrect as the ellipsoid is rotated!
double [][] fake_corrs = {corrs[6],null,null,null,corrs[7],null}; //
lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level, // int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
} else if (imgdtt_params.pcorr_use_hv) { // correct way to combine
// use hor pairs as one (top), use vert pairs as one (left)
double [][] fake_corrs = {corrs[8],null,corrs[9],null,corrs[4],corrs[4]}; // hor, null, vert, diagm, diago
lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params,
fake_corrs, // double [][] corrs,
0x11, // imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level, // int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
} else { // old way, 6 pairs top, bottom, left, right, diagm, diago
lma = corr2d.corrLMA(
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level, // int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
}
double [] lma_disparity_strength = null;
double max_disp_diff_lma = 3.0;
if (lma != null) {
lma_disparity_strength = lma.getDisparityStrength();
if (Math.abs(lma_disparity_strength[0] - disp_str[0] ) > max_disp_diff_lma) {
if (globalDebugLevel > -1) {
System.out.println("Crazy LMA for tileX="+tileX+", tileY="+tileY+": disparity="+disp_str[0]+",lma_disparity_strength[0]="+lma_disparity_strength[0]);
}
lma = null;
}
}
if (lma != null) {
double [] mod_disparity_diff = null;
double [][] dir_corr_strength = null;
lma_disparity_strength = lma.getDisparityStrength();
if (tile_lma_debug_level > 0){
System.out.println(String.format("Tile X/Y = %d/%d LMA disparity = %7.4f, strength = %7.4f",
tileX, tileY,
lma_disparity_strength[0],lma_disparity_strength[1]));
}
disparity_map[DISPARITY_INDEX_POLY] [tIndex] = lma_disparity_strength[0];
// if enabled overwrite - replace DISPARITY_INDEX_CM and DISPARITY_STRENGTH_INDEX
if (imgdtt_params.mix_corr_poly) { //true
disp_str[0] = lma_disparity_strength[0];
disp_str[1] = lma_disparity_strength[1];
disparity_map[DISPARITY_INDEX_CM] [tIndex] = disp_str[0];
disparity_map[DISPARITY_STRENGTH_INDEX] [tIndex] = disp_str[1];
if (Double.isNaN(disparity_map[DISPARITY_STRENGTH_INDEX][tIndex])) {
System.out.println("BUG: 2. disparity_map[DISPARITY_STRENGTH_INDEX][tIndex] should not be NaN");
}
}
// store debug data
// if strong enough and enabled - try to improve far objects
// temporarily removed strength requirement for debugging, restore later to make faster
/// if ((imgdtt_params.fo_correct && (strength > imgdtt_params.fo_min_strength)) || (clt_mismatch != null)) {
if ((imgdtt_params.fo_correct && (disp_str[1] > 0 * imgdtt_params.fo_min_strength)) || (clt_mismatch != null)) {
// try all dirs:
dir_corr_strength = corr2d.corr4dirsLMA(
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
-disp_str[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level, // int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
if ((tile_lma_debug_level > 0) && (dir_corr_strength != null)) {
double [] nan2 = {Double.NaN, Double.NaN, Double.NaN, Double.NaN};
for (int ii = 0; ii < dir_corr_strength.length; ii++) {
if (dir_corr_strength[ii] == null) dir_corr_strength[ii] = nan2;
}
System.out.println(String.format("corr4dirsLMA -> ↔: %7.4f (%7.4f) ↕:%7.4f (%7.4f) ⇖:%7.4f (%7.4f) ⇗:%7.4f (%7.4f)",
dir_corr_strength[0][0],dir_corr_strength[0][1],dir_corr_strength[1][0],dir_corr_strength[1][1],
dir_corr_strength[2][0],dir_corr_strength[2][1],dir_corr_strength[3][0],dir_corr_strength[3][1]));
}
mod_disparity_diff = corr2d.foregroundCorrect(
imgdtt_params.fo_far, // boolean bg,
imgdtt_params.fo_ortho, // boolean ortho,
dir_corr_strength, // double [] dir_disp,
disp_str[0], // double full_disp,
imgdtt_params.fo_min_strength, // double min_strength,
imgdtt_params.fo_min_eff, // double min_eff,
imgdtt_params.fo_min_eff_ratio, // double min_eff_ratio,
imgdtt_params.fo_max_hwidth, // double max_hwidth, // = 3.0; // maximal half-width disparity direction to try to correct far objects
imgdtt_params.fo_min_diff, // double fo_min_diff,
imgdtt_params.fo_overcorrection, // double fo_overcorrection,
imgdtt_params.fo_lim_overcorr, // double fo_lim_overcorr,
(tile_lma_debug_level > 0) ); // boolean debug);
// Do not use modified far object distance when mismatch is measured
if ((mod_disparity_diff[0] != disp_str[0]) && (clt_mismatch == null)){ // if it changed
if (imgdtt_params.fo_correct && (disp_str[1] > imgdtt_params.fo_min_strength)) { // always
disp_str[0] = mod_disparity_diff[0];
disparity_map[DISPARITY_INDEX_CM] [tIndex] = disp_str[0];
}
}
}
if (tile_lma_debug_level > -1) {
System.out.println("debug12348973591");
}
if (clt_mismatch != null) { // mod_disparity_diff should be calculated
// bypass difference or zero strength if disparity difference is too high (will influence mismatch correction)
// but setting it too low will make it impossible to correct larger mismatches. Maybe multi-pass?
if (mod_disparity_diff[2] <= imgdtt_params.mismatch_max_diff) { // may be NaN, will fail test as intended
if (tile_lma_debug_level > -1) {
System.out.println("debug12348973590");
}
double [] mismatch_result = null;
boolean need_CM = true;
if (imgdtt_params.ly_poly) { // not used in lwir
mismatch_result = corr2d.mismatchPairs( // returns x-xcenter, y, strength (sign same as disparity)
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
all_pairs, // int pair_mask, // which pairs to process
-disp_str[0], // double xcenter, // preliminary center x in pixels for largest baseline
max_corr_radius, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level,// int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
// check if got crazy poly, then retry with CM
boolean has_NaN = false;
boolean need_dbg = false;
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
if (Double.isNaN(mismatch_result[3*dir + 0]) || Double.isNaN(mismatch_result[3*dir + 1])) {
has_NaN = true;
} else if ((mismatch_result[3*dir + 2] != 0.0) &&
((Math.abs(mismatch_result[3*dir + 0]) > imgdtt_params.ly_crazy_poly) ||
(Math.abs(mismatch_result[3*dir + 1]) > imgdtt_params.ly_crazy_poly))) {
mismatch_result[3*dir + 2] = 0;
has_NaN = true;
need_dbg = true;
}
}
need_CM = imgdtt_params.ly_poly_backup && has_NaN;
if (need_dbg && (imgdtt_params.lma_debug_level > 0)) {
System.out.println("Failed polynomial mismatch for tileX="+tileX+", tileY="+tileY);
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
System.out.println(String.format("%d: dxy[%d]=%f8.5, dxy[%d]=%f8.5 strength=%7.5f",
dir, (dir*2+0), mismatch_result[dir*3 + 0], (dir*2 + 1), mismatch_result[dir*3 + 1], mismatch_result[dir*3 + 2]));
}
}
}
// TODO: use magic_scale for CM?
if (need_CM) { // if poly was off or gave crazy poly
mismatch_result = corr2d.mismatchPairsCM( // returns x-xcenter, y, strength (sign same as disparity)
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
all_pairs, // int pair_mask, // which pairs to process
-disp_str[0], // double xcenter, // preliminary center x in pixels for largest baseline
max_corr_radius, // imgdtt_params.ortho_vasw_pwr, // radius, // positive - within that distance, negative - within 2*(-radius)+1 square
tile_lma_debug_level,// int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
if (imgdtt_params.ly_poly && (imgdtt_params.lma_debug_level > 0)) {
System.out.println("Corrected by CM failed polynomial mismatch for tileX="+tileX+", tileY="+tileY);
for (int dir = 0; dir < (mismatch_result.length/3); dir ++) {
System.out.println(String.format("%d: dxy[%d]=%f8.5, dxy[%d]=%f8.5 strength=%7.5f",
dir, (dir*2+0), mismatch_result[dir*3 + 0], (dir*2 + 1), mismatch_result[dir*3 + 1], mismatch_result[dir*3 + 2]));
}
}
}
if (tile_lma_debug_level > 0) {
System.out.println("Lazy eye mismatch:");
for (int np = 0; np < mismatch_result.length/3; np++) {
System.out.println(String.format("%2d: dx = %7.4f, dy = %7.4f, strength = %7.4f,",
np, mismatch_result[3 * np + 0], mismatch_result[3 * np + 1], mismatch_result[3 * np + 2]));
}
}
for (int np = 0; np < clt_mismatch.length/3; np++) if (np < mismatch_result.length/3){
clt_mismatch[3 * np + 0 ][tIndex] = mismatch_result[3 * np + 0 ];
clt_mismatch[3 * np + 1 ][tIndex] = mismatch_result[3 * np + 1 ];
clt_mismatch[3 * np + 2 ][tIndex] = mismatch_result[3 * np + 2 ];
}
}
}
}
}
*/
} // end of if (corr_stat != null)
/*
if (corr_mode == 0) extra_disparity = disparity_map[DISPARITY_INDEX_INT][tIndex];
......
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