Skip to content

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

using disparity planes OK

parent 44cb1d7e
Show whitespace changes
Inline Side-by-side
Showing with 1051 additions and 393 deletions
src/main/java/com/elphel/imagej/tileprocessor/GroundPlane.java
View file @ b4d24102
...@@ -23,6 +23,7 @@ import ij.text.TextWindow; ...@@ -23,6 +23,7 @@ import ij.text.TextWindow;
public class GroundPlane { public class GroundPlane {
public static final String GROUND_PLANE_PREFIX = "groundplane"; public static final String GROUND_PLANE_PREFIX = "groundplane";
public double [][] to_ground_xyzatr = null; public double [][] to_ground_xyzatr = null;
public double [] disparity_plane = null;
public double frac_good = Double.NaN; // fraction of "ground" tiles used to find local ground plane public double frac_good = Double.NaN; // fraction of "ground" tiles used to find local ground plane
public double [] lla = null; // GPS LLA public double [] lla = null; // GPS LLA
public double [] quat_enu = null; // IMS orientation quaternion == d2.getQEnu() public double [] quat_enu = null; // IMS orientation quaternion == d2.getQEnu()
...@@ -38,13 +39,18 @@ public class GroundPlane { ...@@ -38,13 +39,18 @@ public class GroundPlane {
public double [][] getToGroundPlane(){ public double [][] getToGroundPlane(){
return to_ground_xyzatr; return to_ground_xyzatr;
} }
public double [] getDisparityTilts(){
return disparity_plane;
}
public GroundPlane() {} public GroundPlane() {}
public GroundPlane( public GroundPlane(
double [][] to_ground_xyzatr, double [][] to_ground_xyzatr,
double [] disparity_plane,
double frac_good, double frac_good,
double [] lla, double [] lla,
double [] quat_enu) { double [] quat_enu) {
this.to_ground_xyzatr = new double [][] {to_ground_xyzatr[0].clone(),to_ground_xyzatr[1].clone()}; // will fail if any is null - both needed this.to_ground_xyzatr = new double [][] {to_ground_xyzatr[0].clone(),to_ground_xyzatr[1].clone()}; // will fail if any is null - both needed
this.disparity_plane = disparity_plane;
this.frac_good = frac_good; this.frac_good = frac_good;
this.lla = lla.clone(); this.lla = lla.clone();
this.quat_enu = quat_enu.clone(); this.quat_enu = quat_enu.clone();
...@@ -143,7 +149,7 @@ public class GroundPlane { ...@@ -143,7 +149,7 @@ public class GroundPlane {
String header = "timestamp\tX\tY\tZ\tAzimuth\tTilt\tRoll\tGood"+ String header = "timestamp\tX\tY\tZ\tAzimuth\tTilt\tRoll\tGood"+
"\tLatitude\tLongitude\tAltitude"+ "\tLatitude\tLongitude\tAltitude"+
"\tqenu[0]\tqenu[1]\tqenu[2]\tqenu[3]"+ "\tqenu[0]\tqenu[1]\tqenu[2]\tqenu[3]"+
"\tcam-A\tcam-T\tcam-R"; "\tcam-A\tcam-T\tcam-R\tdisp-TX\tdisp-TY\tdisp_center";
StringBuffer sb = new StringBuffer(); StringBuffer sb = new StringBuffer();
for (String ts:planes_map.keySet()) { for (String ts:planes_map.keySet()) {
GroundPlane gp = planes_map.get(ts); GroundPlane gp = planes_map.get(ts);
...@@ -160,7 +166,8 @@ public class GroundPlane { ...@@ -160,7 +166,8 @@ public class GroundPlane {
sb.append(ts+"\t"+xyzatr[0][0]+"\t"+xyzatr[0][1]+"\t"+xyzatr[0][2]+"\t"+xyzatr[1][0]+"\t"+xyzatr[1][1]+"\t"+xyzatr[1][2]); sb.append(ts+"\t"+xyzatr[0][0]+"\t"+xyzatr[0][1]+"\t"+xyzatr[0][2]+"\t"+xyzatr[1][0]+"\t"+xyzatr[1][1]+"\t"+xyzatr[1][2]);
sb.append("\t"+frac_good+"\t"+lla[0]+"\t"+lla[1]+"\t"+lla[2]); sb.append("\t"+frac_good+"\t"+lla[0]+"\t"+lla[1]+"\t"+lla[2]);
sb.append("\t"+qenu[0]+"\t"+qenu[1]+"\t"+qenu[2]+"\t"+qenu[3]); sb.append("\t"+qenu[0]+"\t"+qenu[1]+"\t"+qenu[2]+"\t"+qenu[3]);
sb.append("\t"+ref_abs_atr_enu[0]+"\t"+ref_abs_atr_enu[1]+"\t"+ref_abs_atr_enu[2]+"\n"); sb.append("\t"+ref_abs_atr_enu[0]+"\t"+ref_abs_atr_enu[1]+"\t"+ref_abs_atr_enu[2]);
sb.append("\t"+gp.disparity_plane[0]+"\t"+gp.disparity_plane[1]+"\t"+gp.disparity_plane[2]+"\n");
} }
if (path!=null) { if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): ""; String footer=(comment != null) ? ("Comment: "+comment): "";
...@@ -181,6 +188,9 @@ public class GroundPlane { ...@@ -181,6 +188,9 @@ public class GroundPlane {
double [] d = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"to_ground_xyzatr")); double [] d = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"to_ground_xyzatr"));
to_ground_xyzatr = new double [][] {{d[0],d[1],d[2]},{d[3],d[4],d[5]}}; to_ground_xyzatr = new double [][] {{d[0],d[1],d[2]},{d[3],d[4],d[5]}};
} }
if (properties.getProperty(prefix+"disparity_plane")!=null) {
disparity_plane = ErsCorrection.parseDoublesCSV(properties.getProperty(prefix+"disparity_plane"));
}
if (properties.getProperty(prefix+"frac_good")!=null) { if (properties.getProperty(prefix+"frac_good")!=null) {
frac_good = Double.parseDouble(properties.getProperty(prefix+"frac_good")); frac_good = Double.parseDouble(properties.getProperty(prefix+"frac_good"));
} }
...@@ -197,6 +207,7 @@ public class GroundPlane { ...@@ -197,6 +207,7 @@ public class GroundPlane {
properties.setProperty(prefix+"to_ground_xyzatr",String.format("%f, %f, %f, %f, %f, %f", properties.setProperty(prefix+"to_ground_xyzatr",String.format("%f, %f, %f, %f, %f, %f",
to_ground_xyzatr[0][0],to_ground_xyzatr[0][1],to_ground_xyzatr[0][2], to_ground_xyzatr[0][0],to_ground_xyzatr[0][1],to_ground_xyzatr[0][2],
to_ground_xyzatr[1][0],to_ground_xyzatr[1][1],to_ground_xyzatr[1][2])); to_ground_xyzatr[1][0],to_ground_xyzatr[1][1],to_ground_xyzatr[1][2]));
properties.setProperty(prefix+"disparity_plane",String.format("%f, %f, %f",disparity_plane[0],disparity_plane[1], disparity_plane[2]));
properties.setProperty(prefix+"frac_good",String.format("%f",frac_good)); properties.setProperty(prefix+"frac_good",String.format("%f",frac_good));
properties.setProperty(prefix+"lla",String.format("%f, %f, %f",lla[0],lla[1], lla[2])); properties.setProperty(prefix+"lla",String.format("%f, %f, %f",lla[0],lla[1], lla[2]));
properties.setProperty(prefix+"quat_enu",String.format("%f, %f, %f, %f", properties.setProperty(prefix+"quat_enu",String.format("%f, %f, %f, %f",
...@@ -206,6 +217,7 @@ public class GroundPlane { ...@@ -206,6 +217,7 @@ public class GroundPlane {
public static GroundPlane getGroundPlane( public static GroundPlane getGroundPlane(
CLTParameters clt_parameters, CLTParameters clt_parameters,
final QuadCLT ref_scene, final QuadCLT ref_scene,
final double [] gnd_disp, // if not null
final int debugLevel) { final int debugLevel) {
final double gnd_percent_low = 0.01; // discard lowest overliers final double gnd_percent_low = 0.01; // discard lowest overliers
final double gnd_percent_high= 0.2; // 0.9; // high ground percentile final double gnd_percent_high= 0.2; // 0.9; // high ground percentile
...@@ -227,6 +239,7 @@ public class GroundPlane { ...@@ -227,6 +239,7 @@ public class GroundPlane {
normal_damping, // final double normal_damping, normal_damping, // final double normal_damping,
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 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, dbg_title, // final String dbg_title,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
if (to_ground_xyzatr_frac == null) { if (to_ground_xyzatr_frac == null) {
...@@ -234,8 +247,26 @@ public class GroundPlane { ...@@ -234,8 +247,26 @@ public class GroundPlane {
} }
double [][] to_ground_xyzatr = {to_ground_xyzatr_frac[0],to_ground_xyzatr_frac[1]}; double [][] to_ground_xyzatr = {to_ground_xyzatr_frac[0],to_ground_xyzatr_frac[1]};
double frac_good = to_ground_xyzatr_frac[2][0]; double frac_good = to_ground_xyzatr_frac[2][0];
// Until fixed, run independently
double [] disparity_plane = getDisparityPlaneDualPass( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
ref_scene, // final QuadCLT ref_Clt,
gnd_percent_low, // final double gnd_percent_low,
gnd_percent_high, // final double gnd_percent_high,
gnd_max_high_over, // final double gnd_max_high_over,
min_good1, // final int min_good1, // minimal good tiles after pass1
max_abs_diff, // final double max_abs_diff, // maximal absolute disparity difference from the plane
max_rel_diff, // final double max_rel_diff, // maximal relative disparity difference from the plane
normal_damping, // final double normal_damping,
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+"-disp", // final String dbg_title,
debugLevel); // final int debugLevel)
return new GroundPlane( return new GroundPlane(
to_ground_xyzatr, // double [][] to_ground_xyzatr, to_ground_xyzatr, // double [][] to_ground_xyzatr,
disparity_plane, // double [] disparity_plane,
frac_good, // double frac_good, frac_good, // double frac_good,
ref_scene.getLla(), // double [] lla, ref_scene.getLla(), // double [] lla,
ref_scene.getQEnu()); // double [] quat_enu) { ref_scene.getQEnu()); // double [] quat_enu) {
...@@ -254,6 +285,7 @@ public class GroundPlane { ...@@ -254,6 +285,7 @@ public class GroundPlane {
final double normal_damping, final double normal_damping,
final int width, final int width,
final boolean [] good_tiles, // null or boolean[data.length] // should all be false final boolean [] good_tiles, // null or boolean[data.length] // should all be false
final double [] gnd_disp, // if not null
final String dbg_title, final String dbg_title,
final int debugLevel) { final int debugLevel) {
boolean print_histogram = debugLevel > 0; boolean print_histogram = debugLevel > 0;
...@@ -473,14 +505,14 @@ public class GroundPlane { ...@@ -473,14 +505,14 @@ public class GroundPlane {
to_ground_xyzatr_centered[1] to_ground_xyzatr_centered[1]
}; };
*/ */
if (dbg_title != null) { if ((dbg_title != null) || (gnd_disp != null)) {
// rotate ref_disparity and show // rotate ref_disparity and show
double [][] pXpYD_ground = OpticalFlow.transformToScenePxPyD( double [][] pXpYD_ground = OpticalFlow.transformToScenePxPyD(
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null) IN TILES 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 ref_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
virtual_camera_from_camera,// final double [][] scene_xyzatr, // camera center in world coordinates, camera orientation relative to world frame 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) ref_Clt); // final QuadCLT scene_QuadClt)
double [] gnd_disparity = new double [ref_disparity.length]; double [] gnd_disparity = (gnd_disp != null) ? gnd_disp : new double [ref_disparity.length];
Arrays.fill(gnd_disparity, Double.NaN); Arrays.fill(gnd_disparity, Double.NaN);
for (int i = 0; i <ref_disparity.length; i++) { for (int i = 0; i <ref_disparity.length; i++) {
double [] xyd = pXpYD_ground[i]; double [] xyd = pXpYD_ground[i];
...@@ -488,6 +520,7 @@ public class GroundPlane { ...@@ -488,6 +520,7 @@ public class GroundPlane {
gnd_disparity[i] = xyd[2]; gnd_disparity[i] = xyd[2];
} }
} }
if (dbg_title != null) {
double [][] wxyz_gnd = OpticalFlow.transformToWorldXYZ( double [][] wxyz_gnd = OpticalFlow.transformToWorldXYZ(
gnd_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity gnd_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
...@@ -526,15 +559,7 @@ public class GroundPlane { ...@@ -526,15 +559,7 @@ public class GroundPlane {
dbg_data, // double [][] data, dbg_data, // double [][] data,
width, // int width, // int tilesX, width, // int width, // int tilesX,
dbg_data[0].length/width);// int height, // int tilesY, dbg_data[0].length/width);// int height, // int tilesY,
/* }
ShowDoubleFloatArrays.showArrays(
dbg_data,
width,
height,
true,
dbg_title+"-ground_disparity",
dbg_titles);
*/
} }
ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered); ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered);
...@@ -547,10 +572,259 @@ public class GroundPlane { ...@@ -547,10 +572,259 @@ public class GroundPlane {
// return to_ground_xyzatr_centered; // return to_ground_xyzatr_centered;
} }
public static double [] getDisparityPlaneDualPass( // returns to_ground_xyzatr (rotated around the ground point nadir of teh drone)
final QuadCLT ref_Clt,
final double gnd_percent_low,
final double gnd_percent_high,
final double gnd_max_high_over,
final int min_good1, // minimal good tiles after pass1
final double max_abs_diff, // maximal absolute disparity difference from the plane
final double max_rel_diff, // maximal relative disparity difference from the plane
final double normal_damping,
final int width,
final boolean [] good_tiles, // null or boolean[data.length] // should all be false
final double [] gnd_disp, // if not null
final String dbg_title,
final int debugLevel) {
boolean print_histogram = debugLevel > 0;
double blur_frac = 0.01; // 0.03
double weight_frac = 0.3; // multiply weight by w= 1/(1 + (err/k_max_diff)^2)
boolean use_lma = true;
int num_bins = 1000;
String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
String dbg_title2 = (dbg_title != null) ? (dbg_title + "-stage2") : null;
String dbg_title3 = (dbg_title != null) ? (dbg_title + "-stage3") : null;
double [][] dls = ref_Clt.getDLS();
if (dls==null) {
return null;
}
double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
final double [] ref_disparity = ds[0];
final double [] ref_strength = ds[1];
final double inf_disp_ref = ref_Clt.getDispInfinityRef();
if (inf_disp_ref != 0) {
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): applying disparity at infinity correctio, subrtacting "+inf_disp_ref);
}
for (int i = 0; i < ref_disparity.length; i++) {
ref_disparity[i] -= inf_disp_ref;
}
}
final int height = ref_disparity.length / width;
final double [] xy0= {width/2, height/2};
double avg_val = weightedAverage (
ref_disparity, // double [] data, // may have NaNs
ref_strength); // double [] weights)
double hist_low = 0.0;
double hist_high = 1.5 * avg_val;
double [] hist = getHistogram1d(
ref_disparity, // double [] data, // may have NaNs
ref_strength, // double [] weights,
hist_low, // double hist_low,
hist_high, // double hist_high,
num_bins, // int num_bins,
debugLevel); // int debugLevel)
double [] fractions = {gnd_percent_low, gnd_percent_high};
double [] percentiles = getPercentiles(
hist, // double [] hist,
fractions); // double [] fractions)
double low_lim = hist_low + percentiles[0] * (hist_high-hist_low);
double high_lim = hist_low + percentiles[1] * (hist_high-hist_low);
if (high_lim > (low_lim + gnd_max_high_over)) {
high_lim = low_lim + gnd_max_high_over;
}
boolean [] mask = new boolean [ref_disparity.length];
int num_good1=0;
for (int i = 0; i < mask.length; i++) {
if ((ref_disparity[i] >= low_lim) && (ref_disparity[i] <= high_lim)) {
mask[i] = true;
num_good1++;
}
}
// first mask may be small (if the image was tilted a lot)
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): num_good1="+num_good1+
" low_lim="+low_lim+" high_lim="+high_lim+" fractions=["+fractions[0]+","+fractions[1]+"]");
}
double [] tilts_stage1 = getPlane( // {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
ref_disparity, // final double [] data,
mask, // final boolean [] mask,
ref_strength, // final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping, // final double normal_damping,
ref_Clt, // final QuadCLT dbg_scene,
dbg_title1); // String dbg_title)
if (tilts_stage1 == null) {
System.out.println("getPlaneDualPass(): tilts_stage1= null");
return null;
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass() stage1: tiltX="+tilts_stage1[0]+
" tiltY="+tilts_stage1[1]+" offset="+tilts_stage1[2]+" fraction good1="+(1.0*num_good1/ref_disparity.length)+" num_good1="+num_good1);
}
boolean [] mask2 = new boolean [ref_disparity.length];
int num_good2 = 0;
double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
if (debugLevel >-3) {
System.out.println("getPlaneDualPass() stage1: max_abs_diff="+max_abs_diff+", max_rel_diff="+max_rel_diff+
", low_lim="+ low_lim+ ", max_diff="+max_diff);
}
for (int i = 0; i < ref_disparity.length; i++) {
int x = i % width;
int y = i / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
double plane = tilts_stage1[0] * dx + tilts_stage1[1] * dy + tilts_stage1[2];
// data_tilted[i] = ref_disparity[i] - plane;
double d_tilted = ref_disparity[i] - plane;
if (Math.abs(d_tilted) <= max_diff) {
mask2[i] = true;
num_good2++;
}
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
}
// re-calculate disparity tilts with mask2
double [] tilts_stage2 = getPlane( // {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
ref_disparity, // final double [] data,
mask2, // final boolean [] mask,
ref_strength, // final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping, // final double normal_damping,
ref_Clt, // final QuadCLT dbg_scene,
dbg_title2); // String dbg_title)
if (tilts_stage2 == null) {
System.out.println("getPlaneDualPass(): tilts_stage2= null");
return null;
}
double [] data_tilted = new double[ref_disparity.length];
for (int i = 0; i < ref_disparity.length; i++) {
int x = i % width;
int y = i / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
double plane = tilts_stage2[0] * dx + tilts_stage2[1] * dy + tilts_stage2[2];
data_tilted[i] = ref_disparity[i] - plane;
}
// int num_bins2 = 100
double [] hist2 = getHistogram1d(
data_tilted, // double [] data, // may have NaNs
ref_strength, // double [] weights,
-max_diff, // double hist_low,
max_diff, // double hist_high,
num_bins, // int num_bins,
debugLevel); // int debugLevel)
double [] hist2_bkp = print_histogram? hist2.clone() : null;
(new DoubleGaussianBlur()).blur1Direction(
hist2, // double [] pixels,
hist2.length, // int width,
1, // int height,
blur_frac * num_bins, // double sigma,
0.02, // double accuracy,
true); // boolean xDirection
int hist_max = 0;
for (int i = 1; i < hist2.length; i++) {
if (hist2[i] > hist2[hist_max]) {
hist_max = i;
}
}
if (print_histogram) {
System.out.println("blur_frac="+blur_frac+", max_diff="+max_diff);
System.out.println("index, histogram,lpf_histogram");
for (int i = 0; i < hist2.length; i++) {
System.out.println(i+", "+hist2_bkp[i]+", "+hist2[i]);
}
}
double val = -max_diff + 2 * max_diff * hist_max/hist2.length;
boolean [] mask3 = new boolean[ref_disparity.length];
double max_diff2 = max_diff*max_diff;
double [] weights = ref_strength.clone();
int num_good3 = 0;
for (int i = 0; i < mask3.length; i++) {
double d = data_tilted[i] - val;
double k = d/(weight_frac * max_diff);
weights[i] *= 1.0/(1.0 + k*k);
if (d*d < max_diff2) {
mask3[i] = true;
num_good3++;
}
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): num_good3="+num_good3+ " max_diff="+max_diff);
}
double [] tilts_stage3 = getPlane( // {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
ref_disparity, // final double [] data,
mask3, // final boolean [] mask,
weights, // final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping, // final double normal_damping,
ref_Clt, // final QuadCLT dbg_scene,
dbg_title3); // String dbg_title)
if (tilts_stage3 == null) {
System.out.println("getPlaneDualPass(): tilts_stage3= null");
return null;
}
/*
double [] disparity_plane = new double[ref_disparity.length];
for (int i = 0; i < ref_disparity.length; i++) {
int x = i % width;
int y = i / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
disparity_plane[i] = tilts_stage3[0] * dx + tilts_stage3[1] * dy + tilts_stage3[2];
}
return disparity_plane;
*/
return new double [] {tilts_stage3[0],tilts_stage3[1], tilts_stage3[2], 1.0*num_good3/ref_disparity.length};
}
public static double [] rotateRefDisparity(
final QuadCLT ref_Clt,
double [] ref_disparity,
final double [][] scene_xyzatr) {
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
scene_xyzatr,// final double [][] scene_xyzatr, // camera center in world coordinates, camera orientation relative to world frame
ref_Clt); // final QuadCLT scene_QuadClt)
double [] rot_disparity =new double [ref_disparity.length];
Arrays.fill(rot_disparity, Double.NaN);
for (int i = 0; i <ref_disparity.length; i++) {
double [] xyd = pXpYD_ground[i];
if (xyd != null) {
rot_disparity[i] = xyd[2];
}
}
return rot_disparity;
}
public static double [][] prepareTerrainRender( public static double [][] prepareTerrainRender(
final CLTParameters clt_parameters, final CLTParameters clt_parameters,
final QuadCLT ref_Clt, final QuadCLT ref_Clt,
final int debugLevel) { final int debugLevel) {
boolean use_disparity_plane = debugLevel < 1000; // until fixed with 3D
GroundPlane gp = ref_Clt.getGroundPlane(); GroundPlane gp = ref_Clt.getGroundPlane();
if (gp == null) { if (gp == null) {
System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane == null."); System.out.println("prepareTerrainRender(): scene "+ref_Clt.toString()+" GroundPlane == null.");
...@@ -563,18 +837,43 @@ public class GroundPlane { ...@@ -563,18 +837,43 @@ public class GroundPlane {
} }
double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
double altitude = ground_xyzatr[0][2]; double altitude = ground_xyzatr[0][2];
double true_disparity = ref_Clt.getGeometryCorrection().getDisparityFromZ(-altitude); double true_disparity = ref_Clt.getGeometryCorrection().getDisparityFromZ(-altitude);
double corrected_disparity = true_disparity+ref_Clt.getDispInfinityRef(); double corrected_disparity = true_disparity+ref_Clt.getDispInfinityRef();
double [] terrain = new double [ref_Clt.getTilesX()*ref_Clt.getTilesY()]; int tilesX = ref_Clt.getTilesX();
Arrays.fill(terrain, corrected_disparity); int tilesY = ref_Clt.getTilesY();
double [] terrain_plane = new double [tilesX*tilesY];
if (use_disparity_plane) {
int [] xy0 = {tilesX/2, tilesY/2};
double inf_disparity = ref_Clt.getDispInfinityRef();
double [] disparity_tilts = gp.getDisparityTilts();
for (int i = 0; i < terrain_plane.length; i++) {
int x = i % tilesX;
int y = i / tilesX;
double dx = x-xy0[0];
double dy = y-xy0[1];
terrain_plane[i] = disparity_tilts[0] * dx + disparity_tilts[1] * dy + disparity_tilts[2] + inf_disparity;
}
} else {
Arrays.fill(terrain_plane, corrected_disparity); // +0.3); // 260); // 0.256);
}
double [][] virt_camera = virtCameraFromToGround( // virtual to reference
to_ground_xyzatr); // double [][] to_ground_xyzatr
double [][] stereo_xyzatr = ErsCorrection.invertXYZATR(virt_camera);
// double [][] center_rot_xyzatr = {new double [3],stereo_xyzatr[1]};
/*
double [] terrain_plane = rotateRefDisparity(
ref_Clt, // final QuadCLT ref_Clt,
terrain_plane, // double [] terrain,
stereo_xyzatr); // virt_camera); // final double [][] scene_xyzatr)
*/
ref_Clt.setTerrain( ref_Clt.setTerrain(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
terrain, // double [] terrain, terrain_plane, // double [] terrain,
debugLevel); // int debugLevel) debugLevel); // int debugLevel)
double [][] virt_camera = virtCameraFromToGround( return stereo_xyzatr;
to_ground_xyzatr); // double [][] to_ground_xyzatr
return virt_camera;
} }
public static double [][] virtCameraFromToGround( public static double [][] virtCameraFromToGround(
...@@ -583,6 +882,9 @@ public class GroundPlane { ...@@ -583,6 +882,9 @@ public class GroundPlane {
double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]}); double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz); double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
/// double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(virtual_camera_from_ground_xyz, ground_xyzatr);
// double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
virtual_camera_from_camera[1] = to_ground_xyzatr[1];
return virtual_camera_from_camera; return virtual_camera_from_camera;
} }
/* /*
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ b4d24102
...@@ -1771,7 +1771,7 @@ public class OpticalFlow { ...@@ -1771,7 +1771,7 @@ public class OpticalFlow {
// use offsX, offsY as fractional shift and for data interpolation // use offsX, offsY as fractional shift and for data interpolation
if (offsX >= .5) offsX -= 1.0; if (offsX >= .5) offsX -= 1.0;
if (offsY >= .5) offsY -= 1.0; if (offsY >= .5) offsY -= 1.0;
// flowXY_frac[iMTile] = new double [] {offsX, offsY}; // flowXY_frac[iMTile] = new double [] {offsX, offsY};
flowXY_frac[iMTile] = new double [] {-offsX, -offsY}; flowXY_frac[iMTile] = new double [] {-offsX, -offsY};
double min_tX = Double.NaN, max_tX = Double.NaN, min_tY = Double.NaN, max_tY = Double.NaN; double min_tX = Double.NaN, max_tX = Double.NaN, min_tY = Double.NaN, max_tY = Double.NaN;
for (int iY = 0; iY < fullTileSize; iY++) { for (int iY = 0; iY < fullTileSize; iY++) {
...@@ -1803,7 +1803,7 @@ public class OpticalFlow { ...@@ -1803,7 +1803,7 @@ public class OpticalFlow {
} }
int iwidth = imax_tX - imin_tX + 1; int iwidth = imax_tX - imin_tX + 1;
int iheight = imax_tY - imin_tY + 1; int iheight = imax_tY - imin_tY + 1;
//// if ((iwidth <= 0) || (iheight <= 0)) { //// if ((iwidth <= 0) || (iheight <= 0)) {
if ((iwidth <= 1) || (iheight <= 1)) { if ((iwidth <= 1) || (iheight <= 1)) {
System.out.println ("prepareSceneTiles(): iwidth ="+iwidth+", iheight ="+iheight+", min_tX="+min_tX+", imin_tY="+imin_tY+", max_tX="+max_tX+", imax_tY="+imax_tY); System.out.println ("prepareSceneTiles(): iwidth ="+iwidth+", iheight ="+iheight+", min_tX="+min_tX+", imin_tY="+imin_tY+", max_tX="+max_tX+", imax_tY="+imax_tY);
continue; continue;
...@@ -2064,7 +2064,7 @@ public class OpticalFlow { ...@@ -2064,7 +2064,7 @@ public class OpticalFlow {
} }
// System.out.println("fillTilesNans() DONE."); // System.out.println("fillTilesNans() DONE.");
return scene_tiles; return scene_tiles;
} }
...@@ -6450,9 +6450,11 @@ public class OpticalFlow { ...@@ -6450,9 +6450,11 @@ public class OpticalFlow {
// All done with refining orientations and disparities // All done with refining orientations and disparities
boolean air_mode_en = clt_parameters.imp.air_mode_en; // fast airplane mode boolean air_mode_en = clt_parameters.imp.air_mode_en; // fast airplane mode
if (air_mode_en) { if (air_mode_en) {
double [] gnd_disp = new double [master_CLT.getTilesX() * master_CLT.getTilesY()];
GroundPlane gp = GroundPlane.getGroundPlane( GroundPlane gp = GroundPlane.getGroundPlane(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
master_CLT, // final QuadCLT ref_scene, master_CLT, // final QuadCLT ref_scene,
gnd_disp, // final double [] gnd_disp, // if not null
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
master_CLT.setGroundPlane(gp); master_CLT.setGroundPlane(gp);
master_CLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) master_CLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
...@@ -6519,6 +6521,7 @@ public class OpticalFlow { ...@@ -6519,6 +6521,7 @@ public class OpticalFlow {
normal_damping, // final double normal_damping, normal_damping, // final double normal_damping,
master_CLT.getTilesX(),// final int width, master_CLT.getTilesX(),// final int width,
good_tiles, // final boolean [] good_tiles, // null or boolean[data.length] // should all be false good_tiles, // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
null, // final double [] gnd_disp, // if not null
dbg_title, // final String dbg_title, dbg_title, // final String dbg_title,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
...@@ -7365,18 +7368,25 @@ public class OpticalFlow { ...@@ -7365,18 +7368,25 @@ public class OpticalFlow {
if (export_terrain_sequence && !clt_parameters.imp.lock_position) { if (export_terrain_sequence && !clt_parameters.imp.lock_position) {
double [] stereo_offset = ZERO3; double [] stereo_offset = ZERO3;
double [] stereo_atr = null; double [] stereo_atr = null;
double [][] ground_normal_pose = GroundPlane.prepareTerrainRender( double [][] gnp = GroundPlane.prepareTerrainRender(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
master_CLT, // final QuadCLT ref_Clt, master_CLT, // final QuadCLT ref_Clt,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
if (ground_normal_pose != null) { double [][] ignp = null;
stereo_offset = new double [] {-ground_normal_pose[0][0],-ground_normal_pose[0][1],-ground_normal_pose[0][2]}; if (gnp != null) {
stereo_atr = ground_normal_pose[1]; ignp = ErsCorrection.invertXYZATR(gnp);
// stereo_offset = new double [] {-ground_normal_pose[0][0],-ground_normal_pose[0][1],-ground_normal_pose[0][2]};
stereo_offset = gnp[0]; // stereo_pose[0];
// stereo_atr = gnp[1]; // stereo_pose[1];
stereo_atr = new double [] {-ignp[1][0],-ignp[1][1],-ignp[1][2]}; // stereo_pose[1];
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("Using airplane mode terrane as a single horizontal plane"); System.out.println("Using airplane mode terrane as a single horizontal plane");
System.out.println("stereo_offset = ["+stereo_offset[0]+", "+stereo_offset[1]+", "+stereo_offset[2]+"]");
System.out.println("stereo_atr = ["+stereo_atr[0]+ ", "+stereo_atr[1]+ ", "+stereo_atr[2]+ "]");
// TODO add provisions to the non-flat terrain -modify terrain slice, keeping ground_normal_pose as a horizontal plane. // TODO add provisions to the non-flat terrain -modify terrain slice, keeping ground_normal_pose as a horizontal plane.
System.out.println("ignp[0] = ["+ignp[0][0]+", "+ignp[0][1]+", "+ignp[0][2]+"]");
System.out.println("ignp[1] = ["+ignp[1][0]+", "+ignp[1][1]+", "+ignp[1][2]+"]");
} }
} }
combo_dsn_final =master_CLT.restoreComboDSI(true); combo_dsn_final =master_CLT.restoreComboDSI(true);
if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) { if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) {
...@@ -7384,8 +7394,8 @@ public class OpticalFlow { ...@@ -7384,8 +7394,8 @@ public class OpticalFlow {
} else { } else {
double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN]; double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN];
/* /* */
String scenes_suffix_test = master_CLT.getImageName()+"-TERRAIN-TEST"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN"; String scenes_suffix_test = master_CLT.getImageName()+"-TERRAIN-DISP"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_test = renderSceneSequence( ImagePlus imp_terrain_test = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas, master_CLT.hasCenterClt(), // boolean mode_cuas,
...@@ -7409,7 +7419,9 @@ public class OpticalFlow { ...@@ -7409,7 +7419,9 @@ public class OpticalFlow {
master_CLT.saveImagePlusInModelDirectory( master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_test); // imp_scenes); // ImagePlus imp) imp_terrain_test); // imp_scenes); // ImagePlus imp)
*/ /*
String scenes_suffix = master_CLT.getImageName()+"-TERRAIN"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN"; String scenes_suffix = master_CLT.getImageName()+"-TERRAIN"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain = renderSceneSequence( ImagePlus imp_terrain = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
...@@ -7422,8 +7434,9 @@ public class OpticalFlow { ...@@ -7422,8 +7434,9 @@ public class OpticalFlow {
null, // Rectangle fov_tiles, null, // Rectangle fov_tiles,
1, // int mode3d, 1, // int mode3d,
false, // boolean toRGB, false, // boolean toRGB,
stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z} ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas) null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][] {stereo_offset,stereo_atr}, //
1, // int sensor_mask, 1, // int sensor_mask,
scenes_suffix, // String suffix, scenes_suffix, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity, terrain_disparity, // selected_disparity, // double [] ref_disparity,
...@@ -7434,7 +7447,328 @@ public class OpticalFlow { ...@@ -7434,7 +7447,328 @@ public class OpticalFlow {
master_CLT.saveImagePlusInModelDirectory( master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain); // imp_scenes); // ImagePlus imp) imp_terrain); // imp_scenes); // ImagePlus imp)
String scenes_suffix_mod = master_CLT.getImageName()+"-TERRAIN-MOD"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_mod = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // new double [] {-ignp[0][0],-ignp[0][1],-ignp[0][2]}, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][] {{-ignp[0][0],-ignp[0][1],-ignp[0][2]},stereo_atr}, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
1, // int sensor_mask,
scenes_suffix_mod, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_mod); // imp_scenes); // ImagePlus imp)
String scenes_suffix_mod1 = master_CLT.getImageName()+"-TERRAIN-MOD1"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_mod1 = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // new double [] {ignp[0][0],-ignp[0][1],ignp[0][2]}, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][] {{ignp[0][0],-ignp[0][1],ignp[0][2]}, stereo_atr},// stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
1, // int sensor_mask,
scenes_suffix_mod1, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_mod1); // imp_scenes); // ImagePlus imp)
String scenes_suffix_mod2 = master_CLT.getImageName()+"-TERRAIN-MOD2"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_mod2 = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // new double [] {-gnp[0][0], gnp[0][1], -gnp[0][2]}, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][]{{-gnp[0][0], gnp[0][1], -gnp[0][2]},stereo_atr},
1, // int sensor_mask,
scenes_suffix_mod2, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_mod2); // imp_scenes); // ImagePlus imp)
String scenes_suffix_move = master_CLT.getImageName()+"-TERRAIN-MOVE"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_move = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][] {stereo_offset, ZERO3},
1, // int sensor_mask,
scenes_suffix_move, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_move); // imp_scenes); // ImagePlus imp)
String scenes_suffix_rot = master_CLT.getImageName()+"-TERRAIN-ROT"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_rot = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
new double [][] {ZERO3,stereo_atr},
1, // int sensor_mask,
scenes_suffix_rot, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_rot); // imp_scenes); // ImagePlus imp)
*/
/*
String scenes_suffix_inv = master_CLT.getImageName()+"-TERRAIN-INV"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] {-stereo_offset[0],-stereo_offset[1],-stereo_offset[1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] {-stereo_atr[0], -stereo_atr[1], -stereo_atr[1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv); // imp_scenes); // ImagePlus imp)
String scenes_suffix_inv2 = master_CLT.getImageName()+"-TERRAIN-INV2"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv2 = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] {-stereo_offset[0],-stereo_offset[1],-stereo_offset[1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] { stereo_atr[0], stereo_atr[1], stereo_atr[1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv2, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv2); // imp_scenes); // ImagePlus imp)
String scenes_suffix_inv3 = master_CLT.getImageName()+"-TERRAIN-INV3"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv3 = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] { stereo_offset[0], stereo_offset[1], stereo_offset[1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] {-stereo_atr[0], -stereo_atr[1], -stereo_atr[1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv3, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv3); // imp_scenes); // ImagePlus imp)
// ************** same for w/o inversion ****************
String scenes_suffix_ni = master_CLT.getImageName()+"-TERRAIN-NI"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_ni = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
gnp[0], // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
gnp[1], // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_ni, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_ni); // imp_scenes); // ImagePlus imp)
String scenes_suffix_inv_ni = master_CLT.getImageName()+"-TERRAIN-INV_NI"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv_ni = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] {-gnp[0][0], -gnp[0][1], -gnp[0][1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] {-gnp[1][0], -gnp[1][1], -gnp[1][1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv_ni, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv_ni); // imp_scenes); // ImagePlus imp)
String scenes_suffix_inv2_ni = master_CLT.getImageName()+"-TERRAIN-INV2-NI"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv2_ni = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] {-gnp[0][0], -gnp[0][1], -gnp[0][1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] { gnp[1][0], gnp[1][1], gnp[1][1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv2_ni, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv2_ni); // imp_scenes); // ImagePlus imp)
String scenes_suffix_inv3_ni = master_CLT.getImageName()+"-TERRAIN-INV3-NI"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_inv3_ni = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
new double [] { gnp[0][0], gnp[0][1], gnp[0][1]}, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
new double [] {-gnp[1][0], -gnp[1][1], -gnp[1][1]}, //stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
1, // int sensor_mask,
scenes_suffix_inv3_ni, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_inv3_ni); // imp_scenes); // ImagePlus imp)
*/
} }
if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) { if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) {
System.out.println ("No terrain data available"); System.out.println ("No terrain data available");
} else { } else {
...@@ -8904,12 +9238,53 @@ public class OpticalFlow { ...@@ -8904,12 +9238,53 @@ public class OpticalFlow {
return min_max_xyzatr; return min_max_xyzatr;
} }
public static ImagePlus renderSceneSequence( public static ImagePlus renderSceneSequence(
CLTParameters clt_parameters, CLTParameters clt_parameters,
boolean mode_cuas, boolean mode_cuas,
boolean um_mono, boolean um_mono,
boolean caculate_average, // now only with float pixels boolean calculate_average, // now only with float pixels
float [][] average_slice, // [channel][pixel]
boolean subtract_average,
int running_average,
Rectangle fov_tiles,
int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG
boolean toRGB,
double [] stereo_xyz, // offset reference camera {x,y,z}
double [] stereo_atr_in, // offset reference orientation (cuas)
int sensor_mask,
String suffix_in,
double [] ref_disparity,
QuadCLT [] quadCLTs,
QuadCLT ref_scene, // int ref_index,
int threadsMax,
int debugLevel) {
return renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
mode_cuas, // boolean mode_cuas,
um_mono, // boolean um_mono,
calculate_average, // boolean calculate_average, // now only with float pixels
average_slice, //float [][] average_slice, // [channel][pixel]
subtract_average, // boolean subtract_average,
running_average, // int running_average,
fov_tiles, // Rectangle fov_tiles,
mode3d, // int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG
toRGB, // boolean toRGB,
stereo_xyz, // double [] stereo_xyz, // offset reference camera {x,y,z}
stereo_atr_in, // double [] stereo_atr_in, // offset reference orientation (cuas)
null, // double [][] post_rotate,
sensor_mask, // int sensor_mask,
suffix_in, // String suffix_in,
ref_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
ref_scene, // QuadCLT ref_scene, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel)
}
public static ImagePlus renderSceneSequence(
CLTParameters clt_parameters,
boolean mode_cuas,
boolean um_mono,
boolean calculate_average, // now only with float pixels
float [][] average_slice, // [channel][pixel] float [][] average_slice, // [channel][pixel]
boolean subtract_average, boolean subtract_average,
int running_average, int running_average,
...@@ -8918,6 +9293,7 @@ public class OpticalFlow { ...@@ -8918,6 +9293,7 @@ public class OpticalFlow {
boolean toRGB, boolean toRGB,
double [] stereo_xyz, // offset reference camera {x,y,z} double [] stereo_xyz, // offset reference camera {x,y,z}
double [] stereo_atr_in, // offset reference orientation (cuas) double [] stereo_atr_in, // offset reference orientation (cuas)
double [][] post_rotate,
int sensor_mask, int sensor_mask,
String suffix_in, String suffix_in,
double [] ref_disparity, double [] ref_disparity,
...@@ -8925,17 +9301,17 @@ public class OpticalFlow { ...@@ -8925,17 +9301,17 @@ public class OpticalFlow {
QuadCLT ref_scene, // int ref_index, QuadCLT ref_scene, // int ref_index,
int threadsMax, int threadsMax,
int debugLevel) { int debugLevel) {
boolean insert_average = ((average_slice != null) && ((average_slice[0] != null)) )|| caculate_average; // now only with float pixels boolean insert_average = ((average_slice != null) && ((average_slice[0] != null)) )|| calculate_average; // now only with float pixels
boolean corr_raw_ers = true; boolean corr_raw_ers = true;
double [] stereo_atr = (stereo_atr_in != null)? stereo_atr_in: ZERO3; // maybe later play with rotated camera double [] stereo_atr = (stereo_atr_in != null)? stereo_atr_in: ZERO3; // maybe later play with rotated camera
/// boolean mode_cuas = (stereo_atr[0] != 0) || (stereo_atr[1] != 0) || (stereo_atr[2] != 0); /// boolean mode_cuas = (stereo_atr[0] != 0) || (stereo_atr[1] != 0) || (stereo_atr[2] != 0);
// boolean um_mono = clt_parameters.imp.um_mono; // boolean um_mono = clt_parameters.imp.um_mono;
double um_sigma = clt_parameters.imp.um_sigma; double um_sigma = clt_parameters.imp.um_sigma;
double um_weight = clt_parameters.imp.um_weight; double um_weight = clt_parameters.imp.um_weight;
boolean mb_en = clt_parameters.imp.mb_en && (fov_tiles==null) && (mode3d > 0); boolean mb_en = clt_parameters.imp.mb_en && (fov_tiles==null) && (mode3d > 0);
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
double mb_max_gain = clt_parameters.imp.mb_max_gain; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel double mb_max_gain = clt_parameters.imp.mb_max_gain; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
/// insert_average = (mode3d == 1); // merged /// insert_average = (mode3d == 1); // merged
insert_average &= (mode3d == 1); // merged insert_average &= (mode3d == 1); // merged
if (mode3d != 1) { if (mode3d != 1) {
running_average = 0; running_average = 0;
...@@ -8960,7 +9336,7 @@ public class OpticalFlow { ...@@ -8960,7 +9336,7 @@ public class OpticalFlow {
} }
if (mode_cuas) { if (mode_cuas) {
suffix+="-CUAS"; // add properties too? include offsets suffix+="-CUAS"; // add properties too? include offsets
/// suffix+=String.format("%6f:%6f:%6f", stereo_atr[0],stereo_atr[1],stereo_atr[2]); /// suffix+=String.format("%6f:%6f:%6f", stereo_atr[0],stereo_atr[1],stereo_atr[2]);
} }
if (!mb_en) { if (!mb_en) {
suffix+="-NOMB"; // no motion blur suffix+="-NOMB"; // no motion blur
...@@ -9059,6 +9435,15 @@ public class OpticalFlow { ...@@ -9059,6 +9435,15 @@ public class OpticalFlow {
scene_xyz = combo_xyzatr[0]; scene_xyz = combo_xyzatr[0];
scene_atr = combo_xyzatr[1]; scene_atr = combo_xyzatr[1];
} }
if (!mode_cuas && (post_rotate != null)) {
double [][] combo_xyzatr = ErsCorrection.combineXYZATR(
scene_xyz, // double [] reference_xyz,
scene_atr, // double [] reference_atr,
post_rotate[0], // double [] scene_xyz,
post_rotate[1]); // double [] scene_atr)
scene_xyz = combo_xyzatr[0];
scene_atr = combo_xyzatr[1];
}
int sm = merge_all? -1: sensor_mask; int sm = merge_all? -1: sensor_mask;
ImagePlus imp_scene = null; ImagePlus imp_scene = null;
double [][] dxyzatr_dt = null; double [][] dxyzatr_dt = null;
...@@ -9217,35 +9602,6 @@ public class OpticalFlow { ...@@ -9217,35 +9602,6 @@ public class OpticalFlow {
} }
} }
} }
/*
// Apply unsharp mask here, in parallel
if (um_mono && !toRGB) {
final ImageStack fstack_scenes = stack_scenes;
final int nSlices = fstack_scenes.getSize();
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 nSlice = ai.getAndIncrement(); nSlice < nSlices; nSlice = ai.getAndIncrement()) {
FloatProcessor fp = (FloatProcessor) fstack_scenes.getProcessor(nSlice+1);
float [] fpixels = (float[]) fstack_scenes.getPixels(nSlice+1);
float [] fpixels_orig = fpixels.clone();
(new GaussianBlur()).blurFloat(
fp, // FloatProcessor ip,
um_sigma, // double sigmaX,
um_sigma, // double sigmaY,
0.01); // double accuracy)
for (int i = 0; i < fpixels.length; i++) {
fpixels[i] = fpixels_orig[i] - fum_weight * fpixels[i];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
*/
ImagePlus imp_scenes = new ImagePlus(suffix, stack_scenes); ImagePlus imp_scenes = new ImagePlus(suffix, stack_scenes);
imp_scenes.getProcessor().resetMinAndMax(); imp_scenes.getProcessor().resetMinAndMax();
// Apply unsharp mask here, in parallel // Apply unsharp mask here, in parallel
......
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