Skip to content

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

Parallel projection, not yet done with center projection bounds with IMS

parent 8b5bd41b
Show whitespace changes
Inline Side-by-side
Showing with 783 additions and 124 deletions
src/main/java/com/elphel/imagej/cameras/CLTParameters.java
View file @ 019655f7
......@@ -470,6 +470,9 @@ public class CLTParameters {
// parameters for ground maps generation
public boolean gmap_render_hdr = true; // generate textures w/o normalization to generate undistorted
public boolean gmap_en = true; // generate ground map from a drone (enables gmap_render_hdr)
public boolean gmap_parallel_proj = true; // Use parallel projection (map)
public boolean gmap_update_range = false; // for parallel only
// extracting ground projection plane
public boolean gmap_use_lma = true; // ;
public double gmap_discard_low = 0.01; // fraction of all pixels
......@@ -1613,6 +1616,9 @@ public class CLTParameters {
properties.setProperty(prefix+"gmap_render_hdr", this.gmap_render_hdr+""); // boolean
properties.setProperty(prefix+"gmap_en", this.gmap_en+""); // boolean
properties.setProperty(prefix+"gmap_parallel_proj", this.gmap_parallel_proj+""); // boolean
properties.setProperty(prefix+"gmap_update_range", this.gmap_update_range+""); // boolean
properties.setProperty(prefix+"gmap_use_lma", this.gmap_use_lma+""); // boolean
properties.setProperty(prefix+"gmap_discard_low", this.gmap_discard_low+""); // double
properties.setProperty(prefix+"gmap_discard_high", this.gmap_discard_high+""); // double
......@@ -2629,6 +2635,8 @@ public class CLTParameters {
if (properties.getProperty(prefix+"gmap_render_hdr")!=null) this.gmap_render_hdr=Boolean.parseBoolean(properties.getProperty(prefix+ "gmap_render_hdr"));
if (properties.getProperty(prefix+"gmap_en")!=null) this.gmap_en=Boolean.parseBoolean(properties.getProperty(prefix+ "gmap_en"));
if (properties.getProperty(prefix+"gmap_parallel_proj")!=null) this.gmap_parallel_proj=Boolean.parseBoolean(properties.getProperty(prefix+"gmap_parallel_proj"));
if (properties.getProperty(prefix+"gmap_update_range")!=null) this.gmap_update_range=Boolean.parseBoolean(properties.getProperty(prefix+ "gmap_update_range"));
if (properties.getProperty(prefix+"gmap_use_lma")!=null) this.gmap_use_lma=Boolean.parseBoolean(properties.getProperty(prefix+ "gmap_use_lma"));
if (properties.getProperty(prefix+"gmap_discard_low")!=null) this.gmap_discard_low=Double.parseDouble(properties.getProperty(prefix+ "gmap_discard_low"));
if (properties.getProperty(prefix+"gmap_discard_high")!=null) this.gmap_discard_high=Double.parseDouble(properties.getProperty(prefix+ "gmap_discard_high"));
......@@ -3875,10 +3883,17 @@ public class CLTParameters {
gd.addCheckbox ("glTF use alpha blend", this.gltf_alpha_blend);
gd.addMessage ("Ground map export");
gd.addMessage ("Use of IMS/GNSS data is "+(imp.ims_use? "ENABLED":"DISABLED")+" (see 'IMS' configuration tab)");
gd.addCheckbox ("Generate/save linear 32-bit textures", this.gmap_render_hdr, // true; // enable change FG pixel to opaque from transparent
"Generate and save textures w/o normalization as 32-bit Tiff files. This option will be automatically activated if ground plane maps are used.");
gd.addCheckbox ("Render ground-plane maps", this.gmap_en, // true; // enable change FG pixel to opaque from transparent
"Render fixed-scale projection of the 3D model to a ground plane, such as for the UAS-generated imagery.");
gd.addCheckbox ("Parallel (not center) projection (maps)", this.gmap_parallel_proj, // true; // enable change FG pixel to opaque from transparent
"Parallel-project objects to a plane surface. If unchecked - use center (from the camera) projection.");
gd.addCheckbox ("Fit all model (parallel only)", this.gmap_update_range, // true; // enable change FG pixel to opaque from transparent
"Recalculate (increase) image size to fit all model elements. Unchecked - limit by plane intersection with the camera FOV.");
gd.addCheckbox ("Use only LMA disparity for ground", this.gmap_use_lma, // true; // enable change FG pixel to opaque from transparent
"Use only LMA-generated disparity to find the ground plane to project the map.");
gd.addNumericField("Discard low fraction", this.gmap_discard_low, 4,6,"",
......@@ -5103,6 +5118,8 @@ public class CLTParameters {
this.gmap_render_hdr= gd.getNextBoolean();
this.gmap_en= gd.getNextBoolean();
this.gmap_parallel_proj= gd.getNextBoolean();
this.gmap_update_range= gd.getNextBoolean();
this.gmap_use_lma= gd.getNextBoolean();
this.gmap_discard_low= gd.getNextNumber();
this.gmap_discard_high= gd.getNextNumber();
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 019655f7
......@@ -2738,6 +2738,20 @@ public class OpticalFlow {
public static double [][] transformToWorldXYZ(
final double [] disparity_ref, // invalid tiles - NaN in disparity
final QuadCLT quadClt, // now - may be null - for testing if scene is rotated ref
int threadsMax){
return transformToWorldXYZ(
disparity_ref, // invalid tiles - NaN in disparity
quadClt, // now - may be null - for testing if scene is rotated ref
ZERO3,
ZERO3,
threadsMax);
}
public static double [][] transformToWorldXYZ(
final double [] disparity_ref, // invalid tiles - NaN in disparity
final QuadCLT quadClt, // now - may be null - for testing if scene is rotated ref
double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
int threadsMax)
{
TileProcessor tp = quadClt.getTileProcessor();
......@@ -2764,8 +2778,8 @@ public class OpticalFlow {
centerY, // double py, // pixel coordinate Y in the reference view
disparity, // double disparity, // reference disparity
true, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
ZERO3, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
ZERO3); // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
reference_xyz, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
reference_atr); // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
}
}
};
......@@ -5251,6 +5265,52 @@ public class OpticalFlow {
ego_comment); // String comment);
}
}
boolean test_ground = false; // true;
if (test_ground) {
final boolean use_lma = clt_parameters.gmap_use_lma ; // true; // ;
final double discard_low = clt_parameters.gmap_discard_low ; //0.01; // fraction of all pixels
final double discard_high = clt_parameters.gmap_discard_high ; //0.5; // fraction of all pixels
final double discard_adisp = clt_parameters.gmap_discard_adisp ; //0.2; // discard above/below this fraction of average height
final double discard_rdisp = clt_parameters.gmap_discard_rdisp ; //0.02; // discard above/below this fraction of average height
final double pix_size = clt_parameters.gmap_pix_size ; //0.005; // hdr_x0y0, // in meters
final int max_image_width = clt_parameters.gmap_max_image_width ; //4000; // 3200; // increase pixel size as a power of 2 until image fits
// 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 [][] to_ground_xyzatr = quadCLTs[ref_index].getGroundNoIms(
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
quadCLTs[ref_index].getErsCorrection().printVectors (to_ground_xyzatr[0], to_ground_xyzatr[1]);
double [][] to_ground_xyzatr_ims = quadCLTs[ref_index].getGroundIms(
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
quadCLTs[ref_index].getErsCorrection().printVectors (to_ground_xyzatr_ims[0], to_ground_xyzatr_ims[1]);
System.out.println("getGroundIms Done");
}
if (test_ers) { // only debug feature
test_ers0 = quadCLTs.length -1; // make it always == reference !
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 019655f7
......@@ -338,9 +338,64 @@ public class QuadCLTCPU {
return z_avg;
}
// cac
public double [][] getGround(
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
) {
boolean ims_use = clt_parameters.imp.ims_use;
if (!ims_use || (did_ins_2 == null)) {
System.out.println("getGroundIns(): no INS data available.");
ims_use=false;
}
if (ims_use) {
return getGroundIms(
clt_parameters,
use_lma,
use_parallel_proj, // boolean use_parallel_proj,
disparity_offset,
discard_low, // fraction of all pixels
discard_high, // fraction of all pixels
discard_adisp, // discard above/below this fraction of average height
discard_rdisp, // discard above/below this fraction of average height
pix_size, // in meters
max_image_width,// increase pixel size as a power of 2 until image fits
x0y0, // initialize to double[2] to return width, height
whs, // initialize to int[3] to return {width, height, scale reduction}
debug_level);
} else {
return getGroundNoIms(
clt_parameters,
use_lma,
use_parallel_proj, // boolean use_parallel_proj,
disparity_offset,
discard_low, // fraction of all pixels
discard_high, // fraction of all pixels
discard_adisp, // discard above/below this fraction of average height
discard_rdisp, // discard above/below this fraction of average height
pix_size, // in meters
max_image_width,// increase pixel size as a power of 2 until image fits
x0y0, // initialize to double[2] to return width, height
whs, // initialize to int[3] to return {width, height, scale reduction}
debug_level);
}
}
public double [][] getGroundNoIms(
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
......@@ -474,18 +529,20 @@ public class QuadCLTCPU {
System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]);
}
double [][] ground_xyxatr = new double [][] {
{0, 0, z_tilts[0]},
{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
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_xyxatr = ErsCorrection.invertXYZATR(ground_xyxatr);
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] = 0.0;
else {
if (!Double.isNaN(ref_disparity[i])) {
ref_disparity[i] -= disparity_offset;
if (ref_disparity[i] <=0) {
ref_disparity[i] = Double.NaN;
}
}
}
wxyz = OpticalFlow.transformToWorldXYZ(
......@@ -498,11 +555,15 @@ public class QuadCLTCPU {
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_xyxatr, // double [][] reference_xyzatr,
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
}
......@@ -542,15 +603,268 @@ public class QuadCLTCPU {
}
double [] dronexyz =ErsCorrection.applyXYZATR(
to_ground_xyxatr, // double [][] reference_xyzatr,
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(
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
) {
boolean ims_use = clt_parameters.imp.ims_use;
if (!ims_use || (did_ins_2 == null)) {
System.out.println("getGroundIns(): no INS data available.");
return null;
}
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
// double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
Did_ins_2 d2 = did_ins_2;
double [] cam_quat_enu =Imx5.quaternionImsToCam(
d2.getQEnu(),
ims_mount_atr,
ims_ortho);
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] dls = getDLS();
if (dls==null) {
return null;
}
double min_disparity = 0.2;
double max_disparity = 15;
double [][] xy_range = new double[3][];
double agl = getAGL(
dls[use_lma?1:0].clone(), // double [] ref_disparity,
dls[2], // double [] ref_strength,
scene_abs_atr_enu, // double [] reference_atr,
disparity_offset, // double disparity_offset,
min_disparity, // double min_disparity,
max_disparity, // double max_disparity,
discard_low, // double discard_low, // fraction of all pixels
discard_high, // double discard_high, // fraction of all pixels
xy_range, // double [][] xy_range, // should be double[2][];
debug_level); // int debug_level)
// camera position and orientation relative to the same lat/long on the ground below it
// double [][] scene_abs_xyzatr_enu = new double [][] {new double[] {0,0,-agl}, scene_abs_atr_enu};
double [][] scene_abs_xyzatr_enu = new double [][] {new double[] {0,0,use_parallel_proj?0:-agl}, scene_abs_atr_enu};
// double [][] ground_xyzatr = new double [][] {
// {0, 0, z_tilts[0]},
// {Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(scene_abs_xyzatr_enu);
if ((xy_range[0] == null) || (xy_range[1] == null)) {
return null; // no points at all?
}
// FIXME: still something wrong with non-parallel bounds with IMS, noIMS - correct *****************
if (x0y0!=null) {
if (!use_parallel_proj) {
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
xy_range[i][j] -= xy_range[2][i];
}
}
}
x0y0[0] = xy_range[0][0]; // null
x0y0[1] = xy_range[1][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((xy_range[0][1] - xy_range[0][0])/use_pix_size) + 1;
height = (int) Math.floor((xy_range[1][1] - xy_range[1][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_xyxatr; // from the camera coordinates to in-plane coordiantes
return to_ground_xyzatr;
}
public double getAGL(
double [] ref_disparity, // will be modified
double [] ref_strength,
double [] reference_atr,
double disparity_offset,
double min_disparity,
double max_disparity,
double discard_low, // fraction of all pixels
double discard_high, // fraction of all pixels
double [][] xy_range, // should be double[2][];
int debug_level) {
boolean show_wxyz=false;
// find average AGL excluding high objects
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 sw=0, swd=0;
boolean [] good_tiles = new boolean[ref_disparity.length];
for (int i = 0; i < ref_disparity.length; i++) if (!Double.isNaN(ref_disparity[i]) && (ref_strength[i] > 0)){ // some where NaN!
double d = ref_disparity[i] - disparity_offset;
ref_disparity[i] = d;
if ((ref_disparity[i] >= min_disparity) && (ref_disparity[i] <= max_disparity)) {
sw += ref_strength[i];
swd += ref_strength[i]*d;
good_tiles[i] = true;
}
}
double avg_disparity = swd/sw;
double average_Z= getGeometryCorrection().getZFromDisparity(avg_disparity);
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
OpticalFlow.ZERO3, // reference_xyz, // this view position in world coordinates (typically ZERO3)
reference_atr, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
THREADS_MAX); // int threadsMax)
if (show_wxyz) {
double [][] dbg_img = new double[3][wxyz.length];
for (int i = 0; i < dbg_img.length; i++) {
Arrays.fill(dbg_img[i], Double.NaN);
}
for (int i = 0; i < wxyz.length; i++) if (wxyz[i] != null) {
for (int j = 0; j < 3; j++) {
dbg_img[j][i] = wxyz[i][j];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
tp.getTilesX(),
tp.getTilesY(),
true,
"wxyz",
new String[] {"x","y","z"}); // dsrbg_titles);
}
double hist_low = hist_rlow * average_Z;
double hist_high = hist_rhigh * average_Z;
double k = num_bins / (hist_high - hist_low);
double [] hist = new double [num_bins];
sw = 0;
swd = 0;
for (int i = 0; i < wxyz.length; i++) if (good_tiles[i] && (wxyz[i] != null)) { // && !Double.isNaN(wxyz[i][2])){
double z = -wxyz[i][2];
double w = ref_strength[i];
int bin = (int) (k * (z - hist_low));
if ((bin >= 0) && (bin < num_bins)) {
sw += w;
swd += w * z;
hist[bin] += w;
if (xy_range != null) {
if (xy_range[0]== null) {
xy_range[0] = new double[] {wxyz[i][0],wxyz[i][0]}; // x, x
xy_range[1] = new double[] {wxyz[i][1],wxyz[i][1]}; // y, y
}
if (wxyz[i][0] < xy_range[0][0]) xy_range[0][0] = wxyz[i][0];
else if (wxyz[i][0] > xy_range[0][1]) xy_range[0][1] = wxyz[i][0];
if (wxyz[i][1] < xy_range[1][0]) xy_range[1][0] = wxyz[i][1];
else if (wxyz[i][1] > xy_range[1][1]) xy_range[1][1] = wxyz[i][1];
}
}
}
if (sw == 0) {
if (debug_level > -3) {
System.out.println("Could not find ground - sum weights==0.0");
}
return Double.NaN;
}
// double dl = discard_low * sw;
// double dh = discard_high * sw;
double dl = discard_high * sw;
double dh = discard_low * 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;
sw = 0.0;
swd = 0.0;
for (int i = 0; i < wxyz.length; i++) if (good_tiles[i] && (wxyz[i] != null)) { // && !Double.isNaN(wxyz[i][2])){
double z = -wxyz[i][2];
double w = ref_strength[i];
if ((ref_strength[i] > 0) && (z >= thr_low) && (z <= thr_high)) {
numgood++;
sw += w;
swd += w * z;
}
}
if (numgood < min_good) {
if (debug_level > -3) {
System.out.println("Could not find ground - number of good tiles = "+numgood+" < "+min_good);
}
return Double.NaN; // too few good
}
double agl = swd/sw;
double [] center_xyz = ErsCorrection.applyXYZATR(
OpticalFlow.ZERO3, // reference_xyz, // this view position in world coordinates (typically ZERO3)
reference_atr, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
new double [] {0,0,-agl});
if (xy_range != null) {
xy_range[2] = center_xyz;
}
return agl;
}
public double [][] getDLS(){ // get disparity, disparity_lma, strength
if (dsi == null) {
// System.out.println("dsi== null, use spawnQuadCLT(), restoreFromModel(), ... to set it");
......
src/main/java/com/elphel/imagej/tileprocessor/TexturedModel.java
View file @ 019655f7
......@@ -2454,6 +2454,8 @@ public class TexturedModel {
{
final boolean map_en = clt_parameters.gmap_en;
final boolean render_hdr = clt_parameters.gmap_render_hdr || map_en;// true; //false; // true; // generate textures w/o normalization to generate undistorted
final boolean use_parallel_proj = clt_parameters.gmap_parallel_proj; // Use parallel projection (map)
final boolean update_range = clt_parameters.gmap_update_range; // for parallel only
final boolean use_lma = clt_parameters.gmap_use_lma ; // true; // ;
final double discard_low = clt_parameters.gmap_discard_low ; //0.01; // fraction of all pixels
......@@ -2585,26 +2587,6 @@ public class TexturedModel {
double[][] ds_fg_bg = {ds_fg[0], ds_bg[0].clone()};
double[][] ss_fg_bg = {ds_fg[1], ds_bg[1]};
/*
for (int i = 0; i < dls_bg[0].length; i++) {
if (Math.abs(ds_fg_bg[1][i]-ds_fg_bg[0][i]) < tex_fg_bg) {
ds_fg_bg[1][i] = Double.NaN;
}
}
if (debugLevel > -2) {
System.out.println("Limiting non-infinity objects' disparity to "+min_obj_disparity+
", infinity_disparity = "+infinity_disparity);
}
for (int nl = 0; nl < ds_fg_bg.length; nl++) {
for (int i = 0; i < dls_bg[nl].length; i++) {
if ((nl != sky_layer) || !sky_tiles[i]) {
if (ds_fg_bg[nl][i] < min_obj_disparity) {
ds_fg_bg[nl][i] = min_obj_disparity;
}
}
}
}
*/
if (show_bs_debug) {
String [] dbg_titles = {"FG","BG","BS"};
double [][] dbg_img = {ds_fg_bg[0], ds_fg_bg[1],combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_BLUE_SKY].clone()};
......@@ -3000,8 +2982,10 @@ public class TexturedModel {
if (render_hdr) {
int [] hdr_whs = new int[3];
double [] hdr_x0y0 = new double[2];
double [][] to_ground_xyxatr = scenes[ref_index].getGround(
double [][] to_ground_xyzatr = scenes[ref_index].getGround(
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
......@@ -3012,24 +2996,59 @@ public class TexturedModel {
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
if (to_ground_xyxatr == null) {
if (to_ground_xyzatr == null) {
System.out.println("***** Failed to detect flat ground surface, skipping rendering and output files generation *****");
} else {
double scaled_pixel_size = pix_size * hdr_whs[2];
if (use_parallel_proj) {
if (update_range ) {
double [][] bounds = Render3D.getBounds(
tri_meshes, // final ArrayList<TriMesh> tri_meshes,
to_ground_xyzatr, // final double [][] xyzatr_toground,
debugLevel); // int debugLevel) // debug level
hdr_x0y0[0] = bounds[0][0];
hdr_x0y0[1] = bounds[1][0];
hdr_whs[0] = (int) Math.ceil((bounds[0][1]-bounds[0][0])/scaled_pixel_size);
hdr_whs[1] = (int) Math.ceil((bounds[1][1]-bounds[1][0])/scaled_pixel_size);
if (debugLevel > -2) {
System.out.println("Updated parameters for rendering:top left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]");
System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]);
System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm");
}
} else {
if (debugLevel > -2) {
System.out.println("Keeping conservative parameters for rendering:top left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]");
System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]);
System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm");
}
}
}
Render3D render3D = new Render3D (
//x3d_dir, // String x3d_dir,
//ref_scene.correctionsParameters.getModelName(ref_scene.getImageName()), // String model_name,
scenes[ref_index], // QuadCLT ref_scene, // all coordinates relative to this scene
to_ground_xyxatr, // double [][] plane_xyzatr, // projection plane center relative to reference scene
pix_size * hdr_whs[2], // double pixel_size, // in meters
to_ground_xyzatr, // double [][] plane_xyzatr, // projection plane center relative to reference scene
scaled_pixel_size, // double pixel_size, // in meters
hdr_x0y0, // double [] x0_y0, // usually negative - top-left point of the output render
hdr_whs[0], // int out_width, // output rendered image width in pixels
hdr_whs[1]); // int out_height); // output rendered image height in pixels
boolean last_is_alpha = true; // last channel in textures slices is alpha
double [][] full_render =render3D.render3dPlane(
double [][] full_render;
if (use_parallel_proj) {
full_render =render3D.render3dPlaneParallelProj(
tri_meshes, // final ArrayList<TriMesh> tri_meshes,
last_is_alpha, // final boolean last_is_alpha,
// scenes[ref_index], //final QuadCLT ref_scene, // all coordinates relative to this scene
debugLevel); //int debugLevel)
} else {
full_render =render3D.render3dPlaneCenterProj(
tri_meshes, // final ArrayList<TriMesh> tri_meshes,
last_is_alpha, // final boolean last_is_alpha,
scenes[ref_index], //final QuadCLT ref_scene, // all coordinates relative to this scene
// scenes[ref_index], //final QuadCLT ref_scene, // all coordinates relative to this scene
debugLevel); //int debugLevel)
}
// String model_name = ref_scene.correctionsParameters.getModelName(ref_scene.getImageName());
String suffix ="-RECT";
if (clt_parameters.tex_um) {
......
src/main/java/com/elphel/imagej/x3d/export/Render3D.java
View file @ 019655f7
......@@ -44,6 +44,7 @@ public class Render3D {
final int out_height; // output rendered image height in pixels
// final String x3d_dir;
// final String model_name;
final double [] x0_y0;
final Vector3D ground_origin;
final Vector3D ground_x;
final Vector3D ground_y;
......@@ -65,6 +66,7 @@ public class Render3D {
this.out_width = out_width;
this.out_height = out_height;
this.toground = toground;
this.x0_y0 = x0_y0; // used in parallel projection
this.tocam = ErsCorrection.invertXYZATR(this.toground); // null
// ground plane x0, y0 in camera coordinates
ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1], 0.0}));
......@@ -80,7 +82,7 @@ public class Render3D {
xy_offs = new double[] {ground_x.dotProduct(ground_origin), ground_y.dotProduct(ground_origin)};
}
public double [] projectToPlaneLinear(Vector3D v3) { // get ground place pixel coordinate from camera x,y,z
public double [] projectToPlaneLinear(Vector3D v3) { // get ground plane pixel coordinate from camera x,y,z
double z = ground_normal_unit.dotProduct(v3);
Vector3D in_plane = v3.scalarMultiply(above_ground/z);
double [] xy = new double[3];
......@@ -90,7 +92,7 @@ public class Render3D {
return xy;
}
public double [] projectToPlanePixels(Vector3D v3) { // get ground place pixel coordinate from camera x,y,z
public double [] projectToPlanePixels(Vector3D v3) { // get ground plane pixel coordinate from camera x,y,z
double z = ground_normal_unit.dotProduct(v3);
Vector3D in_plane = v3.scalarMultiply(above_ground/z);
double [] xy = new double[3];
......@@ -190,10 +192,257 @@ public class Render3D {
return img_cropped;
}
public double [][] render3dPlane(
public static double [][] getBounds(
final ArrayList<TriMesh> tri_meshes,
final double [][] xyzatr_toground,
int debugLevel){ // debug level
double [][] bounds = new double[3][];
final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
final double [][][] minmaxes_xyz = new double [threads.length][3][];
for (TriMesh mesh:tri_meshes) {
final double [][] coords = mesh.getCoordinates();
ai.set(0);
ati.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int ti = ati.getAndIncrement();
double [][] minmax_xyz = minmaxes_xyz[ti]; // this thread rendered results
for (int indx = ai.getAndIncrement(); indx < coords.length; indx = ai.getAndIncrement()) {
double [] xyz = ErsCorrection.applyXYZATR(xyzatr_toground, coords[indx]);
// maybe no need to check for Double.isNaN()?
if ((xyz != null) && !Double.isNaN(xyz[0]) && !Double.isNaN(xyz[1]) && !Double.isNaN(xyz[2])) {
if (minmax_xyz[0] == null) {
for (int j = 0; j < xyz.length; j++) {
minmax_xyz[j] = new double[] {xyz[j],xyz[j]};
}
}
for (int j = 0; j < minmax_xyz.length; j++) {
minmax_xyz[j][0] = Math.min(minmax_xyz[j][0], xyz[j]);
minmax_xyz[j][1] = Math.max(minmax_xyz[j][1], xyz[j]);
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
for (double [][] mm_xyz: minmaxes_xyz) if (mm_xyz[0] != null){
if (bounds[0] == null) {
for (int j = 0; j < bounds.length; j++) {
bounds[j] = new double [] {mm_xyz[j][0],mm_xyz[j][1]};
}
}
for (int j = 0; j < bounds.length; j++) {
bounds[j][0] = Math.min(bounds[j][0], mm_xyz[j][0]);
bounds[j][1] = Math.max(bounds[j][1], mm_xyz[j][1]);
}
}
return bounds;
}
public double [][] render3dPlaneParallelProj(
final ArrayList<TriMesh> tri_meshes,
final boolean last_is_alpha,
// final QuadCLT ref_scene, // all coordinates relative to this scene - not used
int debugLevel){ // debug level
// TODO: add crop - add to the caller
if ((tri_meshes == null) || tri_meshes.isEmpty() || (tri_meshes.get(0).getTexturePixels() == null)) {
return null;
}
final int dbg_ipix=1673752;
// get total number of triangles
int num_tri=0;
for (TriMesh tri: tri_meshes) {
num_tri += tri.getTriangles().length;
}
int indx=0;
int num_mesh = 0;
final int [][] tri_index = new int[num_tri][2];
for (TriMesh tri: tri_meshes) {
int num_tri_mesh = tri.getTriangles().length;
for (int i = 0; i < num_tri_mesh; i++)
{
tri_index[indx][0] = num_mesh;
tri_index[indx++][1] = i;
}
num_mesh++;
}
if (debugLevel > -2) {
System.out.println("Prepare to render "+num_tri+" triangles in "+num_mesh+" meshes");
}
final int z_index = tri_meshes.get(0).getTexturePixels().length;
final double [][] full_rendered = new double[z_index][out_width * out_height];
int alpha_index = last_is_alpha ? (z_index - 1) : z_index;
for (int chn = 0; chn < alpha_index; chn++) {
Arrays.fill(full_rendered[chn], Double.NaN);
}
// create z-buffer array per each thread, in the end - merge them
final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
final double [][][] rendered = new double [threads.length][full_rendered[0].length][];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int ti = ati.getAndIncrement();
double [][] rend = rendered[ti]; // this thread rendered results
for (int indx = ai.getAndIncrement(); indx < tri_index.length; indx = ai.getAndIncrement()) {
TriMesh tri=tri_meshes.get(tri_index[indx][0]); // mesh to process;
int tri_indx = tri_index[indx][1]; // triangle index in a mesh
double [][] texture = tri.getTexturePixels();
int texture_width= tri.getTextureWidth();
int texture_height= tri.getTextureHeight();
double[][] mesh_coord = tri.getCoordinates();
double[][] mesh_tex_coord = tri.getTexCoord();
int [] triangle = tri.getTriangles()[tri_indx];
double [][] tri_out2= new double[3][];
double [][] tri_text2= new double[3][];
double [][] min_max_xyz = new double[3][2];
for (int i = 0; i < 3; i++) {
// tri_out2[i] = projectToPlanePixels(new Vector3D(mesh_coord[triangle[i]]));
double [] gxyz=ErsCorrection.applyXYZATR(toground, mesh_coord[triangle[i]]);
tri_out2[i] = new double [] {
pixel_per_m * (gxyz[0] - x0_y0[0]),
pixel_per_m * (gxyz[1] - x0_y0[1]),
-pixel_per_m * gxyz[2] // make it positive?
};
//[2] - distance from the camera in "pixels" - same linear scale as on the ground. lower values obscure higher.
tri_text2[i] = mesh_tex_coord[triangle[i]];
for (int j = 0; j < 3; j++) {
if ((i==0) || (tri_out2[i][j] < min_max_xyz[j][0])) min_max_xyz[j][0] = tri_out2[i][j];
if ((i==0) || (tri_out2[i][j] > min_max_xyz[j][1])) min_max_xyz[j][1] = tri_out2[i][j];
}
}
// Check plane direction
double [] d01 = new double [] {tri_out2[1][0]-tri_out2[0][0], tri_out2[1][1]-tri_out2[0][1]};
double [] d02 = new double [] {tri_out2[2][0]-tri_out2[0][0], tri_out2[2][1]-tri_out2[0][1]};
if (!cross2ccw(d02,d01)) {
continue;
}
int ipx_min = (int) Math.floor(min_max_xyz[0][0]);
int ipx_max = (int) Math.ceil (min_max_xyz[0][1]);
int ipy_min = (int) Math.floor(min_max_xyz[1][0]);
int ipy_max = (int) Math.ceil (min_max_xyz[1][1]);
// apply bounds
if (ipx_min < 0) ipx_min = 0;
if (ipy_min < 0) ipy_min = 0;
if (ipx_max >= out_width) ipx_max = out_width - 1;
if (ipy_max >= out_height) ipy_max = out_height - 1;
if ((ipx_min > ipx_max) || (ipy_min > ipy_max)) {
continue; // triangle completely outside rendering are
}
// vector from 1 to 2
double [] t01 = new double [] {tri_text2[1][0]-tri_text2[0][0], tri_text2[1][1]-tri_text2[0][1]};
double [] t02 = new double [] {tri_text2[2][0]-tri_text2[0][0], tri_text2[2][1]-tri_text2[0][1]};
double [][] orto2 = orthonorm2(d01, d02);
double [] d12 = new double [] {tri_out2[2][0]-tri_out2[1][0], tri_out2[2][1]-tri_out2[1][1]};
for (int ipy = ipy_min; ipy <= ipy_max; ipy++) {
for (int ipx = ipx_min; ipx <= ipx_max; ipx++) {
// check it is inside triangle
double [] d0p = new double[] {ipx-tri_out2[0][0],ipy-tri_out2[0][1]};
if (!cross2ccw(d0p,d01)) continue;
if (!cross2ccw(d02,d0p)) continue;
double [] d1p = new double[] {ipx-tri_out2[1][0],ipy-tri_out2[1][1]};
if (!cross2ccw(d1p,d12)) continue;
int ipix = ipx +(out_height - 1 -ipy) * out_width; // Y goes down
if (ipix== dbg_ipix) {
System.out.println("ipix="+ipix);
}
// See if the rendered pixel is closer than the closest of the corners
if ((rend[ipix] != null ) && (rend[ipix][z_index] < min_max_xyz[2][0])) {
continue;
}
double kx = dot2(d0p, orto2[0]);
double ky = dot2(d0p, orto2[1]);
// interpolate z
double z_interp = tri_out2[0][2] + kx * (tri_out2[1][2]-tri_out2[0][2]) + ky*(tri_out2[1][2]-tri_out2[0][2]);
if ((rend[ipix] != null ) && (rend[ipix][z_index] < z_interp)) {
continue;
}
// Get corresponding texture coordinates
double text_x = tri_text2[0][0] + kx * t01[0] + ky*t02[0]; // texture relative coordinates (0,1)
double text_y = tri_text2[0][1] + kx * t01[1] + ky*t02[1]; // y - up!
double px = text_x * texture_width - 0.5; // (0.0,0.0) - center of top-left texture pixel
double py = (1.0-text_y) * texture_height -0.5;
int ipx0 = (int) Math.floor(px);
int ipy0 = (int) Math.floor(py);
double fx = px - ipx0;
double fy = py - ipy0;
int ipx1 = ipx0+1;
int ipy1 = ipy0+1;
if ((ipx1 < 0) || (ipy1 < 0) || (ipx0 >= texture_width) || (ipy0 >= texture_width)) {
continue; // outside bounds
}
// limit if just on the edge
if (ipx0 < 0) ipx0=ipx1;
if (ipy0 < 0) ipy0=ipy1;
if (ipx1 >= texture_width) ipx1=ipx0;
if (ipy1 >= texture_height) ipy1=ipy0;
int indx00 = ipx0+texture_width*ipy0;
int indx10 = ipx1+texture_width*ipy0;
int indx01 = ipx0+texture_width*ipy1;
int indx11 = ipx1+texture_width*ipy1;
double [] pix_val = new double[z_index+1];
pix_val[z_index] = z_interp;
for (int chn = 0; chn < z_index; chn++) {
pix_val[chn] =
(1.0 - fy) * (1.0 - fx) * texture[chn][indx00] +
(1.0 - fy) * ( fx) * texture[chn][indx10] +
( fy) * (1.0 - fx) * texture[chn][indx01] +
( fy) * ( fx) * texture[chn][indx11];
}
// handle alpha
if (last_is_alpha && (pix_val[z_index-1] < 0.5)) {
continue; // low alpha -> transparent
}
rend[ipix] = pix_val;
}
}
// min_max_xyz[2]
//num_col_chn
// projectToPlanePixels
// getCoordinates()
//getTexCoord()
}
}
};
}
ImageDtt.startAndJoin(threads);
// merge partial renders:
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int indx = ai.getAndIncrement(); indx < full_rendered[0].length; indx = ai.getAndIncrement()) {
double z = Double.NaN;
for (int sub_render = 0; sub_render < rendered.length; sub_render++) if (rendered[sub_render][indx] != null){
if (!(rendered[sub_render][indx][z_index] <= z)) { // OK previous NaN
z = rendered[sub_render][indx][z_index];
for (int chn = 0; chn < z_index; chn++) {
full_rendered[chn][indx] = rendered[sub_render][indx][chn];
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return full_rendered;
}
public double [][] render3dPlaneCenterProj(
final ArrayList<TriMesh> tri_meshes,
final boolean last_is_alpha,
final QuadCLT ref_scene, // all coordinates relative to this scene
// final QuadCLT ref_scene, // all coordinates relative to this scene - not used
int debugLevel){ // debug level
// TODO: add crop - add to the caller
if ((tri_meshes == null) || tri_meshes.isEmpty() || (tri_meshes.get(0).getTexturePixels() == null)) {
......
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