Skip to content

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

working on ground planes

parent 50aab4a2
Expand all Show whitespace changes
Inline Side-by-side
Showing with 856 additions and 1 deletion
src/main/java/com/elphel/imagej/tileprocessor/GroundPlane.java 0 → 100644
View file @ 0107a23d
This diff is collapsed.
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 0107a23d
......@@ -6415,6 +6415,58 @@ public class OpticalFlow {
boolean use_parallel_proj = false; // true;
int [] hdr_whs = new int[3];
double [] hdr_x0y0 = new double[2];
double [][] dls = master_CLT.getDLS();
if (dls==null) {
return null;
}
double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
final double gnd_percent_low = 0.01; // discard lowest overliers
final double gnd_percent_high= 0.9; // high ground percentile
final double gnd_max_high_over = 0.5; // pix = make dependent on average disparity?
final int min_good1 = 50; // minimal good tiles after pass1
final double max_abs_diff = 0.05; // maximal absolute disparity difference from the plane
final double max_rel_diff = 0.1; // maximal relative disparity difference from the plane
final double normal_damping = 0.001; // pull to horizontal if not enough data
final boolean [] good_tiles = new boolean[ds[0].length];
String dbg_title =master_CLT.getImageName()+"-ground tilts";
double [] plane_tilts = GroundPlane.getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good
ds[0], // final double [] data,
ds[1], // final double [] weights,
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,
master_CLT.getTilesX(),// final int width,
good_tiles, // final boolean [] good_tiles, // null or boolean[data.length] // should all be false
dbg_title, // final String dbg_title,
debugLevel); // final int debugLevel)
if (debugLevel >-3) {
System.out.println("Ground plane: tiltX="+plane_tilts[0]+
" tiltY="+plane_tilts[1]+" offset="+plane_tilts[2]+" fraction good="+plane_tilts[3]);
}
double [][] to_ground_xyzatr_airplane = master_CLT.getGroundNoImsAirplane(
clt_parameters, // CLTParameters clt_parameters,
use_lma, // boolean use_lma,
use_parallel_proj, // boolean use_parallel_proj,
range_disparity_offset,// double range_disparity_offset
discard_low, // double discard_low, // fraction of all pixels
discard_high, // double discard_high, // fraction of all pixels
discard_adisp, // double discard_adisp, // discard above/below this fraction of average height
discard_rdisp, // double discard_rdisp // discard above/below this fraction of average height
pix_size, // double pix_size, // in meters
max_image_width, // int max_image_width // increase pixel size as a power of 2 until image fits
hdr_x0y0, // double [] x0y0, // initialize to double[2] to return width, height
hdr_whs, // int [] whs, // initialize to int[3] to return {width, height, scale reduction}
debugLevel); // int debug_level
double [][] to_ground_xyzatr = master_CLT.getGroundNoIms(
clt_parameters, // CLTParameters clt_parameters,
use_lma, // boolean use_lma,
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 0107a23d
......@@ -2837,6 +2837,231 @@ public class QuadCLTCPU {
return to_ground_xyzatr; // from the camera coordinates to in-plane coordiantes
}
public double [][] getGroundNoImsAirplane( // return to_ground_xyzatr;
CLTParameters clt_parameters,
boolean use_lma,
boolean use_parallel_proj,
double disparity_offset,
double discard_low, // fraction of all pixels
double discard_high, // fraction of all pixels
double discard_adisp, // discard above/below this fraction of average height
double discard_rdisp, // discard above/below this fraction of average height
double pix_size, // in meters
int max_image_width,// increase pixel size as a power of 2 until image fits
double [] x0y0, // initialize to double[2] to return width, height
int [] whs, // initialize to int[3] to return {width, height, scale reduction}
int debug_level
) {
double [] z_tilts = null;
double normal_damping = 0.001; // pull to horizontal if not enough data
double hist_rlow = 0.5;
double hist_rhigh = 2.0;
int min_good = 20; //number of good tiles
double rel_hight = 0.2; // when calculating scale, ignore objects far from plane
int num_bins = 1000;
double [][] dls = getDLS();
if (dls==null) {
return null;
}
double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
double sw=0, swd=0;
for (int i = 0; i < ds[0].length; i++) if (!Double.isNaN(ds[0][i])){
ds[0][i] -= disparity_offset;
sw += ds[1][i];
swd += ds[0][i] * ds[1][i];
}
double disp_avg = swd/sw;
double hist_low = 0; // hist_rlow * disp_avg;
double hist_high = 2.0 * disp_avg; // hist_rhigh * disp_avg;
double k = num_bins / (hist_high - hist_low);
double [] hist = new double [num_bins];
sw = 0;
swd = 0;
for (int i = 0; i < ds[0].length; i++) if (!Double.isNaN(ds[0][i])){
double d = ds[0][i];
double w = ds[1][i];
int bin = (int) (k * (d - hist_low));
if ((bin >= 0) && (bin < num_bins)) {
sw += w;
swd += w * d;
hist[bin] += w;
}
}
if (sw == 0) {
if (debug_level > -3) {
System.out.println("Could not find ground - sum weights==0.0");
}
return null;
}
double dl = discard_low * sw;
double dh = discard_high * sw;
double sh = 0.0;
int indx = 0;
for (; indx < num_bins; indx++) {
sh+= hist[indx];
if (sh >= dl) break;
}
double shp = sh- hist[indx];
// step back from the overshoot indx
double thr_low = hist_low+ (indx - (sh - dl)/(sh-shp))/num_bins *(hist_high-hist_low);
indx = num_bins-1;
sh = 0.0;
for (; indx >= 0; indx--) {
sh += hist[indx];
if (sh >= dh) {
break;
}
}
shp = sh- hist[indx];
double thr_high = hist_low+ (indx + (sh - dh)/(sh-shp))/num_bins *(hist_high-hist_low);
int numgood = 0;
boolean [] good = new boolean[ds[0].length];
sw = 0.0;
swd = 0.0;
for (int i = 0; i < ds[0].length; i++) if (
(ds[1][i] > 0) && (ds[0][i] >= thr_low) && (ds[0][i] <= thr_high)) {
good[i] = true;
numgood++;
sw += ds[1][i];
swd += ds[1][i] * ds[0][i];
}
if (numgood < min_good) {
if (debug_level > -3) {
System.out.println("Could not find ground - number of good tiles = "+numgood+" < "+min_good);
}
return null; // too few good
}
// fit plane
double [] ref_disparity = ds[0].clone();
for (int i = 0; i < ref_disparity.length; i++) {
if (!good[i]) {
ref_disparity[i] = Double.NaN;
}
}
double [][] wxyz = OpticalFlow.transformToWorldXYZ(
ref_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
(QuadCLT) this, // final QuadCLT quadClt, // now - may be null - for testing if scene is rotated ref
THREADS_MAX); // int threadsMax)
numgood = 0;
for (int i = 0; i < wxyz.length; i++) if (wxyz[i] != null) {
numgood++;
}
double [][][] mdata = new double [numgood][][];
int mindx = 0;
for (int i = 0; i < wxyz.length; i++) if (wxyz[i] != null){
mdata[mindx] = new double[3][];
mdata[mindx][0] = new double [2];
mdata[mindx][0][0] = wxyz[i][0];
mdata[mindx][0][1] = wxyz[i][1];
mdata[mindx][1] = new double [1];
mdata[mindx][1][0] = wxyz[i][2];
mdata[mindx][2] = new double [1];
mdata[mindx][2][0] = ds[1][i];
mindx++;
}
PolynomialApproximation pa = new PolynomialApproximation();
double [] damping = new double [] {normal_damping, normal_damping};
double[][] approx2d = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // double [] damping, null OK
-1); // debug level
z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
if (debug_level > -3) {
System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]);
}
double [][] ground_xyzatr = new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
// It is approximate for small angles. OK for now
double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(ground_xyzatr);
double [][] ground_xyzatr1 = new double [][] {{0, 0, z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
double [][] to_ground_xyzatr1 = ErsCorrection.invertXYZATR(ground_xyzatr1);
// recalculate coordinates for all pixels including weak ones
ref_disparity = dls[0].clone();
for (int i = 0; i < ref_disparity.length; i++) {
if (!Double.isNaN(ref_disparity[i])) {
ref_disparity[i] -= disparity_offset;
if (ref_disparity[i] <=0) {
ref_disparity[i] = Double.NaN;
}
}
}
wxyz = OpticalFlow.transformToWorldXYZ(
ref_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
(QuadCLT) this, // final QuadCLT quadClt, // now - may be null - for testing if scene is rotated ref
THREADS_MAX); // int threadsMax)
double [][] plane_xyz=new double[wxyz.length][];
double [] x_min_max = null; // new int[2];
double [] y_min_max = null; // new int[2];
for (int i = 0; i < wxyz.length; i++) if (wxyz[i] != null) {
double [] wxyz3=new double[] {wxyz[i][0],wxyz[i][1],wxyz[i][2]};
plane_xyz[i] =ErsCorrection.applyXYZATR(
to_ground_xyzatr, // double [][] reference_xyzatr,
wxyz3); // double [][] scene_xyzatr)
double x = plane_xyz[i][0];
double y = plane_xyz[i][1];
double z = plane_xyz[i][2];
if (use_parallel_proj) {
z+=to_ground_xyzatr1[0][2];
}
//TODO - for use_parallel_proj subtract transformed Z and
if (Math.abs(z)/Math.abs(z_tilts[0]) > rel_hight){
continue; // outlier Z
}
if (x_min_max == null) x_min_max = new double[] {x,x};
if (y_min_max == null) y_min_max = new double[] {y,y};
if (x < x_min_max[0]) x_min_max[0] = x;
else if (x > x_min_max[1]) x_min_max[1] = x;
if (y < y_min_max[0]) y_min_max[0] = y;
else if (y > y_min_max[1]) y_min_max[1] = y;
}
if ((x_min_max == null) || (y_min_max == null)) {
return null; // no points at all?
}
if (x0y0!=null) {
x0y0[0] = x_min_max[0]; // null
x0y0[1] = y_min_max[0];
}
int scale = 0;
double use_pix_size = pix_size;
int width, height;
do {
scale = (scale==0) ? 1 : scale * 2;
use_pix_size = scale * pix_size;
width = (int) Math.floor((x_min_max[1]-x_min_max[0])/use_pix_size) + 1;
height = (int) Math.floor((y_min_max[1]-y_min_max[0])/use_pix_size) + 1;
} while ((width > max_image_width) || (height > max_image_width));
if (whs != null) {
whs[0] = width;
whs[1] = height;
whs[2] = scale;
}
if (debug_level > -2) {
System.out.println("Parameters for rendering:top left corner=["+x0y0[0]+"m, "+x0y0[1]+"m]");
System.out.println(" : width="+whs[0]+"pix, height="+whs[1]+"pix, scale level="+whs[2]);
System.out.println(" : pixel size: ="+(1000*use_pix_size)+"mm");
}
double [] dronexyz =ErsCorrection.applyXYZATR(
to_ground_xyzatr, // double [][] reference_xyzatr,
new double [3] ); // double [][] scene_xyzatr)
if (debug_level > -2) {
System.out.println("Drone position relative to the ground plane: x="+
dronexyz[0]+"m, y="+dronexyz[1]+"m, z="+dronexyz[2]+"m");
}
return to_ground_xyzatr; // from the camera coordinates to in-plane coordiantes
}
public double [][] getGroundIms( // return to_ground_xyzatr;
CLTParameters clt_parameters,
boolean use_lma,
......
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