Skip to content

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

First SfM tests - 0.05m from 75m AGL

parent b3dc65da
Hide whitespace changes
Inline Side-by-side
Showing with 418 additions and 147 deletions
src/main/java/com/elphel/imagej/tileprocessor/Correlation2d.java
View file @ e61d71b1
......@@ -2506,6 +2506,89 @@ public class Correlation2d {
}
public static double [] getMaxProjCm(
double [] data,
int data_width, // = 2 * transform_size - 1;
double radius, // 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
int refine, // re-center window around new maximum. 0 -no refines (single-pass)
double [] direction_XY,
boolean debug)
{
int data_height = data.length/data_width;
int center_xy = (data_width - 1)/2; // = transform_size - 1;
double x0 = center_xy, y0 = center_xy;
int imax= 0;
for (int i= 1; i < data.length;i++) {
if (data[i] > data[imax]) {
imax = i;
}
}
double mx = data[imax];
int ix0 = imax % data_width;
int iy0 = imax / data_width;
x0 = ix0;
y0 = iy0;
double disp0 = x0 * direction_XY[0] + y0 * direction_XY[1];
// if (fpn_mask != null
//calculate as "center of mass"
if (radius == 0) {
double s0 = 0, sx=0,sy = 0, sdisp = 0;
for (int iy = 0; iy < data_height; iy++) {
double y = iy - y0;
for (int ix = 0; ix < data_width; ix++) {
double x = ix - x0;
double d = data[iy * data_width + ix];
if (d > 0) { // ignore negative
s0 += d;
sx += d * x;
sy += d * y;
sdisp += d * (x * direction_XY[0] + y * direction_XY[1]);
} else if (Double.isNaN(d)) {
System.out.println("NaN in getMaxProjCm()");
return null;
}
}
}
x0 += sx / s0;
y0 += sy / s0;
disp0 += sdisp / s0;
} else {
double radius2 = radius*radius;
for (int nref = 0; nref <= refine; nref++) {
double s0 = 0, sx=0, sy = 0, sdisp = 0;
for (int iy = 0; iy < data_height; iy++) {
double y = iy - y0;
for (int ix = 0; ix < data_width; ix++) {
double x = ix - x0;
double r2= x*x + y*y;
if (r2 < radius2) {
double r = Math.sqrt(r2);
double d = data[iy * data_width + ix];
if (d > 0) { // ignore negative
d *= Math.cos(0.5*Math.PI*r/radius);
s0 += d;
sx += d * x;
sy += d * y;
sdisp += d * (x * direction_XY[0] + y * direction_XY[1]);
}
}
}
}
x0 += sx / s0;
y0 += sy / s0;
disp0 += sdisp / s0;
}
}
// double [] rslt = {x0 - center_xy, y0 - center_xy, mx};
double [] rslt = {disp0 - center_xy * (direction_XY[0] + direction_XY[1]), mx};
if (debug){
System.out.println("getMaxProjCm() -> "+rslt[0]+":"+rslt[1]);
}
return rslt;
}
/**
* Analyze 1d correlation (single centerline of the 3D phase correlation combined output
......
src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java
View file @ e61d71b1
......@@ -1077,7 +1077,7 @@ public class GeometryCorrection {
debugLevel); // int debugLevel)
int pars = jt_delta.length;
int tilesX = qc_main.tp.getTilesX();
int tilesY = qc_main.tp.getTilesX();
int tilesY = qc_main.tp.getTilesY(); // 10.14.2023: was getTilesX()!
String [] titles = new String[6 * pars];
double [][] dbg_data = new double [6 * pars][tilesY * tilesX];
for (int i = 0; i < dbg_data.length; i++) for (int j = 0; j < dbg_data[i].length; j++) dbg_data[i][j]=Double.NaN;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ e61d71b1
......@@ -4892,14 +4892,21 @@ public class OpticalFlow {
mb_max_gain, // double mb_max_gain,
batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel)
QuadCLT[] scenes_pair = new QuadCLT[]{
quadCLTs[ref_index - 1],
quadCLTs[earliest_scene]};
StructureFromMotion.sfmPair(
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(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs[ref_index], // final QuadCLT ref_scene,
scenes_pair, // final QuadCLT [] scenes,
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)
......
src/main/java/com/elphel/imagej/tileprocessor/StructureFromMotion.java
View file @ e61d71b1
......@@ -229,27 +229,24 @@ public class StructureFromMotion {
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 boolean sfmPair( // ref/scene
public static double [][] sfmPair( // ref/scene
final CLTParameters clt_parameters,
final QuadCLT ref_scene,
final QuadCLT [] scenes,
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) {
boolean show_2d_correlations = true;
boolean show_2d_correlations = false; // true;
boolean show_disp_corr = false; //true;
boolean show_disp_seq = true;
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};
......@@ -303,104 +300,242 @@ public class StructureFromMotion {
if (ref_disparity == null) {
ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
// 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 [][]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();
}
}
}
}
// 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 false;
}
scenes_xyzatr[nscene] = new double[][] {
ers_reference.getSceneXYZ(ts),
ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt[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[nscene][i] == null) {
System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
scenes_xyzatr_dt[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 ;
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
*/
double [][] dpXYddisp = getSfmDpxDpyDdisp(
scenes_xyzatr, // final double [][][] scenes_xyzatr,
scenes, // final QuadCLT[] scenes,
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
TDCorrTile [] pairs_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, //final double[][][] scenes_xyzatr, // 2 scenes
scenes_xyzatr_dt,// 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
TDCorrTile [] neibs_TD = TDCorrTile.calcNeibs(
pairs_TD, // TDCorrTile [] tiles,
tilesX, // final int tilesX,
neib_weights_od, //double [] neib_weights_od, // {orhto, diag}
true); // final boolean process_all
double [][][] corr2d_PD = new double [2][][];
corr2d_PD[0] = TDCorrTile.convertTDtoPD(
ref_scene.getGPU(), // final GpuQuad gpuQuad,
pairs_TD, // final TDCorrTile [] tiles,
0xFE, // final int corr_type, // 0xFE
corr_fz_inter, // final double gpu_fat_zero,
debugLevel); // final int debug_level
corr2d_PD[1] = 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
//
if (show_2d_correlations) {
String [] dbg_titles = {"combo", "neibs"};
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)
ShowDoubleFloatArrays.showArrays(
dbg_2d_corrs,
tilesX * (corr_size + 1),
tilesY * (corr_size + 1),
true,
"corr2d-"+scenes[0].getImageName()+"-"+scenes[0].getImageName(),
dbg_titles);
}
return true;
}
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}
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,
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);
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;
}
public static double [][] getSfmDisparityCorrectionStrength(
final CLTParameters clt_parameters,
final double [][] corr2d_PD,
final double [][] dpXYddisp,
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);
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)
disp_corr[nTile] = corr_cm; // may be null
}
}
};
}
ImageDtt.startAndJoin(threads);
return disp_corr;
}
......@@ -596,8 +731,6 @@ public class StructureFromMotion {
}
return tiles;
}
......@@ -610,10 +743,16 @@ public class StructureFromMotion {
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_DZ,
ErsCorrection.DP_DSZ};
ErsCorrection.DP_DD};
for (int i: par_indices) {
param_select[i]=true;
}
......@@ -636,8 +775,6 @@ public class StructureFromMotion {
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];
......@@ -645,7 +782,7 @@ public class StructureFromMotion {
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 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);
......@@ -664,30 +801,53 @@ 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] = k* (last_jt[1][2*nTile + 0]-last_jt[0][2*nTile + 0]); //dpX [DP_DSZ] - [DP_SZ]
dpXYddisp[nTile][1] = k* (last_jt[1][2*nTile + 1]-last_jt[0][2*nTile + 1]); //dpY [DP_DSZ] - [DP_SZ]
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[] scenes,
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 int debug_level
){
boolean debug_img = 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_DZ,
ErsCorrection.DP_DSZ};
ErsCorrection.DP_DD};
for (int i: par_indices) {
param_select[i]=true;
}
......@@ -696,20 +856,25 @@ public class StructureFromMotion {
false, // clt_parameters.ilp.ilma_thread_invariant);
0.0); // always no disparity
for (int nscene = 0; nscene < scenes.length; nscene++) {
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, // boolean first_run,
debug_level); // final int debug_level)
last_jts[nscene] = intersceneLma. getLastJT(); // alternating x,y for each selected parameters
// 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, // boolean first_run,
debug_level); // final int debug_level)
last_jts[nscene] = intersceneLma. getLastJT(); // alternating x,y for each selected parameters
}
}
......@@ -720,7 +885,7 @@ public class StructureFromMotion {
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 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);
......@@ -739,19 +904,36 @@ public class StructureFromMotion {
continue;
}
dpXYddisp[nTile] = new double [2];
double k = ddz_disp_scale/disp_eff/disp_eff;
// double k = ddz_disp_scale/disp_eff/disp_eff;
// TODO: get disparity or Z for scene?
dpXYddisp[nTile][0] = k* (
(last_jts[1][1][2*nTile + 0] -last_jts[1][0][2*nTile + 0])-
(last_jts[0][1][2*nTile + 0] -last_jts[0][0][2*nTile + 0])); //dpX [DP_DSZ] - [DP_SZ]
dpXYddisp[nTile][1] = k* (
(last_jts[1][1][2*nTile + 1]-last_jts[1][1][2*nTile + 1])-
(last_jts[0][1][2*nTile + 1]-last_jts[0][1][2*nTile + 1])); //dpY [DP_DSZ] - [DP_SZ]
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;
}
......
src/main/java/com/elphel/imagej/tileprocessor/TDCorrTile.java
View file @ e61d71b1
......@@ -347,7 +347,6 @@ public class TDCorrTile {
final int [] corr_indices = new int [num_tiles];
final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger anum_tiles = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
......
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