Skip to content

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

Fixed display of the ground planes

parent d786d9fe
Show whitespace changes
Inline Side-by-side
Showing with 178 additions and 71 deletions
src/main/java/com/elphel/imagej/cuas/Cuas.java
View file @ d7ef934d
......@@ -1311,6 +1311,69 @@ public class Cuas {
return ref_pXpYD;
}
/**
* Re-sample disparity map to match uniform grid on the virtual view, such as
* the view orthogonal to the terrain tangent/average surface
* @param clt_parameters configuration parameters
* @param disparity_ref known disparity for the camera reference frame
* @param scene_xyzatr virtual view center position and orientation relative to the reference frame
* @param reference_QuadClt scene instance for the reference frame
* @param around number of tiles to look around when interpolating using average planes
* @param sigma reduce weight of far samples e-times at this distance
* @param num_refines number of refine passes
* @param debugSuffix image name suffix to generate debug images
* @return array of per-tile disparity (NaN for undefined tiles)
*/
public static double [] getDisparityVirtual(
CLTParameters clt_parameters,
double [] disparity_ref,
final double [][] scene_xyzatr, // camera center and orientation in world (reference frame) coordinates
final QuadCLT reference_QuadClt,
final int around, // 2 search around for interpolation
final double sigma,
final int num_refines,
final String debugSuffix){
double [][] reference_pXpYD =
transformFromVirtual(
clt_parameters, // CLTParameters clt_parameters,
disparity_ref, // double [] disparity_ref,
scene_xyzatr[0], // final double [] scene_xyz, // camera center in world (reference) coordinates
scene_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world (reference) frame
reference_QuadClt, // final QuadCLT reference_QuadClt,
around, // final int around, // 2 search around for interpolation
sigma, // final double sigma,
num_refines, // final int num_refines,
debugSuffix); // final String debugSuffix)
// get scene pXpYD corresponding to reference_pXpYD
double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
reference_pXpYD, // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
scene_xyzatr[0], // final double [] scene_xyz, // camera center in world (reference) coordinates
scene_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world (reference) frame
reference_QuadClt, // final QuadCLT reference_QuadClt) //
null); // final QuadCLT scene_QuadClt) //
double [] scene_disparity = new double [scene_pXpYD.length];
Arrays.fill(scene_disparity, Double.NaN);
for (int ntile = 0; ntile < scene_disparity.length; ntile++) if (scene_pXpYD[ntile] != null) {
scene_disparity[ntile] = scene_pXpYD[ntile][2];
}
return scene_disparity;
}
/**
* Re-sample disparity map to match uniform grid on the virtual view, producing a non-uniform
* coordinates+disparity in the reference scene that corresponds to the uniform grid in the virtual view.
* @param clt_parameters configuration parameters
* @param disparity_ref known disparity for the camera reference frame
* @param scene_xyzatr virtual view center position and orientation relative to the reference frame
* @param reference_QuadClt scene instance for the reference frame
* @param around number of tiles to look around when interpolating using average planes
* @param sigma reduce weight of far samples e-times at this distance
* @param num_refines number of refine passes
* @param debugSuffix image name suffix to generate debug images
* @return array of non-uniform grid triplets of pixel X, pixel Y and disparity for the reference scene
* corresponding to the uniform grid in a virtual camera view
*/
public static double [][] transformFromVirtual(
CLTParameters clt_parameters,
......@@ -1322,7 +1385,7 @@ public class Cuas {
final double sigma,
final int num_refines,
final String debugSuffix){
final boolean debug = (debugSuffix != null) && !clt_parameters.batch_run;
final boolean debug = (debugSuffix != null); // && !clt_parameters.batch_run;
final double sigma2 = 2*sigma*sigma;
final double normal_damping = 0.001; // pull to horizontal if not enough data
final double [] damping = new double [] {normal_damping, normal_damping};
......@@ -1363,9 +1426,6 @@ public class Cuas {
System.arraycopy(disparity_ref1, 0, disparity_ref, 0, disparity_ref.length);
}
final String debugTitle=debug?reference_QuadClt.getImageName()+"-"+debugSuffix : null;
String [] debug_frame_titles = {"X", "Y", "D"};
String [] debug_titles = new String[num_refines+1];
......@@ -1385,10 +1445,16 @@ public class Cuas {
for (int nTile = 0; nTile < tiles_around; nTile++) {
interp_list.add(new ArrayList<Integer>(initial_capacity));
}
// create uniform grid for initial interpolations
/*
* create uniform grid for initial interpolations
*/
final double [][] reference_pXpYD = new double [tiles][];
for (int nrefine = 0; nrefine <= num_refines; nrefine++) {
if (nrefine == 0) {
/*
* The first iteration of the reference_pXpYD is just a uniform grid with
* reference disparity
*/
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
......@@ -1408,18 +1474,26 @@ public class Cuas {
};
}
ImageDtt.startAndJoin(threads);
} else {
// get scene pXpYD corresponding to reference_pXpYD
/*
* Next iterations create uniform grid on the virtual scene by the bi-linear
* interpolation using small patches around the grid points
* Start with creatring non-iniform grid scene_pXpYD, corresponding to the
* current reference_pXpYD (also non-uniform for all but the very first iteration)
*
* tiles_around - larger grid
*/
final double [][] scene_pXpYD = OpticalFlow.transformToScenePxPyD(
reference_pXpYD, // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
scene_xyz, // final double [] scene_xyz, // camera center in world (reference) coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world (reference) frame
reference_QuadClt, // final QuadCLT reference_QuadClt) //
null); // final QuadCLT scene_QuadClt) //
// deep clone reference_pXpYD
final double [][] reference_pXpYD_next = new double [tiles][];
/*
* Initialize lists of the scene tiles containing converted reference tiles in the vicinity
* of scene uniform grid
*/
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
......@@ -1431,7 +1505,11 @@ public class Cuas {
};
}
ImageDtt.startAndJoin(threads);
// not multithreaded
/*
* Populate lists in a single thread. Each reference_pXpYD tile projection scene_pXpYD
* gets to some uniform-grit tile. Some of the uniform grid tiles gets multiple reference
* sources, others - none.
*/
for (int nTile = 0; nTile < tiles; nTile++) if (scene_pXpYD[nTile] != null) {
int tileX = (int) Math.floor (scene_pXpYD[nTile][0]/transform_size);
int tileY = (int) Math.floor (scene_pXpYD[nTile][1]/transform_size);
......@@ -1442,7 +1520,9 @@ public class Cuas {
interp_list.get(tile_around).add(nTile);
}
}
// interpolate reference_pXpYD_next
/*
* interpolate reference_pXpYD_next
*/
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
......@@ -1464,7 +1544,9 @@ public class Cuas {
}
int mindx = 0;
if (num_samples >= min_samples) {
// inter/extrapolate with regularization (for few samples) and weights
/*
* inter/extrapolate with regularization (for few samples) and weights
*/
double [][][] mdata = new double [num_samples][3][];
for (int dy = -around; dy <= around; dy++) {
for (int dx = -around; dx <= around; dx++) {
......@@ -1552,13 +1634,14 @@ public class Cuas {
}
}
if (debug) {
ShowDoubleFloatArrays.showArraysHyperstack(
ImagePlus img_debug = ShowDoubleFloatArrays.showArraysHyperstack(
debug_data, // double[][][] pixels,
tilesX, // int width,
debugTitle, // String title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
debug_titles, // String [] titles, // all slices*frames titles or just slice titles or null
debug_frame_titles, // String [] frame_titles, // frame titles or null
true); // boolean show)
false); // true); // boolean show)
reference_QuadClt.saveImagePlusInModelDirectory(img_debug);
}
......
src/main/java/com/elphel/imagej/tileprocessor/GroundPlane.java
View file @ d7ef934d
......@@ -16,6 +16,7 @@ import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.cuas.Cuas;
import com.elphel.imagej.ims.Imx5;
import ij.text.TextWindow;
......@@ -226,11 +227,13 @@ public class GroundPlane {
final boolean [] good_tiles = new boolean[ref_scene.getTilesX()*ref_scene.getTilesY()];
// String dbg_title =ref_scene.getImageName()+"-ground_tilts";
String dbg_title ="-ground_tilts";
String dbg_title = clt_parameters.imp.fgnd_debug ? (clt_parameters.imp.fgnd_debug_prefix+ "_tilts"): null; // "-ground_tilts";
String dbg_title_virt = clt_parameters.imp.fgnd_debug ? clt_parameters.imp.fgnd_debug_prefix: null; // "-ground_tilts";
String dbg_title_disp = null; // clt_parameters.imp.fgnd_debug ? (clt_parameters.imp.fgnd_debug_prefix + "_disp"): null; // "-ground_tilts";
double [][] to_ground_xyzatr_frac= getPlaneDualPassMetric( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_Clt,
ref_scene.getTilesX(),// final int width,
ref_scene.getTilesX(), // final int width,
good_tiles, // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
gnd_disp, // final double [] gnd_disp, // if not null
dbg_title, // final String dbg_title,
......@@ -249,9 +252,9 @@ public class GroundPlane {
double [] disparity_plane = getDisparityPlaneSinglePass( // returns to_ground_xyzatr (rotated around the ground point nadir of the drone)
clt_parameters, // final CLTParameters clt_parameters,
ref_scene, // final QuadCLT ref_Clt,
ref_scene.getTilesX(),// final int width,
ref_scene.getTilesX(), // final int width,
good_tiles, // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
dbg_title+"-disp", // final String dbg_title,
dbg_title_disp, // final String dbg_title,
debugLevel); // final int debugLevel)
return new GroundPlane(
......@@ -467,6 +470,7 @@ public class GroundPlane {
ref_Clt, // final QuadCLT quadClt, // now - may be null - for testing if scene is rotated ref
ImageDtt.THREADS_MAX); // int threadsMax)
boolean use_parallel_proj = false; // true; // maybe make false to get z-offset to the ground to avoid moving
String dbg_title_metric = (dbg_title != null) ?(dbg_title+"-metric"):null;
double [][] to_ground_xyzatr = getPlane( // from the camera coordinates to in-plane coordinates
wxyz, // final double [][] wxyz,
use_parallel_proj, // final boolean use_parallel_proj, // for parallel xyz is 0, otherwise - point on the ground under the camera
......@@ -474,12 +478,13 @@ public class GroundPlane {
weights, // final double [] weight,
width, // final int width,
normal_damping, // final double normal_damping,
dbg_title+"-metric", // final String dbg_title,
dbg_title_metric, // final String dbg_title,
debugLevel); // final int debugLevel) {
if (to_ground_xyzatr == null) {
return new double [][] {{0,0,0},{0,0,0},{0}};
}
String dbg_title_macro = (dbg_title != null) ?(dbg_title+"-macroplanes"):null;
double [][] macroplanes = getMacroPlanes( // no weights
wxyz, // final double [][] wxyz,
......@@ -489,7 +494,7 @@ public class GroundPlane {
width, // final int tilesX,
mtile_size, // final int mtile_size,
normal_damping, // final double normal_damping,
dbg_title+"-macroplanes", // final String dbg_title,
dbg_title_macro, // final String dbg_title,
debugLevel); // final int debugLevel) {
boolean [] macro_mask = getMaskFromMacro(
mask3, // boolean [] mask_in,
......@@ -510,7 +515,7 @@ public class GroundPlane {
weights, // final double [] weight,
width, // final int width,
normal_damping, // final double normal_damping,
dbg_title+"-metric", // final String dbg_title,
dbg_title_metric, // final String dbg_title,
debugLevel); // final int debugLevel) {
if (to_ground_xyzatr == null) {
return new double [][] {{0,0,0},{0,0,0},{0}};
......@@ -525,7 +530,7 @@ public class GroundPlane {
width, // final int tilesX,
mtile_size, // final int mtile_size,
normal_damping, // final double normal_damping,
dbg_title+"-macroplanes", // final String dbg_title,
dbg_title_macro, // final String dbg_title,
debugLevel); // final int debugLevel) {
double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
......@@ -533,51 +538,30 @@ public class GroundPlane {
double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
// FIXME: Next is correct (at least approximately, "-"/invert, order,), find out why
System.out.println("FIXME: Inverted X, Y, kept Z, and ATR. Find out why.");
// double [][] vcfs_test = {
// {-virtual_camera_from_camera[0][0], -virtual_camera_from_camera[0][1], virtual_camera_from_camera[0][2]},
// { virtual_camera_from_camera[1][0], virtual_camera_from_camera[1][1], virtual_camera_from_camera[1][2]}};
/*
double [][] vcfs_test = {
{virtual_camera_from_camera[0][0], virtual_camera_from_camera[0][1], virtual_camera_from_camera[0][2]},
{ to_ground_xyzatr[1][0], to_ground_xyzatr[1][1], to_ground_xyzatr[1][2]}};
if ((dbg_title != null) || (gnd_disp != null)) {
// expand macrotiles results to match tiles
// rotate ref_disparity and show
/*
double [][] pXpYD_ground = OpticalFlow.transformToScenePxPyD(
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null) IN TILES
ref_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
vcfs_test, // virtual_camera_from_camera,// final double [][] scene_xyzatr, // camera center in world coordinates, camera orientation relative to world frame
ref_Clt); // final QuadCLT scene_QuadClt)
double [][] ivcfs_test =ErsCorrection.invertXYZATR(vcfs_test);
*/
double [][] fgnd_pXpYD = OpticalFlow.refDisparityFromGroundDisparity(
ref_disparity, // final double [] ground_disparity,
vcfs_test, // final double [][] ground_xyzatr,
ref_Clt); //final QuadCLT ref_scene)
double [][] pXpYD_ground = OpticalFlow.transformToScenePxPyD(
fgnd_pXpYD, // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
ground_xyzatr[0], // final double [] scene_xyz, // camera center in world (reference) coordinates
ground_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world (reference) frame
ref_Clt, // final QuadCLT reference_QuadClt) //
null); // final QuadCLT scene_QuadClt) //
/*
ref_pXpYD = OpticalFlow.transformToScenePxPyD(
fgnd_pXpYD, // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
ground_xyzatr[0], // final double [] scene_xyz, // camera center in world (reference) coordinates
ground_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world (reference) frame
ref_scene, // final QuadCLT reference_QuadClt) //
null); // final QuadCLT scene_QuadClt) //
double [][] virtual_camera_from_camera_mod = {
{-virtual_camera_from_camera[0][0], -virtual_camera_from_camera[0][1], virtual_camera_from_camera[0][2]},
virtual_camera_from_camera[1] };
*/
double [] gnd_disparity = (gnd_disp != null) ? gnd_disp.clone() : new double [ref_disparity.length];
Arrays.fill(gnd_disparity, Double.NaN);
for (int i = 0; i <ref_disparity.length; i++) {
double [] xyd = pXpYD_ground[i];
if (xyd != null) {
gnd_disparity[i] = xyd[2];
}
}
if ((dbg_title != null) || (gnd_disp != null)) {
int around = clt_parameters.imp.fgnd_virt_around; // 2;
double around_sigma = clt_parameters.imp.fgnd_virt_sigma; // 4.0;
int num_virtual_refines = clt_parameters.imp.fgnd_virt_refine; // 4;
String debugSuffix= (dbg_title != null) ? (dbg_title+"_virtual") : null; // "virtual_gnd"; // null
double [] gnd_disparity = Cuas.getDisparityVirtual(
clt_parameters, // CLTParameters clt_parameters,
ref_disparity, // double [] disparity_ref,
virtual_camera_from_camera_mod, // ivcfs_test, // final double [] scene_xyz, // camera center in world (reference) coordinates
ref_Clt, // final QuadCLT reference_QuadClt,
around, // final int around, // 2 search around for interpolation
around_sigma, // final double sigma,
num_virtual_refines, // final int num_refines,
debugSuffix); // final String debugSuffix)
if (dbg_title != null) {
int tilesX = width;
int tilesY = wxyz.length/width;
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ d7ef934d
......@@ -1080,6 +1080,11 @@ min_str_neib_fpn 0.35
public double fgnd_ct_high = 1.0; // highest CT level relative to the ground
public double fgnd_ct_step = 0.2; // CT step
public int fgnd_virt_around = 2; // virtual views interpolate around
public double fgnd_virt_sigma = 4.0; // virtual views sigma
public int fgnd_virt_refine = 4; // virtual views number of refines
public boolean fgnd_debug = true; // generate ground planes debug images
public String fgnd_debug_prefix = "-ground"; // ground planes debug prefix (will add -tilts, -virtual
// TODO: move next parameters elsewhere - they are not the motion blur ones.
......@@ -3180,6 +3185,19 @@ min_str_neib_fpn 0.35
gd.addNumericField("CT scan step", this.fgnd_ct_step, 5,7,"m",
"CT scan step.");
gd.addMessage("Calculate disparty for the virtual views orthogonal to the ground plane surface");
gd.addNumericField("Virtual views interpolate around", this.fgnd_virt_around, 0,3,"pix",
"Interpolate using this number of tiles around.");
gd.addNumericField("Virtual views sigma", this.fgnd_virt_sigma, 5,7,"pix",
"Exponentially reduce weight of the far tiles (radius).");
gd.addNumericField("Virtual views number of refines", this.fgnd_virt_refine, 0,3,"",
"Iterate conversion from the known reference frame this number of times.");
gd.addMessage("Ground planes debug images");
gd.addCheckbox ("Generate ground planes debug images", this.fgnd_debug,
"Always adjust Z (altitude) when running interscene LMA. False - same as X and Y.");
gd.addStringField("Ground planes debug images prefix", this.fgnd_debug_prefix, 40,
"Ground planes debug imaghes prefix (Will add -tilts, -virtual).");
gd.addTab("LMA sequence","Interscene LMA sequence control");
gd.addMessage("Parameters for control of the LMA pose adjustment sequence");
gd.addNumericField("Pose readjust number for full mb_gain", this.mb_gain_index_pose, 0,3,"",
......@@ -4507,6 +4525,12 @@ min_str_neib_fpn 0.35
this.fgnd_ct_high = gd.getNextNumber();
this.fgnd_ct_step = gd.getNextNumber();
this.fgnd_virt_around = (int) gd.getNextNumber();
this.fgnd_virt_sigma = gd.getNextNumber();
this.fgnd_virt_refine = (int) gd.getNextNumber();
this.fgnd_debug = gd.getNextBoolean();
this.fgnd_debug_prefix = gd.getNextString();
this.mb_gain_index_pose = (int) gd.getNextNumber();
this.mb_gain_index_depth =(int) gd.getNextNumber();
......@@ -5723,6 +5747,12 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"fgnd_ct_high", this.fgnd_ct_high+""); // double
properties.setProperty(prefix+"fgnd_ct_step", this.fgnd_ct_step+""); // double
properties.setProperty(prefix+"fgnd_virt_around", this.fgnd_virt_around+""); // int
properties.setProperty(prefix+"fgnd_virt_sigma", this.fgnd_virt_sigma+""); // double
properties.setProperty(prefix+"fgnd_virt_refine", this.fgnd_virt_refine+""); // int
properties.setProperty(prefix+"fgnd_debug", this.fgnd_debug+""); // boolean
properties.setProperty(prefix+"fgnd_debug_prefix", this.fgnd_debug_prefix+""); // String
properties.setProperty(prefix+"mb_gain_index_pose", this.mb_gain_index_pose+""); // int
properties.setProperty(prefix+"mb_gain_index_depth", this.mb_gain_index_depth+""); // int
......@@ -6923,6 +6953,12 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"fgnd_ct_high")!=null) this.fgnd_ct_high=Double.parseDouble(properties.getProperty(prefix+"fgnd_ct_high"));
if (properties.getProperty(prefix+"fgnd_ct_step")!=null) this.fgnd_ct_step=Double.parseDouble(properties.getProperty(prefix+"fgnd_ct_step"));
if (properties.getProperty(prefix+"fgnd_virt_around")!=null) this.fgnd_virt_around=Integer.parseInt(properties.getProperty(prefix+"fgnd_virt_around"));
if (properties.getProperty(prefix+"fgnd_virt_sigma")!=null) this.fgnd_virt_sigma=Double.parseDouble(properties.getProperty(prefix+"fgnd_virt_sigma"));
if (properties.getProperty(prefix+"fgnd_virt_refine")!=null) this.fgnd_virt_refine=Integer.parseInt(properties.getProperty(prefix+"fgnd_virt_refine"));
if (properties.getProperty(prefix+"fgnd_debug")!=null) this.fgnd_debug=Boolean.parseBoolean(properties.getProperty(prefix+"fgnd_debug"));
if (properties.getProperty(prefix+"fgnd_debug_prefix")!= null) fgnd_debug_prefix=(String) properties.getProperty(prefix+"fgnd_debug_prefix");
if (properties.getProperty(prefix+"mb_gain_index_pose")!=null) this.mb_gain_index_pose=Integer.parseInt(properties.getProperty(prefix+"mb_gain_index_pose"));
if (properties.getProperty(prefix+"mb_ers_index")!=null) this.mb_ers_index=Integer.parseInt(properties.getProperty(prefix+"mb_ers_index"));
if (properties.getProperty(prefix+"mb_ers_y_index")!=null) this.mb_ers_y_index=Integer.parseInt(properties.getProperty(prefix+"mb_ers_y_index"));
......@@ -8102,6 +8138,12 @@ min_str_neib_fpn 0.35
imp.fgnd_ct_high = this.fgnd_ct_high;
imp.fgnd_ct_step = this.fgnd_ct_step;
imp.fgnd_virt_around = this.fgnd_virt_around;
imp.fgnd_virt_sigma = this.fgnd_virt_sigma;
imp.fgnd_virt_refine = this.fgnd_virt_refine;
imp.fgnd_debug = this.fgnd_debug;
imp.fgnd_debug_prefix = this.fgnd_debug_prefix;
imp.mb_gain_index_pose = this.mb_gain_index_pose;
imp.mb_gain_index_depth = this.mb_gain_index_depth;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ d7ef934d
......@@ -3059,8 +3059,6 @@ public class OpticalFlow {
return pXpYD;
}
//TODO: refine inter-scene pose to accommodate refined disparity map
/**
* Removing BG tiles that are not visible because of the FG ones
......
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