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
Hide 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
package com.elphel.imagej.tileprocessor;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
public class GroundPlane {
public static double [][] getPlaneDualPassMetric( // 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 String dbg_title,
final int debugLevel) {
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;
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 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++;
}
}
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,
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);
}
double [] data_tilted = new double[ref_disparity.length];
boolean [] mask2 = new boolean [ref_disparity.length];
int num_good2 = 0;
double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
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;
if (Math.abs(data_tilted[i]) <= max_diff) {
mask2[i] = true;
num_good2++;
}
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
}
// go to world metric with the same mask
double [][] wxyz = OpticalFlow.transformToWorldXYZ(
ref_disparity, // final double [] disparity_ref, // invalid tiles - NaN in disparity
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
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
mask2, // final boolean [] mask,
ref_strength, // final double [] weight,
width, // final int width,
normal_damping, // final double normal_damping,
dbg_title+"-metric", // final String dbg_title,
debugLevel); // final int debugLevel) {
double [][] to_ground_xyz = {to_ground_xyzatr[0],new double[3]};
double [][] to_ground_atr = {new double[3], to_ground_xyzatr[0]};
double [][] from_ground_xyz = ErsCorrection.invertXYZATR(to_ground_xyz);
// calculate rotated around ground relative to the reference frame (see if z-sign is correct, or swap to/from
//combineXYZATR
double [][] to_ground_xyzatr1 = ErsCorrection.combineXYZATR(to_ground_xyz, to_ground_atr);
double [][] to_ground_xyzatr_centered = ErsCorrection.combineXYZATR(to_ground_xyzatr1, from_ground_xyz);
if (dbg_title != null) {
// rotate ref_disparity and show
}
return to_ground_xyzatr_centered;
}
public static double [] getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good
final double [] data,
final double [] weights,
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 String dbg_title,
final int debugLevel) {
int num_bins = 1000;
String dbg_title1 = (dbg_title != null) ? (dbg_title + "-stage1") : null;
String dbg_title2 = (dbg_title != null) ? (dbg_title + "-stage2") : null;
final int height = data.length / width;
final double [] xy0= {width/2, height/2};
double avg_val = weightedAverage (
data, // double [] data, // may have NaNs
weights); // double [] weights)
double hist_low = 0.0;
double hist_high = 1.5 * avg_val;
double [] hist = getHistogram1d(
data, // double [] data, // may have NaNs
weights, // 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 [data.length];
int num_good1=0;
for (int i = 0; i < mask.length; i++) {
if ((data[i] >= low_lim) && (data[i] <= high_lim)) {
mask[i] = true;
num_good1++;
}
}
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
data, // final double [] data,
mask, // final boolean [] mask,
weights, // final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping, // final double normal_damping,
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/data.length)+" num_good1="+num_good1);
}
double [] data_tilted = new double[data.length];
boolean [] mask2 = new boolean [data.length];
int num_good2 = 0;
double max_diff = Math.min(max_abs_diff, max_rel_diff * low_lim);
for (int i = 0; i < data.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] = data[i] - plane;
if (Math.abs(data_tilted[i]) <= max_diff) {
mask2[i] = true;
num_good2++;
}
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): num_good2="+num_good2+ " max_diff="+max_diff);
}
double [] tilts_stage2 = getPlane( // {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
data_tilted, // final double [] data,
mask2, // final boolean [] mask,
weights, // final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping, // final double normal_damping,
dbg_title2); // String dbg_title)
if (tilts_stage2 == null) {
System.out.println("getPlaneDualPass(): tilts_stage2= null, using results of stage1");
double [] rslt_tilts = {tilts_stage1[0], tilts_stage1[1], tilts_stage1[2], 1.0 * num_good2/data.length};
return rslt_tilts;
}
if (debugLevel >-3) {
System.out.println("getPlaneDualPass() stage2: tiltX="+tilts_stage2[0]+
" tiltY="+tilts_stage2[1]+" offset="+tilts_stage2[2]+" fraction good2="+(1.0*num_good2/data.length)+" num_good2="+num_good2);
}
double [] rslt_tilts = {tilts_stage1[0]+tilts_stage2[0],tilts_stage1[1]+tilts_stage2[1],tilts_stage1[2]+tilts_stage2[2], 0};
int num_good =0;
if (good_tiles != null) {
Arrays.fill(good_tiles, false);
}
for (int i = 0; i < data.length; i++) {
int x = i % width;
int y = i / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
double plane = rslt_tilts[0] * dx + rslt_tilts[1] * dy + rslt_tilts[2];
double diff = data[i] - plane;
if (Math.abs(diff) <= max_diff) {
num_good++;
if (good_tiles != null) {
good_tiles[i] = true;
}
}
}
rslt_tilts[3] = 1.0*num_good/width/height;
if (debugLevel >-3) {
System.out.println("getPlaneDualPass(): tiltX="+rslt_tilts[0]+
" tiltY="+rslt_tilts[1]+" offset="+rslt_tilts[2]+" fraction good="+rslt_tilts[3]+" num_good="+num_good);
}
if (dbg_title != null) {
String [] dbg_titles={"disparity","tilted","masked","strength"};
double [][] dbg_img = new double [dbg_titles.length][width*height];
for (int i = 0; i < data.length; i++) {
int x = i % width;
int y = i / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
double plane = rslt_tilts[0] * dx + rslt_tilts[1] * dy; // + rslt_tilts[2];
dbg_img[0][i]= data[i];
dbg_img[1][i]= data[i]-plane;
dbg_img[2][i]= ((good_tiles != null) && good_tiles[i]) ? (data[i]-plane) : Double.NaN;
if (weights != null) {
dbg_img[3][i]= weights[i];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
width,
height,
true,
dbg_title+"-result",
dbg_titles);
}
return rslt_tilts;
}
// copied from OrthoMap.java
/**
* Fit a plane to the input data, relative to the specified point in Rectangle wnd
* @param data double input data array
* @param mask mask that disables somew input data (or null)
* @param weight optional weights array (same size as data and mask)
* @param width width of the rectangle
* @param xy0 a pair of x0, y0 - origin where the plane is referenced too
* @return double array of {tiltx, tilty, offset, xy0[0] and xy0[1]).
*/
public static double [] getPlane(
final double [] data,
final boolean [] mask,
final double [] weight,
final int width,
final double [] xy0,
final double normal_damping
) {
return getPlane(
data, // final double [] data,
mask, // final boolean [] mask,
weight, //final double [] weight,
width, // final int width,
xy0, // final double [] xy0,
normal_damping,
null); // String dbg_title)
}
public static double [] getPlane(
final double [] data,
final boolean [] mask,
final double [] weight,
final int width,
final double [] xy0,
final double normal_damping,
String dbg_title) {
double [] damping = Double.isNaN(normal_damping) ? null : (new double [] {normal_damping, normal_damping});
final double [][][] mdatai = new double [data.length][][];
AtomicInteger anum_good = new AtomicInteger(0);
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if ((mask == null) || mask[nPix]){
double d = data[nPix];
if (!Double.isNaN(d)) {
int x = nPix % width;
int y = nPix / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
int mindx = anum_good.getAndIncrement();
mdatai[mindx] = new double[3][];
mdatai[mindx][0] = new double [2];
mdatai[mindx][0][0] = dx;
mdatai[mindx][0][1] = dy;
mdatai[mindx][1] = new double [1];
mdatai[mindx][1][0] = d;
mdatai[mindx][2] = new double [1];
mdatai[mindx][2][0] = (weight == null)? 1.0 : weight[nPix];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
final double [][][] mdata = new double [anum_good.get()][][];
System.arraycopy(mdatai, 0, mdata, 0, mdata.length);
double[][] approx2d = (new PolynomialApproximation()).quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // damping, // double [] damping, null OK
-1); // debug level
if (approx2d != null) {
if (dbg_title != null) {
double [] plane=new double[data.length];
double [] diff=new double[data.length];
double [] dweight = (weight != null) ? weight: new double[data.length];
double [] dmask = new double[data.length];
double [] masked = new double[data.length];
Arrays.fill(masked, Double.NaN);
for (int nPix = 0; nPix < plane.length; nPix++) {
int x = nPix % width;
int y = nPix / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
plane[nPix] = approx2d[0][2]+approx2d[0][0]*dx+approx2d[0][1]*dy;
diff[nPix] = data[nPix]-plane[nPix];
dmask[nPix] = ((mask == null) || Double.isNaN(data[nPix]))? Double.NaN : (mask[nPix]?2:1);
if ((mask == null) || mask[nPix]) {
masked[nPix] = diff[nPix];
}
}
ShowDoubleFloatArrays.showArrays(
new double[][] {data,plane,diff,masked,dweight,dmask},
width,
data.length/width,
true,
dbg_title,
new String[] {"data","approx","diff","masked","weight","mask"});
}
return new double[] {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
}
return null;
}
public static double [][] getPlane( // from the camera coordinates to in-plane coordinates
final double [][] wxyz,
final boolean use_parallel_proj, // for parallel xyz is 0, otherwise - point on the ground under the camera
final boolean [] mask,
final double [] weight,
final int width,
final double normal_damping,
final String dbg_title,
final int debugLevel) {
double [] z_tilts = null;
double [] damping = Double.isNaN(normal_damping) ? null : (new double [] {normal_damping, normal_damping});
final double [][][] mdatai = new double [wxyz.length][][];
AtomicInteger anum_good = new AtomicInteger(0);
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < wxyz.length; nPix = ai.getAndIncrement()) if ((mask == null) || mask[nPix]){
if (wxyz[nPix] != null) {
int mindx = anum_good.getAndIncrement();
mdatai[mindx] = new double[3][];
mdatai[mindx][0] = new double [2];
mdatai[mindx][0][0] = wxyz[nPix][0];
mdatai[mindx][0][1] = wxyz[nPix][1];
mdatai[mindx][1] = new double [1];
mdatai[mindx][1][0] = wxyz[nPix][1];
mdatai[mindx][2] = new double [1];
mdatai[mindx][2][0] = (weight == null)? 1.0 : weight[nPix];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
final double [][][] mdata = new double [anum_good.get()][][];
System.arraycopy(mdatai, 0, mdata, 0, mdata.length);
double[][] approx2d = (new PolynomialApproximation()).quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // damping, // double [] damping, null OK
-1); // debug level
if (approx2d == null) {
System.out.println("getPlane() approx2d== null");
return null;
}
z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
if (debugLevel > -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);
return to_ground_xyzatr; // from the camera coordinates to in-plane coordinates
/*
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);
*/
/*
if (dbg_title != null) {
double [] plane=new double[data.length];
double [] diff=new double[data.length];
double [] dweight = (weight != null) ? weight: new double[data.length];
double [] dmask = new double[data.length];
double [] masked = new double[data.length];
Arrays.fill(masked, Double.NaN);
for (int nPix = 0; nPix < plane.length; nPix++) {
int x = nPix % width;
int y = nPix / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
plane[nPix] = approx2d[0][2]+approx2d[0][0]*dx+approx2d[0][1]*dy;
diff[nPix] = data[nPix]-plane[nPix];
dmask[nPix] = ((mask == null) || Double.isNaN(data[nPix]))? Double.NaN : (mask[nPix]?2:1);
if ((mask == null) || mask[nPix]) {
masked[nPix] = diff[nPix];
}
}
ShowDoubleFloatArrays.showArrays(
new double[][] {data,plane,diff,masked,dweight,dmask},
width,
data.length/width,
true,
dbg_title,
new String[] {"data","approx","diff","masked","weight","mask"});
}
*/
// return new double[] {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
}
static double [] getHistogram1d (
double [] data, // may have NaNs
double [] weights,
double hist_low,
double hist_high,
int num_bins,
int debugLevel) {
if (weights == null) {
weights = new double [data.length];
Arrays.fill(weights, 1.0);
}
double k = num_bins / (hist_high - hist_low);
double [] hist = new double [num_bins];
for (int i = 0; i < data.length; i++) if (!Double.isNaN(data[i])){
double d = data[i];
double w = weights[i];
int bin = (int) (k * (d - hist_low));
if ((bin >= 0) && (bin < num_bins)) {
hist[bin] += w;
}
}
return hist;
}
static double [] getPercentiles(
double [] hist,
double [] fractions) {
double [] percentiles = new double [fractions.length];
double [] hist_cumul = new double [hist.length+1];
for (int i = 0; i < hist.length; i++) {
hist_cumul[i+1] = hist_cumul[i] + hist[i];
}
double s = hist_cumul[hist_cumul.length-1];
for (int nfrac = 0; nfrac < fractions.length; nfrac++) {
double f = fractions[nfrac];
double p = 0;
double v = f * s;
int indx = 0;
for (; indx < hist_cumul.length; indx++) {
if (hist_cumul[indx] > v) {
break;
}
}
if (indx == 0) {
p = 0;
} else if (indx >= hist_cumul.length) {
p = 1;
} else {
double frac = (v - hist_cumul[indx - 1])/(hist_cumul[indx] - hist_cumul[indx - 1]);
p= (indx - 1 + frac)/hist.length;
}
percentiles[nfrac] = p;
}
return percentiles;
}
static double weightedAverage (
double [] data, // may have NaNs
double [] weights) {
double sw = 0, swd = 0;
for (int i = 0; i < data.length; i++) if (!Double.isNaN(data[i])){
double d = data[i];
double w = (weights != null) ? weights[i] : 1.0;
swd += d * w;
sw += w;
}
return swd/sw;
}
}
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 0107a23d
......@@ -6414,7 +6414,59 @@ public class OpticalFlow {
// final double range_disparity_offset = clt_parameters.imp.range_disparity_offset;// double range_disparity_offset
boolean use_parallel_proj = false; // true;
int [] hdr_whs = new int[3];
double [] hdr_x0y0 = new double[2];
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