Commit 019655f7 authored by Andrey Filippov's avatar Andrey Filippov

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

parent 8b5bd41b
......@@ -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();
......
......@@ -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 !
......
......@@ -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");
......
......@@ -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) {
......
......@@ -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