Skip to content

I
imagej-elphel
Commit 3f6c6c63 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

plane detection

parent 8904657f
Show whitespace changes
Inline Side-by-side
Showing with 1677 additions and 104 deletions
src/main/java/CLTPass3d.java
View file @ 3f6c6c63
...@@ -516,7 +516,7 @@ public class CLTPass3d{ ...@@ -516,7 +516,7 @@ public class CLTPass3d{
} }
public void setSuperTiles( public SuperTiles setSuperTiles(
double step_near, double step_near,
double step_far, double step_far,
double step_threshold, double step_threshold,
...@@ -536,6 +536,7 @@ public class CLTPass3d{ ...@@ -536,6 +536,7 @@ public class CLTPass3d{
strength_floor, strength_floor,
strength_pow, strength_pow,
stBlurSigma); stBlurSigma);
return this.superTiles;
} }
public double [] showDisparityHistogram() public double [] showDisparityHistogram()
{ {
......
src/main/java/DisparityProcessor.java
View file @ 3f6c6c63
...@@ -398,7 +398,7 @@ public class DisparityProcessor { ...@@ -398,7 +398,7 @@ public class DisparityProcessor {
ImageDtt.startAndJoin(threads); ImageDtt.startAndJoin(threads);
double [][] dbg_data = {measured_disparity,disp_data[0],disp_data[1],dbg_pull[0],dbg_pull[1],dbg_pull[2],strength}; double [][] dbg_data = {measured_disparity,disp_data[0],disp_data[1],dbg_pull[0],dbg_pull[1],dbg_pull[2],strength};
String [] titles = {"measured","[0]","[1]","avg","meas","pull","strength"}; String [] titles = {"measured","[0]","[1]","avg","meas","pull","strength"};
if ((debugLevel>0) && ((pass ==0) || (pass >= (num_passes-2)))){ if ((debugLevel> 2) && ((pass ==0) || (pass >= (num_passes-2)))){
sdfa_instance.showArrays(dbg_data,tilesX, tilesY, true, "disp_smoothed",titles); sdfa_instance.showArrays(dbg_data,tilesX, tilesY, true, "disp_smoothed",titles);
} }
...@@ -978,7 +978,9 @@ public class DisparityProcessor { ...@@ -978,7 +978,9 @@ public class DisparityProcessor {
int ty1 = ty + TRANSITIONS[tDir][dirChoice][1]; int ty1 = ty + TRANSITIONS[tDir][dirChoice][1];
int tDir1 = TRANSITIONS[tDir][dirChoice][2]; int tDir1 = TRANSITIONS[tDir][dirChoice][2];
// Was already here (in the same direction? // Was already here (in the same direction?
if (field[ty1][tx1][tDir1] == FLD_WALKED) { if ((tx1 < 0) || (ty1 < 0) || (tx1 > tilesX) || (ty1 > tilesY)){
prohib = true;
} else if (field[ty1][tx1][tDir1] == FLD_WALKED) { // 325
break walking; break walking;
} else if (field[ty1][tx1][tDir1] == FLD_EMPTY) { } else if (field[ty1][tx1][tDir1] == FLD_EMPTY) {
tx = tx1; tx = tx1;
...@@ -1103,7 +1105,7 @@ public class DisparityProcessor { ...@@ -1103,7 +1105,7 @@ public class DisparityProcessor {
geom[nTile] = new int [8]; geom[nTile] = new int [8];
int [] dbg_geom = geom[nTile]; int [] dbg_geom = geom[nTile];
int [] dbg_neib = {neighbors[nTile - tilesX],neighbors[nTile + 1] , neighbors[nTile + tilesX],neighbors[nTile - 1]}; // int [] dbg_neib = {neighbors[nTile - tilesX],neighbors[nTile + 1] , neighbors[nTile + tilesX],neighbors[nTile - 1]}; // out of bounds
int neib = neighbors[nTile]; int neib = neighbors[nTile];
if (neib != 0b11111111){ // do nothing for internal tiles if (neib != 0b11111111){ // do nothing for internal tiles
int tileY = nTile/tilesX; int tileY = nTile/tilesX;
......
src/main/java/EyesisCorrectionParameters.java
View file @ 3f6c6c63
...@@ -2028,6 +2028,7 @@ public class EyesisCorrectionParameters { ...@@ -2028,6 +2028,7 @@ public class EyesisCorrectionParameters {
public double min_clstr_max = 0.25; // Minimal maximal strength of the cluster public double min_clstr_max = 0.25; // Minimal maximal strength of the cluster
public int fill_gaps = 4; // same as in grow - 1: 4 directions by 1 step, 2: 8 directions by 1 step. +2*n - alternating hor/vert public int fill_gaps = 4; // same as in grow - 1: 4 directions by 1 step, 2: 8 directions by 1 step. +2*n - alternating hor/vert
public int fill_final = 50; // same as fill_gaps, on the final pass
public int min_clstr_block = 3; // number of tiles in a cluster to block (just non-background?) public int min_clstr_block = 3; // number of tiles in a cluster to block (just non-background?)
public int bgnd_grow = 2; // number of tiles to grow (1 - hor/vert, 2 - hor/vert/diagonal) public int bgnd_grow = 2; // number of tiles to grow (1 - hor/vert, 2 - hor/vert/diagonal)
...@@ -2113,7 +2114,7 @@ public class EyesisCorrectionParameters { ...@@ -2113,7 +2114,7 @@ public class EyesisCorrectionParameters {
public double stStepNear = 0.5; // Disaprity histogram step for near objects public double stStepNear = 0.5; // Disaprity histogram step for near objects
public double stStepThreshold = 1.0; // Disaprity threshold to switch cfrom linear to logarithmic steps public double stStepThreshold = 1.0; // Disaprity threshold to switch cfrom linear to logarithmic steps
public double stMinDisparity = 0.0; // Minimal disparity (center of a bin) public double stMinDisparity = 0.0; // Minimal disparity (center of a bin)
public double stMaxDisparity = 50.0; // Maximal disparity (center of a bin) public double stMaxDisparity = 15.0; // Maximal disparity (center of a bin)
public double stFloor = 0.15; // Subtract from strength, discard negative public double stFloor = 0.15; // Subtract from strength, discard negative
public double stPow = 1.0; // raise strength to this power public double stPow = 1.0; // raise strength to this power
public double stSigma = 1.5; // Blur disparity histogram (sigma in bins) public double stSigma = 1.5; // Blur disparity histogram (sigma in bins)
...@@ -2141,6 +2142,13 @@ public class EyesisCorrectionParameters { ...@@ -2141,6 +2142,13 @@ public class EyesisCorrectionParameters {
public double grow_disp_step = 6.0; // Increase disparity (from maximal tried) if nothing found in that tile // TODO: handle enclosed dips? public double grow_disp_step = 6.0; // Increase disparity (from maximal tried) if nothing found in that tile // TODO: handle enclosed dips?
public double grow_min_diff = 0.5; // Grow more only if at least one channel has higher variance from others for the tile public double grow_min_diff = 0.5; // Grow more only if at least one channel has higher variance from others for the tile
public double plDispNorm = 3.0; // Normalize disparities to the average if above (now only for eigenvalue comparison)
public int plMinPoints = 5; // Minimal number of points for plane detection
public double plTargetEigen = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
public double plFractOutliers = 0.3; // Maximal fraction of outliers to remove
public int plMaxOutliers = 20; // Maximal number of outliers to remove
// other debug images // other debug images
public boolean show_ortho_combine = false; // Show 'ortho_combine' public boolean show_ortho_combine = false; // Show 'ortho_combine'
public boolean show_refine_supertiles = false; // show 'refine_disparity_supertiles' public boolean show_refine_supertiles = false; // show 'refine_disparity_supertiles'
...@@ -2266,6 +2274,7 @@ public class EyesisCorrectionParameters { ...@@ -2266,6 +2274,7 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"min_clstr_max", this.min_clstr_max +""); properties.setProperty(prefix+"min_clstr_max", this.min_clstr_max +"");
properties.setProperty(prefix+"fill_gaps", this.fill_gaps+""); properties.setProperty(prefix+"fill_gaps", this.fill_gaps+"");
properties.setProperty(prefix+"fill_final", this.fill_final+"");
properties.setProperty(prefix+"min_clstr_block", this.min_clstr_block+""); properties.setProperty(prefix+"min_clstr_block", this.min_clstr_block+"");
properties.setProperty(prefix+"bgnd_grow", this.bgnd_grow+""); properties.setProperty(prefix+"bgnd_grow", this.bgnd_grow+"");
...@@ -2372,6 +2381,12 @@ public class EyesisCorrectionParameters { ...@@ -2372,6 +2381,12 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"grow_disp_step", this.grow_disp_step +""); properties.setProperty(prefix+"grow_disp_step", this.grow_disp_step +"");
properties.setProperty(prefix+"grow_min_diff", this.grow_min_diff +""); properties.setProperty(prefix+"grow_min_diff", this.grow_min_diff +"");
properties.setProperty(prefix+"plDispNorm", this.plDispNorm +"");
properties.setProperty(prefix+"plMinPoints", this.plMinPoints+"");
properties.setProperty(prefix+"plTargetEigen", this.plTargetEigen +"");
properties.setProperty(prefix+"plFractOutliers", this.plFractOutliers +"");
properties.setProperty(prefix+"plMaxOutliers", this.plMaxOutliers+"");
properties.setProperty(prefix+"show_ortho_combine", this.show_ortho_combine+""); properties.setProperty(prefix+"show_ortho_combine", this.show_ortho_combine+"");
properties.setProperty(prefix+"show_refine_supertiles", this.show_refine_supertiles+""); properties.setProperty(prefix+"show_refine_supertiles", this.show_refine_supertiles+"");
properties.setProperty(prefix+"show_bgnd_nonbgnd", this.show_bgnd_nonbgnd+""); properties.setProperty(prefix+"show_bgnd_nonbgnd", this.show_bgnd_nonbgnd+"");
...@@ -2490,6 +2505,7 @@ public class EyesisCorrectionParameters { ...@@ -2490,6 +2505,7 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"fill_gaps")!=null) this.fill_gaps=Integer.parseInt(properties.getProperty(prefix+"fill_gaps")); if (properties.getProperty(prefix+"fill_gaps")!=null) this.fill_gaps=Integer.parseInt(properties.getProperty(prefix+"fill_gaps"));
if (properties.getProperty(prefix+"fill_final")!=null) this.fill_final=Integer.parseInt(properties.getProperty(prefix+"fill_final"));
if (properties.getProperty(prefix+"min_clstr_block")!=null) this.min_clstr_block=Integer.parseInt(properties.getProperty(prefix+"min_clstr_block")); if (properties.getProperty(prefix+"min_clstr_block")!=null) this.min_clstr_block=Integer.parseInt(properties.getProperty(prefix+"min_clstr_block"));
if (properties.getProperty(prefix+"bgnd_grow")!=null) this.bgnd_grow=Integer.parseInt(properties.getProperty(prefix+"bgnd_grow")); if (properties.getProperty(prefix+"bgnd_grow")!=null) this.bgnd_grow=Integer.parseInt(properties.getProperty(prefix+"bgnd_grow"));
...@@ -2593,6 +2609,12 @@ public class EyesisCorrectionParameters { ...@@ -2593,6 +2609,12 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"grow_disp_step")!=null) this.grow_disp_step=Double.parseDouble(properties.getProperty(prefix+"grow_disp_step")); if (properties.getProperty(prefix+"grow_disp_step")!=null) this.grow_disp_step=Double.parseDouble(properties.getProperty(prefix+"grow_disp_step"));
if (properties.getProperty(prefix+"grow_min_diff")!=null) this.grow_min_diff=Double.parseDouble(properties.getProperty(prefix+"grow_min_diff")); if (properties.getProperty(prefix+"grow_min_diff")!=null) this.grow_min_diff=Double.parseDouble(properties.getProperty(prefix+"grow_min_diff"));
if (properties.getProperty(prefix+"plDispNorm")!=null) this.plDispNorm=Double.parseDouble(properties.getProperty(prefix+"plDispNorm"));
if (properties.getProperty(prefix+"plMinPoints")!=null) this.plMinPoints=Integer.parseInt(properties.getProperty(prefix+"plMinPoints"));
if (properties.getProperty(prefix+"plTargetEigen")!=null) this.plTargetEigen=Double.parseDouble(properties.getProperty(prefix+"plTargetEigen"));
if (properties.getProperty(prefix+"plFractOutliers")!=null) this.plFractOutliers=Double.parseDouble(properties.getProperty(prefix+"plFractOutliers"));
if (properties.getProperty(prefix+"plMaxOutliers")!=null) this.plMaxOutliers=Integer.parseInt(properties.getProperty(prefix+"plMaxOutliers"));
if (properties.getProperty(prefix+"show_ortho_combine")!=null) this.show_ortho_combine=Boolean.parseBoolean(properties.getProperty(prefix+"show_ortho_combine")); if (properties.getProperty(prefix+"show_ortho_combine")!=null) this.show_ortho_combine=Boolean.parseBoolean(properties.getProperty(prefix+"show_ortho_combine"));
if (properties.getProperty(prefix+"show_refine_supertiles")!=null) this.show_refine_supertiles=Boolean.parseBoolean(properties.getProperty(prefix+"show_refine_supertiles")); if (properties.getProperty(prefix+"show_refine_supertiles")!=null) this.show_refine_supertiles=Boolean.parseBoolean(properties.getProperty(prefix+"show_refine_supertiles"));
if (properties.getProperty(prefix+"show_bgnd_nonbgnd")!=null) this.show_bgnd_nonbgnd=Boolean.parseBoolean(properties.getProperty(prefix+"show_bgnd_nonbgnd")); if (properties.getProperty(prefix+"show_bgnd_nonbgnd")!=null) this.show_bgnd_nonbgnd=Boolean.parseBoolean(properties.getProperty(prefix+"show_bgnd_nonbgnd"));
...@@ -2725,6 +2747,7 @@ public class EyesisCorrectionParameters { ...@@ -2725,6 +2747,7 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Minimal maximal strength of the cluster", this.min_clstr_max, 3); gd.addNumericField("Minimal maximal strength of the cluster", this.min_clstr_max, 3);
gd.addNumericField("Fill gaps betsween clusters, see comments for 'grow'", this.fill_gaps, 0); gd.addNumericField("Fill gaps betsween clusters, see comments for 'grow'", this.fill_gaps, 0);
gd.addNumericField("Same as fill_gaps above, on the final pass", this.fill_final, 0);
gd.addNumericField("Number of tiles in a cluster to block (just non-background?)", this.min_clstr_block, 0); gd.addNumericField("Number of tiles in a cluster to block (just non-background?)", this.min_clstr_block, 0);
gd.addNumericField("Number of tiles to grow tile selection (1 - hor/vert, 2 - hor/vert/diagonal)", this.bgnd_grow, 0); gd.addNumericField("Number of tiles to grow tile selection (1 - hor/vert, 2 - hor/vert/diagonal)", this.bgnd_grow, 0);
...@@ -2835,6 +2858,13 @@ public class EyesisCorrectionParameters { ...@@ -2835,6 +2858,13 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Trust measured disparity within +/- this value", this.grow_disp_trust, 6); gd.addNumericField("Trust measured disparity within +/- this value", this.grow_disp_trust, 6);
gd.addNumericField("Increase disparity (from maximal tried) if nothing found in that tile", this.grow_disp_step, 6); gd.addNumericField("Increase disparity (from maximal tried) if nothing found in that tile", this.grow_disp_step, 6);
gd.addNumericField("Grow more only if at least one channel has higher variance from others for the tile", this.grow_min_diff, 6); gd.addNumericField("Grow more only if at least one channel has higher variance from others for the tile", this.grow_min_diff, 6);
gd.addMessage ("--- Planes detection ---");
gd.addNumericField("Normalize disparities to the average if above", this.plDispNorm, 6);
gd.addNumericField("Minimal number of points for plane detection", this.plMinPoints, 0);
gd.addNumericField("Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below", this.plTargetEigen, 6);
gd.addNumericField("Maximal fraction of outliers to remove", this.plFractOutliers, 6);
gd.addNumericField("Maximal number of outliers to remove", this.plMaxOutliers, 0);
gd.addMessage ("--- Other debug images ---"); gd.addMessage ("--- Other debug images ---");
gd.addCheckbox ("Show 'ortho_combine'", this.show_ortho_combine); gd.addCheckbox ("Show 'ortho_combine'", this.show_ortho_combine);
gd.addCheckbox ("Show 'refine_disparity_supertiles'", this.show_refine_supertiles); gd.addCheckbox ("Show 'refine_disparity_supertiles'", this.show_refine_supertiles);
...@@ -2960,6 +2990,7 @@ public class EyesisCorrectionParameters { ...@@ -2960,6 +2990,7 @@ public class EyesisCorrectionParameters {
this.min_clstr_max= gd.getNextNumber(); this.min_clstr_max= gd.getNextNumber();
this.fill_gaps= (int) gd.getNextNumber(); this.fill_gaps= (int) gd.getNextNumber();
this.fill_final= (int) gd.getNextNumber();
this.min_clstr_block= (int) gd.getNextNumber(); this.min_clstr_block= (int) gd.getNextNumber();
this.bgnd_grow= (int) gd.getNextNumber(); this.bgnd_grow= (int) gd.getNextNumber();
...@@ -3065,6 +3096,12 @@ public class EyesisCorrectionParameters { ...@@ -3065,6 +3096,12 @@ public class EyesisCorrectionParameters {
this.grow_disp_step= gd.getNextNumber(); this.grow_disp_step= gd.getNextNumber();
this.grow_min_diff= gd.getNextNumber(); this.grow_min_diff= gd.getNextNumber();
this.plDispNorm= gd.getNextNumber();
this.plMinPoints= (int) gd.getNextNumber();
this.plTargetEigen= gd.getNextNumber();
this.plFractOutliers= gd.getNextNumber();
this.plMaxOutliers= (int) gd.getNextNumber();
this.show_ortho_combine= gd.getNextBoolean(); this.show_ortho_combine= gd.getNextBoolean();
this.show_refine_supertiles= gd.getNextBoolean(); this.show_refine_supertiles= gd.getNextBoolean();
this.show_bgnd_nonbgnd= gd.getNextBoolean(); // first on second pass this.show_bgnd_nonbgnd= gd.getNextBoolean(); // first on second pass
......
src/main/java/GeometryCorrection.java
View file @ 3f6c6c63
...@@ -329,8 +329,13 @@ public class GeometryCorrection { ...@@ -329,8 +329,13 @@ public class GeometryCorrection {
} }
/* /**
* Get real world coordinates from pixel coordinates and nominal disparity * Get real world coordinates from pixel coordinates and nominal disparity
* @param px horizontal pixel coordinate (right)
* @param py vertical pixel coordinate (down)
* @param disparity nominal disparity (pixels)
* @param correctDistortions true: correct lens distortions, false - no lens distortions
* @return {x, y, z} in meters
*/ */
public double [] getWorldCoordinates( public double [] getWorldCoordinates(
double px, double px,
...@@ -350,9 +355,224 @@ public class GeometryCorrection { ...@@ -350,9 +355,224 @@ public class GeometryCorrection {
double [] xyz = {x,y,z}; double [] xyz = {x,y,z};
return xyz; return xyz;
} }
/* Just for testing using delta instead of d */
public double [][] getWorldJacobian(
double px,
double py,
double disparity,
boolean correctDistortions, // correct distortion (will need corrected background too !)
double delta)
{
double [] dpxpy0 = {disparity,px,py};
double [][] jacobian = new double [3][3];
double [] xyz0 = getWorldCoordinates(dpxpy0[1],dpxpy0[2],dpxpy0[0],correctDistortions);
for (int i = 0; i< 3; i++){
double [] dpxpy = dpxpy0.clone();
dpxpy[i] += delta;
double [] xyz = getWorldCoordinates(dpxpy[1],dpxpy[2],dpxpy[0],correctDistortions);
for (int j = 0; j<3; j++){
jacobian[j][i] = (xyz[j] - xyz0[j])/delta;
}
}
return jacobian;
}
/**
* Get jacobian matrix - derivatives of real world [x,y,z] (in meters, right, up, to camera) by [disparity, px,py] (in pixels disparity, right, down)
* @param px horizontal pixel coordinate (right)
* @param py vertical pixel coordinate (down)
* @param disparity nominal disparity (pixels)
* @param correctDistortions true: correct lens distortions, false - no lens distortions
* @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}}
*/
public double [][] getWorldJacobian(
double px,
double py,
double disparity,
boolean correctDistortions, // correct distortion (will need corrected background too !)
boolean debug)
{
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double d_pXcd_d_px = 1.0;
double d_pYcd_d_py = 1.0;
double pXcd2_pYcd2 = pXcd*pXcd + pYcd*pYcd;
double rD = Math.sqrt(pXcd2_pYcd2)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rrD = rD / this.distortionRadius;
double d_rRD_d_px = pXcd * rD / pXcd2_pYcd2 / this.distortionRadius;
double d_rRD_d_py = pYcd * rD / pXcd2_pYcd2 / this.distortionRadius;
double rND2R = correctDistortions?(getRByRDist(rrD, false)): 1.0;
double rrND = rND2R * rrD; // relative to distortion radius non-distorted radius
// k = rD/r
double d_k_d_rrND = correctDistortions?getDerivRDistFromR(rrND):0.0;
double d_rND2R_d_rrD = - rND2R * rND2R * d_k_d_rrND / ( d_k_d_rrND * rrND + 1.0/ rND2R); // rrND);
/*
double d_rND2R_d_rrD0 = correctDistortions?(getDerivRByRDist(rrD, false)): 0.0;
double d_rND2R_d_rrD1 = correctDistortions?(getDerivRByRDist(rrD, false, 0.00001)): 0.0;
if (debug) {
System.out.println("getWorldJacobian(): d_rND2R_d_rrD="+d_rND2R_d_rrD+", d_rND2R_d_rrD0="+d_rND2R_d_rrD0+", d_rND2R_d_rrD1="+d_rND2R_d_rrD1 );
}
*/
double d_rND2R_d_px = d_rND2R_d_rrD * d_rRD_d_px;
double d_rND2R_d_py = d_rND2R_d_rrD * d_rRD_d_py;
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
double d_pXc_d_px = pXcd * d_rND2R_d_px + d_pXcd_d_px * rND2R; // incorrect
double d_pYc_d_py = pYcd * d_rND2R_d_py + d_pYcd_d_py * rND2R; // incorrect
double d_pXc_d_py = pXcd * d_rND2R_d_py; // incorrect (too small)?
double d_pYc_d_px = pYcd * d_rND2R_d_px; // incorrect (too small)?
double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (disparity * 0.001*this.pixelSize); // "+" - near, "-" far
double kx = SCENE_UNITS_SCALE * this.disparityRadius / disparity;
double ky = -SCENE_UNITS_SCALE * this.disparityRadius / disparity;
double x = kx * pXc;
double y = ky * pYc;
double d_z_d_disparity = -z / disparity;
double d_x_d_disparity = -x / disparity;
double d_y_d_disparity = -y / disparity;
// d_z_d_px == d_z_d_py ==0
double [][] jacobian =
{ {d_x_d_disparity, kx * d_pXc_d_px, kx * d_pXc_d_py},
{d_y_d_disparity, ky * d_pYc_d_px, ky * d_pYc_d_py},
{d_z_d_disparity, 0.0, 0.0}};
return jacobian; // xyz;
}
/**
* Get pixel disparity and coordinates from the real world coordinates (in meters)
* @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
* @param correctDistortions true: correct lens distortions, false - no lens distortions
* @return {disparity, px, py} (right, down)
*/
public double [] getImageCoordinates(
double [] xyz,
boolean correctDistortions) // correct distortion (will need corrected background too !)
{
double x = xyz[0];
double y = xyz[1];
double z = xyz[2];
double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm
double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels
double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels
double rND = Math.sqrt(pXc*pXc + pYc*pYc)*0.001*this.pixelSize; // mm
double rD2RND = correctDistortions?getRDistByR(rND/this.distortionRadius):1.0;
double px = pXc * rD2RND + 0.5 * this.pixelCorrectionWidth; // distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double py = pYc * rD2RND + 0.5 * this.pixelCorrectionHeight; // in pixels
double [] dxy = {disparity, px, py};
return dxy;
}
/* Just for testing using delta instead of d */
public double [][] getImageJacobian(
double [] xyz0,
boolean correctDistortions, // correct distortion (will need corrected background too !)
double delta)
{
double [][] jacobian = new double [3][3];
double [] dpxpy0 = getImageCoordinates(xyz0,correctDistortions);
for (int i = 0; i< 3; i++){
double [] xyz = xyz0.clone();
xyz[i] += delta;
double [] dpxpy = getImageCoordinates(xyz,correctDistortions);
for (int j = 0; j<3; j++){
jacobian[j][i] = (dpxpy[j] - dpxpy0[j])/delta;
}
}
return jacobian;
}
/**
* Get jacobian matrix - derivatives of [disparity, px,py] (in pixels disparity, right, down) by real world [x,y,z] (in meters, right, up, to camera)
* @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
* @param correctDistortions true: correct lens distortions, false - no lens distortions
* @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}}
*/
public double [][] getImageJacobian(
double [] xyz,
boolean correctDistortions,
int debugLevel)
{
if (debugLevel > 0){
System.out.println("getImageJacobian():");
}
double x = xyz[0];
double y = xyz[1];
double z = xyz[2];
double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
double d_disparity_d_z = -disparity / z; // no dependence on x,y
// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm
double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double d_pXc_d_z = -pXc / z; // pXc/disparity * d_disparity_d_z = pXc/disparity * (-disparity / z);
double d_pYc_d_z = -pYc / z; // pYc/disparity * d_disparity_d_z = pYc/disparity * (-disparity / z);
double d_pXc_d_x = disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double d_pYc_d_y = -disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double rND2 = pXc*pXc + pYc*pYc;
double rrND = Math.sqrt(rND2)*0.001*this.pixelSize/this.distortionRadius; // relative to distortion radius;
double d_rrND_d_pXc = pXc*rrND/rND2;
double d_rrND_d_pYc = pYc*rrND/rND2;
double rD2RND = correctDistortions?getRDistByR(rrND):1.0;
double d_rD2RND_d_rrND = correctDistortions?getDerivRDistFromR(rrND) : 0.0;
/*
double d_rD2RND_d_rrND0 = correctDistortions?getDerivRDistFromR(rrND,0.000001) : 0.0;
if (debugLevel > 0){
System.out.println("getImageJacobian():, d_rD2RND_d_rrND0 = "+d_rD2RND_d_rrND0+" ("+0.000001+"), d_rD2RND_d_rrND="+d_rD2RND_d_rrND);
}
*/
double p_px_d_x = d_pXc_d_x * (pXc * (d_rD2RND_d_rrND * d_rrND_d_pXc) + rD2RND); // / (0.001 * this.pixelSize);
double p_py_d_y = d_pYc_d_y * (pYc * (d_rD2RND_d_rrND * d_rrND_d_pYc) + rD2RND); // / (0.001 * this.pixelSize);
double p_px_d_y = pXc * (d_rD2RND_d_rrND * d_rrND_d_pYc * d_pYc_d_y); // / (0.001 * this.pixelSize);
double p_py_d_x = pYc * (d_rD2RND_d_rrND * d_rrND_d_pXc * d_pXc_d_x); // / (0.001 * this.pixelSize);
double d_rrND_d_z = d_rrND_d_pXc * d_pXc_d_z + d_rrND_d_pYc * d_pYc_d_z;
double p_px_d_z = (pXc * (d_rD2RND_d_rrND * d_rrND_d_z) + (d_pXc_d_z * rD2RND)); // / (0.001 * this.pixelSize);
double p_py_d_z = (pYc * (d_rD2RND_d_rrND * d_rrND_d_z) + (d_pYc_d_z * rD2RND)); // / (0.001 * this.pixelSize);
double [][] jacobian = {
{ 0.0, 0.0, d_disparity_d_z},
{p_px_d_x,p_px_d_y, p_px_d_z},
{p_py_d_x,p_py_d_y, p_py_d_z}};
return jacobian;
}
/* /*
* Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image * Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image
* and generic diparity, measured in pixels * and generic disparity, measured in pixels
*/ */
public double [][] getPortsCoordinates( public double [][] getPortsCoordinates(
...@@ -452,6 +672,51 @@ public class GeometryCorrection { ...@@ -452,6 +672,51 @@ public class GeometryCorrection {
return true; return true;
} }
/**
* Get relative (to distortion radius) distorted radius ratio to non-distorted from non-distorted
* @param r non-distorted radius (1.0 is 2.8512mm)
* @return ratio of distorted to non-distorted radius
*/
public double getRDistByR(double r) // relative to distortion radius
{
boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
double k;
if (use8){
k=(((((((this.distortionA8)*r+this.distortionA7)*r+this.distortionA6)*r+this.distortionA5)*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d;
} else {
k=(((this.distortionA5*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d;
}
return k;
}
/**
* Get derivative of relative (to distortion radius) d_Rdist/d_R from relative (to distortion radius) non-distorted radius
* @param r non-distorted relative radius
* @return derivative d_Rdist/d_R from (relative to relative)
*/
public double getDerivRDistFromR(double r) // relative to distortion radius
{
boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
// double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
double drDistDr;
if (use8){
drDistDr=(((((((7*this.distortionA8)*r + 6*this.distortionA7)*r + 5*this.distortionA6)*r + 4*this.distortionA5)*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC); // +d;
} else {
// drDistDr=(((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC)*r;
drDistDr=((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC;
}
return drDistDr;
}
public double getDerivRDistFromR(double r, double delta) // relative to distortion radius
{
return (getRDistByR(r+delta) -getRDistByR(r))/delta;
}
public double getRByRDist(double rDist, boolean debug){ public double getRByRDist(double rDist, boolean debug){
// add exceptions; // add exceptions;
if (this.rByRDist==null) { if (this.rByRDist==null) {
...@@ -479,6 +744,50 @@ public class GeometryCorrection { ...@@ -479,6 +744,50 @@ public class GeometryCorrection {
return result; return result;
} }
/*
public double getDerivRByRDist(double rDist, boolean debug, double delta){
return (getRByRDist(rDist+delta, false) - getRByRDist(rDist, false))/delta;
}
public double getDerivRByRDist(double rDist, boolean debug){
// add exceptions;
if (this.rByRDist==null) {
calcReverseDistortionTable();
if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): this.rByRDist==null");
// return Double.NaN;
}
if (rDist<0) {
if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): rDist<0");
return Double.NaN;
}
int index=(int) Math.floor(rDist/this.stepR);
if (index>=(this.rByRDist.length-1)) {
if (debug) System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): index="+index+">="+(this.rByRDist.length-1));
return Double.NaN;
}
double result=this.rByRDist[index+1]-this.rByRDist[index];
if ((index > 0) || (index < (this.rByRDist.length-2))){ // interpolate
double a = rDist/this.stepR-index; // fractional part, 0..1.0
if ( a <= 0.5){
result= 2 * (0.5 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
a * (this.rByRDist[index+1] - this.rByRDist[index-1]);
} else {
result= 2 * (1.0 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
(a - 0.5) * (this.rByRDist[index+2] - this.rByRDist[index]);
}
}
if (Double.isNaN(result)){
if (debug) System.out.println("this.rByRDist["+index+"]="+this.rByRDist[index]);
if (debug) System.out.println("this.rByRDist["+(index+1)+"]="+this.rByRDist[index+1]);
if (debug) System.out.println("rDist="+rDist);
if (debug) System.out.println("(rDist/this.stepR="+(rDist/this.stepR));
}
return result/this.stepR;
}
*/
} }
src/main/java/QuadCLT.java
View file @ 3f6c6c63
...@@ -5059,7 +5059,7 @@ public class QuadCLT { ...@@ -5059,7 +5059,7 @@ public class QuadCLT {
geometryCorrection, geometryCorrection,
threadsMax, // maximal number of threads to launch threadsMax, // maximal number of threads to launch
updateStatus, updateStatus,
debugLevel); 2); // debugLevel);
// get images for predefined regions and disparities. First - with just fixed scans 1 .. list.size() // get images for predefined regions and disparities. First - with just fixed scans 1 .. list.size()
tp.showScan( tp.showScan(
......
src/main/java/SuperTiles.java
View file @ 3f6c6c63
...@@ -25,15 +25,19 @@ import java.util.concurrent.atomic.AtomicInteger; ...@@ -25,15 +25,19 @@ import java.util.concurrent.atomic.AtomicInteger;
public class SuperTiles{ public class SuperTiles{
TileProcessor tileProcessor; TileProcessor tileProcessor;
double step_near;
double step_far; double step_far;
double step_threshold; double step_near;
double step_threshold_far; // relative to min_disparity
double step_threshold_near; // relative to min_disparity
double bin_far; // bin value (before rounding) matching (min_disparity + step_threshold_far)
double bin_near; // bin value (before rounding) matching (min_disparity + step_threshold_near)
double min_disparity; double min_disparity;
double max_disparity; double max_disparity;
double strength_floor; double strength_floor;
double strength_pow; double strength_pow;
double stBlurSigma; double stBlurSigma;
int numBins; int numBins;
double [] bin_centers;
double [][] disparityHistograms = null; double [][] disparityHistograms = null;
double [] stStrength = null; // per super-tile correlation strength double [] stStrength = null; // per super-tile correlation strength
double [][][] maxMinMax = null; double [][][] maxMinMax = null;
...@@ -42,7 +46,6 @@ public class SuperTiles{ ...@@ -42,7 +46,6 @@ public class SuperTiles{
// TileProcessor.CLTPass3d cltPass3d; // TileProcessor.CLTPass3d cltPass3d;
CLTPass3d cltPass3d; CLTPass3d cltPass3d;
public SuperTiles( public SuperTiles(
// TileProcessor.CLTPass3d cltPass3d,
CLTPass3d cltPass3d, CLTPass3d cltPass3d,
double step_near, double step_near,
double step_far, double step_far,
...@@ -57,18 +60,65 @@ public class SuperTiles{ ...@@ -57,18 +60,65 @@ public class SuperTiles{
this.tileProcessor = cltPass3d.getTileProcessor(); this.tileProcessor = cltPass3d.getTileProcessor();
this.step_near = step_near; this.step_near = step_near;
this.step_far = step_far; this.step_far = step_far;
this.step_threshold = step_threshold; this.step_threshold_far = step_threshold;
this.min_disparity = min_disparity; this.min_disparity = min_disparity;
this.max_disparity = max_disparity; this.max_disparity = max_disparity;
this.strength_floor = strength_floor; this.strength_floor = strength_floor;
this.strength_pow = strength_pow; this.strength_pow = strength_pow;
this.stBlurSigma = stBlurSigma; this.stBlurSigma = stBlurSigma;
// this.numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1; // this.step_threshold_near = this.step_threshold_far * this.step_far / step_near;
this.numBins = (int) ((max_disparity - min_disparity)/step_far) + 1; this.step_threshold_near = this.step_threshold_far * step_near / this.step_far ;
// getDisparityHistograms(); // calculate and blur supertileas this.bin_far = this.step_threshold_far / this.step_far;
getDisparityHistograms(null); // calculate and blur supertileas (for all, not just selected?) this.bin_near = this.step_threshold_far / this.step_far * (Math.log(this.step_near/this.step_far) + 1);
int bin_max = disparityToBin(max_disparity);
numBins = bin_max + 1;
bin_centers = new double [numBins];
int bin = 0;
for (bin = 0; bin < numBins; bin ++){
bin_centers[bin] = binToDisparity(bin);
}
getDisparityHistograms(null); // calculate and blur supertiles (for all, not just selected?)
if (tileProcessor.globalDebugLevel > -1){
System.out.println("SuperTiles(): min_disparity = "+min_disparity+", max_disparity="+max_disparity);
System.out.println("SuperTiles(): step_far = "+step_far+", step_near="+step_near);
System.out.println("SuperTiles(): numBins = "+numBins+", bin_far="+bin_far+", bin_near = "+bin_near);
System.out.println("SuperTiles(): step_threshold_far = "+step_threshold_far+", step_threshold_near="+step_threshold_near);
for (int i = 0; i<numBins; i++){
System.out.println(i+": "+bin_centers[i]+", "+disparityToBin(bin_centers[i]));
}
//
}
}
public int disparityToBin(double disparity){ // not filtered
double x = disparity - min_disparity;
int bin;
if (x < step_threshold_far){
bin = (int) Math.round(x/step_far);
} else if (x < step_threshold_near){
bin = (int) Math.round(step_threshold_far / step_far * (Math.log(x / step_threshold_far) + 1));
} else {
bin = (int) Math.round(bin_near + (x - step_threshold_near) / step_near);
}
// if (bin < 0) bin = 0;
// else if (bin >= numBins) bin = numBins - 1;
return bin;
}
public double binToDisparity(double bin){
double d;
if (bin < bin_far ) {
d = bin * step_far;
} else if (bin < bin_near){
d = step_threshold_far * Math.exp(bin * step_far / step_threshold_far - 1.0);
} else {
d = step_threshold_near + (bin - bin_near) * step_near;
}
return min_disparity + d;
} }
private double [][] getLapWeights(){ private double [][] getLapWeights(){
final int superTileSize = tileProcessor.superTileSize; final int superTileSize = tileProcessor.superTileSize;
final double [][] lapWeight = new double [2 * superTileSize][2 * superTileSize]; final double [][] lapWeight = new double [2 * superTileSize][2 * superTileSize];
...@@ -123,10 +173,8 @@ public class SuperTiles{ ...@@ -123,10 +173,8 @@ public class SuperTiles{
final int stilesX = (tilesX + superTileSize -1)/superTileSize; final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize; final int stilesY = (tilesY + superTileSize -1)/superTileSize;
final int nStiles = stilesX * stilesY; final int nStiles = stilesX * stilesY;
// final int numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1; /// final double dMin = min_disparity - step_disparity/2;
final double dMin = min_disparity - step_disparity/2; /// final double dMax = dMin + numBins * step_disparity;
final double dMax = dMin + numBins * step_disparity;
// final double [][] dispHist = new double [nStiles][numBins];
final double [][] dispHist = new double [nStiles][]; // now it will be sparse final double [][] dispHist = new double [nStiles][]; // now it will be sparse
final double [] strengthHist = new double [nStiles]; final double [] strengthHist = new double [nStiles];
final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax); final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
...@@ -160,7 +208,9 @@ public class SuperTiles{ ...@@ -160,7 +208,9 @@ public class SuperTiles{
if (w > 0.0){ if (w > 0.0){
if (strength_pow != 1.0) w = Math.pow(w, strength_pow); if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
w *= lapWeight[tY][tX]; w *= lapWeight[tY][tX];
int bin = (int) ((d-dMin)/step_disparity); // int bin = (int) ((d-dMin)/step_disparity);
// ignore too near/ too far
int bin = disparityToBin(d);
if ((bin >= 0) && (bin < numBins)){ // maybe collect below min and above max somewhere? if ((bin >= 0) && (bin < numBins)){ // maybe collect below min and above max somewhere?
hist[bin] += w; // +1] hist[bin] += w; // +1]
sw +=w; sw +=w;
...@@ -218,6 +268,7 @@ public class SuperTiles{ ...@@ -218,6 +268,7 @@ public class SuperTiles{
ImageDtt.startAndJoin(threads); ImageDtt.startAndJoin(threads);
} }
// returns odd-length array of max/min (x, strength) pairs // returns odd-length array of max/min (x, strength) pairs
public double [][][] getMaxMinMax(){ public double [][][] getMaxMinMax(){
// first find all integer maximums, and if the top is flat - use the middle. If not flat - use 2-nd degree polynomial // first find all integer maximums, and if the top is flat - use the middle. If not flat - use 2-nd degree polynomial
if (disparityHistograms == null) getDisparityHistograms(); if (disparityHistograms == null) getDisparityHistograms();
...@@ -312,10 +363,10 @@ public class SuperTiles{ ...@@ -312,10 +363,10 @@ public class SuperTiles{
if (numMax > 0) { if (numMax > 0) {
maxMinMax[nsTile] = new double[numMax * 2 - 1][2]; maxMinMax[nsTile] = new double[numMax * 2 - 1][2];
for (int i = 0; i < maxMinMax[nsTile].length; i++){ for (int i = 0; i < maxMinMax[nsTile].length; i++){
maxMinMax[nsTile][i][0] = mmm[i][0]; maxMinMax[nsTile][i][0] = binToDisparity(mmm[i][0]); // convert to actual disparity !
maxMinMax[nsTile][i][1] = mmm[i][1] * stStrength[nsTile]; maxMinMax[nsTile][i][1] = mmm[i][1] * stStrength[nsTile];
} }
if (globalDebugLevel > -1 ) { if (globalDebugLevel > 0 ) { //************
System.out.println(nsTile+": "+dh[0]+" "+dh[1]+" "+dh[2]+" "+dh[3]+" "+dh[4]+" "+ System.out.println(nsTile+": "+dh[0]+" "+dh[1]+" "+dh[2]+" "+dh[3]+" "+dh[4]+" "+
dh[5]+" "+dh[6]+" "+dh[7]+" "+dh[8]+" "+dh[9]+ " "+dh[10]+" "+dh[11]+" "+dh[12]+" "+dh[13]+" "+dh[14]+" "+ dh[5]+" "+dh[6]+" "+dh[7]+" "+dh[8]+" "+dh[9]+ " "+dh[10]+" "+dh[11]+" "+dh[12]+" "+dh[13]+" "+dh[14]+" "+
dh[15]+" "+dh[16]+" "+dh[17]+" "+dh[18]+" "+dh[19]+ " "+dh[20]); dh[15]+" "+dh[16]+" "+dh[17]+" "+dh[18]+" "+dh[19]+ " "+dh[20]);
...@@ -549,7 +600,8 @@ public class SuperTiles{ ...@@ -549,7 +600,8 @@ public class SuperTiles{
} }
} }
//maxIndex is what we need. Which strength to use - individual or accumulated? Use individual. //maxIndex is what we need. Which strength to use - individual or accumulated? Use individual.
bgDisparity[nsTile] = min_disparity + mmm[2 * maxIndex][0] * step_disparity; /// bgDisparity[nsTile] = min_disparity + mmm[2 * maxIndex][0] * step_disparity;
bgDisparity[nsTile] = binToDisparity (mmm[2 * maxIndex][0]);
bgStrength[nsTile] = mmm[2 * maxIndex][1]; bgStrength[nsTile] = mmm[2 * maxIndex][1];
} }
} }
...@@ -578,11 +630,8 @@ public class SuperTiles{ ...@@ -578,11 +630,8 @@ public class SuperTiles{
final int stilesX = (tilesX + superTileSize -1)/superTileSize; final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize; final int stilesY = (tilesY + superTileSize -1)/superTileSize;
final int nStiles = stilesX * stilesY; final int nStiles = stilesX * stilesY;
// final int numBins = (int) ((max_disparity - min_disparity)/step_disparity) + 1;
// final double [][] dispHist = new double [nStiles][numBins];
final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax); final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
final AtomicInteger ai = new AtomicInteger(0); final AtomicInteger ai = new AtomicInteger(0);
// final int st_end = st_start + superTileSize;
final int superTileSize2 = 2 * superTileSize; final int superTileSize2 = 2 * superTileSize;
final double [][] lapWeight = getLapWeights(); final double [][] lapWeight = getLapWeights();
final double [] tileDisparity = new double [tilesY * tilesX]; // assuming all 0.0 final double [] tileDisparity = new double [tilesY * tilesX]; // assuming all 0.0
...@@ -652,7 +701,595 @@ public class SuperTiles{ ...@@ -652,7 +701,595 @@ public class SuperTiles{
return bgTileDispStrength; return bgTileDispStrength;
} }
public void processPlanes(
final boolean [] selected, // or null
final double min_disp,
final boolean invert_disp, // use 1/disparity
final double plDispNorm,
final int debugLevel)
{
final int tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
final int tileSize = tileProcessor.getTileSize();
final double [] disparity = cltPass3d.getDisparity();
final double [] strength = cltPass3d.getStrength();
final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize;
final int nStiles = stilesX * stilesY;
final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int st_start = -superTileSize/2;
final int superTileSize2 = 2 * superTileSize;
final double [][] lapWeight = getLapWeights();
final int len2 = superTileSize2*superTileSize2;
final double [] double_zero = new double [len2];
final boolean [] boolean_zero = new boolean [len2];
final int debug_stile = 18 * stilesX + 25;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TilePlanes tpl = new TilePlanes(tileSize,superTileSize);
double [] stDisparity = new double [superTileSize2*superTileSize2];
double [] stStrength = new double [superTileSize2*superTileSize2];
boolean [] stSel = new boolean [superTileSize2*superTileSize2];
for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
int stileY = nsTile / stilesX;
int stileX = nsTile % stilesX;
double sw = 0.0; // sum weights
double [] hist = new double [numBins];
int tY0 = stileY * superTileSize + st_start;
int tX0 = stileX * superTileSize + st_start;
System.arraycopy(double_zero, 0, stDisparity, 0, len2);
System.arraycopy(double_zero, 0, stStrength, 0, len2);
System.arraycopy(boolean_zero, 0, stSel, 0, len2);
for (int tY = 0; tY < superTileSize2; tY++){
int tileY = tY0 +tY;
if ((tileY >= 0) && (tileY < tilesY)) {
for (int tX = 0; tX < superTileSize2; tX++){
int tileX = tX0 +tX;
if ((tileX >= 0) && (tileX < tilesX)) {
int indx = tileY*tilesX + tileX;
double d = disparity[indx];
if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
if (invert_disp){
d = 1.0/d;
}
double w = strength[indx] - strength_floor;
if (w > 0.0){
if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
w *= lapWeight[tY][tX];
int indx_out = tY * superTileSize2 + tX;
stDisparity[indx_out] = d;
stStrength[indx_out] = w;
stSel[indx_out] = true;
sw +=w;
}
}
}
}
}
}
if (sw >0){
// int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
// int dl = (stileY == 17) ? 1 : 0;
int dl = (stileY >= 0) ? 1 : 0;
double [][][] rslt = tpl.getCovar(
stDisparity,
stStrength,
stSel,
plDispNorm,
0); // dl); // debugLevel);
if (dl > 0) {
int numPoints = (int) rslt[2][0][2];
double kz = rslt[2][0][1];
double swc = rslt[2][0][0];
double [] szxy = rslt[2][1];
double [][] eig_val = rslt[0];
double [][] eig_vect = rslt[1];
if (swc > 1.0) {
System.out.println("Processing planes, nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
}
}
/*
double [][][] rslt = {
eig.getD().getArray(),
eig.getV().getArray(),
{
{sw,kz},
{swz, swx, swy}}};
return rslt;
*/
if (dl > 1) {
System.out.println("Processing planes with average disparity");
double ss = 0.0, sd = 0.0;
for (int i = 0; i < stDisparity.length; i++){
if (!Double.isNaN(stDisparity[i]) && stSel[i]){
ss += stStrength[i];
sd += stStrength[i] * stDisparity[i];
}
}
if (ss > 0) {
sd /= ss;
}
for (int i = 0; i < stDisparity.length; i++){
stDisparity[i] = sd;
}
tpl.getCovar(
stDisparity,
stStrength,
stSel,
plDispNorm,
dl); // debugLevel);
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
public void processPlanes1(
final boolean [] selected, // or null
final double min_disp,
final boolean invert_disp, // use 1/disparity
final double plDispNorm,
final int debugLevel)
{
if (maxMinMax == null) getMaxMinMax();
final int np_min = 5; // minimal number of points to consider
final int tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
final int tileSize = tileProcessor.getTileSize();
final double [] disparity = cltPass3d.getDisparity();
final double [] strength = cltPass3d.getStrength();
final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize;
final int nStiles = stilesX * stilesY;
final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int st_start = -superTileSize/2;
final int superTileSize2 = 2 * superTileSize;
final double [][] lapWeight = getLapWeights();
final int len2 = superTileSize2*superTileSize2;
final double [] double_zero = new double [len2];
final boolean [] boolean_zero = new boolean [len2];
final int debug_stile = 18 * stilesX + 25;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TilePlanes tpl = new TilePlanes(tileSize,superTileSize);
double [] stDisparity = new double [superTileSize2*superTileSize2];
double [] stStrength = new double [superTileSize2*superTileSize2];
boolean [] stSel = new boolean [superTileSize2*superTileSize2];
for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
int stileY = nsTile / stilesX;
int stileX = nsTile % stilesX;
double sw = 0.0; // sum weights
double [] hist = new double [numBins];
int tY0 = stileY * superTileSize + st_start;
int tX0 = stileX * superTileSize + st_start;
System.arraycopy(double_zero, 0, stDisparity, 0, len2);
System.arraycopy(double_zero, 0, stStrength, 0, len2);
System.arraycopy(boolean_zero, 0, stSel, 0, len2);
for (int tY = 0; tY < superTileSize2; tY++){
int tileY = tY0 +tY;
if ((tileY >= 0) && (tileY < tilesY)) {
for (int tX = 0; tX < superTileSize2; tX++){
int tileX = tX0 +tX;
if ((tileX >= 0) && (tileX < tilesX)) {
int indx = tileY*tilesX + tileX;
double d = disparity[indx];
if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
if (invert_disp){
d = 1.0/d;
}
double w = strength[indx] - strength_floor;
if (w > 0.0){
if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
w *= lapWeight[tY][tX];
int indx_out = tY * superTileSize2 + tX;
stDisparity[indx_out] = d;
stStrength[indx_out] = w;
stSel[indx_out] = true;
sw +=w;
}
}
}
}
}
}
if (sw >0){
// int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
// int dl = (stileY == 17) ? 1 : 0;
int dl = (stileY >= 0) ? 1 : 0;
double [][][] rslt = tpl.getCovar(
stDisparity,
stStrength,
stSel,
plDispNorm,
0); // dl); // debugLevel);
double swc_common = 0.0;
if (dl > 0) {
int numPoints = (int) rslt[2][0][2];
double kz = rslt[2][0][1];
double swc = rslt[2][0][0];
double [] szxy = rslt[2][1];
double [][] eig_val = rslt[0];
double [][] eig_vect = rslt[1];
swc_common = swc;
if (swc > 1.0) {
System.out.println("Processing planes, nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
}
}
double [][] mm = maxMinMax[nsTile];
if (mm.length > 1) { // multiple maximums - separate into multiple selections
boolean [][] stSels = new boolean[(mm.length +1)/2][stSel.length];
for (int i = 0; i< stSel.length; i++) if (stSel[i]){
double d = stDisparity[i];
loop:{
for (int m = 0; m < (stSels.length-1); m++){
if (d < mm[2 * m + 1][0]){
stSels[m][i] = true;
break loop;
}
}
stSels[stSels.length-1][i] = true;
}
}
double [][][][] rslts = new double [stSels.length][][][];
int [] np = new int[stSels.length];
String dbg_str = "";
for (int m = 0; m < stSels.length; m++){
for (int i =0; i < stSels[m].length; i++){
if (stSels[m][i]) np[m]++;
}
dbg_str +=" "+np[m];
if (m < stSels.length -1){
dbg_str +="("+mm[2*m+1][0]+")";
}
}
if (swc_common > 1.0) {
System.out.println("Processing subplanes:"+dbg_str+", nsTile="+nsTile);
}
for (int m = 0; m < stSels.length; m++) if (np[m] > np_min) {
rslts[m] = tpl.getCovar(
stDisparity,
stStrength,
stSels[m],
plDispNorm,
0); // dl); // debugLevel);
if (dl > 0) {
if (rslts[m] != null) {
int numPoints = (int) rslts[m][2][0][2];
double kz = rslts[m][2][0][1];
double swc = rslts[m][2][0][0];
double [] szxy = rslts[m][2][1];
double [][] eig_val = rslts[m][0];
double [][] eig_vect = rslts[m][1];
if (swc_common > 1.0) { // reduce output
System.out.println("Processing subplane["+m+"], nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+", numPoints="+numPoints+
", kz = "+kz+", sw = "+sw+ ", swc = "+swc+ ", center=["+szxy[0]+","+szxy[1]+","+szxy[2]+"]"+
", eig_val = {"+eig_val[0][0]+","+eig_val[1][1]+","+eig_val[2][2]+"}"+
", eig_vect[0] = {"+eig_vect[0][0]+","+eig_vect[1][0]+","+eig_vect[2][0]+"}");
}
} else {
System.out.println("Processing subplane["+m+"], nsTile="+nsTile+", stileX="+stileX+", stileY="+stileY+" RETURNED NULL");
}
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
public void processPlanes2(
final boolean [] selected, // or null
final double min_disp,
final boolean invert_disp, // use 1/disparity
final double plDispNorm,
final int plMinPoints, // = 5; // Minimal number of points for plane detection
final double plTargetEigen, // = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
final double plFractOutliers, // = 0.3; // Maximal fraction of outliers to remove
final int plMaxOutliers, // = 20; // Maximal number of outliers to remove
final GeometryCorrection geometryCorrection,
final boolean correct_distortions,
final int debugLevel)
{
if (maxMinMax == null) getMaxMinMax();
// final int np_min = 5; // minimal number of points to consider
final int tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
final int tileSize = tileProcessor.getTileSize();
final double [] disparity = cltPass3d.getDisparity();
final double [] strength = cltPass3d.getStrength();
final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize;
final int nStiles = stilesX * stilesY;
final Thread[] threads = ImageDtt.newThreadArray(tileProcessor.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int st_start = -superTileSize/2;
final int superTileSize2 = 2 * superTileSize;
final double [][] lapWeight = getLapWeights();
final int len2 = superTileSize2*superTileSize2;
final double [] double_zero = new double [len2];
final boolean [] boolean_zero = new boolean [len2];
// final int debug_stile = 18 * stilesX + 25;
// final int debug_stile = 20 * stilesX + 24;
final int debug_stile = 17 * stilesX + 10;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TilePlanes tpl = new TilePlanes(tileSize,superTileSize, geometryCorrection);
double [] stDisparity = new double [superTileSize2*superTileSize2];
double [] stStrength = new double [superTileSize2*superTileSize2];
boolean [] stSel = new boolean [superTileSize2*superTileSize2];
for (int nsTile = ai.getAndIncrement(); nsTile < nStiles; nsTile = ai.getAndIncrement()) {
int stileY = nsTile / stilesX;
int stileX = nsTile % stilesX;
int [] sTiles = {stileX, stileY};
double sw = 0.0; // sum weights
double [] hist = new double [numBins];
int tY0 = stileY * superTileSize + st_start;
int tX0 = stileX * superTileSize + st_start;
System.arraycopy(double_zero, 0, stDisparity, 0, len2);
System.arraycopy(double_zero, 0, stStrength, 0, len2);
System.arraycopy(boolean_zero, 0, stSel, 0, len2);
for (int tY = 0; tY < superTileSize2; tY++){
int tileY = tY0 +tY;
if ((tileY >= 0) && (tileY < tilesY)) {
for (int tX = 0; tX < superTileSize2; tX++){
int tileX = tX0 +tX;
if ((tileX >= 0) && (tileX < tilesX)) {
int indx = tileY*tilesX + tileX;
double d = disparity[indx];
if (!Double.isNaN(d) && (d >= min_disp) &&((selected == null) || selected[indx])){
if (invert_disp){
d = 1.0/d;
}
double w = strength[indx] - strength_floor;
if (w > 0.0){
if (strength_pow != 1.0) w = Math.pow(w, strength_pow);
w *= lapWeight[tY][tX];
int indx_out = tY * superTileSize2 + tX;
stDisparity[indx_out] = d;
stStrength[indx_out] = w;
stSel[indx_out] = true;
sw +=w;
}
}
}
}
}
}
if (sw >0){
// int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
// int dl = ((stileY == 17) && (stileX > 4)) ? 1 : 0;
// int dl = (stileY >= 0) ? 1 : 0;
int dl1 = (nsTile == debug_stile-1) ? 3 : 0;
// int dl = (nsTile == debug_stile) ? 3 : 0;
int dl = ((stileY >= 15) && (stileY <= 18) && (stileX >= 5) && (stileX <= 31)) ? 1 : 0;
// int dl = ((stileY == 17) && (stileX == 27) ) ? 3 : 0;
boolean [] sel_all = stSel.clone();
TilePlanes.PlaneData pd = tpl.getPlane(
sTiles,
stDisparity,
stStrength,
sel_all,
0); // debugLevel);
if (pd != null) {
//correct_distortions
double swc_common = pd.getWeight();
if (dl > 0) {
if (swc_common > 1.0) {
System.out.println("Processing planes["+nsTile+"]"+
", stileX="+stileX+
", stileY="+stileY+
", numPoints="+ pd.getNumPoints()+
", sw = "+swc_common+
", swc = "+pd.getWeight()+
", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
}
}
// now try to remove outliers
int max_outliers = (int) Math.round(pd.getNumPoints() * plFractOutliers);
if (max_outliers > plMaxOutliers) max_outliers = plMaxOutliers;
double targetV = plTargetEigen;
double z0 = pd.getZxy()[0];
if ((plDispNorm > 0.0) && (z0 > plDispNorm)) {
double dd = (plDispNorm + z0)/ plDispNorm; // > 1
targetV *= dd * dd; // > original
}
if (pd.getValues()[0] > targetV) {
pd = tpl.removePlaneOutliers(
pd,
sTiles,
stDisparity,
stStrength,
sel_all,
targetV, // double targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
max_outliers, // int maxRemoved, // maximal number of tiles to remove (not a constant)
plMinPoints, // int minLeft, // minimal number of tiles to keep
dl1); // 0); // debugLevel);
if (dl > 0) {
if (swc_common > 1.0) {
System.out.println("Removed outliers["+nsTile+"]"+
", stileX="+stileX+
", stileY="+stileY+
", numPoints="+ pd.getNumPoints()+
", sw = "+swc_common+
", swc = "+pd.getWeight()+
", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
}
}
} // nothing to do if already OK
if (dl > 0) {
System.out.println("Calculating World normal["+nsTile+"]");
}
double [] norm_xyz = pd.getWorldXYZ(
correct_distortions,
dl);
if (dl > 0) {
System.out.println("World normal["+nsTile+"] = {"+
norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
}
// now try for each of the disparity-separated clusters
double [][] mm = maxMinMax[nsTile];
if (mm.length > 1) { // multiple maximums - separate into multiple selections
boolean [][] stSels = new boolean[(mm.length +1)/2][stSel.length];
for (int i = 0; i< stSel.length; i++) if (stSel[i]){
double d = stDisparity[i];
loop:{
for (int m = 0; m < (stSels.length-1); m++){
if (d < mm[2 * m + 1][0]){
stSels[m][i] = true;
break loop;
}
}
stSels[stSels.length-1][i] = true;
}
}
double [][][][] rslts = new double [stSels.length][][][];
int [] np = new int[stSels.length];
String dbg_str = "";
for (int m = 0; m < stSels.length; m++){
for (int i =0; i < stSels[m].length; i++){
if (stSels[m][i]) np[m]++;
}
dbg_str +=" "+np[m];
if (m < stSels.length -1){
dbg_str +="("+mm[2*m+1][0]+")";
}
}
if ((dl > 0) && (swc_common > 1.0)) {
System.out.println("Processing subplanes["+nsTile+"]:"+dbg_str);
}
for (int m = 0; m < stSels.length; m++) if (np[m] > plMinPoints) {
pd = tpl.getPlane(
sTiles,
stDisparity,
stStrength,
stSels[m],
0); // debugLevel);
if (pd != null) {
if (dl > 0) {
if (swc_common > 1.0) {
System.out.println("Processing subplane["+nsTile+"]["+m+"]"+
", stileX="+stileX+
", stileY="+stileY+
", numPoints="+ pd.getNumPoints()+
", sw = "+swc_common+
", swc = "+pd.getWeight()+
", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
}
}
// now try to remove outliers
max_outliers = (int) Math.round(pd.getNumPoints() * plFractOutliers);
if (max_outliers > plMaxOutliers) max_outliers = plMaxOutliers;
targetV = plTargetEigen;
z0 = pd.getZxy()[0];
if ((plDispNorm > 0.0) && (z0 > plDispNorm)) {
double dd = (plDispNorm + z0)/ plDispNorm; // > 1
targetV *= dd * dd; // > original
}
if (pd.getValues()[0] > targetV) {
pd = tpl.removePlaneOutliers(
pd,
sTiles,
stDisparity,
stStrength,
stSels[m],
targetV, // double targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
max_outliers, // int maxRemoved, // maximal number of tiles to remove (not a constant)
plMinPoints, // int minLeft, // minimal number of tiles to keep
dl1); // 0); // debugLevel);
if (dl > 0) {
if (swc_common > 1.0) {
System.out.println("Removed outliers["+nsTile+"]["+m+"]"+
", stileX="+stileX+
", stileY="+stileY+
", numPoints="+ pd.getNumPoints()+
", sw = "+swc_common+
", swc = "+pd.getWeight()+
", center=["+pd.getZxy()[0]+","+pd.getZxy()[1]+","+pd.getZxy()[2]+"]"+
", eig_val = {"+pd.getValues()[0]+","+pd.getValues()[1]+","+pd.getValues()[2]+"}"+
", eig_vect[0] = {"+pd.getVector()[0]+","+pd.getVector()[1]+","+pd.getVector()[2]+"}");
}
}
}
norm_xyz = pd.getWorldXYZ(
correct_distortions);
if (dl > 0) {
System.out.println("World normal["+nsTile+"]["+m+"] = {"+
norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
}
}
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
} // end of class SuperTiles } // end of class SuperTiles
src/main/java/TilePlanes.java 0 → 100644
View file @ 3f6c6c63
import Jama.EigenvalueDecomposition;
import Jama.Matrix;
/**
**
** TilePlanes - detect planes in tile clusters
**
** Copyright (C) 2017 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** TilePlanes.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/>.
** -----------------------------------------------------------------------------**
**
*/
public class TilePlanes {
private int tileSize = 0; // 8;
private int stSize = 0; // 8;
GeometryCorrection geometryCorrection = null;
public TilePlanes(
int tileSize,
int stSize,
GeometryCorrection geometryCorrection){
this.tileSize = tileSize;
this.stSize = stSize;
this.geometryCorrection =geometryCorrection;
}
public TilePlanes(
int tileSize,
int stSize){
this.tileSize = tileSize;
this.geometryCorrection = null;
}
public class PlaneData{
GeometryCorrection geometryCorrection = null;
boolean correctDistortions = false;
double [] zxy; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
double [][] vectors = null; // [3][3] - re-ordered/re-directed eigenvectors(transposed): [0] - plane normal, most Z-like, towards camera, [1] - X-like, [2] - Y-like
double [] values = null; // [3] -eigenvalues
int num_points = 0;
double weight = 0.0;
double [] world_xyz = null; // world coordinates of the nearest point of the plane, in meters
double [] daxy = null; // disparity and 2 relative angles (ax and ay) corresponding to fisheye view, near (0,0) scale is pixel size
int tileSize;
int superTileSize;
int [] sTileXY = null; // X and Y indices of this superTile in the image
public PlaneData (
int [] sTileXY,
int tileSize,
int superTileSize,
GeometryCorrection geometryCorrection)
{
this.geometryCorrection = geometryCorrection;
this.tileSize = tileSize;
this.superTileSize = superTileSize;
this.sTileXY = sTileXY.clone();
}
public void setCorrectDistortions(
boolean correctDistortions)
{
this.correctDistortions = correctDistortions;
}
public boolean getCorrectDistortions() {
return correctDistortions;
}
public int[] getSTileXY() {
return sTileXY;
}
public double[] getZxy() {
return zxy;
}
public void setZxy(double[] zxy) {
this.zxy = zxy;
}
public double[][] getVectors() {
return vectors;
}
public double[] getVector() {
return vectors[0];
}
public void setVectors(double[][] vectors) {
this.vectors = vectors;
}
public double[] getValues() {
return values;
}
public double getValue() {
return values[0];
}
public void setValues(double[] values) {
this.values = values;
}
public void setValues(double v1, double v2, double v3) {
this.values = new double[3];
this.values[0] = v1;
this.values[1] = v2;
this.values[2] = v3;
}
public int getNumPoints() {
return num_points;
}
public void setNumPoints(int num_points) {
this.num_points = num_points;
}
public double getWeight() {
return weight;
}
public void setWeight(double weight) {
this.weight = weight;
}
public double [] getWorldXYZ(boolean correct_distortions)
{
return getWorldXYZ(correct_distortions,0);
}
/**
* Cross product of 2 3-d vectors as column matrices
* @param v1
* @param v2
* @return v1 x v2 as a column (3x1) matrix
*/
public Matrix cross3d (Matrix v1, Matrix v2)
{
double [][]av1 = v1.getArray();
double [][]av2 = v2.getArray();
double [][] ar = {
{av1[1][0]*av2[2][0] - av1[2][0]*av2[1][0]},
{av1[2][0]*av2[0][0] - av1[0][0]*av2[2][0]},
{av1[0][0]*av2[1][0] - av1[1][0]*av2[0][0]}};
return new Matrix (ar);
}
public double [] getWorldXYZ(
boolean correct_distortions,
int debugLevel)
{
double delta = 0.0001;
if (world_xyz != null) return world_xyz;
setCorrectDistortions(correct_distortions);
// get pixel coordinates of the plane origin point
double px = tileSize*(superTileSize * sTileXY[0] + superTileSize/2) + zxy[1]; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
double py = tileSize*(superTileSize * sTileXY[1] + superTileSize/2) + zxy[2];
double disp = zxy[0];
Matrix xyz = new Matrix(geometryCorrection.getWorldCoordinates(
px,
py,
disp,
this.correctDistortions),3); // column matrix
Matrix dpxpy = new Matrix(vectors[0],3); // 3 rows, 1 column
if (debugLevel > 0){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"), correctDistortions="+correctDistortions+", xyz= {"+
xyz.get(0, 0)+","+xyz.get(1, 0)+","+xyz.get(2, 0)+"}");
// xyz.print(10, 6); // w,d
// double [] dpxpy = geometryCorrection.getImageCoordinates(xyz.getColumnPackedCopy(),this.correctDistortions);
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): disp, px, py="+disp+","+px+","+py); // + ", reversed:"+dpxpy[0]+","+dpxpy[1]+","+dpxpy[2]);
Matrix xyz1 = new Matrix(geometryCorrection.getWorldCoordinates(
px + delta * dpxpy.get(1, 0),
py + delta * dpxpy.get(2, 0),
disp + delta * dpxpy.get(0, 0),
this.correctDistortions),3); // column matrix
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+") delta diff: {"+
((xyz1.get(0, 0)- xyz.get(0, 0))/delta)+","+
((xyz1.get(1, 0)- xyz.get(1, 0))/delta)+","+
((xyz1.get(2, 0)- xyz.get(2, 0))/delta)+"}");
}
Matrix jacobian = new Matrix(geometryCorrection.getWorldJacobian(
px,
py,
disp,
this.correctDistortions,
(debugLevel > 2)
));
// convert both orthogonal axes, normalize their cross product
Matrix v1 = jacobian.times(new Matrix(vectors[1],3)); // 3 rows, 1 column
Matrix v2 = jacobian.times(new Matrix(vectors[2],3)); // 3 rows, 1 column
Matrix norm_xyz = cross3d(v1,v2);
// norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
// norm_xyz = jacobian.times(dpxpy); // plane normal vector in world xyz
if (debugLevel > 0){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz={"+
norm_xyz.get(0, 0)+", "+norm_xyz.get(1, 0)+", "+norm_xyz.get(2, 0)+"}, (dpxpy={"+
dpxpy.get(0, 0)+ ", "+dpxpy.get(1, 0)+ ", "+dpxpy.get(2, 0)+"})");
}
if (debugLevel > 2){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): dpxpy=");
dpxpy.print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian=");
jacobian.print(10, 6); // w,d
/*
Matrix jacobian1 = new Matrix(geometryCorrection.getWorldJacobian(
px,
py,
disp,
this.correctDistortions,
0.00001));
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1=");
jacobian1.print(10, 6); // w,d
*/
}
if (debugLevel > 2){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz=");
norm_xyz.print(10, 6); // w,d
}
if (debugLevel > 2){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image jacobian=");
Matrix img_jacobian = new Matrix(geometryCorrection.getImageJacobian(
xyz.getColumnPackedCopy(),
this.correctDistortions,
1));
img_jacobian.print(10, 6); // w,d
/*
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image jacobian1 (0.000001)=");
Matrix img_jacobian1 = new Matrix(geometryCorrection.getImageJacobian(
xyz.getColumnPackedCopy(),
this.correctDistortions,
0.000001));
img_jacobian1.print(10, 6); // w,d
*/
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian)=");
jacobian.times(img_jacobian).print(10, 6); // w,d
/*
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian1)=");
jacobian.times(img_jacobian1).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian)=");
jacobian1.times(img_jacobian).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian1)=");
jacobian1.times(img_jacobian1).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image_jacobian.inverse()=");
img_jacobian.inverse().print(10, 6); // w,d
*/
}
norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
if (debugLevel > 0){
System.out.println("+getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): unit plane normal={"+
norm_xyz.get(0, 0)+", "+norm_xyz.get(1, 0)+", "+norm_xyz.get(2, 0)+"})");
}
if (debugLevel > 2){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz (normalized) =");
norm_xyz.print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): xyz.times(norm_xyz.transpose()).get(0,0) ="+xyz.times(norm_xyz.transpose()).get(0,0));
}
// convert plane normal vector to world coordinates
//world_xyz
world_xyz = norm_xyz.times(-(xyz.times(norm_xyz.transpose()).get(0,0))).getColumnPackedCopy();
return world_xyz;
}
}
/**
* Calculate covariance matrix for a subset of tile data (disparities)
* Subtract weight floor from weight
* @param data data array - square (2*stSize) * (2*stSize)
* @param weight per sample weight (should have floor already subtracted)
* @param select sample selection
* @return covariance (diagonal) matrix: [0]: disparity, [1]: d<disparity>/dx, [2]: d<disparity>/dy,
*/
public double [][][] getCovar(
double [] data,
double [] weight,
boolean [] select,
double plDispNorm, // Normalize disparities to the average if above
int debugLevel){
int stSize2 = 2 * stSize;
// Matrix covar = new Matrix(3,3);
double [][] acovar = new double [3][3];
int numPoints = 0;
double sw =0.0, swz = 0.0, swx = 0.0, swy = 0.0;
for (int indx = 0; indx < data.length; indx++){
if (select[indx] && (weight[indx] > 0)){
numPoints++;
double w = weight[indx];
double d = data[indx];
int x = ((indx % stSize2) - stSize) * tileSize; // in pixels, not in tiles
int y = ((indx / stSize2) - stSize) * tileSize;
sw += w;
swz += w * d;
swx += w * x;
swy += w * y;
}
}
if (sw == 0.0) {
return null;
}
swz /= sw;
swx /= sw;
swy /= sw;
if (debugLevel > 0){
System.out.println("getCovar(): sw = "+sw +", swz = "+swz +", swx = "+swx +", swy = "+swy);
}
// TODO: scale disparity to make same scale for 3 axes?
double kz = ((plDispNorm > 0.0) && (swz > plDispNorm)) ? (plDispNorm / swz) : 1.0;
for (int indx = 0; indx < data.length; indx++){
if (select[indx] && (weight[indx] > 0)){
double w = weight[indx] / sw;
double d = kz * (data[indx] - swz);
double wd = w*d;
double x = ((indx % stSize2) - stSize) * tileSize - swx;
double y = ((indx / stSize2) - stSize) * tileSize - swy;
acovar [0][0] += wd * d;
acovar [0][1] += wd * x;
acovar [0][2] += wd * y;
acovar [1][1] += w * x * x;
acovar [1][2] += w * x * y;
acovar [2][2] += w * y * y;
}
}
acovar [1][0] = acovar [0][1];
acovar [2][0] = acovar [0][2];
acovar [2][1] = acovar [1][2];
Matrix covar = new Matrix(acovar);
EigenvalueDecomposition eig = covar.eig();
if (debugLevel > 0){
System.out.println("getCovar(): covarianvce matrix, number of used points:"+numPoints);
covar.print(10, 6); // w,d
System.out.println("getCovar(): eigenvalues");
eig.getD().print(10, 6); // w,d
System.out.println("getCovar(): eigenvectors");
eig.getV().print(10, 6); // w,d
}
double [][][] rslt = {
eig.getD().getArray(),
eig.getV().getArray(),
{
{sw,kz,numPoints},
{swz, swx, swy}}};
return rslt;
}
public PlaneData getPlane(
int [] sTileXY,
double [] data,
double [] weight,
boolean [] select,
int debugLevel){
double [][][] rslt = getCovar(
data,
weight,
select,
0.0,
0); // debugLevel);
if (rslt == null) return null;
int numPoints = (int) rslt[2][0][2];
// double kz = rslt[2][0][1]; // == 1.0
double swc = rslt[2][0][0];
double [] szxy = rslt[2][1];
double [][] eig_val = rslt[0];
double [][] eig_vect = rslt[1];
// find vector most orthogonal to view // (anyway it all works with that assumption), make it first
// TODO?
int oindx = 0;
for (int i = 1; i <3; i++){
if (Math.abs(eig_vect[0][i]) > Math.abs(eig_vect[0][oindx])){
oindx = i;
}
}
// Find two other axis - "mostly X" (horizontal) and "mostly Y" (vertical)
int vindx = (oindx == 0)? 1 : 0;
int hindx = (oindx == 0)? 2 : ((oindx == 1) ? 2 : 1);
if (Math.abs(eig_vect[2][vindx]) < Math.abs(Math.abs(eig_vect[2][hindx]))){
int tmp = vindx;
vindx = hindx;
hindx = tmp;
}
PlaneData pd = new PlaneData(
sTileXY,
this.tileSize,
this.stSize,
this.geometryCorrection);
pd.setZxy(szxy);
pd.setValues(eig_val[oindx][oindx],eig_val[hindx][hindx],eig_val[vindx][vindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
double [][] plane = {
{eig_vect[0][oindx],eig_vect[1][oindx],eig_vect[2][oindx]}, // plane normal to camera
{eig_vect[0][vindx],eig_vect[1][vindx],eig_vect[2][vindx]}, // "horizontal" axis // to detect skinny planes and poles
{eig_vect[0][hindx],eig_vect[1][hindx],eig_vect[2][hindx]}}; // "vertical" axis // to detect skinny planes and poles
// Make normal be towards camera (positive disparity), next vector - positive in X direction (right), last one - in positive Y (down)
for (int v = 0; v <3; v++) {
if (plane[v][v] < 0.0) for (int i = 0; i < 3; i ++) plane[v][i] = -plane[v][i];
}
pd.setVectors (plane);
pd.setNumPoints (numPoints);
pd.setWeight (swc);
return pd;
}
public PlaneData removePlaneOutliers(
PlaneData pd, // already found or null
int [] sTileXY, // may be null if pd is not null
double [] data,
double [] weight,
boolean [] select, // will be modified
double targetEigen, // target eigenvalue for primary axis (is disparity-dependent, so is non-constant)
int maxRemoved, // maximal number of tiles to remove (not a constant)
int minLeft, // minimal number of tiles to keep
int debugLevel){
int stSize2 = 2 * stSize;
if (pd == null) {
pd = getPlane(
sTileXY,
data,
weight,
select,
debugLevel);
}
if (maxRemoved > (pd.getNumPoints() - minLeft)) maxRemoved = pd.getNumPoints() - minLeft;
int numRemoved = 0;
for (; (pd.getValue() > targetEigen) && (numRemoved < maxRemoved); numRemoved++){
if (debugLevel > 2){
System.out.println("removePlaneOutliers(): numRemoved = "+numRemoved+" eigenValue = "+pd.getValue()+" target = "+targetEigen);
}
// make a plane and find the worst (largest disparity difference) tile
// z = -(x*Vx + y*Vy)/Vz
double worst_d2 = 0.0;
int worst_index = -1;
double [] v = pd.getVector();
double [] zxy0 = pd.getZxy();
for (int indx = 0; indx < data.length; indx++){
if (select[indx] && (weight[indx] > 0)){
// double w = weight[indx];
double x = ((indx % stSize2) - stSize) - zxy0[1];
double y = ((indx / stSize2) - stSize) - zxy0[2];
double d = data[indx];
d -= zxy0[0];
d += (x * v[1]+y*v[2])/v[0];
double d2 = d*d;
if (d2 > worst_d2){
worst_d2 = d2;
worst_index = indx;
}
}
}
if (worst_index < 0) {
System.out.println("This is a BUG in removePlaneOutliers()");
break;
}
select[worst_index] = false;
pd = getPlane( // re-calculate with point removed
pd.getSTileXY(),
data,
weight,
select,
debugLevel);
}
return pd;
}
}
src/main/java/TileProcessor.java
View file @ 3f6c6c63
...@@ -60,6 +60,8 @@ public class TileProcessor { ...@@ -60,6 +60,8 @@ public class TileProcessor {
} }
public int getTilesX() {return tilesX;}; public int getTilesX() {return tilesX;};
public int getTilesY() {return tilesY;}; public int getTilesY() {return tilesY;};
public int getTileSize() {return tileSize;};
public int getSuperTileSize() {return superTileSize;};
public void setTrustedCorrelation(double trustedCorrelation) public void setTrustedCorrelation(double trustedCorrelation)
{ {
this.trustedCorrelation = trustedCorrelation; this.trustedCorrelation = trustedCorrelation;
...@@ -1530,9 +1532,33 @@ public class TileProcessor { ...@@ -1530,9 +1532,33 @@ public class TileProcessor {
public void fillGaps( // grows, then shrinks public void fillGaps( // grows, then shrinks
int depth, // same as grow - odd - 4 directions, even - 8 int depth, // same as grow - odd - 4 directions, even - 8
boolean poison, // do not fill gaps that even touch prohibited
boolean [] tiles, boolean [] tiles,
boolean [] prohibit){ boolean [] prohibit){
boolean [] orig_tiles = tiles.clone(); boolean [] orig_tiles = tiles.clone();
if (poison && (prohibit != null)) {
growTiles(
depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
tiles,
null);
boolean [] poisoned = prohibit.clone();
for (int i =0; i< tiles.length; i++) poisoned[i] &= tiles[i] && !orig_tiles[i];
growTiles(
depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
poisoned,
orig_tiles);
// invert selection, so "grow" will be "shrink"
for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
growTiles(
depth,
tiles,
null);
// invert selection again (restore)
for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] && !poisoned[i];
} else {
growTiles( growTiles(
depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
tiles, tiles,
...@@ -1546,7 +1572,7 @@ public class TileProcessor { ...@@ -1546,7 +1572,7 @@ public class TileProcessor {
// invert selection again (restore) // invert selection again (restore)
for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i]; for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i];
// for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] || orig_tiles[i]; // and or with original (why result does not have some of original - only with prohibit? // for (int i =0; i< tiles.length; i++) tiles[i] = !tiles[i] || orig_tiles[i]; // and or with original (why result does not have some of original - only with prohibit?
if (prohibit != null) { // second pass w/o prohoibit, then end results if (prohibit != null) { // second pass w/o prohibit, then and results
growTiles( growTiles(
depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more depth, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
orig_tiles, orig_tiles,
...@@ -1561,6 +1587,7 @@ public class TileProcessor { ...@@ -1561,6 +1587,7 @@ public class TileProcessor {
for (int i =0; i< tiles.length; i++) tiles[i] &= !orig_tiles[i]; for (int i =0; i< tiles.length; i++) tiles[i] &= !orig_tiles[i];
} }
} }
}
public void growTiles( public void growTiles(
int grow, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more int grow, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
boolean [] tiles, boolean [] tiles,
...@@ -1854,16 +1881,33 @@ public class TileProcessor { ...@@ -1854,16 +1881,33 @@ public class TileProcessor {
final double this_sure, // minimal strength to be considered definitely background final double this_sure, // minimal strength to be considered definitely background
final double this_maybe, // maximal strength to ignore as non-background final double this_maybe, // maximal strength to ignore as non-background
final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
final EyesisCorrectionParameters.CLTParameters clt_parameters, // final EyesisCorrectionParameters.CLTParameters clt_parameters,
// final int threadsMax, // maximal number of threads to launch // final int threadsMax, // maximal number of threads to launch
// final boolean updateStatus,
final boolean show_filter_scan,
final int min_clstr_seed, // clt_parameters.min_clstr_seed
final double min_clstr_weight, // double min_weight // minimal total weight of the cluster
final double min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
final int min_clstr_lone, // int min_area, // minimal number of pixels
final int fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
final int poison_gaps, // second (usually larger than fill_gaps) when any touch of prohibited cell invalidates patching
final boolean zero_gap_strength, // set strength to zero when covering gaps
final int debugLevel final int debugLevel
) )
{ {
/*
clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
clt_parameters.min_clstr_lone, // int min_area, // minimal number of pixels
clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
*/
final int dbg_tile = 49468; // x = 220, y = 152 (pavement line) final int dbg_tile = 49468; // x = 220, y = 152 (pavement line)
final boolean show_filter_scan = clt_parameters.show_filter_scan || (debugLevel > 1); final boolean show_scan = show_filter_scan || (debugLevel > 1);
showDoubleFloatArrays sdfa_instance = null; showDoubleFloatArrays sdfa_instance = null;
if ((debugLevel > -1) || show_filter_scan) sdfa_instance = new showDoubleFloatArrays(); // just for debugging? if ((debugLevel > -1) || show_scan) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
if (debugLevel > 0){ if (debugLevel > 0){
System.out.println("FilterScan(,,"+disparity_far+", " +disparity_near+", "+ sure_smth); System.out.println("FilterScan(,,"+disparity_far+", " +disparity_near+", "+ sure_smth);
} }
...@@ -1916,7 +1960,7 @@ public class TileProcessor { ...@@ -1916,7 +1960,7 @@ public class TileProcessor {
boolean[] dbg_before_small = null; boolean[] dbg_before_small = null;
boolean[] dbg_before_lone = null; boolean[] dbg_before_lone = null;
boolean[] dbg_before_gaps = null; boolean[] dbg_before_gaps = null;
if (clt_parameters.min_clstr_seed > 1){ if (min_clstr_seed > 1){
// TODO: check - now no limit on the strength of the offending selections, only on these onses // TODO: check - now no limit on the strength of the offending selections, only on these onses
...@@ -1924,44 +1968,64 @@ public class TileProcessor { ...@@ -1924,44 +1968,64 @@ public class TileProcessor {
false, //true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left false, //true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left
far_tiles, // boolean [] tiles_src, // selected tiles, will modified far_tiles, // boolean [] tiles_src, // selected tiles, will modified
null, // double [] weights_src, // or null null, // double [] weights_src, // or null
clt_parameters.min_clstr_seed, // int min_area, // minimal number of pixels min_clstr_seed, // int min_area, // minimal number of pixels
0.0, // clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster 0.0, // clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
0.0); // clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster 0.0); // clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster
removeSmallClusters( removeSmallClusters(
false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left
near_tiles, // boolean [] tiles_src, // selected tiles, will modified near_tiles, // boolean [] tiles_src, // selected tiles, will modified
null, // double [] weights_src, // or null null, // double [] weights_src, // or null
clt_parameters.min_clstr_seed, // int min_area, // minimal number of pixels min_clstr_seed, // int min_area, // minimal number of pixels
0.0, // clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster 0.0, // clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
0.0); // clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster 0.0); // clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster
if ((sdfa_instance!=null) && show_filter_scan) dbg_before_small = these_tiles.clone(); if ((sdfa_instance!=null) && show_scan) dbg_before_small = these_tiles.clone();
// only remove far outstanding clusters // only remove far outstanding clusters
removeSmallClusters( // remove single-tile clusters - anywhere removeSmallClusters( // remove single-tile clusters - anywhere
false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left
these_tiles, // boolean [] tiles_src, // selected tiles, will modified these_tiles, // boolean [] tiles_src, // selected tiles, will modified
this_strength, // orig_strength, // null, // double [] weights_src, // or null this_strength, // orig_strength, // null, // double [] weights_src, // or null
clt_parameters.min_clstr_seed, // 2, // int min_area, // minimal number of pixels min_clstr_seed, // 2, // int min_area, // minimal number of pixels
clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
debugLevel); debugLevel);
if ((sdfa_instance!=null) && show_filter_scan) dbg_before_lone = these_tiles.clone(); if ((sdfa_instance!=null) && show_scan) dbg_before_lone = these_tiles.clone();
removeLoneClusters( removeLoneClusters(
false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left false, // true, // boolean diag_en, // enable diagonal directions, false only up, dowm, right,left
these_tiles, // boolean [] tiles_src, // selected tiles, will modified these_tiles, // boolean [] tiles_src, // selected tiles, will modified
this_strength, // orig_strength, // null, // double [] weights_src, // or null this_strength, // orig_strength, // null, // double [] weights_src, // or null
clt_parameters.min_clstr_lone, // int min_area, // minimal number of pixels min_clstr_lone, // int min_area, // minimal number of pixels
clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
debugLevel); debugLevel);
if ((sdfa_instance!=null) && show_filter_scan) dbg_before_gaps = these_tiles.clone(); if ((sdfa_instance!=null) && show_scan) dbg_before_gaps = these_tiles.clone();
prohibit = far_tiles; // do not fill gaps over known background/far tiles boolean [] pre_gap = null;
if (clt_parameters.fill_gaps > 0) { if ((fill_gaps > 0) || (poison_gaps > 0)) {
if (zero_gap_strength) pre_gap = these_tiles.clone();
prohibit = far_tiles.clone(); // do not fill gaps over known background/far tiles
if (fill_gaps > 0) {
fillGaps( // grows, then shrinks fillGaps( // grows, then shrinks
clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8 fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
false, // boolean poison, // do not fill gaps that even touch prohibited
these_tiles, // boolean [] tiles,
prohibit);
}
if (poison_gaps > 0) { // only block for when poison_gaps is enabled (last filtering with high fill_gap value)
for (int i = 0; i< prohibit.length; i++) prohibit[i] |= block_propagate[i];
fillGaps( // grows, then shrinks
poison_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
true, // boolean poison, // do not fill gaps that even touch prohibited
these_tiles, // boolean [] tiles, these_tiles, // boolean [] tiles,
prohibit); prohibit);
} }
if (zero_gap_strength) {
for (int i = 0; i < these_tiles.length; i++) if (these_tiles[i] && !pre_gap[i]){
this_strength[i] = 0.0;
}
}
}
//zero_gap_strength
} }
double [] this_disparity_masked = this_disparity.clone(); double [] this_disparity_masked = this_disparity.clone();
...@@ -1970,7 +2034,7 @@ public class TileProcessor { ...@@ -1970,7 +2034,7 @@ public class TileProcessor {
} }
if ((sdfa_instance!=null) && show_filter_scan){ if ((sdfa_instance!=null) && show_scan){
int [] enum_clusters = enumerateClusters( int [] enum_clusters = enumerateClusters(
true, // boolean diag_en, true, // boolean diag_en,
...@@ -1979,7 +2043,7 @@ public class TileProcessor { ...@@ -1979,7 +2043,7 @@ public class TileProcessor {
String [] titles = {"masked","map","orig_map","hor_map","vert_map","bg_sel","far", String [] titles = {"masked","map","orig_map","hor_map","vert_map","bg_sel","far",
"before_small","before_lone","before_gaps","these","near","block", "before_small","before_lone","before_gaps","these","near","block",
"strength","hor-strength","vert-strength", "strength","hor-strength","vert-strength",
"diff0","diff1","diff2","diff3", "enum_clusters", "disp_cm", "disp_poly", "disp_hor", "disp_vert"}; "diff0","diff1","diff2","diff3", "enum_clusters", "disp_cm", "disp_poly", "disp_hor", "disp_vert", "poison"};
double [][] dbg_img = new double[titles.length][tilesY * tilesX]; double [][] dbg_img = new double[titles.length][tilesY * tilesX];
for (int i = 0; i<dbg_img[0].length;i++){ for (int i = 0; i<dbg_img[0].length;i++){
dbg_img[ 0][i] = this_disparity_masked[i]; dbg_img[ 0][i] = this_disparity_masked[i];
...@@ -2006,6 +2070,7 @@ public class TileProcessor { ...@@ -2006,6 +2070,7 @@ public class TileProcessor {
dbg_img[18][i] = these_diffs[2][i]; dbg_img[18][i] = these_diffs[2][i];
dbg_img[19][i] = these_diffs[3][i]; dbg_img[19][i] = these_diffs[3][i];
dbg_img[20][i] = enum_clusters[i]; dbg_img[20][i] = enum_clusters[i];
dbg_img[25][i] = prohibit[i] ? 1 : -1;
} }
dbg_img[ 2] = scan.getDisparity(1); dbg_img[ 2] = scan.getDisparity(1);
dbg_img[ 3] = scan.getDisparity(2); dbg_img[ 3] = scan.getDisparity(2);
...@@ -2019,6 +2084,7 @@ public class TileProcessor { ...@@ -2019,6 +2084,7 @@ public class TileProcessor {
dbg_img[24] = scan. disparity_map[ImageDtt.DISPARITY_INDEX_VERT]; dbg_img[24] = scan. disparity_map[ImageDtt.DISPARITY_INDEX_VERT];
sdfa_instance.showArrays(dbg_img, tilesX, tilesY, true, "FilterScan"+clt_3d_passes.size(),titles); sdfa_instance.showArrays(dbg_img, tilesX, tilesY, true, "FilterScan"+clt_3d_passes.size(),titles);
System.out.println("FilterScan"+clt_3d_passes.size());
} }
return these_tiles; return these_tiles;
...@@ -2303,10 +2369,13 @@ public class TileProcessor { ...@@ -2303,10 +2369,13 @@ public class TileProcessor {
clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster clt_parameters.min_clstr_max); // double min_max_weight // minimal value of the maximal strengh in the cluster
dbg_before_gaps = these_tiles.clone(); dbg_before_gaps = these_tiles.clone();
prohibit = far_tiles; // do not fill gaps over known background/far tiles prohibit = far_tiles.clone(); // do not fill gaps over known background/far tiles
for (int i = 0; i< prohibit.length; i++) prohibit[i] |= block_propagate[i];
if (clt_parameters.fill_gaps > 0) { if (clt_parameters.fill_gaps > 0) {
fillGaps( // grows, then shrinks fillGaps( // grows, then shrinks
clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8 clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
false, // boolean poison, // do not fill gaps that even touch prohibited
these_tiles, // boolean [] tiles, these_tiles, // boolean [] tiles,
prohibit); prohibit);
} }
...@@ -2504,9 +2573,14 @@ public class TileProcessor { ...@@ -2504,9 +2573,14 @@ public class TileProcessor {
this_sure, // final double this_sure, // minimal strength to be considered definitely background this_sure, // final double this_sure, // minimal strength to be considered definitely background
this_maybe, // final double this_maybe, // maximal strength to ignore as non-background this_maybe, // final double this_maybe, // maximal strength to ignore as non-background
sure_smth, // final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd sure_smth, // final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
clt_parameters, clt_parameters.show_filter_scan,
// final int threadsMax, // maximal number of threads to launch clt_parameters.min_clstr_seed, // clt_parameters.min_clstr_seed
// final boolean updateStatus, clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
clt_parameters.min_clstr_lone, // int min_area, // minimal number of pixels
clt_parameters.fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
0, // final int poison_gaps, // Do not fill gaps that have even single "poisoned" tile
false, // final boolean zero_gap_strength, // set strength to zero when covering gaps
debugLevel); debugLevel);
...@@ -2602,7 +2676,6 @@ public class TileProcessor { ...@@ -2602,7 +2676,6 @@ public class TileProcessor {
0.5 * disparity_far, 0.5 * disparity_far,
2.0 * disparity_near, 2.0 * disparity_near,
debugLevel); debugLevel);
dbg_outlayers = new double[outlayers.length]; dbg_outlayers = new double[outlayers.length];
for (int i = 0; i < outlayers.length; i++){ for (int i = 0; i < outlayers.length; i++){
...@@ -2759,6 +2832,7 @@ public class TileProcessor { ...@@ -2759,6 +2832,7 @@ public class TileProcessor {
showDoubleFloatArrays sdfa_instance = null; showDoubleFloatArrays sdfa_instance = null;
if (debugLevel > -1) sdfa_instance = new showDoubleFloatArrays(); // just for debugging? if (debugLevel > -1) sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
//TODO: for next passes - combine all selected for previous passes (all passes with smaller disparity) //TODO: for next passes - combine all selected for previous passes (all passes with smaller disparity)
boolean show_st = clt_parameters.stShow || (debugLevel > 1);
boolean [] these_tiles; boolean [] these_tiles;
int [] replaced = null; // +1 - hor, +2 - vert int [] replaced = null; // +1 - hor, +2 - vert
...@@ -2813,12 +2887,17 @@ public class TileProcessor { ...@@ -2813,12 +2887,17 @@ public class TileProcessor {
this_sure, // final double this_sure, // minimal strength to be considered definitely background this_sure, // final double this_sure, // minimal strength to be considered definitely background
this_maybe, // final double this_maybe, // maximal strength to ignore as non-background this_maybe, // final double this_maybe, // maximal strength to ignore as non-background
sure_smth, // final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd sure_smth, // final double sure_smth, // if 2-nd worst image difference (noise-normalized) exceeds this - do not propagate bgnd
clt_parameters, true, // clt_parameters.show_filter_scan,
clt_parameters.min_clstr_seed, // clt_parameters.min_clstr_seed
clt_parameters.min_clstr_weight, // double min_weight // minimal total weight of the cluster
clt_parameters.min_clstr_max, // double min_max_weight // minimal value of the maximal strengh in the cluster
clt_parameters.min_clstr_lone, // int min_area, // minimal number of pixels
clt_parameters.fill_gaps, // fill_gaps, // int depth, // same as grow - odd - 4 directions, even - 8
clt_parameters.fill_final, // final boolean poison_gaps, // Do not fill gaps that have even single "poisoned" tile
true, // final boolean zero_gap_strength, // set strength to zero when covering gaps
// final int threadsMax, // maximal number of threads to launch // final int threadsMax, // maximal number of threads to launch
// final boolean updateStatus, // final boolean updateStatus,
2); //debugLevel); 2); //debugLevel);
} else { } else {
these_tiles= combineHorVertDisparity( these_tiles= combineHorVertDisparity(
scan_prev, // final CLTPass3d scan, scan_prev, // final CLTPass3d scan,
...@@ -2847,7 +2926,7 @@ public class TileProcessor { ...@@ -2847,7 +2926,7 @@ public class TileProcessor {
double [] dbg_outlayers = null; double [] dbg_outlayers = null;
// boolean [] grown = these_tiles.clone(); // boolean [] grown = these_tiles.clone();
if (use_supertiles) { if (use_supertiles || show_st) {
String [] dbg_st_titles = {"raw", "blurred"+clt_parameters.stSigma,"max-min-max"}; String [] dbg_st_titles = {"raw", "blurred"+clt_parameters.stSigma,"max-min-max"};
double [][] dbg_hist = new double[dbg_st_titles.length][]; double [][] dbg_hist = new double[dbg_st_titles.length][];
...@@ -2862,7 +2941,7 @@ public class TileProcessor { ...@@ -2862,7 +2941,7 @@ public class TileProcessor {
0.0); // NO BLUR double stBlurSigma) 0.0); // NO BLUR double stBlurSigma)
dbg_hist[0] = scan_prev.showDisparityHistogram(); dbg_hist[0] = scan_prev.showDisparityHistogram();
scan_prev.setSuperTiles( SuperTiles st = scan_prev.setSuperTiles(
clt_parameters.stStepNear, // double step_disparity, clt_parameters.stStepNear, // double step_disparity,
clt_parameters.stStepFar, // double step_near, clt_parameters.stStepFar, // double step_near,
clt_parameters.stStepThreshold, // double step_threshold, clt_parameters.stStepThreshold, // double step_threshold,
...@@ -2878,6 +2957,39 @@ public class TileProcessor { ...@@ -2878,6 +2957,39 @@ public class TileProcessor {
int hist_width0 = scan_prev.showDisparityHistogramWidth(); int hist_width0 = scan_prev.showDisparityHistogramWidth();
int hist_height0 = dbg_hist[0].length/hist_width0; int hist_height0 = dbg_hist[0].length/hist_width0;
if (show_st){
sdfa_instance.showArrays(dbg_hist, hist_width0, hist_height0, true, "disparity_supertiles_histograms",dbg_st_titles);
}
st.processPlanes2(
null, // final boolean [] selected, // or null
0.3, // final double min_disp,
false, // final boolean invert_disp, // use 1/disparity
clt_parameters.plDispNorm, // = 2.0; // Normalize disparities to the average if above
clt_parameters.plMinPoints, // = 5; // Minimal number of points for plane detection
clt_parameters.plTargetEigen, // = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
clt_parameters.plFractOutliers, // = 0.3; // Maximal fraction of outliers to remove
clt_parameters.plMaxOutliers, // = 20; // Maximal number of outliers to remove
geometryCorrection,
clt_parameters.correct_distortions,
debugLevel); // final int debugLevel)
/*
st.processPlanes1(
null, // final boolean [] selected, // or null
0.3, // final double min_disp,
false, // final boolean invert_disp, // use 1/disparity
clt_parameters.plDispNorm, // = 2.0; // Normalize disparities to the average if above
debugLevel); // final int debugLevel)
st.processPlanes(
null, // final boolean [] selected, // or null
0.3, // final double min_disp,
true, // final boolean invert_disp, // use 1/disparity
clt_parameters.plDispNorm, // = 2.0; // Normalize disparities to the average if above - here wrong
debugLevel); // final int debugLevel)
*/
if (use_supertiles) {
scan_prev.getBgDispStrength( // calculate (check non-null)? scan_prev.getBgDispStrength( // calculate (check non-null)?
clt_parameters.stMinBgDisparity, // final double minBgDisparity, clt_parameters.stMinBgDisparity, // final double minBgDisparity,
clt_parameters.stMinBgFract); // final double minBgFract); clt_parameters.stMinBgFract); // final double minBgFract);
...@@ -2894,7 +3006,8 @@ public class TileProcessor { ...@@ -2894,7 +3006,8 @@ public class TileProcessor {
if (dbg_with_super_disp != null) dbg_with_super_disp = dbg_with_super_disp.clone(); // else no super disparity available if (dbg_with_super_disp != null) dbg_with_super_disp = dbg_with_super_disp.clone(); // else no super disparity available
} }
// replace weak outlaye tiles with weighted averages (modifies disparity) }
// replace weak outlier tiles with weighted averages (modifies disparity)
boolean[] outlayers = scan_prev.replaceWeakOutlayers( boolean[] outlayers = scan_prev.replaceWeakOutlayers(
null, // final boolean [] selection, null, // final boolean [] selection,
clt_parameters.outlayerStrength , //final double weakStrength, // strength to be considered weak, subject to this replacement clt_parameters.outlayerStrength , //final double weakStrength, // strength to be considered weak, subject to this replacement
...@@ -3124,7 +3237,7 @@ public class TileProcessor { ...@@ -3124,7 +3237,7 @@ public class TileProcessor {
} }
if (numCycle >= (clt_parameters.tiNumCycles -2)) { // only heal during last 2 passes? if (numCycle >= (clt_parameters.tiNumCycles -2)) { // only heal during last 2 passes?
if (clt_parameters.tiHealSame > 0){ if (clt_parameters.tiHealSame > 0){
int numHealed = dp. healSame( // returns number of new ortho connections int numHealed = dp.healSame( // returns number of new ortho connections // ty = 325
neighbors, neighbors,
clt_parameters.tiHealSame , // int maxlen, clt_parameters.tiHealSame , // int maxlen,
// just to fill in diagonals // just to fill in diagonals
...@@ -3363,14 +3476,14 @@ public class TileProcessor { ...@@ -3363,14 +3476,14 @@ public class TileProcessor {
if (debugLevel > -1){ if (debugLevel > -1){
System.out.println("secondPassSetup(): created "+ numScans+ " FPGA passes."); System.out.println("secondPassSetup(): created "+ numScans+ " FPGA passes.");
} }
if (debugLevel > 0){ if (debugLevel > 10){ // null pointer
String [] titles = new String [clt_3d_passes.size()]; String [] titles = new String [clt_3d_passes.size()];
double [][] disparities = new double [titles.length][tilesX*tilesY]; double [][] disparities = new double [titles.length][tilesX*tilesY];
for (int i = 0; i < titles.length; i++) { for (int i = 0; i < titles.length; i++) {
titles[i] = i+"_scan"; titles[i] = i+"_scan";
double [][] disparityTiles = clt_3d_passes.get(i).disparity; double [][] disparityTiles = clt_3d_passes.get(i).disparity;
for (int ty = 0; ty < tilesY; ty++) for (int tx = 0; tx < tilesX; tx++){ for (int ty = 0; ty < tilesY; ty++) for (int tx = 0; tx < tilesX; tx++){
disparities[i][ty*tilesX+tx] = disparityTiles[ty][tx]; disparities[i][ty*tilesX+tx] = disparityTiles[ty][tx]; // null pointer
} }
} }
sdfa_instance.showArrays(disparities, tilesX, tilesY, true, "disparities_scans",titles); sdfa_instance.showArrays(disparities, tilesX, tilesY, true, "disparities_scans",titles);
......
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