/**
**
** StructureFromMotion - calculate depth from scene sequences
**
** Copyright (C) 2023 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** StructureFromMotion.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.tileprocessor.sfm;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GPUTileProcessor;
//import com.elphel.imagej.cameras.ColorProcParameters;
//import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
//import com.elphel.imagej.ims.Did_ins_2;
//import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.tileprocessor.Correlation2d;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.Interscene;
import com.elphel.imagej.tileprocessor.IntersceneLma;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import com.elphel.imagej.tileprocessor.TDCorrTile;
import com.elphel.imagej.tileprocessor.TileNeibs;
import ij.ImagePlus;
public class StructureFromMotion {
public static double [] ZERO3 = {0.0,0.0,0.0};
public static int THREADS_MAX = 100; // maximal number of threads to launch
public static final int MODE_STRONG=3;
public static final int MODE_NEIB= 2;
public static final int MODE_WEAK= 1;
public static final int MODE_NONE= 0;
// class SfmCorr{
// double sfm_gain;
// double [] corr_ind; // {disparity, strength}
// double [] corr_neib; // {disparity, strength}
// }
public static boolean sfmPair_ref_debug( // ref/scene
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT scene,
double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
double range_disparity_offset = clt_parameters.imp.range_disparity_offset;
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
int margin = clt_parameters.imp.margin;
int sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
int tilesX = ref_scene.getTileProcessor().getTilesX();
int tilesY = ref_scene.getTileProcessor().getTilesY();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
boolean [] reliable_ref = null;
boolean use_combo_dsi = true;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [][] ref_xyzatr_dt= {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()
};
for (int i = 0; i < ref_xyzatr_dt.length; i++) {
if (ref_xyzatr_dt[i] == null) {
System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
ref_xyzatr_dt[i] = ZERO3.clone();
}
}
double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity);
double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
ref_scene, // QuadCLT scene,
debugLevel);
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
/* use conditioned!
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
*/
}
double [][] ref_pXpYD = null;
double [] mb_ref_disparity =null;
mb_ref_disparity = interscene_ref_disparity;
if (mb_ref_disparity == null) {
mb_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
// ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
mb_ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
// should have at least next or previous non-null
double [][] mb_vectors_ref = null;
TpTask[][] tp_tasks_ref = null;
String ts = scene.getImageName();
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
System.out.println("sfmPair(): no pose for timestamp "+ts);
return false;
}
double [][] scene_xyzatr = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
double [][] scene_xyzatr_dt= {
ers_reference.getSceneErsXYZ_dt(ts),
ers_reference.getSceneErsATR_dt(ts)
};
for (int i = 0; i < scene_xyzatr_dt.length; i++) {
if (scene_xyzatr_dt[i] == null) {
System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
scene_xyzatr_dt[i] = ZERO3.clone();
}
}
double [][] mb_vectors = null;
if (mb_en) {
// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
// double [][] dxyzatr_dt_ref = ref_xyzatr_dt;
mb_vectors_ref = OpticalFlow.getMotionBlur(
ref_scene, // QuadCLT ref_scene,
ref_scene, // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
ZERO3, // double [] camera_xyz,
ZERO3, // double [] camera_atr,
ref_xyzatr_dt[0], // double [] camera_xyz_dt,
ref_xyzatr_dt[1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
tp_tasks_ref = Interscene.setReferenceGPU (
clt_parameters, // CLTParameters clt_parameters,
ref_scene, // QuadCLT ref_scene,
mb_ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
ref_pXpYD, // double [][] ref_pXpYD,
reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors_ref, // double [][] mb_vectors, // now [2][ntiles];
debugLevel); // int debugLevel)
if (mb_en) {
mb_vectors = OpticalFlow.getMotionBlur(
ref_scene, // QuadCLT ref_scene,
scene, // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
scene_xyzatr[0], // double [] camera_xyz,
scene_xyzatr[1], // double [] camera_atr,
scene_xyzatr_dt[0], // double [] camera_xyz_dt,
scene_xyzatr_dt[1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
int [] fail_reason = new int[1];
double [] ref_disparity = null;
float [][][] facc_2d_img = new float [1][][]; // set it to null?
// show debug image
@SuppressWarnings("unused")
double [][] dpXYddisp = getSfmDpxDpyDdisp(
new double[][][] {new double[2][3], scene_xyzatr},// final double [][][] scenes_xyzatr,
ref_scene, // scene_pairs[0][0], // final QuadCLT scene0,
scene, // scene_pairs[0][1], // final QuadCLT scene1,
ref_scene, // final QuadCLT ref_QuadClt,
ref_pXpYD, // final double [][] ref_pXpYD, // centers
range_disparity_offset, // final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
batch_mode, // final boolean batch_mode,
debugLevel); // final int debug_level
@SuppressWarnings("unused")
double [][][] coord_motion = Interscene.interCorrPair( // new double [tilesY][tilesX][][];
clt_parameters, // CLTParameters clt_parameters,
false, // use3D, // boolean use3D, // generate disparity difference
true, // fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close
mb_max_gain, // double mb_max_gain,
null, // min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
ref_scene, // QuadCLT ref_scene, // Scene for which DSI (ref_disparity) is calculated
ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
ref_scene, // QuadCLT first_scene,
ref_pXpYD, // double [][] pXpYD_ref, // pXpYD for the reference scene
tp_tasks_ref[0], // TpTask[] tp_tasks_ref, // only (main if MB correction) tasks for FPN correction
scene, // QuadCLT scene_QuadCLT,
scene_xyzatr[0], // xyz
scene_xyzatr[1], // pose[1], // atr
reliable_ref, // ****null, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
sensor_mask_inter, // final int sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
facc_2d_img, // final float [][][] accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][] accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
null, // final float [][] dbg_corr_fpn,
true, // near_important, // boolean near_important, // do not reduce weight of the near tiles
false, // boolean all_fpn, // do not lower thresholds for non-fpn (used during search)
false, // initial_adjust, // boolean initial_adjust,
mb_vectors, // double [][] mb_vectors, // now [2][ntiles];
clt_parameters.imp.debug_level, // int imp_debug_level,
debugLevel); // 1); // -1); // int debug_level);
return true;
}
public static double [][] sfmPair_debug( // debug scene pairs
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT [][] scene_pairs,
double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
boolean show_2d_correlations = false; // true;
boolean show_disp_corr = false; //true;
boolean show_disp_seq = true;
double sfm_min_gain = 10.0;
double sfm_min_frac = 0.5;
int num_readjust = 5;
double min_strength = 0.4;
double range_disparity_offset = clt_parameters.imp.range_disparity_offset;
// 16 puts scale same as with older code
double corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());
final int corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
double [] neib_weights_od = {0.7, 0.5};
int tilesX = ref_scene.getTileProcessor().getTilesX();
int tilesY = ref_scene.getTileProcessor().getTilesY();
// int tile_size = ref_scene.getTileProcessor().getTileSize();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
// boolean [] reliable_ref = null;
boolean use_combo_dsi = true;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [][] ref_xyzatr = new double [][] {ZERO3,ZERO3};
double [][] ref_xyzatr_dt= {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()
};
for (int i = 0; i < ref_xyzatr_dt.length; i++) {
if (ref_xyzatr_dt[i] == null) {
System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
ref_xyzatr_dt[i] = ZERO3.clone();
}
}
double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity);
double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
ref_scene, // QuadCLT scene,
debugLevel);
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
/* use conditioned!
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
*/
}
double [] ref_disparity = interscene_ref_disparity;
if (ref_disparity == null) {
ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
double [][]disp_adj = new double [num_readjust+1][];
disp_adj[0] = ref_disparity.clone();
final int num_pairs = scene_pairs.length;
for (int ntry = 0; ntry < num_readjust; ntry++) {
// only for derivatives
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
TDCorrTile [][] pairs_TD_all = new TDCorrTile [2*(scene_pairs.length + 1)][];
final double[][][][] scenes_xyzatr = new double[num_pairs][2][][]; // 2 scenes
final double[][][][] scenes_xyzatr_dt = new double[num_pairs][2][][];// 2 scenes
for (int npair = 0; npair < num_pairs; npair++) {
QuadCLT [] scenes = scene_pairs[npair];
// should have at least next or previous non-null
// final double[][][] scenes_xyzatr = new double[scenes.length][][]; // 2 scenes
// final double[][][] scenes_xyzatr_dt = new double[scenes.length][][];// 2 scenes
for (int nscene = 0; nscene < scenes.length; nscene++) {
String ts = scenes[nscene].getImageName();
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
System.out.println("sfmPair(): no pose for timestamp "+ts);
return null;
}
scenes_xyzatr[npair][nscene] = new double[][] {
ers_reference.getSceneXYZ(ts),
ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt[npair][nscene]= new double[][] {
ers_reference.getSceneErsXYZ_dt(ts),
ers_reference.getSceneErsATR_dt(ts)};
for (int i = 0; i < scenes_xyzatr_dt[nscene].length; i++) {
if (scenes_xyzatr_dt[npair][nscene][i] == null) {
System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
}
}
}
}
double [][] dpXYddisp = getSfmDpxDpyDdisp(
scenes_xyzatr[0], // final double [][][] scenes_xyzatr,
scene_pairs[0][0], // final QuadCLT scene0,
scene_pairs[0][1], // final QuadCLT scene1,
ref_scene, // final QuadCLT ref_QuadClt,
ref_pXpYD, // final double [][] ref_pXpYD, // centers
range_disparity_offset, // final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
batch_mode, // final boolean batch_mode,
debugLevel); // final int debug_level
for (int npair = 0; npair < num_pairs; npair++) {
QuadCLT [] scenes = scene_pairs[npair];
pairs_TD_all[npair] = sfmPair_TD( // scene0/scene1
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_scene,
ref_disparity, // final double [] ref_disparity, // here can not be null
ref_xyzatr, // final double [][] ref_xyzatr, // {ZERO3,ZERO3};
ref_xyzatr_dt, // final double [][] ref_xyzatr_dt,
scenes, // final QuadCLT [] scenes, // 2 scenes
scenes_xyzatr[npair], //final double[][][] scenes_xyzatr, // 2 scenes
scenes_xyzatr_dt[npair],// final double[][][] scenes_xyzatr_dt,// 2 scenes
mb_max_gain, // double mb_max_gain,
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel);
// get neibs in TD
}
// accumulate all pairs
pairs_TD_all[num_pairs] = TDCorrTile.cloneTiles(pairs_TD_all[0]);
for (int npair = 1; npair < num_pairs; npair++) {
TDCorrTile.accumulate (
pairs_TD_all[num_pairs], // final TDCorrTile [] dst,
pairs_TD_all[npair], // final TDCorrTile [] src,
1.0); // final double src_weight);
}
// create neighbors for individual and combo
for (int npair = 0; npair <= num_pairs; npair++) {
pairs_TD_all[num_pairs + 1 + npair] = TDCorrTile.calcNeibs(
pairs_TD_all[npair], // pairs_TD, // TDCorrTile [] tiles,
tilesX, // final int tilesX,
neib_weights_od, //double [] neib_weights_od, // {orhto, diag}
null, // double [][] corr_pd,
0.0, // double neib_too_strong,
true); // final boolean process_all
}
double [][][] corr2d_PD = new double [pairs_TD_all.length][][];
for (int npair = 0; npair < pairs_TD_all.length; npair++) {
corr2d_PD[npair] = TDCorrTile.convertTDtoPD(
ref_scene.getGPU(), // final GpuQuad gpuQuad,
pairs_TD_all[npair], // final TDCorrTile [] tiles,
0xFE, // final int corr_type, // 0xFE
corr_fz_inter, // final double gpu_fat_zero,
debugLevel); // final int debug_level
}
//
if (show_2d_correlations) {
double [][] dbg_2d_corrs = ImageDtt.corr_partial_dbg( // not used in lwir
corr2d_PD, // final double [][][] corr_data, // [layer][tile][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
tilesX, // final int tilesX,
corr_size, //final int corr_size, // 15
clt_parameters.corr_border_contrast, // final double border_contrast,
debugLevel); // final int globalDebugLevel)
String [] dbg_titles= new String[dbg_2d_corrs.length];
for (int npair = 0; npair < num_pairs; npair++) {
dbg_titles[npair] = "single-"+npair;
dbg_titles[npair+num_pairs+1] = "neibs-"+npair;
}
dbg_titles[num_pairs] = "single-avg"+num_pairs;
dbg_titles[2 * num_pairs + 1] = "neibs-avg"+num_pairs;
ShowDoubleFloatArrays.showArrays(
dbg_2d_corrs,
tilesX * (corr_size + 1),
tilesY * (corr_size + 1),
true,
"corr2d-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs,
dbg_titles);
}
double [][] corr2d_PD_use = corr2d_PD[num_pairs];
final double centroid_radius = clt_parameters.imp.centroid_radius; // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
final int n_recenter = clt_parameters.imp.n_recenter; // when cosine window, re-center window this many times
double [][] disp_corr = getSfmDisparityCorrectionStrength(
clt_parameters, // final CLTParameters clt_parameters,
corr2d_PD_use, // final double [][] corr2d_PD,
dpXYddisp, // final double [][] dpXYddisp,
sfm_min_gain, // double sfm_min_gain,
sfm_min_frac, //final double sfm_min_frac,
centroid_radius, // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
n_recenter, // final int n_recenter, // when cosine window, re-center window this many times
corr_size, //final int corr_size, // 15
debugLevel); // final int debugLevel);
if (disp_corr == null) {
if (debugLevel > -3) {
System.out.println("sfmPair_debug(): not enough SfM tiles, bailing out");
}
return null;
}
for (int nTile = 0; nTile< disp_corr.length; nTile++) if (disp_corr[nTile] != null){
if (disp_corr[nTile][1] > min_strength) {
ref_disparity[nTile] += disp_corr[nTile][0];
}
}
// replace weak tiles with average?
disp_adj[ntry + 1] = ref_disparity.clone();
if (show_disp_corr) {
double [][] dbg_2d_corr = new double [3][disp_corr.length];
for (int i = 0; i < dbg_2d_corr.length; i++) {
Arrays.fill(dbg_2d_corr[i], Double.NaN);
}
for (int nTile = 0; nTile < disp_corr.length; nTile++) if (disp_corr[nTile] != null) {
dbg_2d_corr[0][nTile] = disp_corr[nTile][0];
dbg_2d_corr[1][nTile] = disp_corr[nTile][1];
}
dbg_2d_corr[2] = ref_disparity.clone();
String [] dbg_titles = {"disp_corr","strength", "new_disp"};
ShowDoubleFloatArrays.showArrays(
dbg_2d_corr,
tilesX,
tilesY,
true,
"disp_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs,
dbg_titles);
}
} // ntry
if (show_disp_seq) {
// String [] dbg_titles = new String[disp_adj.length]; // {"disp_corr","strength", "new_disp"};
ShowDoubleFloatArrays.showArrays(
disp_adj,
tilesX,
tilesY,
true,
"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs);
// dbg_titles);
}
//combo_dsn_final
// Re-calc BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
String rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
return combo_dsn_final;
}
@Deprecated
public static double [][] sfmPair( // clean this up from extra data
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT [][] scene_pairs,
final double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
boolean show_disp_corr = false; //true;
boolean show_disp_seq = true;
final double sfm_min_gain = clt_parameters.imp.sfm_min_gain;
final double sfm_min_frac = clt_parameters.imp.sfm_min_frac;
final int num_readjust = clt_parameters.imp.sfm_readjust; // 5;
final double min_strength1 = clt_parameters.imp.sfm_min_str1; // 0.4; // update if correction strength exceeds
final double min_strength16= clt_parameters.imp.sfm_min_str16; // 0.4; // update if correction strength exceeds
final double neib_too_strong1 = clt_parameters.imp.sfm_neib_too_str1; // 0.4; // do not count neighbors stronger than that
final double neib_too_strong16 = clt_parameters.imp.sfm_neib_too_str16; // 0.4; // do not count neighbors stronger than that
final int sfm_shrink = clt_parameters.imp.sfm_shrink; // 5;
final double fade_sigma = clt_parameters.imp.sfm_fade_sigma; // 3.0; // fade SfM gains at the edges
final boolean use_neibs = clt_parameters.imp.sfm_use_neibs; // true;
final double min_neib_strength1= clt_parameters.imp.sfm_neib_str1; // 0.5; // update if no-individual and neibs correction strength exceeds
final double min_neib_strength16= clt_parameters.imp.sfm_neib_str16; // 0.5; // update if no-individual and neibs correction strength exceeds
final double prev_sfm_frac = clt_parameters.imp.sfm_prev_frac; // 0.6; // update if new sfm gain > this fraction of the old one
final double same_weight = clt_parameters.imp.sfm_same_weight; // 0.8;
final double range_disparity_offset = clt_parameters.imp.range_disparity_offset;
final double centroid_radius = clt_parameters.imp.centroid_radius; // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
final int n_recenter = clt_parameters.imp.n_recenter; // when cosine window, re-center window this many times
// 16 puts scale same as with older code
final double corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());
// final int corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
// final double [] neib_weights_od = {0.7, 0.5};
int tilesX = ref_scene.getTileProcessor().getTilesX();
int tilesY = ref_scene.getTileProcessor().getTilesY();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
boolean use_combo_dsi = true;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [][] ref_xyzatr = new double [][] {ZERO3,ZERO3};
double [][] ref_xyzatr_dt= {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()
};
for (int i = 0; i < ref_xyzatr_dt.length; i++) {
if (ref_xyzatr_dt[i] == null) {
System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
ref_xyzatr_dt[i] = ZERO3.clone();
}
}
double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity);
double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
if (combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] == null) {
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =
new double [combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP].length];
if (debugLevel > -3) {
System.out.println("sfmPair(): Initializing missing COMBO_DSN_INDX_SFM_GAIN data.");
}
}
final double [] ref_sfm_gain = combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN].clone();
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
ref_scene, // QuadCLT scene,
debugLevel);
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
/* use conditioned!
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
*/
}
if (interscene_ref_disparity == null) {
interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
final double [] ref_disparity = interscene_ref_disparity;
double [][] disp_adj = show_disp_seq? (new double [num_readjust+1][]):null;
if (disp_adj != null) {
disp_adj[0] = ref_disparity.clone();
}
final int max_pairs_scale = 16;
final int num_pairs = scene_pairs.length;
final double strength_frac16 = (num_pairs < max_pairs_scale)? (Math.sqrt(num_pairs)-1)/(Math.sqrt(max_pairs_scale)-1): 1.0;
final double min_strength = min_strength1 * (1.0-strength_frac16) + min_strength16* strength_frac16;
final double neib_too_strong = neib_too_strong1 * (1.0-strength_frac16) + neib_too_strong16* strength_frac16;
final double min_neib_strength = min_neib_strength1 * (1.0-strength_frac16) + min_neib_strength16* strength_frac16;
final double[][][][] scenes_xyzatr = new double[num_pairs][2][][]; // 2 scenes
final double[][][][] scenes_xyzatr_dt = new double[num_pairs][2][][];// 2 scenes
for (int npair = 0; npair < num_pairs; npair++) {
QuadCLT [] scenes = scene_pairs[npair];
for (int nscene = 0; nscene < scenes.length; nscene++) {
String ts = scenes[nscene].getImageName();
if (ts.equals(ref_scene.getImageName())) {
scenes_xyzatr[npair][nscene] = ref_xyzatr;
scenes_xyzatr_dt[npair][nscene]= ref_xyzatr_dt;
} else {
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
System.out.println("sfmPair(): no pose for timestamp "+ts);
return null;
}
scenes_xyzatr[npair][nscene] = new double[][] {
ers_reference.getSceneXYZ(ts),
ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt[npair][nscene]= new double[][] {
ers_reference.getSceneErsXYZ_dt(ts),
ers_reference.getSceneErsATR_dt(ts)};
for (int i = 0; i < scenes_xyzatr_dt[npair][nscene].length; i++) {
if (scenes_xyzatr_dt[npair][nscene][i] == null) {
System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
}
}
}
}
}
for (int ntry = 0; ntry < num_readjust; ntry++) {
SfmCorr [] sfmCorr = getSfmCorr(
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_scene,
ref_disparity, // final double [] ref_disparity,
ref_xyzatr, // final double[][] ref_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
ref_xyzatr_dt, // final double[][] ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
scene_pairs, // final QuadCLT[][] scene_pairs,
scenes_xyzatr, // final double[][][][] scenes_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
scenes_xyzatr_dt, // final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
sfm_min_gain, // final double sfm_min_gain,
sfm_min_frac, // final double sfm_min_frac,
mb_max_gain, // final double mb_max_gain,
range_disparity_offset, // final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ; final double mb_max_gain,
corr_fz_inter, // final double corr_fz_inter,
use_neibs, // final boolean use_neibs,
neib_too_strong, // final double neib_too_strong,
centroid_radius, // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
n_recenter, // final int n_recenter, // when cosine window, re-center window this many times
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel)
if (sfmCorr == null) {
if (debugLevel > -3) {
System.out.println("sfmPair(): not enough SfM tiles, bailing out");
}
return null;
}
if ((sfm_shrink > 0) || (fade_sigma > 0.0)) {
shrinkFadeSfmGain(
sfm_shrink, // final int sfm_shrink,
fade_sigma, // final double fade_sigma,
sfmCorr, // final SfmCorr [] sfmCorr,
tilesX); // final int tilesX)
}
if (show_disp_corr) {
String [] dbg_titles = {"corr", "corr_str","neib_corr", "neib_str","old_sfm","new_sfm"};
double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorr.length];
for (int i = 0; i < dbg_2d_corr.length; i++) {
Arrays.fill(dbg_2d_corr[i], Double.NaN);
}
dbg_2d_corr[4] = ref_sfm_gain.clone();
for (int nTile = 0; nTile < sfmCorr.length; nTile++) if (sfmCorr[nTile] != null) {
dbg_2d_corr[5][nTile] = sfmCorr[nTile].sfm_gain;
if (sfmCorr[nTile].corr_ind != null) {
dbg_2d_corr[0][nTile] = sfmCorr[nTile].corr_ind[0];
dbg_2d_corr[1][nTile] = sfmCorr[nTile].corr_ind[1];
}
if (sfmCorr[nTile].corr_neib != null) {
dbg_2d_corr[2][nTile] = sfmCorr[nTile].corr_neib[0];
dbg_2d_corr[3][nTile] = sfmCorr[nTile].corr_neib[1];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_2d_corr,
tilesX,
tilesY,
true,
"sfm_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs+"-"+String.format("%02d", ntry),
dbg_titles);
}
// combine corrections
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final double wscale = -0.5/Math.log(prev_sfm_frac);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement())
if (sfmCorr[nTile] != null){
if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
double w = 1.0;
if (ref_sfm_gain[nTile] > 0) {
w = same_weight + wscale*Math.log(sfmCorr[nTile].sfm_gain/ref_sfm_gain[nTile]);
}
if (w < 0.0) w = 0;
else if (w > 1.0) w = 1.0;
// Update some strength too?
if ((sfmCorr[nTile].corr_ind != null) && (sfmCorr[nTile].corr_ind[1] > min_strength)) {
ref_disparity[nTile] += w *sfmCorr[nTile].corr_ind[0];
// TODO: add same (as above) for strengths?
ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorr[nTile].sfm_gain;
} else if ((sfmCorr[nTile].corr_neib != null) && (sfmCorr[nTile].corr_neib[1] > min_neib_strength)) {
ref_disparity[nTile] += w *sfmCorr[nTile].corr_neib[0];
// TODO: add same (as above) for strengths?
ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorr[nTile].sfm_gain;
}
}
/*
if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
if (prev_sfm_frac*sfmCorr[nTile].sfm_gain >= ref_sfm_gain[nTile]){ // copy 100%
// Update some strength too?
if ((sfmCorr[nTile].corr_ind != null) && (sfmCorr[nTile].corr_ind[1] > min_strength)) {
ref_disparity[nTile] += sfmCorr[nTile].corr_ind[0];
ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain;
} else if ((sfmCorr[nTile].corr_neib != null) && (sfmCorr[nTile].corr_neib[1] > min_neib_strength)) {
ref_disparity[nTile] += sfmCorr[nTile].corr_neib[0];
ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain;
}
}
}
*/
}
}
};
}
ImageDtt.startAndJoin(threads);
// replace weak tiles with average?
if (disp_adj != null) {
disp_adj[ntry + 1] = ref_disparity.clone();
}
} // ntry
if (disp_adj != null) {
ShowDoubleFloatArrays.showArrays(
disp_adj,
tilesX,
tilesY,
true,
"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs);
}
// Re-calc BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] = ref_sfm_gain.clone();
String rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
return combo_dsn_final;
}
/**
* Filter reference disparity from NaN (average immediate neighbors), dips and bumps
* @param disp_in
* @param max_dip maximal dip relative to lowest neighbor (0 - any)
* @param max_bump maximal bump relative to highest neighbor (0 - any)
* @param filter_nan replace NaN by neighbors average
* @param tilesX
* @return filtered disparity
*/
public static double [] filterRefDisparity(
final double [] disp_in,
final double max_dip,
final double max_bump,
final boolean filter_nan,
final int tilesX ) {
final double [] disp_out = disp_in.clone();
final int tilesY = disp_in.length / tilesX;
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TileNeibs tn = new TileNeibs(tilesX,tilesY);
for (int nTile = ai.getAndIncrement(); nTile < disp_in.length; nTile = ai.getAndIncrement()) {
int navg = 0;
double s = 0;
if (Double.isNaN(disp_in[nTile])) {
for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
int tile1 = tn.getNeibIndex(nTile, dir);
if ((tile1 >=0) && !Double.isNaN(disp_in[tile1])){
s += disp_in[tile1];
navg++;
}
}
if (navg > 0) {
disp_out[nTile] = s/navg;
}
} else {
double mn = Double.NaN, mx = Double.NaN;
for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
int tile1 = tn.getNeibIndex(nTile, dir);
if (tile1 >=0){
double d = disp_in[tile1];
if (!Double.isNaN(d)) {
if (Double.isNaN(mn)) {
mn = d;
mx = d;
} else {
mn = Math.min(mn, d);
mx = Math.max(mn, d);
}
s += d;
navg++;
}
}
}
if (navg > 0) {
if ( ((max_dip > 0) && (disp_in[nTile] < (mn - max_dip))) ||
((max_bump > 0) && (disp_in[nTile] > (mx + max_bump)))) {
disp_out[nTile] = s/navg;
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return disp_out;
}
public static double [][] sfmPairsSet(
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT [][][] scene_pairs_sets,
final double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
boolean save_disp_seq = clt_parameters.imp.sfm_save_seq; //true;
boolean show_disp_seq = clt_parameters.imp.sfm_show_seq &= !batch_mode; //true;
boolean show_disp_corr_ind = clt_parameters.imp.sfm_show_corr_ind &= !batch_mode; //false; //true;
boolean show_disp_corr = clt_parameters.imp.sfm_show_corr &= !batch_mode; //false; //true;
final int dbg_tile = -( 52+58*80);
final double sfm_min_base = clt_parameters.imp.sfm_min_base;
final double sfm_min_gain = clt_parameters.imp.sfm_min_gain;
final double sfm_min_frac = clt_parameters.imp.sfm_min_frac;
final int num_readjust = clt_parameters.imp.sfm_readjust; // 5;
final double min_strength1 = clt_parameters.imp.sfm_min_str1; // 0.4; // update if correction strength exceeds
final double min_strength16= clt_parameters.imp.sfm_min_str16; // 0.4; // update if correction strength exceeds
final double neib_too_strong1 = clt_parameters.imp.sfm_neib_too_str1; // 0.4; // do not count neighbors stronger than that
final double neib_too_strong16 = clt_parameters.imp.sfm_neib_too_str16; // 0.4; // do not count neighbors stronger than that
final int sfm_shrink = clt_parameters.imp.sfm_shrink; // 5;
final double fade_sigma = clt_parameters.imp.sfm_fade_sigma; // 3.0; // fade SfM gains at the edges
final boolean use_neibs = clt_parameters.imp.sfm_use_neibs; // true;
final double min_neib_strength1= clt_parameters.imp.sfm_neib_str1; // 0.5; // update if no-individual and neibs correction strength exceeds
final double min_neib_strength16= clt_parameters.imp.sfm_neib_str16; // 0.5; // update if no-individual and neibs correction strength exceeds
final double prev_sfm_frac = clt_parameters.imp.sfm_prev_frac; // 0.6; // update if new sfm gain > this fraction of the old one
final double same_weight = clt_parameters.imp.sfm_same_weight; // 0.8;
final double range_disparity_offset = clt_parameters.imp.range_disparity_offset;
final double centroid_radius = clt_parameters.imp.centroid_radius; // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
final int n_recenter = clt_parameters.imp.n_recenter; // when cosine window, re-center window this many times
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
// 16 puts scale same as with older code
final double corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());
// TODO: assign !
final double inf_disp = clt_parameters.imp.disparity_corr; // disparity at infinity
final int extrapolate_steps = clt_parameters.imp.sfm_extrap_steps;
final int extrapolate_radius = clt_parameters.imp.sfm_extrap_radius;
final boolean average_neibs = clt_parameters.imp.sfm_average_neibs; //false;
final boolean fill_weak = clt_parameters.imp.sfm_fill_weak; // false;
final boolean extrapolate = clt_parameters.imp.sfm_extrapolate; // false;
final double max_dip = clt_parameters.imp.sfm_max_dip; // 0.1;
final double max_bump = clt_parameters.imp.sfm_max_bump; // 0.5;
final boolean filter_nan = clt_parameters.imp.sfm_filter_nan; // true;
final int tilesX = ref_scene.getTileProcessor().getTilesX();
final int tilesY = ref_scene.getTileProcessor().getTilesY();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
final double zdisp = ers_reference.getDisparityFromZ(1.0); // product of z and disparity
boolean use_combo_dsi = true;
double [][] ref_xyzatr = new double [][] {ZERO3,ZERO3};
double [][] ref_xyzatr_dt= {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()
};
for (int i = 0; i < ref_xyzatr_dt.length; i++) {
if (ref_xyzatr_dt[i] == null) {
System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
ref_xyzatr_dt[i] = ZERO3.clone();
}
}
// double [] disparity_raw = new double [tilesX * tilesY];
// Arrays.fill(disparity_raw,clt_parameters.disparity);
double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
if (combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] == null) {
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =
new double [combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP].length];
if (debugLevel > -3) {
System.out.println("sfmPair(): Initializing missing COMBO_DSN_INDX_SFM_GAIN data.");
}
}
final double [] ref_sfm_gain = combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN].clone();
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
ref_scene, // QuadCLT scene,
debugLevel);
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0]; // .clone(); // use_lma_dsi ?
/* use conditioned!
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
*/
}
if (interscene_ref_disparity == null) {
interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
boolean filter_ref = filter_nan || (max_dip > 0) || (max_bump > 0);
final double [] ref_disparity = filter_ref ?
filterRefDisparity(
interscene_ref_disparity, // final double [] disp_in,
max_dip, // final double max_dip,
max_bump, // final double max_bump,
filter_nan, // final boolean filter_nan,
tilesX ): // final int tilesX)
interscene_ref_disparity.clone();
double [][] disp_adj = (show_disp_seq || save_disp_seq)? (new double [num_readjust+2][]):null;
if (disp_adj != null) {
disp_adj[0] = ref_disparity.clone();
}
final int num_sets = scene_pairs_sets.length;
final int [] num_set_pairs = new int [num_sets];
final double[][][][][] scenes_xyzatr_sets = new double[num_sets][][][][];
final double[][][][][] scenes_xyzatr_dt_sets = new double[num_sets][][][][];
final int num_pairs0 = scene_pairs_sets[0].length; // normally each set length is the same. Used for scaling min strengths.
final int max_pairs_scale = 16;
final double strength_frac16 = (num_pairs0 > 1) ?(
(num_pairs0 < max_pairs_scale)? (Math.sqrt(num_pairs0)-1)/(Math.sqrt(max_pairs_scale)-1): 1.0) : 0;
final double min_strength = min_strength1 * (1.0-strength_frac16) + min_strength16* strength_frac16;
final double neib_too_strong = neib_too_strong1 * (1.0-strength_frac16) + neib_too_strong16* strength_frac16;
final double min_neib_strength = min_neib_strength1 * (1.0-strength_frac16) + min_neib_strength16* strength_frac16;
final int min_neibs_fill = 4;
for (int nset=0; nset < num_sets; nset++) {
int num_pairs = scene_pairs_sets[nset].length;
num_set_pairs[nset] = num_pairs;
scenes_xyzatr_sets[nset]= new double[num_pairs][2][][]; // 2 scenes
scenes_xyzatr_dt_sets[nset]=new double[num_pairs][2][][];// 2 scenes;
for (int npair = 0; npair < num_pairs; npair++) {
QuadCLT [] scenes = scene_pairs_sets[nset][npair];
for (int nscene = 0; nscene < scenes.length; nscene++) {
String ts = scenes[nscene].getImageName();
if (ts.equals(ref_scene.getImageName())) {
scenes_xyzatr_sets[nset][npair][nscene] = ref_xyzatr;
scenes_xyzatr_dt_sets[nset][npair][nscene]= ref_xyzatr_dt;
} else {
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
System.out.println("sfmPairsSet(): no pose for timestamp "+ts);
return null;
}
scenes_xyzatr_sets[nset][npair][nscene] = new double[][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt_sets[nset][npair][nscene]= new double[][] {ers_reference.getSceneErsXYZ_dt(ts),ers_reference.getSceneErsATR_dt(ts)};
for (int i = 0; i < scenes_xyzatr_dt_sets[nset][npair][nscene].length; i++) {
if (scenes_xyzatr_dt_sets[nset][npair][nscene][i] == null) {
System.out.println("sfmPairsSet(): scenes_xyzatr_dt_sets["+nset+"]["+i+"] == null");
scenes_xyzatr_dt_sets[nset][npair][nscene][i] = ZERO3.clone();
}
}
}
}
}
}
// check each pair has sufficient baseline and contains exactly 2 scenes
int num_good_sets=0;
boolean [] good_set = new boolean [num_sets];
for (int nset=0; nset < num_sets; nset++) {
int num_pairs = scene_pairs_sets[nset].length;
check_set: {
for (int npair = 0; npair < num_pairs; npair++) {
double [][][] scenes_xyzatr= scenes_xyzatr_sets[nset][npair];
if (scenes_xyzatr.length != 2) {
System.out.println("sfmPairsSet(): BUG: Not a pair of scenes: scenes_xyzatr_sets["+nset+"]["+npair+
"].length = "+(scenes_xyzatr.length)+" != 2");
return null;
}
double dx = scenes_xyzatr[1][0][0] - scenes_xyzatr[0][0][0];
double dy = scenes_xyzatr[1][0][1] - scenes_xyzatr[0][0][1];
double base = Math.sqrt(dx*dx + dy*dy);
if (base < sfm_min_base) {
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): stereo base for nset="+nset+", npair="+npair+
" = "+base+" < "+sfm_min_base+".");
}
break check_set; // return null;
}
}
good_set[nset] = true;
num_good_sets++;
}
}
if (num_good_sets == 0) {
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): No good sets with sufficient base found, bailing out from SfM ranging.");
}
return null;
}
for (int ntry = 0; ntry < num_readjust; ntry++) {
final SfmCorr [] sfmCorrCombo = new SfmCorr [tilesX*tilesY];
double [] sum_weights_ind= new double [tilesX*tilesY];
double [] sum_weights_neib=new double [tilesX*tilesY];
/*
* 0 - not to be modified as SfM gain is lower than threshold
* 1 - weak tile, never got individual or neighbor correction
* 2 - corrected by a neighbor
* 3 - strong tile, corrected individually
*/
final int [] corr_mode = new int [tilesX*tilesY];
for (int nset=0; nset < num_sets; nset++) if (good_set[nset]){
SfmCorr [] sfmCorr = getSfmCorr(
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_scene,
ref_disparity, // final double [] ref_disparity,
ref_xyzatr, // final double[][] ref_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
ref_xyzatr_dt, // final double[][] ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
scene_pairs_sets[nset], // final QuadCLT[][] scene_pairs,
scenes_xyzatr_sets[nset], // final double[][][][] scenes_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
scenes_xyzatr_dt_sets[nset],// final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
sfm_min_gain, // final double sfm_min_gain,
sfm_min_frac, // final double sfm_min_frac,
mb_max_gain, // final double mb_max_gain,
range_disparity_offset, // final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ; final double mb_max_gain,
corr_fz_inter, // final double corr_fz_inter,
use_neibs, // final boolean use_neibs,
neib_too_strong, // final double neib_too_strong,
centroid_radius, // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
n_recenter, // final int n_recenter, // when cosine window, re-center window this many times
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel)
if (sfmCorr == null) {
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): not enough SfM tiles, bailing out");
}
continue; //return null;
}
if ((sfm_shrink > 0) || (fade_sigma > 0.0)) {
shrinkFadeSfmGain(
sfm_shrink, // final int sfm_shrink,
fade_sigma, // final double fade_sigma,
sfmCorr, // final SfmCorr [] sfmCorr,
tilesX); // final int tilesX)
}
if (show_disp_corr_ind) {
String [] dbg_titles = {"corr", "corr_str","neib_corr", "neib_str","old_sfm","new_sfm"};
double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorr.length];
for (int i = 0; i < dbg_2d_corr.length; i++) {
Arrays.fill(dbg_2d_corr[i], Double.NaN);
}
dbg_2d_corr[4] = ref_sfm_gain.clone();
for (int nTile = 0; nTile < sfmCorr.length; nTile++) if (sfmCorr[nTile] != null) {
dbg_2d_corr[5][nTile] = sfmCorr[nTile].sfm_gain;
if (sfmCorr[nTile].corr_ind != null) {
dbg_2d_corr[0][nTile] = sfmCorr[nTile].corr_ind[0];
dbg_2d_corr[1][nTile] = sfmCorr[nTile].corr_ind[1];
}
if (sfmCorr[nTile].corr_neib != null) {
dbg_2d_corr[2][nTile] = sfmCorr[nTile].corr_neib[0];
dbg_2d_corr[3][nTile] = sfmCorr[nTile].corr_neib[1];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_2d_corr,
tilesX,
tilesY,
true,
"sfm_corr-"+nset+"-"+scene_pairs_sets[nset][num_set_pairs[nset]-1][1].getImageName()+"-"+
scene_pairs_sets[nset][0][0].getImageName()+"-"+num_set_pairs[nset]+"-"+String.format("%02d", ntry),
dbg_titles);
}
// combine corrections
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) {
if (nTile == dbg_tile) {
System.out.println("sfmPairsSet().1: dbg_tile="+dbg_tile);
}
if (sfmCorr[nTile] != null){
if (sfmCorrCombo[nTile]==null) {
sfmCorrCombo[nTile] = new SfmCorr();
}
if (sfmCorr[nTile].corr_ind != null) {
if (sfmCorrCombo[nTile].corr_ind == null) {
sfmCorrCombo[nTile].corr_ind= new double[2];
}
if (!Double.isNaN(sfmCorr[nTile].corr_ind[0]) && (sfmCorr[nTile].corr_ind[1] > 0)) {
sfmCorrCombo[nTile].corr_ind[0]+= sfmCorr[nTile].corr_ind[0] * sfmCorr[nTile].sfm_gain;
sfmCorrCombo[nTile].corr_ind[1]+= sfmCorr[nTile].corr_ind[1] * sfmCorr[nTile].sfm_gain;
sum_weights_ind[nTile]+=sfmCorr[nTile].sfm_gain;
if (sfmCorr[nTile].sfm_gain > sfmCorrCombo[nTile].sfm_gain) {
sfmCorrCombo[nTile].sfm_gain = sfmCorr[nTile].sfm_gain;
}
}
}
if (sfmCorr[nTile].corr_neib != null) {
if (sfmCorrCombo[nTile].corr_neib == null) {
sfmCorrCombo[nTile].corr_neib= new double[2];
}
if (!Double.isNaN(sfmCorr[nTile].corr_neib[0]) && (sfmCorr[nTile].corr_neib[1] > 0)) {
sfmCorrCombo[nTile].corr_neib[0]+= sfmCorr[nTile].corr_neib[0] * sfmCorr[nTile].sfm_gain;
sfmCorrCombo[nTile].corr_neib[1]+= sfmCorr[nTile].corr_neib[1] * sfmCorr[nTile].sfm_gain;
sum_weights_neib[nTile]+=sfmCorr[nTile].sfm_gain;
if (sfmCorr[nTile].sfm_gain > sfmCorrCombo[nTile].sfm_gain) {
sfmCorrCombo[nTile].sfm_gain = sfmCorr[nTile].sfm_gain;
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
} // for (int nset=0; nset < num_sets; nset++)
double [][] dbg_extra = show_disp_corr? new double [6][]: null;
if (dbg_extra !=null) {
dbg_extra[0] = ref_disparity.clone();
}
// combine corrections
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final double wscale = -0.5/Math.log(prev_sfm_frac);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfmCorrCombo.length; nTile = ai.getAndIncrement()) {
if (nTile == dbg_tile) {
System.out.println("sfmPairsSet().2: dbg_tile="+dbg_tile);
}
if (sfmCorrCombo[nTile] != null){
if (sum_weights_ind[nTile] > 0) { // do it now just for debug images
sfmCorrCombo[nTile].corr_ind[0] /= sum_weights_ind[nTile];
sfmCorrCombo[nTile].corr_ind[1] /= sum_weights_ind[nTile];
}
if (sum_weights_neib[nTile] > 0) {
sfmCorrCombo[nTile].corr_neib[0] /= sum_weights_neib[nTile];
sfmCorrCombo[nTile].corr_neib[1] /= sum_weights_neib[nTile];
}
if (sfmCorrCombo[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
double w = 1.0;
if (ref_sfm_gain[nTile] > 0) {
w = same_weight + wscale*Math.log(sfmCorrCombo[nTile].sfm_gain/ref_sfm_gain[nTile]);
}
if (w < 0.0) w = 0;
else if (w > 1.0) w = 1.0;
// Update some strength too?
if ((sfmCorrCombo[nTile].corr_ind != null) && (sfmCorrCombo[nTile].corr_ind[1] > min_strength)) {
ref_disparity[nTile] += w *sfmCorrCombo[nTile].corr_ind[0];
// TODO: add same (as above) for strengths?
ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorrCombo[nTile].sfm_gain;
corr_mode[nTile] = MODE_STRONG;
} else if ((sfmCorrCombo[nTile].corr_neib != null) && (sfmCorrCombo[nTile].corr_neib[1] > min_neib_strength)) {
ref_disparity[nTile] += w *sfmCorrCombo[nTile].corr_neib[0];
// TODO: add same (as above) for strengths?
ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorrCombo[nTile].sfm_gain;
corr_mode[nTile] = MODE_NEIB;
} else {
corr_mode[nTile] = MODE_WEAK;
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (dbg_extra !=null) {
dbg_extra[1] = ref_disparity.clone();
}
if (average_neibs) {
averageNeighborsDisparity(
ref_disparity, // final double [] ref_disparity,
corr_mode, // final int [] corr_mode,
tilesX, // final int tilesX,
tilesY); // final int tilesY
}
// now fill weak tiles (MODE_WEAK) from MODE_NEIB neighbors
// repeat until any new tiles
// then - try from strong tiles - repeat until new
// ************************************
if (dbg_extra !=null) {
dbg_extra[2] = ref_disparity.clone();
dbg_extra[3] = new double [corr_mode.length];
for (int i = 0; i < corr_mode.length; i++) {
dbg_extra[3][i] = corr_mode[i];
}
}
boolean [] defined = null;
if (fill_weak) {
defined = fillWeakTiles(
ref_disparity, // final double [] ref_disparity,
corr_mode, // final int [] corr_mode,
min_neibs_fill, // final int min_neibs_fill,
tilesX, // final int tilesX,
tilesY); // final int tilesY
if (dbg_extra !=null) {
dbg_extra[4] = ref_disparity.clone();
dbg_extra[5] = new double [corr_mode.length];
for (int i = 0; i < defined.length; i++) {
dbg_extra[5][i] = defined[i]? 1.0:0.0;
}
}
}
// Can be done once after all iteration, but one row can help neighbors
// may make max_steps= 1 for all but the last run
if (extrapolate) {
if (defined == null) {
defined = new boolean[corr_mode.length];
for (int i = 0; i < corr_mode.length; i++) {
defined[i] = (corr_mode[i] > 0);
}
}
int total_filled = extrapolateEdges(
ref_disparity, // final double [] ref_disparity,
defined, // final boolean [] defined,
extrapolate_radius, // final int radius,
extrapolate_steps, // final int max_steps,
tilesX, // final int tilesX,
tilesY, // final int tilesY,
zdisp, //final double zdisp, // product of z and disparity
inf_disp); // final double inf_disp // disparity at infinity
if (debugLevel > -3) {
System.out.println("extrapolateEdges() -> "+total_filled+" new tiles");
}
}
if (show_disp_corr) {
String [] dbg_titles = {
"corr", "corr_str","neib_corr", "neib_str", // 0-3
"disp_before","disp_init","disp_avg","disp_weak","disp_extrap", // 4-8
"old_sfm","new_sfm", "corr_mode","defined"}; //9-12
double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorrCombo.length];
for (int i = 0; i < dbg_2d_corr.length; i++) {
Arrays.fill(dbg_2d_corr[i], Double.NaN);
}
dbg_2d_corr[9] = ref_sfm_gain.clone();
for (int nTile = 0; nTile < sfmCorrCombo.length; nTile++) if (sfmCorrCombo[nTile] != null) {
dbg_2d_corr[10][nTile] = sfmCorrCombo[nTile].sfm_gain;
if (sfmCorrCombo[nTile].corr_ind != null) {
dbg_2d_corr[0][nTile] = sfmCorrCombo[nTile].corr_ind[0];
dbg_2d_corr[1][nTile] = sfmCorrCombo[nTile].corr_ind[1];
}
if (sfmCorrCombo[nTile].corr_neib != null) {
dbg_2d_corr[2][nTile] = sfmCorrCombo[nTile].corr_neib[0];
dbg_2d_corr[3][nTile] = sfmCorrCombo[nTile].corr_neib[1];
}
dbg_2d_corr[11][nTile] = corr_mode[nTile];
}
dbg_2d_corr[ 4] = dbg_extra[0];
dbg_2d_corr[ 5] = dbg_extra[1];
dbg_2d_corr[ 6] = dbg_extra[2];
dbg_2d_corr[ 7] = dbg_extra[4];
dbg_2d_corr[ 8] = ref_disparity.clone();
dbg_2d_corr[12] = dbg_extra[5];
ShowDoubleFloatArrays.showArrays(
dbg_2d_corr,
tilesX,
tilesY,
true,
"sfm_corr-combo-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
scene_pairs_sets[0][0][0].getImageName()+"-"+String.format("%02d", ntry),
dbg_titles);
}
// replace weak tiles with average?
if (disp_adj != null) {
disp_adj[ntry + 1] = ref_disparity.clone();
}
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): Finished pass "+(ntry + 1)+" (of "+ num_readjust+")");
}
} // ntry
if (disp_adj != null) {
disp_adj[disp_adj.length-1] = ref_sfm_gain;
String [] tiltes = new String[disp_adj.length];
tiltes[0] = "orig";
tiltes[disp_adj.length-1] = "gain";
for (int ntry = 0; ntry < num_readjust; ntry++) {
tiltes[ntry + 1]="pass-"+ntry;
}
// String dbg_title = "SfM-disparity-multi-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
// scene_pairs_sets[0][num_set_pairs[0]-1][0].getImageName()+"-"+num_set_pairs.length;
//ref_scene.
String suffix = String.format("-SfM%02d-%02d", num_set_pairs.length, ref_scene.getNumAccum());
String title = ref_scene.getImageName()+suffix;
if (show_disp_seq) {
ShowDoubleFloatArrays.showArrays(
disp_adj,
tilesX,
tilesY,
true,
title,
tiltes);
}
if (save_disp_seq) {
ref_scene.saveDoubleArrayInModelDirectory( // error
suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
}
}
// Re-calc BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] = ref_sfm_gain.clone();
String rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
return combo_dsn_final;
}
public static int extrapolateEdges(
final double [] ref_disparity,
final boolean [] defined,
final int radius,
final int max_steps,
final int tilesX,
final int tilesY,
final double zdisp, // product of z and disparity
final double inf_disp // disparity at infinity
) {
final double [][] weights = new double [radius+1][radius+1];
for (int i =0; i <= radius; i++) {
double y = (1.0 * i) / (radius+1);
for (int j =0; j <= radius; j++) {
double x = (1.0 * j) / (radius+1);
double r = Math.sqrt(x*x + y*y);
if (r < 1.0) {
weights[i][j] = Math.cos(r* Math.PI);
}
}
}
final TileNeibs tn = new TileNeibs(tilesX,tilesY);
final double normal_damping = 0.001; // pull to horizontal if not enough data
final double [] damping = new double [] {normal_damping, normal_damping};
final boolean [] new_defined = defined.clone();
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger acounter = new AtomicInteger(0);
final AtomicInteger afilled = new AtomicInteger(0);
final AtomicInteger ai = new AtomicInteger(0);
int total_filled = 0;
for (int nstep=0; nstep < max_steps; nstep++) {
acounter.set(0); // remains after last step;
afilled.set(0);
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
double [][][] mdata = new double [(2 * radius + 1) * (2 * radius + 1)][][];
PolynomialApproximation pa = new PolynomialApproximation();
for (int nTile = ai.getAndIncrement(); nTile < ref_disparity.length; nTile = ai.getAndIncrement())
if (!defined[nTile]) { // only add new that were not defined before
// find if there is any defined neighbor
boolean got_it = false;
for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
int tile1 = tn.getNeibIndex(nTile, dir);
if ((tile1 >=0) && defined[tile1]){
got_it=true;
break;
}
}
if (got_it) {
Arrays.fill(mdata, null);
int mindx = 0;
for (int idy = -radius; idy <= radius; idy++) {
for (int idx = -radius; idx <= radius; idx++) {
int tile1 = tn.getNeibIndex(nTile, idx, idy);
if ((tile1 >= 0) && defined[tile1]) {
double z = zdisp / (ref_disparity[nTile] + inf_disp);
double w = weights[Math.abs(idy)][Math.abs(idx)];
mdata[mindx] = new double[3][];
mdata[mindx][0] = new double [2];
mdata[mindx][0][0] = idx;
mdata[mindx][0][1] = idy;
mdata[mindx][1] = new double [1];
mdata[mindx][1][0] = z;
mdata[mindx][2] = new double [1];
mdata[mindx][2][0] =w;
mindx++;
}
}
}
double[][] approx2d = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // double [] damping, null OK
-1); // debug level
new_defined[nTile] = true;
// z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
// double z = zdisp / (ref_disparity[nTile] + inf_disp);
double z0 = approx2d[0][2];
ref_disparity[nTile] = zdisp / z0 - inf_disp;
afilled.getAndIncrement();
} else {
acounter.getAndIncrement();
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (afilled.get() > 0) {
total_filled+= afilled.get();
System.arraycopy(new_defined, 0, defined, 0, ref_disparity.length);
} else {
break;
}
}
return total_filled; // acounter.get() - remains not filled
}
public static boolean [] fillWeakTiles(
final double [] ref_disparity,
final int [] corr_mode,
final int min_neibs_fill, // = 4;
final int tilesX,
final int tilesY
){
double [] nwo = {0.7, 0.5};
final double [] dir_weights = new double[] {nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1]};
final TileNeibs tn = new TileNeibs(tilesX,tilesY);
final double [] ref_disparity_in = ref_disparity.clone();
final boolean [] defined = new boolean [tilesX*tilesY];
final boolean [] new_defined = new boolean [tilesX*tilesY];
final int max_try_fill = tilesX+tilesY;
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger acounter = new AtomicInteger(0);
final AtomicInteger afilled = new AtomicInteger(0);
final AtomicInteger ai = new AtomicInteger(0);
for (boolean weak_only: new boolean[] {true, false}) {
// set initial defined, count number of undefined to be filled, update when switching to all from neibs only
// Arrays.fill(defined, false);
ai.set(0);
acounter.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement())
if (!defined[nTile]) { // only add new that were not defined before
switch (corr_mode[nTile] ) {
case MODE_STRONG:
if (weak_only) break;
case MODE_NEIB:
defined[nTile] = true;
break;
case MODE_WEAK:
acounter.getAndIncrement();
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (acounter.get() == 0) {
continue; // no points to adjust
}
// System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
System.arraycopy(defined, 0, new_defined, 0, ref_disparity.length);
for (int nfill = 0; nfill < max_try_fill; nfill++) {
// System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
// System.arraycopy(defined, 0, new_defined, 0, ref_disparity.length);
ai.set(0);
acounter.set(0);
afilled.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement())
if ((corr_mode[nTile] == MODE_WEAK ) && !defined[nTile]){
// acounter.getAndIncrement(); // left to fill
// see if any defined neighbor
int num_neibs = 0;
double sw=0, swd=0;
for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
int tile1 = tn.getNeibIndex(nTile, dir);
if ((tile1 >=0) && defined[tile1]){
double w = dir_weights[dir]; // use strength?
sw += w;
swd += w * ref_disparity_in[tile1];
num_neibs++;
}
}
if (num_neibs >= min_neibs_fill) {
ref_disparity[nTile] = swd/sw;
new_defined[nTile] = true;
afilled.getAndIncrement();
} else {
acounter.getAndIncrement(); // left unfilled
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (afilled.get() > 0) {
System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
System.arraycopy(new_defined, 0, defined, 0, ref_disparity.length);
if (acounter.get() == 0) {
break; // this run filled all remaining
}
} else {
break; // for (int nfill = 0; nfill < max_try_fill; nfill++)
}
}
}
// return acounter.get();
return defined;
}
public static void averageNeighborsDisparity(
final double [] ref_disparity,
final int [] corr_mode,
final int tilesX,
final int tilesY
) {
double [] nwo = {0.7, 0.5};
final double [] dir_weights = new double[] {nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1]};
final TileNeibs tn = new TileNeibs(tilesX,tilesY);
final double [] ref_disparity_in = ref_disparity.clone();
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement())
if (corr_mode[nTile] == MODE_NEIB){ // only neighbors
double sw = 1, swd = ref_disparity_in[nTile];
for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
int tile1 = tn.getNeibIndex(nTile, dir);
if ((tile1 >=0) && (corr_mode[tile1] == MODE_NEIB)){
double w = dir_weights[dir]; // use strength?
sw += w;
swd += w * ref_disparity_in[tile1];
}
}
ref_disparity[nTile] = swd/sw;
}
}
};
}
ImageDtt.startAndJoin(threads);
}
/**
* Calculate SfM disparity correction from a set of scene pairs with approximately
* the same position offset in each pair by consolidating 2D correlation data in
* Transform Domain before converting to the Pixel Domain. Optionally provide such
* correction for averaged with 8 immediate neighbors to deal with very low contrast
* areas. Additionally provide SfM gain (saved with the disparity data) to indicate
* estimated accuracy of disparity data.
* Reference scene disparity and scene poses are provided explicitly to allow tuning.
*
* @param clt_parameters processing parameters
* @param ref_scene reference scene instance
* @param ref_disparity reference disparity array (NaN for undefined)
* @param ref_xyzatr reference scene pose {{x,y,z},{a,t,r}}
* @param ref_xyzatr_dt reference scene pose derivatives (for ERS)
* @param scene_pairs pairs of scene instances to correlate ([1] to [0])
* @param scenes_xyzatr pairs of scene poses relative to the reference one
* @param scenes_xyzatr_dt pairs of scene poses derivatives (for ERS)
* @param sfm_min_gain minimal SfM gain to apply SfM to the depth map
* @param sfm_min_frac minimal fraction of defined tiles to have SfM correction
* @param mb_max_gain motion blur correction maximal gain
* @param range_disparity_offset disparity at actual infinity. Here can be just 0.
* @param corr_fz_inter Fat zero correction. Recommended value:
* (16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome())
* @param use_neibs if true, will generate data for averaging between neighbors
* @param neib_too_strong do not accumulate neighbor if it is too strong
* @param centroid_radius
* @param n_recenter
* @param batch_mode batch mode (suppress all debug images)
* @param debugLevel debug level
* @return per tile array (sparse) of SfmCorr instances, containing tile's SfM gain,
* individual disparity correction (to add to the current disparity)/strength and
* optionally averaged between neighbors correction/strength. Returns null if there
* insufficient tiles with high enough disparity gain.
*/
public static SfmCorr [] getSfmCorr(
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final double [] ref_disparity,
final double[][] ref_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
final double[][] ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
final QuadCLT[][] scene_pairs,
final double[][][][] scenes_xyzatr, // new double[num_pairs][2][][]; // 2 scenes
final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
final double sfm_min_gain,
final double sfm_min_frac,
final double mb_max_gain,
final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ; final double mb_max_gain,
final double corr_fz_inter,
final boolean use_neibs,
final double neib_too_strong,
final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
final int n_recenter, // when cosine window, re-center window this many times
final boolean batch_mode,
final int debugLevel) {
final int num_pairs = scene_pairs.length;
double [] neib_weights_od = {0.7, 0.5};
final int corr_size = 2 * ref_scene.tp.getTileSize() -1;
int tilesX = ref_scene.tp.getTilesX();
// only for derivatives
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
double [][][] dpXYddisp = new double [num_pairs][][]; // will be averaged between all pairs
TDCorrTile [] accum_TD = null;
for (int npair = 0; npair < num_pairs; npair++) {
dpXYddisp[npair] = getSfmDpxDpyDdisp( // will be averaged
scenes_xyzatr[0], // final double [][][] scenes_xyzatr,
scene_pairs[0][0], // final QuadCLT scene0,
scene_pairs[0][1], // final QuadCLT scene1,
ref_scene, // final QuadCLT ref_QuadClt,
ref_pXpYD, // final double [][] ref_pXpYD, // centers
range_disparity_offset, // final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
batch_mode, // final boolean batch_mode,
debugLevel); // final int debug_level
QuadCLT [] scenes = scene_pairs[npair];
TDCorrTile [] pair_TD =sfmPair_TD( // scene0/scene1
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_scene,
ref_disparity, // final double [] ref_disparity, // here can not be null
ref_xyzatr, // final double [][] ref_xyzatr, // {ZERO3,ZERO3};
ref_xyzatr_dt, // final double [][] ref_xyzatr_dt,
scenes, // final QuadCLT [] scenes, // 2 scenes
scenes_xyzatr[npair], //final double[][][] scenes_xyzatr, // 2 scenes
scenes_xyzatr_dt[npair],// final double[][][] scenes_xyzatr_dt,// 2 scenes
mb_max_gain, // double mb_max_gain,
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel);
if (npair == 0 ) {
accum_TD = pair_TD;
} else {
TDCorrTile.accumulate (accum_TD, pair_TD);
}
}
double [][] accum_PD = TDCorrTile.convertTDtoPD(
ref_scene.getGPU(), // final GpuQuad gpuQuad,
accum_TD, // final TDCorrTile [] tiles,
0xFE, // final int corr_type, // 0xFE
corr_fz_inter, // final double gpu_fat_zero,
debugLevel); // final int debug_level
double [][] neibs_PD = null;
if (use_neibs) {
TDCorrTile [] neibs_TD = TDCorrTile.calcNeibs(
accum_TD, // pairs_TD, // TDCorrTile [] tiles,
tilesX, // final int tilesX,
neib_weights_od, // double [] neib_weights_od, // {orhto, diag}
accum_PD, // double [][] corr_pd,
neib_too_strong, // double neib_too_strong,
true); // final boolean process_all
neibs_PD = TDCorrTile.convertTDtoPD(
ref_scene.getGPU(), // final GpuQuad gpuQuad,
neibs_TD, // final TDCorrTile [] tiles,
0xFE, // final int corr_type, // 0xFE
corr_fz_inter, // final double gpu_fat_zero,
debugLevel); // final int debug_level
}
final double [][] dpXYddisp_avg = new double [accum_PD.length][];
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < accum_PD.length; nTile = ai.getAndIncrement()) {
double [] sxy = new double[2];
int na = 0;
for (int npair = 0; npair < num_pairs; npair++) {
if (dpXYddisp[npair][nTile] != null) {
for (int i = 0; i < 2; i++) {
sxy[i]+=dpXYddisp[npair][nTile][i];
}
na++;
}
}
if (na > 0) {
for (int i = 0; i < 2; i++) {
sxy[i] /= na;
}
dpXYddisp_avg[nTile] = sxy;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
final SfmCorr [] sfmCorr = new SfmCorr [accum_PD.length];
final double [][][] data_PD = (neibs_PD == null) ? (new double[][][] {accum_PD}):(new double[][][] {accum_PD,neibs_PD});
boolean show_2d_correlations = debugLevel>10;
if (show_2d_correlations) {
String [] dbg_titles = (neibs_PD == null) ? (new String[] {"accum"}):(new String[] {"accum","neibs"});
double [][] dbg_2d_corrs = ImageDtt.corr_partial_dbg( // not used in lwir
data_PD, // final double [][][] corr_data, // [layer][tile][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
tilesX, // final int tilesX,
corr_size, //final int corr_size, // 15
clt_parameters.corr_border_contrast, // final double border_contrast,
debugLevel); // final int globalDebugLevel)
int tilesY = data_PD[0].length/tilesX;
ShowDoubleFloatArrays.showArrays(
dbg_2d_corrs,
tilesX * (corr_size + 1),
tilesY * (corr_size + 1),
true,
"sfm_corr2d-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
scene_pairs[0][0].getImageName()+"-"+num_pairs+"_fz"+((int) corr_fz_inter),
dbg_titles);
}
final double [][][] disp_corr = new double [data_PD.length][][];
for (int i = 0; i < data_PD.length; i++) {
disp_corr[i] = getSfmDisparityCorrectionStrength(
clt_parameters, // final CLTParameters clt_parameters,
data_PD[i], // final double [][] corr2d_PD,
dpXYddisp_avg, // final double [][] dpXYddisp,
sfm_min_gain, // final double sfm_min_gain,
sfm_min_frac, // final double sfm_min_frac,
centroid_radius, // final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
n_recenter, // final int n_recenter, // when cosine window, re-center window this many times
corr_size, //final int corr_size, // 15
debugLevel); // final int debugLevel);
if (disp_corr[i] == null) {
if (debugLevel > -3) {
System.out.println("getSfmCorr(): not enough SfM tiles, bailing out");
}
return null;
}
}
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < accum_PD.length; nTile = ai.getAndIncrement()) {
// if (nTile==4600) {
// System.out.println("getSfmCorr(): nTile="+nTile);
// }
if ((disp_corr[0][nTile] != null) || (use_neibs && (disp_corr[1][nTile] != null) )) {
sfmCorr[nTile] = new SfmCorr();
double [] sxy = dpXYddisp_avg[nTile];
sfmCorr[nTile].sfm_gain = Math.sqrt(sxy[0]*sxy[0] + sxy[1]*sxy[1]);
sfmCorr[nTile].corr_ind = disp_corr[0][nTile];
if (use_neibs) {
sfmCorr[nTile].corr_neib = disp_corr[1][nTile];
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return sfmCorr;
}
public static void shrinkFadeSfmGain(
final int sfm_shrink,
final double fade_sigma,
final SfmCorr [] sfmCorr,
final int tilesX) {
final int extra = sfm_shrink + (int)Math.ceil(fade_sigma);
final int tilesY = sfmCorr.length / tilesX;
final int tilesX_extra = tilesX+2*extra;
final int tilesY_extra = tilesY+2*extra;
final boolean [] sfm_defined_extra = new boolean [tilesX_extra*tilesY_extra];
final TileNeibs tn = new TileNeibs(tilesX_extra,tilesY_extra);
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) if (sfmCorr[nTile] != null){
int tileX = nTile % tilesX + extra;
int tileY = nTile / tilesX + extra;
int nTile_extra=tileX + tileY * tilesX_extra;
sfm_defined_extra[nTile_extra] = sfmCorr[nTile].sfm_gain > 0;
}
}
};
}
ImageDtt.startAndJoin(threads);
if (sfm_shrink > 0) {
tn.shrinkSelection(
sfm_shrink, // final int shrink, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
sfm_defined_extra, // final boolean [] tiles,
null); // final boolean [] prohibit)
}
if (fade_sigma > 0) {
final double [] gain_scale = new double [sfm_defined_extra.length];
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfm_defined_extra.length; nTile = ai.getAndIncrement()) if (sfm_defined_extra[nTile]){
gain_scale[nTile] = 2.0;
}
}
};
}
ImageDtt.startAndJoin(threads);
DoubleGaussianBlur gb = new DoubleGaussianBlur();
gb.blurDouble(gain_scale, tilesX_extra, tilesY_extra, fade_sigma, fade_sigma, 0.01);
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) if (sfmCorr[nTile] != null){
int tileX = nTile % tilesX + extra;
int tileY = nTile / tilesX + extra;
int nTile_extra=tileX + tileY * tilesX_extra;
if (sfm_defined_extra[nTile_extra]) {
double g = gain_scale[nTile_extra]-1;
sfmCorr[nTile].sfm_gain *= g * g;
} else {
sfmCorr[nTile].sfm_gain = 0.0; // null; // or use sfmCorr[nTile] = null?
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
}
/**
* Calculate disparity correction from the pixel-domain 2D correlation data
* and the SfM gain vector array (nominal disparity pixels per SfM correlation
* pixels in X and Y directions.
* @param clt_parameters processing parameters
* @param corr2d_PD per-tile sparse array of the 2D correlation results
* @param dpXYddisp per-tile sparse array of the X,Y components of the SfM gain
* @param sfm_min_gain minimal SfM gain to apply SfM to the depth map
* @param sfm_min_frac minimal fraction of defined tiles to have SfM correction
* @param centroid_radius windowing for centroid argmax: 0 - use all tile, >0 - cosine
* window around local max
* @param n_recenter number of refining of the argmax
* @param corr_size 2d correlation tile size (side of a square, normally 15)
* @param debugLevel debug level
* @return per tile sparse array of {disparity_correction, strength} pairs. Will return
* null if number of sfm tiles is less than sfm_min_frac of defined tiles
*/
public static double [][] getSfmDisparityCorrectionStrength(
final CLTParameters clt_parameters,
final double [][] corr2d_PD,
final double [][] dpXYddisp,
final double sfm_min_gain,
final double sfm_min_frac,
final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
final int n_recenter, // when cosine window, re-center window this many times
final int corr_size, // 15
final int debugLevel) {
final double [][] disp_corr = new double [corr2d_PD.length][];
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger anum_defined = new AtomicInteger(0);
final AtomicInteger anum_sfm = new AtomicInteger(0);
final double sfm_min_gain2 = sfm_min_gain * sfm_min_gain;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < corr2d_PD.length; nTile = ai.getAndIncrement())
if ((corr2d_PD[nTile] != null) && (dpXYddisp[nTile] != null)){
double [] v = dpXYddisp[nTile];
double l2 = v[0]*v[0]+ v[1]*v[1];
double [] inv_v = {v[0]/l2, v[1]/l2};
double [] corr_cm = Correlation2d.getMaxProjCm(
corr2d_PD[nTile], // double [] data,
corr_size, // int data_width, // = 2 * transform_size - 1;
centroid_radius, // double radius, // 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
n_recenter, // int refine, // re-center window around new maximum. 0 -no refines (single-pass)
inv_v, // double [] direction_XY,
debugLevel > 2); // boolean debug)
if (corr_cm != null){
anum_defined.getAndIncrement();
if (l2 >= sfm_min_gain2) {
anum_sfm.getAndIncrement();
disp_corr[nTile] = corr_cm; // may be null
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if ((anum_sfm.get() == 0) || (anum_sfm.get() < anum_defined.get() * sfm_min_frac)) {
return null;
}
return disp_corr;
}
/**
* Calculate a transform-domain cross-correlation of 2 scenes, keep only all-channels combined
* as a TDCorrTile [] sparse array. Each non-null element contains weight (normally 16 as number of
* channels combined) and 256 elements of the TD representation of the correlation data.
* Reference scene disparity and scene poses are provided explicitly to allow tuning.
* Both scenes are separate from the reference scene (TODO: verify when 1 is the reference)
*
* @param clt_parameters processing parameters
* @param ref_scene reference scene instance
* @param ref_disparity reference disparity array (NaN for undefined)
* @param ref_xyzatr reference scene [2][3] pose {{x,y,z},{a,t,r}}
* @param ref_xyzatr_dt reference scene pose derivatives (for ERS)
* @param scenes pair of scene instances to correlate ([1] to [0])
* @param scenes_xyzatr pair of scene poses relative to the reference one
* @param scenes_xyzatr_dt pair of scene poses derivatives (for ERS)
* @param mb_max_gain motion blur correction maximal gain
* @param batch_mode batch mode (suppress all debug images)
* @param debugLevel debug level
* @return TDCorrTile [] sparse array representing a transform-domain interscene correlation
* result.
*/
public static TDCorrTile [] sfmPair_TD( // scene0/scene1
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final double [] ref_disparity, // here can not be null
final double [][] ref_xyzatr, // {ZERO3,ZERO3};
final double [][] ref_xyzatr_dt,
final QuadCLT [] scenes, // 2 scenes
final double[][][] scenes_xyzatr, // 2 scenes
final double[][][] scenes_xyzatr_dt,// 2 scenes
double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
int imp_debug_level = 0; // MANUALLY change to 2 for debug!
boolean show_render_ref = !batch_mode && clt_parameters.imp.renderRef(imp_debug_level); // false; //true;
boolean show_render_scene = !batch_mode && clt_parameters.imp.renderScene(imp_debug_level); // false; // true;
boolean toRGB = true;
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
int margin = clt_parameters.imp.margin;
int sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
double [][][] scenes_pXpYD = new double [scenes.length][][];
// ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
@SuppressWarnings("unused")
TpTask[][] tp_tasks_ref; // only were used for the FPN removal
double [][][] mb_vectors_scenes = new double [scenes.length][][];
for (int nscene = 0; nscene < scenes.length; nscene++) {
scenes_pXpYD[nscene] = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
scenes_xyzatr[nscene][0], // final double [] scene_xyz, // camera center in world coordinates
scenes_xyzatr[nscene][1], // final double [] scene_atr, // camera orientation relative to world frame
scenes[nscene], // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
if (mb_en) {
mb_vectors_scenes[nscene] = OpticalFlow.getMotionBlur(
ref_scene, // QuadCLT ref_scene,
scenes[nscene], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
scenes_xyzatr[nscene][0], // double [] camera_xyz,
scenes_xyzatr[nscene][1], // double [] camera_atr,
scenes_xyzatr_dt[nscene][0],// double [] camera_xyz_dt,
scenes_xyzatr_dt[nscene][1],// double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
}
// remove tiles from both that are null for any
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < ref_pXpYD.length; nTile = ai.getAndIncrement())
if (ref_pXpYD[nTile] != null){
boolean undefined = false;
for (int nscene = 0; nscene < scenes.length; nscene++) {
if (scenes_pXpYD[nscene][nTile] == null) {
undefined=true;
break;
}
}
if (undefined) {
for (int nscene = 0; nscene < scenes.length; nscene++) {
scenes_pXpYD[nscene][nTile] = null;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
tp_tasks_ref = // were used only for the FPN removal
Interscene.setReferenceGPU (
clt_parameters, // CLTParameters clt_parameters,
scenes[0], // ref_scene, // QuadCLT ref_scene,
null, // ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
scenes_pXpYD[0], // ref_pXpYD, // double [][] ref_pXpYD,
null, // reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
// motion blur compensation
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors_scenes[0],// mb_vectors_ref, // double [][] mb_vectors, // now [2][ntiles];
debugLevel); // int debugLevel)
// is it needed here?
scenes[0].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
ImageDtt image_dtt;
image_dtt = new ImageDtt(
ref_scene.getNumSensors(), // ,
clt_parameters.transform_size,
clt_parameters.img_dtt,
ref_scene.isAux(),
ref_scene.isMonochrome(),
ref_scene.isLwir(),
clt_parameters.getScaleStrength(ref_scene.isAux()),
ref_scene.getGPU());
image_dtt.getCorrelation2d(); // initiate image_dtt.correlation2d, needed if disparity_map != null
//ref_scene.getGPU() can not be null
ref_scene.getGPU().setGpu_debug_level(debugLevel - 4); // monitor GPU ops >=-1
final double disparity_corr = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
final double gpu_sigma_corr = clt_parameters.getGpuCorrSigma(ref_scene.isMonochrome());
final double gpu_sigma_rb_corr = ref_scene.isMonochrome()? 1.0 : clt_parameters.gpu_sigma_rb_corr;
final double gpu_sigma_log_corr = clt_parameters.getGpuCorrLoGSigma(ref_scene.isMonochrome());
TpTask[][] tp_tasks;
if (mb_en && (mb_vectors_scenes[1]!=null)) {
tp_tasks = GpuQuad.setInterTasksMotionBlur(
scenes[1].getNumSensors(),
scenes[1].getErsCorrection().getSensorWH()[0],
!scenes[1].hasGPU(), // final boolean calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
scenes_pXpYD[1], // final double [][] pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
null, // final boolean [] selection, // may be null, if not null do not process unselected tiles
// motion blur compensation
mb_tau, // final double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // final double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_vectors_scenes[1], // final double [][] mb_vectors, //
scenes[1].getErsCorrection(), // final GeometryCorrection geometryCorrection,
disparity_corr, // final double disparity_corr,
margin, // final int margin, // do not use tiles if their centers are closer to the edges
null, // final boolean [] valid_tiles,
THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
} else {
tp_tasks = new TpTask[1][];
tp_tasks[0] = GpuQuad.setInterTasks(
scenes[1].getNumSensors(),
scenes[1].getErsCorrection().getSensorWH()[0],
!scenes[1].hasGPU(), // final boolean calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
scenes_pXpYD[1], // final double [][] pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
null, // selection, // final boolean [] selection, // may be null, if not null do not process unselected tiles
scenes[1].getErsCorrection(), // final GeometryCorrection geometryCorrection,
disparity_corr, // final double disparity_corr,
margin, // final int margin, // do not use tiles if their centers are closer to the edges
null, // final boolean [] valid_tiles,
THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
}
scenes[1].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
// Generate 2D phase correlations from the CLT representation
// generates sum of the per-channel correlations as the last slot.
// updates gpuQuad.gpu_corr_indices, gpuQuad.gpu_corrs_td and some other
if (mb_en && (mb_vectors_scenes[1]!=null)) {
image_dtt.interCorrTDMotionBlur(
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
tp_tasks, // final TpTask[][] tp_tasks,
null, // final float [][][][] fcorr_td, // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
clt_parameters.gpu_sigma_r, // final double gpu_sigma_r, // 0.9, 1.1
clt_parameters.gpu_sigma_b, // final double gpu_sigma_b, // 0.9, 1.1
clt_parameters.gpu_sigma_g, // final double gpu_sigma_g, // 0.6, 0.7
clt_parameters.gpu_sigma_m, // final double gpu_sigma_m, // = 0.4; // 0.7;
gpu_sigma_rb_corr, // final double gpu_sigma_rb_corr, // = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
gpu_sigma_corr, // final double gpu_sigma_corr, // = 0.9;gpu_sigma_corr_m
gpu_sigma_log_corr, // final double gpu_sigma_log_corr, // hpf to reduce dynamic range for correlations
clt_parameters.corr_red, // final double corr_red, // +used
clt_parameters.corr_blue, // final double corr_blue,// +used
sensor_mask_inter, // final int sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
THREADS_MAX, // final int threadsMax, // maximal number of threads to launch
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.interCorrTD(
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
tp_tasks[0], // final TpTask[] tp_tasks,
null, // final float [][][][] fcorr_td, // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
clt_parameters.gpu_sigma_r, // final double gpu_sigma_r, // 0.9, 1.1
clt_parameters.gpu_sigma_b, // final double gpu_sigma_b, // 0.9, 1.1
clt_parameters.gpu_sigma_g, // final double gpu_sigma_g, // 0.6, 0.7
clt_parameters.gpu_sigma_m, // final double gpu_sigma_m, // = 0.4; // 0.7;
gpu_sigma_rb_corr, // final double gpu_sigma_rb_corr, // = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
gpu_sigma_corr, // final double gpu_sigma_corr, // = 0.9;gpu_sigma_corr_m
gpu_sigma_log_corr, // final double gpu_sigma_log_corr, // hpf to reduce dynamic range for correlations
clt_parameters.corr_red, // final double corr_red, // +used
clt_parameters.corr_blue, // final double corr_blue,// +used
sensor_mask_inter, // final int sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
THREADS_MAX, // final int threadsMax, // maximal number of threads to launch
debugLevel); // final int globalDebugLevel);
}
// get TD interscene correlation of 2 scenes, use only combo (all channels) data
TDCorrTile [] tiles = TDCorrTile.getFromGpu(
ref_scene.getGPU());
if (show_render_ref) {
ImagePlus imp_render_ref = scenes[0].renderFromTD (
-1, // final int sensor_mask,
false, // boolean merge_channels,
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
clt_parameters.getRGBParameters(), //EyesisCorrectionParameters.RGBParameters rgbParameters,
null, // int [] wh,
toRGB, // boolean toRGB,
true, //boolean use_reference
"GPU-SHIFTED-REF"); // String suffix)
imp_render_ref.show();
}
if (show_render_scene) { //set toRGB=false
ImagePlus imp_render_scene = scenes[1].renderFromTD (
-1, // final int sensor_mask,
false, // boolean merge_channels,
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
clt_parameters.getRGBParameters(), //EyesisCorrectionParameters.RGBParameters rgbParameters,
null, // int [] wh,
toRGB, // boolean toRGB,
false, //boolean use_reference
"GPU-SHIFTED-SCENE"); // String suffix)
imp_render_scene.show();
}
return tiles;
}
public static double [][] getSfmDpxDpyDdisp(
final double [][][] scenes_xyzatr, // cameras center in world coordinates (or null to use instance)
final QuadCLT scene0,
final QuadCLT scene1,
final QuadCLT ref_QuadClt,
final double [][] ref_pXpYD, // centers
final double range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
final boolean batch_mode,
final int debug_level
){
boolean debug_img = !batch_mode && (debug_level > 0);
double [][] dbg_img = debug_img? (new double[6] [ref_pXpYD.length]):null;
if (dbg_img != null) {
for (int i = 0; i <dbg_img.length;i++) {
Arrays.fill(dbg_img[i],Double.NaN);
}
}
final QuadCLT[] scenes = {scene0, scene1};
boolean[] param_select = new boolean[ErsCorrection.DP_NUM_PARS];
final int [] par_indices = new int[] {
ErsCorrection.DP_DD};
for (int i: par_indices) {
param_select[i]=true;
}
final double [][][] last_jts = new double [scenes.length][][];
IntersceneLma intersceneLma= new IntersceneLma(
false, // clt_parameters.ilp.ilma_thread_invariant);
0.0); // always no disparity
for (int nscene = 0; nscene < scenes.length; nscene++) {
// try if one is reference?
if (scenes[nscene].getImageName().equals(ref_QuadClt.getImageName())) {
last_jts[nscene] = new double[1][2 * ref_pXpYD.length];
} else {
intersceneLma.prepareLMA(
scenes_xyzatr[nscene][0], // final double [] scene_xyz0, // camera center in world coordinates (or null to use instance)
scenes_xyzatr[nscene][1], // final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
null, // final double [] scene_xyz_pull, // if both are not null, specify target values to pull to
null, // final double [] scene_atr_pull, //
scenes[nscene], // final QuadCLT scene_QuadClt,
ref_QuadClt, // final QuadCLT reference_QuadClt,
param_select, // final boolean[] param_select,
null, // final double [] param_regweights,
null, // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
ref_pXpYD, // final double [][] centers, // macrotile centers (in pixels and average disparities
false, // final boolean same_weights,
false, // boolean first_run,
debug_level); // final int debug_level)
last_jts[nscene] = intersceneLma. getLastJT(); // alternating x,y for each selected parameters
}
}
int [] sensor_wh = ref_QuadClt.getGeometryCorrection().getSensorWH();
final double width = sensor_wh[0];
final double height = sensor_wh[1];
final double min_disparity = .05; // -0.5;
final double max_disparity = 100.0;
final double [][] dpXYddisp = new double [ref_pXpYD.length][];
final int debug_tile = -1;
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < ref_pXpYD.length; nTile = ai.getAndIncrement()) if (ref_pXpYD[nTile] != null){
if (nTile == debug_tile) {
System.out.println("getSfmDpxDpyDdisp(): nTile="+nTile);
}
double disp_eff = ref_pXpYD[nTile][2] - range_disparity_offset;
if ( (ref_pXpYD[nTile][0] < 0) || (ref_pXpYD[nTile][0] >= width) ||
(ref_pXpYD[nTile][1] < 0) || (ref_pXpYD[nTile][1] >= height) ||
(disp_eff < min_disparity) || (disp_eff >= max_disparity)) {
continue;
}
dpXYddisp[nTile] = new double [2];
dpXYddisp[nTile][0] = last_jts[1][0][2*nTile + 0]-last_jts[0][0][2*nTile + 0];
dpXYddisp[nTile][1] = last_jts[1][0][2*nTile + 1]-last_jts[0][0][2*nTile + 1];
if (dbg_img != null) {
dbg_img[0][nTile] = dpXYddisp[nTile][0];
dbg_img[1][nTile] = dpXYddisp[nTile][1];
dbg_img[2][nTile] = last_jts[0][0][2*nTile + 0];
dbg_img[3][nTile] = last_jts[0][0][2*nTile + 1];
dbg_img[4][nTile] = last_jts[1][0][2*nTile + 0];
dbg_img[5][nTile] = last_jts[1][0][2*nTile + 1];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (dbg_img != null) {
int tilesX = ref_QuadClt.tp.getTilesX();
int tilesY = ref_QuadClt.tp.getTilesY();
String [] dbg_titles = {"dPx_dd","dPy_dd","dPx0_dd","dPy0_dd","dPx1_dd","dPy1_dd"}; // ,"dPx_dZ0","dPy_dZ0","dPx_dZ","dPy_dZ"};
ShowDoubleFloatArrays.showArrays(
dbg_img,
tilesX,
tilesY,
true,
"getSfmDpxDpyDdisp-"+ref_QuadClt.getImageName(),
dbg_titles);
}
return dpXYddisp;
}
}