Commit bdb4ff93 authored by Andrey Filippov's avatar Andrey Filippov

Implemented ground plane extraction (so far hard-coded parameters) and

rendering rectified 3D model to that plane
parent 9d1a83ef
......@@ -1768,7 +1768,7 @@ public class ErsCorrection extends GeometryCorrection {
int tiles2 = tilesX2 * tilesY2;
if (debug_level > 0) {
String title3 = scene_QuadClt.getImageName()+"-pXpYD_comprison";
String title3 = scene_QuadClt.getImageName()+"-pXpYD_comparison";
String [] titles3 = {"orig","restored","diff"};
double [][] dbg_img3 = new double [titles3.length][tiles2];
for (int i = 0; i < dbg_img3.length; i++) {
......@@ -1858,6 +1858,16 @@ public class ErsCorrection extends GeometryCorrection {
scene_xyzatr[1]);
}
public static double [] applyXYZATR(
double [][] reference_xyzatr,
double [] scene_xyz) {
return applyXYZATR(
reference_xyzatr[0],
reference_xyzatr[1],
scene_xyz);
}
public static double [][] invertXYZATR(
double [][] source_xyzatr){
return invertXYZATR(
......@@ -1883,6 +1893,22 @@ public class ErsCorrection extends GeometryCorrection {
}
// is it correct? Made of combineXYZATR()
public static double [] applyXYZATR(
double [] reference_xyz,
double [] reference_atr,
double [] scene_xyz)
{
Rotation ref_rotation= new Rotation(RotationOrder.YXZ, ROT_CONV, reference_atr[0],reference_atr[1],reference_atr[2]); // null
Vector3D ref_offset = new Vector3D(reference_xyz);
Vector3D scene_offset = new Vector3D(scene_xyz);
Vector3D offset = ref_offset.add(ref_rotation.applyTo(scene_offset));
double [] xyz = offset.toArray();
return xyz;
}
public static double [][] invertXYZATR(
double [] source_xyz,
double [] source_atr)
......
......@@ -100,7 +100,7 @@ public class OpticalFlow {
case FAIL_REASON_LMA: return "FAIL_REASON_LMA";
case FAIL_REASON_INTERSCENE: return "FAIL_REASON_INTERSCENE";
case FAIL_REASON_MIN: return "FAIL_REASON_MIN";
case FAIL_REASON_MAX: return "FAIL_REASON_MAXv";
case FAIL_REASON_MAX: return "FAIL_REASON_MAX";
case FAIL_REASON_NULL: return "FAIL_REASON_NULL";
case FAIL_REASON_EMPTY: return "FAIL_REASON_EMPTY";
case FAIL_REASON_ROLL: return "FAIL_REASON_ROLL";
......@@ -5796,8 +5796,39 @@ public class OpticalFlow {
} // if (generate_mapped) {
// debugging 3D model
int [] whs = new int[3];
double [] x0y0 = new double[2];
if (export3d) { //combo_dsn_final had strength 1.0e-4 where it should not? Reset it?
/* */
boolean use_lma = true; // ;
double discard_low = 0.01; // fraction of all pixels
double discard_high = 0.5; // fraction of all pixels
double discard_adisp = 0.2; // discard above/below this fraction of average height
double discard_rdisp = 0.02; // discard above/below this fraction of average height
double pix_size = 0.005; // hdr_x0y0, // in meters
int max_image_width = 3200; // increase pixel size as a power of 2 until image fits
// boolean crop_empty = true;
// int crop_extra = 20;
// int [] tex_pals = {0,1,2};
// int tex_palette_start = 0; //
// int tex_palette_end = 12;
int [] hdr_whs = new int[3];
double [] hdr_x0y0 = new double[2];
double [][] to_ground_xyxatr = quadCLTs[ref_index].getGround(
use_lma, // boolean use_lma,
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
/**/
boolean ok_3d = TexturedModel.output3d( // quadCLTs have same image name, and everything else
clt_parameters, // CLTParameters clt_parameters,
colorProcParameters, // ColorProcParameters colorProcParameters,
......
......@@ -59,6 +59,7 @@ import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.cameras.EyesisCorrectionParameters;
import com.elphel.imagej.cameras.ThermalColor;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.correction.CorrectionColorProc;
import com.elphel.imagej.correction.EyesisCorrections;
......@@ -219,6 +220,276 @@ public class QuadCLTCPU {
return z_avg;
}
// cac
public double [][] getGround(
boolean use_lma,
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;
final int tilesX=getTileProcessor().getTilesX();
final int tilesY=getTileProcessor().getTilesY();
double hist_rlow = 0.4;
double hist_rhigh = 2.0;
int min_good = 20; //number of good tiles
int min_good_quad = 20; // number of good tiles per quadrantto calculate tilts
int gap_frac2= 20;
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], dls[2]};
double sw=0, swd=0;
for (int i = 0; i < ds[0].length; i++) if (!Double.isNaN(ds[0][i])){
sw += ds[1][i];
swd += ds[0][i] * ds[1][i];
}
double disp_avg = swd/sw;
double hist_low = hist_rlow * disp_avg;
double hist_high = 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];
// double thr_low = shp + (dl - shp)/(sh-shp);
// 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 = shp - (dh-shp)/(sh - shp); // both negative
// double thr_high = hist_low+ (shp + (dh - shp)/(sh-shp))/num_bins *(hist_high-hist_low);
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
}
double z_avg= getGeometryCorrection().getZFromDisparity(swd/sw);
int gap_x_0 = tilesX/2 - tilesX/gap_frac2;
int gap_x_1 = tilesX/2 + tilesX/gap_frac2;
int gap_y_0 = tilesY/2 - tilesY/gap_frac2;
int gap_y_1 = tilesY/2 + tilesY/gap_frac2;
int [][] num_good_quad = new int [2][2];
for (int i = 0; i < good.length; i++) if (good[i]){
int ty = i / tilesX;
if ((ty <= gap_y_0) || (ty >= gap_y_1)) {
int tx = i % tilesX;
if ((tx <= gap_x_0) || (tx >= gap_x_1)) {
int indx_y = (ty > tilesY/2)? 1:0;
int indx_x = (tx > tilesX/2)? 1:0;
num_good_quad[indx_y][indx_x]++;
}
}
}
int num_good_quads = 0;
for (int i = 0; i < num_good_quad.length; i++) {
for (int j = 0; j < num_good_quad[i].length; j++) {
if (num_good_quad[i][j] > min_good_quad) {
num_good_quads++;
}
}
}
if (num_good_quads < 3) {
if (debug_level > -3) {
System.out.println("There are less than 3 quadrants ("+num_good_quads+") having more than "+min_good_quad+" tiles");
System.out.println("Using only level "+z_avg+", ignoring tilt.");
}
z_tilts = new double[] {z_avg, 0.0, 0.0}; // no tilts
// return new double[] {z_avg, 0.0, 0.0}; // no tilts
} else {
// 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[][] approx2d = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
null, // 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]);
}
}
// ground - negative Z. picture right - positive X, picture up - positive Y
// positive tiltX - to the right higher ground (lower altitude above it) or camera tilted left
// positive tiltY - picture up - higher ground (or camera tilted back)
double [][] ground_xyxatr = new double [][] {
{0, 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);
// recalculate coordinates for all pixels including weak ones
double [] 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;
}
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)
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_xyxatr, // 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 (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 (x0y0!=null) {
x0y0[0] = x_min_max[0];
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_xyxatr, // 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");
}
// double pix_size, // in meters
// int max_image_width // increase pixel size as a power of 2 until image fits
// testing world coordinates -> plane coordinates
/*
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] = plane_xyz[i][0];
mdata[mindx][0][1] = plane_xyz[i][1];
mdata[mindx][1] = new double [1];
mdata[mindx][1][0] = plane_xyz[i][2];
mdata[mindx][2] = new double [1];
mdata[mindx][2][0] = ds[1][i];
mindx++;
}
double[][] approx2d_1 = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
null, // double [] damping, null OK
-1); // debug level
*/
return to_ground_xyxatr; // from the camera coordinates to in-plane coordiantes
}
public double [][] getDLS(){ // get disparity, disparity_lma, strength
if (dsi == null) {
// System.out.println("dsi== null, use spawnQuadCLT(), restoreFromModel(), ... to set it");
......@@ -12706,7 +12977,7 @@ public class QuadCLTCPU {
// Save KML and ratings files if they do not exist (so not to overwrite edited ones), make them world-writable
writeKml (null, debugLevel);
writeRatingFile (debugLevel);
/// writeRatingFile (debugLevel); broke!
Runtime.getRuntime().gc();
......@@ -15113,7 +15384,6 @@ public class QuadCLTCPU {
public boolean writePreview(
double [] data,
// boolean overwrite, // correctionsParameters.thumb_overwrite
int debugLevel
)
{
......@@ -15148,7 +15418,6 @@ public class QuadCLTCPU {
(debugLevel > -2) ? debugLevel : 1); // int debugLevel (print what it saves)
return true;
}
public boolean writeLwirPreview(
final CLTParameters clt_parameters,
double [] data,
......@@ -15156,6 +15425,23 @@ public class QuadCLTCPU {
int tex_palette,
String suffix,
int debugLevel) {
return writeLwirPreview(
clt_parameters,
data,
getTileProcessor().getTilesX() *getTileProcessor().getTileSize(),
scene,
tex_palette,
suffix,
debugLevel);
}
public boolean writeLwirPreview(
final CLTParameters clt_parameters,
double [] data,
int width,
QuadCLT scene,
int tex_palette,
String suffix,
int debugLevel) {
if (scene == null) {
scene = (QuadCLT) this;
}
......@@ -15164,7 +15450,7 @@ public class QuadCLTCPU {
rendered_texture[1][i] = Double.isNaN(rendered_texture[0][i])? 0.0: 1.0;
}
double [] minmax = scene.getColdHot(); // used in linearStackToColor (from this current scene)
int width = getTileProcessor().getTilesX() *getTileProcessor().getTileSize();
// int width = getTileProcessor().getTilesX() *getTileProcessor().getTileSize();
int height = data.length/width;
String set_name = getImageName();
if (set_name == null ) {
......@@ -15207,7 +15493,7 @@ public class QuadCLTCPU {
}
/** broke
public boolean writeRatingFile( // USED in lwir
int debugLevel
)
......@@ -15234,7 +15520,7 @@ public class QuadCLTCPU {
List<String> lines = Arrays.asList(correctionsParameters.default_rating+"");
Path path = Paths.get(fname);
try {
Files.write(path,lines, Charset.forName("UTF-8"));
Files.write(path,lines); // , Charset.forName("UTF-8"));
} catch (IOException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
......@@ -15252,7 +15538,8 @@ public class QuadCLTCPU {
}
return true;
}
*/
public void setPassAvgRBGA( // get image from a single pass, return relative path for x3d // USED in lwir
CLTParameters clt_parameters,
int scanIndex,
......
......@@ -40,6 +40,7 @@ import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.correction.EyesisCorrections;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.x3d.export.GlTfExport;
import com.elphel.imagej.x3d.export.Render3D;
import com.elphel.imagej.x3d.export.TriMesh;
import com.elphel.imagej.x3d.export.WavefrontExport;
import com.elphel.imagej.x3d.export.X3dOutput;
......@@ -2450,6 +2451,7 @@ public class TexturedModel {
final boolean updateStatus,
final int debugLevel)
{
final boolean render_hdr = true; //false; // true; // generate textures w/o normalization to generate undistorted
final boolean batch_mode = clt_parameters.multiseq_run; // batch_run;
final boolean gltf_emissive = clt_parameters.gltf_emissive;
final boolean use_alpha_blend = clt_parameters.gltf_alpha_blend;
......@@ -2695,6 +2697,7 @@ public class TexturedModel {
scenes_sel[i] = true;
}
double [][][][] lin_text0 = render_hdr ? new double [1][][][] : null;
double[][][] faded_textures = getInterCombinedTextures( // return ImagePlus[] matching tileClusters[], with alpha
clt_parameters, // final CLTParameters clt_parameters,
......@@ -2708,7 +2711,18 @@ public class TexturedModel {
renormalize, // final boolean re-normalize, // false - use normalizations from previous scenes to keep consistent colors
max_disparity_lim, // final double max_disparity_lim, // 100.0; // do not allow stray disparities above this
min_trim_disparity, // final double min_trim_disparity, // 2.0; // do not try to trim texture outlines with lower disparities
lin_text0, // final double [][][][] lin_textures, // null or [1][][][] to return non-normalized textures
debugLevel); // final int debug_level)
double [][][] lin_textures = (lin_text0 != null) ? lin_text0[0] : null;
if (no_alpha && (lin_textures!= null)){
for (int nslice = 0; nslice < lin_textures.length; nslice++) {
int nalpha = lin_textures[nslice].length-1;
for (int i = 0; i < lin_textures[nslice][nalpha].length ; i++) {
lin_textures[nslice][nalpha][i] = Double.isNaN(lin_textures[nslice][0][i])? 0.0: 1.0;
}
}
}
ImagePlus[] combined_textures = getInterCombinedTextures( // return ImagePlus[] matching tileClusters[], with alpha
clt_parameters, // final CLTParameters clt_parameters,
no_alpha, // final boolean no_alpha,
......@@ -2875,7 +2889,7 @@ public class TexturedModel {
}
// String texturePath = imp_texture_cluster.getTitle()+".png";
String texturePath = imp_texture_cluster.getOriginalFileInfo().fileName;
int lin_width = imp_texture_cluster.getWidth();
double [] scan_disparity = tileClusters[nslice].getSubDisparity(sub_i); // limited to cluster bounds
boolean [] scan_selected = tileClusters[nslice].getSubSelected(sub_i); // limited to cluster bounds
int [] scan_border_int = tileClusters[nslice].getSubBorderInt(sub_i); // limited to cluster bounds
......@@ -2927,6 +2941,8 @@ public class TexturedModel {
wfOutput, // output WSavefront if not null
tri_meshes, // ArrayList<TriMesh> tri_meshes,
texturePath,
(lin_textures == null) ? null: lin_textures[nslice], // double [][] lin_texture, // null or rendering rectilinear hdr image
lin_width, // int lin_width,
"shape_id-"+cluster_index, // id
null, // class
roi, // scan.getTextureBounds(),
......@@ -2963,7 +2979,90 @@ public class TexturedModel {
}
// if (imp_textures[nslice] != null)
} // for (int nslice = 0; nslice < tileClusters.length; nslice++){
if (render_hdr) {
boolean use_lma = true; // ;
double discard_low = 0.01; // fraction of all pixels
double discard_high = 0.5; // fraction of all pixels
double discard_adisp = 0.2; // discard above/below this fraction of average height
double discard_rdisp = 0.02; // discard above/below this fraction of average height
double pix_size = 0.005; // hdr_x0y0, // in meters
int max_image_width = 3200; // increase pixel size as a power of 2 until image fits
boolean crop_empty = true;
int crop_extra = 20;
int [] tex_pals = {0,1,2};
int tex_palette_start = 0;
int tex_palette_end = 12;
int [] hdr_whs = new int[3];
double [] hdr_x0y0 = new double[2];
double [][] to_ground_xyxatr = scenes[ref_index].getGround(
use_lma, // boolean use_lma,
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
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
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(
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)
// String model_name = ref_scene.correctionsParameters.getModelName(ref_scene.getImageName());
String suffix ="-RECT";
if (clt_parameters.tex_um) {
suffix+="-UM"+(clt_parameters.tex_um_sigma)+"_"+(clt_parameters.tex_um_weight);
}
suffix +="-PIX"+pix_size * hdr_whs[2];
scenes[ref_index].saveDoubleArrayInModelDirectory(
suffix+"-FULL", // String suffix,
null, // String [] labels, // or null
full_render, // double [][] data,
hdr_whs[0], // int width, // int tilesX,
hdr_whs[1]); // int height, // int tilesY,
if (crop_empty || (crop_extra > 0)) {
int [] wh = new int[2];
double [][] img_cropped = Render3D.cropRectified(
crop_empty, // boolean crop_empty,
last_is_alpha, // boolean last_is_alpha,
crop_extra, // int crop_extra,
hdr_whs[0], // int width,
wh, // int [] wh, // should be initialized to int [2]
full_render); // double [][] img_src)
scenes[ref_index].saveDoubleArrayInModelDirectory(
suffix+"-CROP", // String suffix,
null, // String [] labels, // or null
img_cropped, // double [][] data,
wh[0], // int width, // int tilesX,
wh[1]); // int height, // int tilesY,
// for (int tex_palette = tex_palette_start; tex_palette <= tex_palette_end; tex_palette++) {
for (int tex_palette: tex_pals) {
scenes[ref_index].writeLwirPreview(
clt_parameters, // final CLTParameters clt_parameters,
img_cropped[0], // double [] data,
wh[0], // int width, // int tilesX,
null, // QuadCLT scene,
tex_palette, // int tex_palette,
suffix+"-CROP"+"-PAL"+tex_palette, // +tex_palette, // String suffix,
debugLevel); // int debugLevel)
}
}
}
if (dbg_mesh_imgs != null) {
ShowDoubleFloatArrays.showArrays(
dbg_mesh_imgs,
......@@ -3045,9 +3144,8 @@ public class TexturedModel {
System.out.println("**** output3d(): likely a bug (not copied?), temporary fix ***");
correctionsParameters.use_set_dirs = true;
}
ref_scene.writeKml (null, debugLevel);
ref_scene.writeRatingFile (debugLevel);
/// ref_scene.writeRatingFile (debugLevel); broke
Runtime.getRuntime().gc();
System.out.println("output3d(): generating 3d output files finished at "+
IJ.d2s(0.000000001*(System.nanoTime()-startStepTime),3)+" sec, --- Free memory25="+Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
......@@ -6992,6 +7090,7 @@ public class TexturedModel {
final boolean renormalize,
final double max_disparity_lim,
final double min_trim_disparity,
final double [][][][] lin_textures, // null or [1][][][] to return non-normalized textures
final int debugLevel)
{
// TODO: ***** scenes with high motion blur also have high ERS to be corrected ! *****
......@@ -7386,7 +7485,20 @@ public class TexturedModel {
tex_um_sigma, // final double um_sigma,
tex_um_weight); // final double um_weight)
}
if (lin_textures != null) {
// duplicate texture before normalization
lin_textures[0] = new double [faded_textures.length][][];
for (int slice = 0; slice < faded_textures.length; slice++) { // slices
if (faded_textures[slice] != null) {
lin_textures[0][slice] = new double[faded_textures[slice].length][];
for (int chn = 0; chn < faded_textures[slice].length; chn++) {
if (faded_textures[slice][chn] != null) {
lin_textures[0][slice][chn] = faded_textures[slice][chn].clone();
}
}
}
}
}
if (save_interm_textures || save_um_texture0) {
double [][] dbg_textures = new double [faded_textures.length * faded_textures[0].length][faded_textures[0][0].length];
String [] dbg_titles = new String[dbg_textures.length];
......@@ -7508,7 +7620,7 @@ public class TexturedModel {
double [] inverted_table = QuadCLTCPU.invertHistogramNormalization(
norm_table, // double [] direct_table, // last is <1.0, first > 0
tex_hist_bins); // int num_bins)
QuadCLTCPU.applyTexturesNormHist(
QuadCLTCPU.applyTexturesNormHist( // does not modify alpha if any
hist_ignore_alpha, // final boolean ignore_alpha
faded_textures, // final double [][][] textures, // [nslices][nchn][i]
cold_hot, // final double [] min_max,
......@@ -7527,7 +7639,7 @@ public class TexturedModel {
dbg_titles[i] = dbg_subtitles[i % dbg_subtitles.length] + "-" + (i / dbg_subtitles.length);
}
if (save_preview) {
ref_scene.writePreview( // may movbe to different (earlier) stage of processing, (search for "-combined_textures")
ref_scene.writePreview( // may move to different (earlier) stage of processing, (search for "-combined_textures")
dbg_textures[0], // double [] data,
debugLevel); // int debugLevel
// Trying different palettes
......
/**
** Render3D - render 3D view to the specified plane
**
** Copyright (C) 2023 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** Render3D.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.x3d.export;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.QuadCLT;
public class Render3D {
static final int THREADS_MAX = 100; // maximal number of threads to launch
static double [] ZERO3 = new double[3];
final double [][] toground;
final double [][] tocam;
final QuadCLT ref_scene;
final double pixel_per_m; // pixels per meter
final int out_width; // output rendered image width in pixels
final int out_height; // output rendered image height in pixels
// final String x3d_dir;
// final String model_name;
final Vector3D ground_origin;
final Vector3D ground_x;
final Vector3D ground_y;
final Vector3D ground_normal;
final Vector3D ground_normal_unit;
final double above_ground;
final double [] xy_offs;
public Render3D (
// String x3d_dir,
// String model_name,
QuadCLT ref_scene, // all coordinates relative to this scene
double [][] toground, // projection plane center relative to reference scene
double pixel_size, // in meters
double [] x0_y0, // usually negative - top-left point of the output render
int out_width, // output rendered image width in pixels
int out_height){ // output rendered image height in pixels
// this.x3d_dir = x3d_dir;
// this.model_name = model_name;
this.ref_scene = ref_scene;
this.pixel_per_m = 1.0 / pixel_size;
this.out_width = out_width;
this.out_height = out_height;
this.toground = toground;
this.tocam = ErsCorrection.invertXYZATR(this.toground);
// double [][] test1 = ErsCorrection.invertXYZATR(this.tocam); //OK
// double [][] test2 = ErsCorrection.invertXYZATR(test1); // OK
// ground plane x0, y0 in camera coordinates
ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1], 0.0}));
Vector3D v3x1 = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0] + 1.0,x0_y0[1], 0.0}));
Vector3D v3y1 = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1]+1.0, 0.0}));
ground_x = v3x1.subtract(this.ground_origin).normalize(); // unity in plane X direction
ground_y = v3y1.subtract(this.ground_origin).normalize(); // unity in plane Y direction
ground_normal = ground_origin.subtract(
ground_x.scalarMultiply(ground_origin.dotProduct(ground_x))).subtract(
ground_y.scalarMultiply(ground_origin.dotProduct(ground_y)));
ground_normal_unit = ground_normal.normalize(); // unitary vector normal and away from the ground plane
above_ground = ground_normal.getNorm();
xy_offs = new double[] {ground_x.dotProduct(ground_origin), ground_y.dotProduct(ground_origin)};
// double [] test_gp=projectToPlanePixels(ground_origin);
}
public double [] projectToPlaneLinear(Vector3D v3) { // get ground place 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];
xy[0] = ground_x.dotProduct(in_plane) - xy_offs[0];
xy[1] = ground_y.dotProduct(in_plane) - xy_offs[1];
xy[2] = z;
return xy;
}
public double [] projectToPlanePixels(Vector3D v3) { // get ground place 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];
xy[0] = pixel_per_m * (ground_x.dotProduct(in_plane) - xy_offs[0]);
xy[1] = pixel_per_m * (ground_y.dotProduct(in_plane) - xy_offs[1]);
xy[2] = pixel_per_m * z; // positive distance from the camera in pixels (same linear scale)
return xy;
}
public static double cross2 (double [] v1, double [] v2) {
return v1[0]*v2[1]-v1[1]*v2[0];
}
public static boolean cross2ccw (double [] v1, double [] v2) {
return (v1[0]*v2[1]-v1[1]*v2[0]) > 0;
}
public static double dot2 (double [] v1, double [] v2) {
return v1[0]*v2[0]+v1[1]*v2[1];
}
public static double [] normalize2 (double [] v) {
double l = FastMath.sqrt (v[0] * v[0] + v[1]*v[1]);
return new double[] {v[0]/l,v[1]/l};
}
public static double [] orthonormSingle2(double [] v1, double [] v2) {
double p11 = dot2(v1, v1);
double p12 = dot2(v1, v2);
double p22 = dot2(v2, v2);
double k = p12/p11;
double a = p11/(p22*p11 - p12 * p12);
return new double[] {a * (v2[0] - v1[0]*k),a * (v2[1] - v1[1]*k)};
}
/**
* p = v1 * (orthonorm2(v1,v2)[0]).dot(p)) + v2 * (orthonorm2(v1,v2)[1]).dot(p))
* @param v1
* @param v2
* @return
*/
public static double [][] orthonorm2(double [] v1, double [] v2) {
return new double [][] {orthonormSingle2(v2,v1), orthonormSingle2(v1,v2)};
}
public static double [][] cropRectified(
boolean crop_empty,
boolean last_is_alpha,
int crop_extra,
int width,
int [] wh, // should be initialized to int [2]
double [][] img_src){
int height = img_src[0].length/width;
int indx_alpha = img_src.length - 1;
int marg_top=0,marg_left=0,marg_bottom=0,marg_right=0;
if (crop_empty) {
int [][] xy_min_max= null; // new int[2][2];
for (int iy = 0; iy<height; iy++) {
for (int ix=0; ix<width; ix++) {
int indx=iy * width +ix;
if (last_is_alpha? (img_src[indx_alpha][indx] > 0.0) : !Double.isNaN(img_src[0][indx])) {
if (xy_min_max == null) {
xy_min_max=new int [][] {{ix,ix},{iy,iy}};
} else {
if (ix < xy_min_max[0][0]) xy_min_max[0][0] = ix;
else if (ix > xy_min_max[0][1]) xy_min_max[0][1] = ix;
if (iy < xy_min_max[1][0]) xy_min_max[1][0] = iy;
else if (iy > xy_min_max[1][1]) xy_min_max[1][1] = iy;
}
}
}
}
marg_top = xy_min_max[1][0];
marg_left = xy_min_max[0][0];
marg_bottom = height - 1 - xy_min_max[1][1];
marg_right = width - 1 - xy_min_max[0][1];
}
marg_top += crop_extra;
marg_left += crop_extra;
marg_bottom += crop_extra;
marg_right += crop_extra;
int out_width = width - marg_left - marg_right;
int out_height = height - marg_top - marg_bottom;
wh[0] = out_width;
wh[1] = out_height;
double [][] img_cropped = new double [img_src.length][out_width*out_height];
for (int row = 0; row < out_height; row++) {
for (int chn = 0; chn < img_cropped.length; chn++) {
System.arraycopy(
img_src[chn],
(row + marg_top) * width + marg_left,
img_cropped[chn],
row * out_width ,
out_width);
}
}
return img_cropped;
}
public double [][] render3dPlane(
final ArrayList<TriMesh> tri_meshes,
final boolean last_is_alpha,
final QuadCLT ref_scene, // all coordinates relative to this scene
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;
}
// 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]]));
//[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
// 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;
}
}
......@@ -51,21 +51,37 @@ public class TriMesh {
{{0,0},{1,0},{ 1,1}}};// TRI_RIGHT_DOWN
public String texture_image;
public double [][] texture_pixels=null; // alternative texture representation [chn][pix]
public int texture_width; // texture width when provided as an array
public double [][] worldXYZ;
public double [][] texCoord;
public int [][] triangles;
public TriMesh (
String texture_image,
double [][] texture_pixels,
int texture_width,
double [][] worldXYZ,
double [][] texCoord,
int [][] triangles) {
this.texture_image = texture_image;
this.texture_image = texture_image;
this.texture_pixels = texture_pixels;
this.texture_width = texture_width;
this.worldXYZ = worldXYZ;
this.texCoord = texCoord;
this.triangles = triangles;
this.triangles = (triangles!=null) ? triangles :new int[0][]; // maybe not needed?
}
public String getImage() {return texture_image;}
int [][] getTriangles() {return triangles;}
public double [][] getTexturePixels(){
return texture_pixels;
}
public int getTextureWidth() {
return texture_width;
}
public int getTextureHeight() {
return texture_pixels[0].length/texture_width;
}
double [][] getTexCoord() {
return texCoord;
}
......@@ -2276,6 +2292,8 @@ public class TriMesh {
if (tri_meshes != null) {
tri_meshes.add(new TriMesh(
texturePath, // String texture_image,
null, // double [][] texture_pixels,
0, // lin_width, // int texture_width,
worldXYZ, // double [][] worldXYZ,
texCoord, // double [][] texCoord,
triangles)); // int [][] triangles
......@@ -2293,6 +2311,8 @@ public class TriMesh {
WavefrontExport wfOutput, // output WSavefront if not null
ArrayList<TriMesh> tri_meshes,
String texturePath,
double [][] lin_texture, // null or rendering rectilinear hdr image
int lin_width,
String id,
String class_name,
Rectangle bounds,
......@@ -2503,6 +2523,8 @@ public class TriMesh {
if (tri_meshes != null) {
tri_meshes.add(new TriMesh(
texturePath, // String texture_image,
lin_texture, // double [][] texture_pixels,
lin_width, // int texture_width,
worldXYZ, // double [][] worldXYZ,
texCoord, // double [][] texCoord,
triangles)); // int [][] triangles
......
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