Skip to content

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

Cleaning up SfM

parent e61d71b1
Hide whitespace changes
Inline Side-by-side
Showing with 560 additions and 216 deletions
src/main/java/com/elphel/imagej/tileprocessor/Correlation2d.java
View file @ 3430abee
......@@ -2506,6 +2506,20 @@ public class Correlation2d {
}
/**
* Find maximum of the 2d array projected on a specified vector using centroid.
* On the first stage integer maximum is found, then several refining operations
* multiply vicinity of the 2d max by a window function and locate the center
* of mass. In parallel (can be optimized) the center of mass is calculated for the
* dot-product of the vector and 2d array coordinates
* @param data square data array (normally result of the 2d phase correlation) in linescan order
* @param data_width size of the square (2 * transform_size - 1 for correlation)
* @param radius window: 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
* @param refine number of refines
* @param direction_XY [x,y] components of the direction to project to
* @param debug debug output if true
* @return a pair of {projection_max, strength}
*/
public static double [] getMaxProjCm(
double [] data,
int data_width, // = 2 * transform_size - 1;
......
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 3430abee
......@@ -153,36 +153,36 @@ public class ErsCorrection extends GeometryCorrection {
// returned arrays have the zero element with coordinates, not derivatives
// Reference parameters
static final int DP_DPX = 0; // dw_dpX, (pix)
static final int DP_DPY = 1; // dw_dpY (pix)
static final int DP_DD = 2; // dw_dd, (pix)
static final int DP_DVAZ = 3; // dw_dvaz, (rad/sec)
static final int DP_DVTL = 4; // dw_dvtl, (rad/sec)
static final int DP_DVRL = 5; // dw_dvrl, (rad/sec)
static final int DP_DVX = 6; // dw_dvx, (m/s)
static final int DP_DVY = 7; // dw_dvy, (m/s)
static final int DP_DVZ = 8; // dw_dvz, (m/s)
static final int DP_DAZ = 9; // dw_daz, (rad)
static final int DP_DTL = 10; // dw_dtl, (rad)
static final int DP_DRL = 11; // dw_drl, (rad)
static final int DP_DX = 12; // dw_dx, (m)
static final int DP_DY = 13; // dw_dy, (m)
static final int DP_DZ = 14; // dw_dz}; (m)
public static final int DP_DPX = 0; // dw_dpX, (pix)
public static final int DP_DPY = 1; // dw_dpY (pix)
public static final int DP_DD = 2; // dw_dd, (pix)
public static final int DP_DVAZ = 3; // dw_dvaz, (rad/sec)
public static final int DP_DVTL = 4; // dw_dvtl, (rad/sec)
public static final int DP_DVRL = 5; // dw_dvrl, (rad/sec)
public static final int DP_DVX = 6; // dw_dvx, (m/s)
public static final int DP_DVY = 7; // dw_dvy, (m/s)
public static final int DP_DVZ = 8; // dw_dvz, (m/s)
public static final int DP_DAZ = 9; // dw_daz, (rad)
public static final int DP_DTL = 10; // dw_dtl, (rad)
public static final int DP_DRL = 11; // dw_drl, (rad)
public static final int DP_DX = 12; // dw_dx, (m)
public static final int DP_DY = 13; // dw_dy, (m)
public static final int DP_DZ = 14; // dw_dz}; (m)
// Scene parameters
static final int DP_DSVAZ =15; // dw_dvaz, (rad/sec)
static final int DP_DSVTL =16; // dw_dvtl, (rad/sec)
static final int DP_DSVRL =17; // dw_dvrl, (rad/sec)
static final int DP_DSVX = 18; // dw_dvx, (m/s)
static final int DP_DSVY = 19; // dw_dvy, (m/s)
static final int DP_DSVZ = 20; // dw_dvz, (m/s)
static final int DP_DSAZ = 21; // dw_daz, (rad)
static final int DP_DSTL = 22; // dw_dtl, (rad)
static final int DP_DSRL = 23; // dw_drl, (rad)
static final int DP_DSX = 24; // dw_dx, (m)
static final int DP_DSY = 25; // dw_dy, (m)
static final int DP_DSZ = 26; // dw_dz}; (m)
static final int DP_NUM_PARS = DP_DSZ+1;
static final int [] DP_ERS_INDICES=
public static final int DP_DSVAZ =15; // dw_dvaz, (rad/sec)
public static final int DP_DSVTL =16; // dw_dvtl, (rad/sec)
public static final int DP_DSVRL =17; // dw_dvrl, (rad/sec)
public static final int DP_DSVX = 18; // dw_dvx, (m/s)
public static final int DP_DSVY = 19; // dw_dvy, (m/s)
public static final int DP_DSVZ = 20; // dw_dvz, (m/s)
public static final int DP_DSAZ = 21; // dw_daz, (rad)
public static final int DP_DSTL = 22; // dw_dtl, (rad)
public static final int DP_DSRL = 23; // dw_drl, (rad)
public static final int DP_DSX = 24; // dw_dx, (m)
public static final int DP_DSY = 25; // dw_dy, (m)
public static final int DP_DSZ = 26; // dw_dz}; (m)
public static final int DP_NUM_PARS = DP_DSZ+1;
public static final int [] DP_ERS_INDICES=
{ DP_DVAZ, DP_DVTL, DP_DVRL,
DP_DVX, DP_DVY, DP_DVZ,
DP_DSVAZ, DP_DSVTL, DP_DSVRL,
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 3430abee
......@@ -55,6 +55,7 @@ import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.jp4.JP46_Reader_camera;
import com.elphel.imagej.tileprocessor.sfm.StructureFromMotion;
import Jama.Matrix;
import ij.ImagePlus;
......@@ -70,7 +71,7 @@ import ij.plugin.filter.GaussianBlur;
public class OpticalFlow {
public static String [] COMBO_DSN_TITLES = {"disp", "strength","disp_lma","num_valid","change",
"disp_bg", "strength_bg","disp_lma_bg","change_bg","disp_fg","disp_bg_all","blue_sky"};
"disp_bg", "strength_bg","disp_lma_bg","change_bg","disp_fg","disp_bg_all","blue_sky","sfm_gain"};
public static int COMBO_DSN_INDX_DISP = 0; // cumulative disparity (from CM or POLY), FG
public static int COMBO_DSN_INDX_STRENGTH = 1; // strength, FG
public static int COMBO_DSN_INDX_LMA = 2; // masked copy from 0 - cumulative disparity
......@@ -83,6 +84,7 @@ public class OpticalFlow {
public static int COMBO_DSN_INDX_DISP_FG = 9; // cumulative disparity (from CM or POLY), FG
public static int COMBO_DSN_INDX_DISP_BG_ALL =10; // cumulative BG disparity (Use FG where no BG is available)
public static int COMBO_DSN_INDX_BLUE_SKY = 11; // Detected featureless infinity (sky)
public static int COMBO_DSN_INDX_SFM_GAIN = 12; // SfM disparity gain pixel/pixel
// move to Interscene class?
// interscene adjustments failure reasons.
......@@ -4884,32 +4886,32 @@ public class OpticalFlow {
if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
}
done_sfm = StructureFromMotion.sfmPair(
/*
done_sfm = StructureFromMotion.sfmPair_ref_debug(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs[ref_index], // final QuadCLT ref_scene,
quadCLTs[earliest_scene], // final QuadCLT scene,
mb_max_gain, // double mb_max_gain,
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel)
*/
int num_avg_pairs = 16; // number of scene pairs to average
QuadCLT[][] scenes_pairs = new QuadCLT[num_avg_pairs][2];
for (int i = 0; i < num_avg_pairs; i++) {
scenes_pairs[i][0] = quadCLTs[ref_index - 1 - i];
scenes_pairs[i][1] = quadCLTs[earliest_scene + num_avg_pairs - 1 - i];
}
// QuadCLT[] scenes_pair = new QuadCLT[]{
// quadCLTs[ref_index - 1],
// quadCLTs[earliest_scene]};
combo_dsn_final = StructureFromMotion.sfmPair(
double [][] sfm_dsn = StructureFromMotion.sfmPair(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs[ref_index], // final QuadCLT ref_scene,
scenes_pairs, // final QuadCLT [][] scenes_pairs,
// scenes_pair, // final QuadCLT [] scenes,
// num_avg_pairs, // final int num_avg_pairs, // number of scene pairs to average
mb_max_gain, // double mb_max_gain,
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel)
if (sfm_dsn != null) {
combo_dsn_final = sfm_dsn;
done_sfm = true;
}
}
if (!done_sfm) { // first pass or sfm failed
// should skip scenes w/o orientation 06/29/2022
......@@ -11543,7 +11545,7 @@ public class OpticalFlow {
}
static double [][] conditionInitialDS(
public static double [][] conditionInitialDS(
boolean use_conf, // use configuration parameters, false - use following
CLTParameters clt_parameters,
double [][] dls,
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 3430abee
......@@ -1521,6 +1521,10 @@ public class QuadCLTCPU {
this.dsi[is_aux?TwoQuadCLT.DSI_BLUE_SKY_AUX:TwoQuadCLT.DSI_BLUE_SKY_MAIN] =
combo_dsi[OpticalFlow.COMBO_DSN_INDX_BLUE_SKY];
}
if ((combo_dsi.length > OpticalFlow.COMBO_DSN_INDX_SFM_GAIN) && (combo_dsi[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] != null)) {
this.dsi[is_aux?TwoQuadCLT.DSI_SFM_GAIN_AUX:TwoQuadCLT.DSI_SFM_GAIN_MAIN] =
combo_dsi[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN];
}
}
......@@ -1568,7 +1572,7 @@ public class QuadCLTCPU {
/**
* Tries to read combo DSI, if successful - sets this.dsi and blue sky
* @param silent
* @return combo DSI if read, null if failed to read. Result has full lenghth
* @return combo DSI if read, null if failed to read. Result has full length
* (OpticalFlow.COMBO_DSN_TITLES.length), missing slices are null
*/
public double [][] restoreComboDSI (boolean silent) {
......@@ -1627,18 +1631,26 @@ public class QuadCLTCPU {
double [] reduced_strength // if not null will return >0 if had to reduce strength (no change if did not reduce)
) {
int NUM_BINS = 1024;
// 10.15.2023 - was error here, readComboDSI (silent) returns combo_dsi, not converted to this.dsi format;
// double [][] main_dsi = use_combo? readComboDSI (silent): readDsiMain();
double [][] main_dsi = null;
boolean silent = false;
double [][] main_dsi = use_combo? readComboDSI (silent): readDsiMain();
if (use_combo) {
readComboDSI (silent);
main_dsi = this.dsi;
} else {
main_dsi = readDsiMain();
}
if (main_dsi == null) {
return null;
}
double [] disparity_lma = main_dsi[isAux()?TwoQuadCLT.DSI_DISPARITY_AUX_LMA:TwoQuadCLT.DSI_DISPARITY_MAIN_LMA];
double [] strength = main_dsi[isAux()?TwoQuadCLT.DSI_STRENGTH_AUX:TwoQuadCLT.DSI_STRENGTH_MAIN];
double [] sfm_gain = main_dsi[isAux()?TwoQuadCLT.DSI_SFM_GAIN_AUX:TwoQuadCLT.DSI_SFM_GAIN_MAIN];
if ((strength == null) || (needs_lma && (disparity_lma == null) )) {
return null;
}
int min_reliable = (int) Math.round (strength.length * min_ref_frac);
strength = strength.clone();
boolean [] reliable = new boolean [strength.length];
for (int i = 0; i < reliable.length; i++) {
......@@ -1654,6 +1666,20 @@ public class QuadCLTCPU {
}
}
}
int num_sfm_gain = 0;
for (int i = 0; i < reliable.length; i++) {
if (sfm_gain[i] >0) {
num_sfm_gain++;
}
}
if (num_sfm_gain > min_reliable) {
for (int i = 0; i < reliable.length; i++) {
if (sfm_gain[i] <= 0){
reliable[i] = false;
strength[i] = 0.0;
}
}
}
int num_reliable = 0;
for (boolean b: reliable) if (b) num_reliable++;
......
src/main/java/com/elphel/imagej/tileprocessor/TDCorrTile.java
View file @ 3430abee
......@@ -66,17 +66,6 @@ public class TDCorrTile {
}
}
/*
if (fcorr_combo_td != null) {
gpuQuad.getCorrTilesTdInterCombo(
fcorr_combo_td); // final float [][] corr_tiles_combo)
}
return;
*/
/**
* Get Transform-domain 2D correlation tile weight needed for fat zero in phase
* correlation and transform to the pixel domain.
......@@ -334,7 +323,6 @@ public class TDCorrTile {
final double gpu_fat_zero,
final int debug_level
){
// final double gpu_fat_zero = clt_parameters.getGpuFatZeroInter(monochrome);
final int corr_size_td = 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE;
final int gpu_corr_rad = GPUTileProcessor.DTT_SIZE -1;
final int [] indices = new int [tiles.length];
......@@ -397,6 +385,13 @@ public class TDCorrTile {
return mapped_corrs;
}
/**
* Get GPU TD data after interscene correlation of 2 scenes (only use
* combo of all channels)
* @param gpuQuad GPU quad instance
* @return TDCorrTile [] array, with weight equal to number of channels
* combined (normally 16)
*/
public static TDCorrTile [] getFromGpu(
GpuQuad gpuQuad) {
int tilesX = gpuQuad.getImageWidth() / GpuQuad.getDttSize();
......@@ -429,12 +424,4 @@ public class TDCorrTile {
ImageDtt.startAndJoin(threads);
return tiles;
}
/*
if (fcorr_combo_td != null) {
gpuQuad.getCorrTilesTdInterCombo(
fcorr_combo_td); // final float [][] corr_tiles_combo)
}
return;
*/
}
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 3430abee
......@@ -80,7 +80,9 @@ public class TwoQuadCLT {
public static int DSI_AVGVAL_AUX = 12;
public static int DSI_BLUE_SKY_MAIN = 13;
public static int DSI_BLUE_SKY_AUX = 14;
public static int DSI_LENGTH = DSI_BLUE_SKY_AUX+1;
public static int DSI_SFM_GAIN_MAIN = 15; // SfM disparity gain pixel/pixel, RGB
public static int DSI_SFM_GAIN_AUX = 16; // SfM disparity gain pixel/pixel, LWIR
public static int DSI_LENGTH = DSI_SFM_GAIN_AUX+1;
public static String DSI_COMBO_SUFFIX = "-DSI_COMBO";
public static String DSI_MAIN_SUFFIX = "-DSI_MAIN";
......@@ -100,7 +102,9 @@ public class TwoQuadCLT {
"avgval_main",
"avgval_aux",
"blue_sky_main",
"blue_sky_aux"};
"blue_sky_aux",
"sfm_gain_main",
"sfm_gain_aux"};
public long startTime; // start of batch processing
public long startSetTime; // start of set processing
......
src/main/java/com/elphel/imagej/tileprocessor/sfm/SfmCorr.java 0 → 100644
View file @ 3430abee
package com.elphel.imagej.tileprocessor.sfm;
class SfmCorr{
double sfm_gain;
double [] corr_ind = null; // {disparity, strength}
double [] corr_neib = null; // {disparity, strength}
}
src/main/java/com/elphel/imagej/tileprocessor/StructureFromMotion.java → src/main/java/com/elphel/imagej/tileprocessor/sfm/StructureFromMotion.java
View file @ 3430abee
......@@ -21,7 +21,7 @@
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.tileprocessor;
package com.elphel.imagej.tileprocessor.sfm;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
......@@ -35,36 +35,44 @@ 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 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
// class SfmCorr{
// double sfm_gain;
// double [] corr_ind; // {disparity, strength}
// double [] corr_neib; // {disparity, strength}
// }
public static boolean sfmPair( // ref/scene
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) {
boolean test_motion_blur = false;
double range_disparity_offset = clt_parameters.imp.range_disparity_offset;
boolean debug2 = !batch_mode; // false; // 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;
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 [] reduced_strength = new double[1];
double [][] ref_xyzatr = new double [][] {ZERO3,ZERO3};
double [][] ref_xyzatr_dt= {
ers_reference.getErsXYZ_dt(),
ers_reference.getErsATR_dt()
......@@ -92,7 +100,6 @@ public class StructureFromMotion {
dls, // double [][] dls
ref_scene, // QuadCLT scene,
debugLevel);
// double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
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 ?
......@@ -106,7 +113,6 @@ public class StructureFromMotion {
}
double [][] ref_pXpYD = null;
double [][] dbg_mb_img = null;
double [] mb_ref_disparity =null;
mb_ref_disparity = interscene_ref_disparity;
if (mb_ref_disparity == null) {
......@@ -120,12 +126,8 @@ public class StructureFromMotion {
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
// if (test_motion_blur) {
// dbg_mb_img = new double[2][];
// }
// should have at least next or previous non-null
int debug_scene = -8;
double [][] mb_vectors_ref = null;
TpTask[][] tp_tasks_ref = null;
String ts = scene.getImageName();
......@@ -145,12 +147,6 @@ public class StructureFromMotion {
}
}
double [][] mb_vectors = null;
/*
scene_xyz_pre = ers_reference.getSceneXYZ(ts);
scene_atr_pre = ers_reference.getSceneATR(ts);
double [][] scene_xyzatr = new double[][] {scene_xyz_pre, scene_atr_pre};
*/
if (mb_en) {
// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
......@@ -193,17 +189,20 @@ public class StructureFromMotion {
int [] fail_reason = new int[1];
double [] ref_disparity = null;
float [][][] facc_2d_img = new float [1][][]; // set it to null?
double [][] dpXYddisp = getSfmDpxDpyDdisp(
scene_xyzatr[0], // final double [] scene_xyz, // camera center in world coordinates (or null to use instance)
scene_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world frame (or null to use instance)
scene, // final QuadCLT scene_QuadClt,
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 ;
debugLevel); // final int debug_level
// 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
......@@ -232,11 +231,10 @@ public class StructureFromMotion {
return true;
}
public static double [][] sfmPair( // ref/scene
public static double [][] sfmPair_debug( // debug scene pairs
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT [][] scene_pairs,
// final int num_avg_pairs, // number of scene pairs to average
double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
......@@ -349,21 +347,8 @@ public class StructureFromMotion {
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
/*
double [][] dpXYddisp_ref = getSfmDpxDpyDdisp(
new double[][][] {new double[2][3],scenes_xyzatr[0][1]},
// scenes_xyzatr[0], // final double [][][] scenes_xyzatr,
ref_scene,
// 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 ;
debugLevel); // final int debug_level
*/
for (int npair = 0; npair < num_pairs; npair++) {
QuadCLT [] scenes = scene_pairs[npair];
......@@ -502,6 +487,408 @@ public class StructureFromMotion {
return combo_dsn_final;
}
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;
int num_readjust = 5;
final double min_strength = 0.4; // update if correction strength exceeds
final boolean use_neibs = true;
final double min_neib_strength= 0.5; // update if no-individual and neibs correction strength exceeds
final double prev_sfm_frac = 0.6; // update if new sfm gain > this fraction of the old one
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 ?
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 num_pairs = scene_pairs.length;
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 ((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();
}
}
}
}
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
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,
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 (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,
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 (sfmCorr[nTile] != null){
if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
// 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;
}
/**
* 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 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 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
*/
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 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 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}
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});
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,
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);
}
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 ((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;
}
/**
* 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 teh SfM gain
* @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
*/
public static double [][] getSfmDisparityCorrectionStrength(
final CLTParameters clt_parameters,
final double [][] corr2d_PD,
......@@ -538,7 +925,27 @@ public class StructureFromMotion {
}
/**
* 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,
......@@ -552,8 +959,8 @@ public class StructureFromMotion {
final boolean batch_mode,
final int debugLevel) {
int imp_debug_level = 0; // MANUALLY change to 2 for debug!
boolean show_render_ref = clt_parameters.imp.renderRef(imp_debug_level); // false; //true;
boolean show_render_scene = clt_parameters.imp.renderScene(imp_debug_level); // false; // true;
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
......@@ -699,6 +1106,7 @@ public class StructureFromMotion {
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());
......@@ -726,108 +1134,11 @@ public class StructureFromMotion {
toRGB, // boolean toRGB,
false, //boolean use_reference
"GPU-SHIFTED-SCENE"); // String suffix)
imp_render_scene.show();
}
return tiles;
}
public static double [][] getSfmDpxDpyDdisp(
final double [] scene_xyz, // camera center in world coordinates (or null to use instance)
final double [] scene_atr, // camera orientation relative to world frame (or null to use instance)
final QuadCLT scene_QuadClt,
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 int debug_level
){
boolean debug_img = debug_level > 0;
double [][] dbg_img = debug_img? (new double[2] [ref_pXpYD.length]):null;
if (dbg_img != null) {
for (int i = 0; i <dbg_img.length;i++) {
Arrays.fill(dbg_img[i],Double.NaN);
}
}
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;
}
IntersceneLma intersceneLma= new IntersceneLma(
false, // clt_parameters.ilp.ilma_thread_invariant);
0.0); // always no disparity
intersceneLma.prepareLMA(
scene_xyz, // final double [] scene_xyz0, // camera center in world coordinates (or null to use instance)
scene_atr, // 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, //
scene_QuadClt, // 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, // boolean first_run,
debug_level); // final int debug_level)
final double [][] last_jt = 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][];
//d_z/d_disp = ddz_disp_scale/disp/disp
// final double ddz_disp_scale = -1.0/ref_QuadClt.getGeometryCorrection().getDisparityFromZ(1.0);
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];
// TODO: get disparity or Z for scene?
dpXYddisp[nTile][0] = last_jt[0][2*nTile + 0];
dpXYddisp[nTile][1] = last_jt[0][2*nTile + 1];
if (dbg_img != null) {
dbg_img[0][nTile] = dpXYddisp[nTile][0];
dbg_img[1][nTile] = dpXYddisp[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"}; // ,"dPx_dZ0","dPy_dZ0","dPx_dZ","dPy_dZ"};
ShowDoubleFloatArrays.showArrays(
dbg_img,
tilesX,
tilesY,
true,
"getSfmDpxDpyDdisp-"+ref_QuadClt.getImageName(),
dbg_titles);
}
return dpXYddisp;
}
public static double [][] getSfmDpxDpyDdisp(
final double [][][] scenes_xyzatr, // cameras center in world coordinates (or null to use instance)
final QuadCLT scene0,
......@@ -835,9 +1146,10 @@ public class StructureFromMotion {
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 = debug_level > 0;
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++) {
......@@ -877,15 +1189,12 @@ public class StructureFromMotion {
}
}
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][];
//d_z/d_disp = ddz_disp_scale/disp/disp
// final double ddz_disp_scale = -1.0/ref_QuadClt.getGeometryCorrection().getDisparityFromZ(1.0);
final int debug_tile = -1;
final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
......@@ -904,8 +1213,6 @@ public class StructureFromMotion {
continue;
}
dpXYddisp[nTile] = new double [2];
// double k = ddz_disp_scale/disp_eff/disp_eff;
// TODO: get disparity or Z for scene?
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) {
......@@ -933,9 +1240,6 @@ public class StructureFromMotion {
"getSfmDpxDpyDdisp-"+ref_QuadClt.getImageName(),
dbg_titles);
}
return dpXYddisp;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment