Commit 7dac691f authored by Andrey Filippov's avatar Andrey Filippov

Developing street pole distance measurement with dual-quad rig

parent 41b89aee
......@@ -21,6 +21,7 @@
** -----------------------------------------------------------------------------**
**
*/
import java.awt.Point;
import java.awt.Rectangle;
import java.util.ArrayList;
import java.util.Collections;
......@@ -29,29 +30,377 @@ import java.util.concurrent.atomic.AtomicInteger;
public class PoleProcessor {
final static double THRESHOLD_LIN = 1.0E-20; // threshold ratio of matrix determinant to norm for linear approximation (det too low - fail)
final static double THRESHOLD_QUAD = 1.0E-30; // threshold ratio of matrix determinant to norm for quadratic approximation (det too low - fail)
BiCamDSI biCamDSI;
ArrayList<PoleCluster> pole_clusters = new ArrayList<PoleCluster>();
int tilesX;
int tilesY;
class PoleCluster{
double mean_disp = 0.0;
double mean_disparity = 0.0;
double target_disparity = 0.0;
double target_previous = 0.0;
double mean_x = 0.0;
double mean_y = 0.0;
double strength = 0.0;
ArrayList <Integer> tiles = new ArrayList<Integer>();
Rectangle bBox = new Rectangle();
Rectangle eBox; // not calculated, assigned by caller
ArrayList<DsPair> ds_list = new ArrayList<DsPair>();
ArrayList<DsPair> ds_selected = new ArrayList<DsPair>();
int layer = -1;
double getMeanDisp() {return mean_disp;}
double xc_center = Double.NaN;
double slope = Double.NaN; // dx/dy
boolean disabled = false;
// int split = -1;
boolean [] pole_selection; // tiles in eBox that belong to the pole
double [][] bg_ds; // background disparity/strength for eBox dimensions;
class DsPair{
int nTile;
double disparity;
double strength;
DsPair(
int nTile,
double disparity,
double strength)
{
this.nTile = nTile;
this.disparity = disparity;
this.strength = strength;
}
}
double getMeanDisparity() {return mean_disparity;}
double getTargetDisparity() {return target_disparity;}
void setTargetDisparity(double disparity) {target_disparity = disparity;}
double getStrength() {return strength;} // to get average strength - need to divide by getNumTiles()
double getMeanX() {return mean_x;}
double getMeanY() {return mean_y;}
int getNumTiles() {return tiles.size();}
ArrayList<Integer> getTiles() {return tiles;}
public Rectangle getBox(boolean extended) {return extended? eBox: bBox;}
public Rectangle getBBox() {return bBox;}
public Rectangle getEBox() {return eBox;}
public void trimEBoxBottom(int height) {eBox.height = height;}
public boolean [] getMask () {return pole_selection;}
public boolean getMask (int dx, int dy) {return pole_selection[dx + dy * eBox.width];}
@Override
public String toString() {
return String.format("{%3d, %3d, %2d, %2d}: %7.5f", eBox.x, eBox.y, eBox.width,eBox.height, target_disparity);
}
//-1 - definitely not,+1 - definitely yes, 0 - uncertain
public int poleFilter() {
// int min_mask = 10; // good - min 12
int min_height = 10; // good - min 12
boolean headlessOK = false;
// double min_fraction = 0.3; // worst 0.3478
double min_fraction = 0.5; // worst 0.3478 , next - 0.57
double min_frac_height = 0.5; // worst 0.25 , next - 0.556
double min_frac_box = 0.7; // worst 0.75 , next - 0.556
/*
boolean unselected_only = true;
double bg_strength = cluster.getAverageBgStrength(unselected_only);
double meas_strength = cluster.getAverageStrength(true); // divided by measured tiles only
double mask_strength = cluster.getAverageStrength(false); // divided by a total number of pole tiles (by mask)
double rstrength = mask_strength/bg_strength;
*/
// if (getMaskSize() < min_mask) return false;
// return 0;
// if (!disabled) return 0;
if (disabled) return -1;
if (eBox.height < min_height) return -1;
if (!headlessOK && (getNumTiles() == 0)) return -1; // skip headless clusters
if (getFractSelected() < min_fraction) return -1;
if (getFractHeight() < min_frac_height) return -1;
if (getFractBoxHeight() < min_frac_box) return -1;
return 0;
}
// get cluster stats for filtering
public int getMaskSize() {
int n =0;
for (int i = 0; i< pole_selection.length; i++) if (pole_selection[i]) n++;
return n;
}
public int getSelectionSize() {
return ds_selected.size();
}
public double getFractSelected() {
return 1.0* ds_selected.size()/getMaskSize();
}
public double getAverageStrength(boolean measured_only) { // denominator - measured - only average among measured tiles, false - all pole mask
double sw = 0.0;
for (DsPair dsp: ds_selected) {
sw += dsp.strength;
}
return sw / (measured_only? ds_selected.size() : getMaskSize());
}
public double getAverageBgStrength( // empty counts as zero strengths
boolean unselected)
{
double sw = 0.0;
int ns = 0;
for (int indx = 0; indx < bg_ds.length; indx++) {
if (!unselected || !pole_selection[indx]) {
ns++;
double [] ds = bg_ds[indx];
if (ds != null){
sw += bg_ds[indx][1];
}
}
}
return sw/ns;
}
public boolean [][] getVertMasks(){
boolean [] y_mask = new boolean[eBox.height];
boolean [] y_selection = new boolean[eBox.height];
for (int i = 0; i < pole_selection.length; i++) {
if (pole_selection[i]) {
y_mask[i/eBox.width] = true;
}
}
for (int dy = 0; dy < (bBox.y-eBox.y); dy++){ // don't count all what is above the seed
y_mask[dy] = false;
}
// remove seed (original head) area
for (int nt:tiles) {
int dy = nt / tilesX - eBox.y;
y_mask[dy] = false;
}
for (DsPair dsp:ds_selected) {
y_selection[dsp.nTile / tilesX - eBox.y] = true;
}
boolean [][] masks = {y_selection, y_mask}; //{ num, denom}
return masks;
}
public double getFractHeight() {
boolean [][] masks = getVertMasks(); // {y_selection, y_mask};
int num_y_mask = 0, num_y_sel = 0;
for (int i = 0; i < eBox.height; i++) {
if (masks[1][i]) {
num_y_mask++;
if (masks[0][i]) num_y_sel++;
}
}
return (num_y_mask > 0) ? (1.0 * num_y_sel / num_y_mask) : 0.0;
}
public double getFractBoxHeight() {
boolean [][] masks = getVertMasks(); // {y_selection, y_mask};
int num_y_sel = 0;
int y_top = bBox.x-eBox.x;
int y_bot = 0;
for (int i = y_top; i < eBox.height; i++) {
if (masks[1][i]) {
y_bot = i;
}
if (masks[0][i]) num_y_sel++;
}
// return 1.0 * num_y_sel / (eBox.height - y_top);
return 1.0 * num_y_sel / (y_bot- y_top + 1);
}
// get longest missing stem over the "visible" stem mask
public int getFreeStemGap() {
boolean [][] masks = getVertMasks(); // {y_selection, y_mask};
int gs = 0;
int ln = masks[1].length;
int max_gap = 0;
int gap_len = 0;
for (; gs < ln -1;) {
for (; gs < ln-1; gs++) {
if (masks[1][gs] && !masks[0][gs]) {
gap_len = 1;
break; // gs now points at the beginning of the gap
}
}
if (gs >= ln -1) {
break; // too late
}
int ge;
for (ge = gs+1; ge < ln; ge++) {
if (masks[1][ge] && !masks[0][ge]) {
gap_len++;
} else if (masks[0][ge]){
break; // ge now points at the detected tile (first after the gap)
}
}
if (gap_len > max_gap) {
max_gap = gap_len;
}
gs = ge;
}
return max_gap;
}
public double getMDisparityRMS() {
double sw = 0.0, swd = 0.0, swd2 = 0.0;
for (DsPair dsp:ds_selected) {
double w = dsp.strength;
sw += w;
double d = dsp.disparity;
swd+=w*d;
swd2 += w*d*d;
}
return Math.sqrt((swd2 * sw + swd*swd)/(sw*sw));
}
public boolean checkRightLeft(
double min_strength,
double disp_adiff,
double disp_rdiff,
double [][] norn_ds)
{
int extra_margin = 2;
int debugLevel = -1;
int dbg_tileX = 128; // 221;// 128;
int dbg_tileY = 87; // 130; // 87;
if (debugLevel > -2) {
if (eBox.contains(new Point(dbg_tileX, dbg_tileY))) {
System.out.println("checkRightLeft(): eBox={"+eBox.x+", "+eBox.y+", "+eBox.width+", "+eBox.height+"}");
System.out.println("checkRightLeft(): eBox={"+eBox.x+", "+eBox.y+", "+eBox.width+", "+eBox.height+"}");
}
}
double disp_diff = disp_adiff + target_disparity*disp_rdiff;
double disp_margin = mean_disparity - disp_diff; // need to have tiles both to right and left from the head with disparity below this margin (or be too weak)
double [] hor_profile = new double[eBox.width+2*extra_margin];
TileNeibs tnImage = biCamDSI.tnImage;
int nTile = eBox.x + eBox.y*tilesX;
for (int dx = -extra_margin; dx<eBox.width+extra_margin; dx++) {
for (int dy = (bBox.y-eBox.y); dy < (bBox.y-eBox.y+bBox.height); dy++) {
int nTile1 = tnImage.getNeibIndex(nTile, dx, dy);
if (nTile1 >= 0) {
if ((norn_ds[1][nTile1] > min_strength) && (norn_ds[1][nTile1] > hor_profile[dx+extra_margin])) {
hor_profile[dx+extra_margin] = norn_ds[0][nTile1];
}
}
}
}
double y = bBox.y-eBox.y + 0.5*bBox.height - 0.5*eBox.height;
double x = xc_center + y* slope + 0.5*eBox.width;
int x0 = (int) Math.round(x);
boolean sep = false;
if ((x0 < 0) || (x0 > hor_profile.length )) {
System.out.println(String.format("Disabling cluster {%d, %d, %d, %d} disp= %f as it is not separated on right or left. x0= %d",
eBox.x, eBox.y, eBox.width, eBox.height, target_disparity, x0));
return false;
}
for (int i = x0+extra_margin; i < hor_profile.length; i++) {
if (hor_profile[i] < disp_margin) {
sep = true;
break;
}
}
if (!sep) {
System.out.println(String.format("Disabling cluster {%d, %d, %d, %d} disp= %f as it is not separated on right. x0= %d",
eBox.x, eBox.y, eBox.width, eBox.height, target_disparity, x0));
return false;
}
sep = false;
for (int i = x0+extra_margin; i >= 0; i--) {
if (hor_profile[i] < disp_margin) {
sep = true;
break;
}
}
if (!sep) {
System.out.println(String.format("Disabling cluster {%d, %d, %d, %d} disp= %f as it is not separated on left. x0= %d",
eBox.x, eBox.y, eBox.width, eBox.height, target_disparity, x0));
}
return sep;
}
public double [] getPoleLine() {
if (Double.isNaN(xc_center)) {
return null;
}
double [] pole_line = {xc_center,slope};
return pole_line;
}
public void resetDsPairs() {
ds_list = new ArrayList<DsPair>();
ds_selected = new ArrayList<DsPair>();
}
public void addDs(
int nTile,
double disparity,
double strength)
{
// ds_list.add(new DsPair(nTile,disparity,strength));
addDs(new DsPair(nTile,disparity,strength));
}
public void addDs(
DsPair dsp)
{
ds_list.add(dsp);
if (pole_selection != null) {
int dx = dsp.nTile % tilesX - eBox.x;
int dy = dsp.nTile / tilesX - eBox.y;
int indx = dy*eBox.width + dx;
if ((indx < pole_selection.length) && pole_selection[indx]) {
ds_selected.add(dsp);
}
}
}
public void removeDs(
int nTile)
{
// may have multiple for the same tile - remove all
for (int nt = 0; nt < ds_list.size(); nt++) {
if (ds_list.get(nt).nTile == nTile) {
ds_list.remove(nt);
nt--; // compensate for loop increment
}
}
}
public double [][] getDsPairs(boolean selected){
double [][] ds = new double [2][selected?ds_selected.size():ds_list.size()];
for (int i = 0; i < ds[0].length; i++) {
DsPair ds_pair = selected? (ds_selected.get(i)):(ds_list.get(i));
ds[0][i] = ds_pair.disparity;
ds[1][i] = ds_pair.strength;
}
return ds;
}
public int [] getDsTiles(boolean selected){
int[] ds_tiles = new int[selected?ds_selected.size():ds_list.size()];
for (int i = 0; i < ds_tiles.length; i++) {
DsPair ds_pair = selected? (ds_selected.get(i)):(ds_list.get(i));
ds_tiles[i] = ds_pair.nTile;
}
return ds_tiles;
}
public void setEBox(
int ext_left,
......@@ -116,7 +465,7 @@ public class PoleProcessor {
TileNeibs tnImage = biCamDSI.tnImage;
int tileX = nTile % tnImage.sizeX;
int tileY = nTile / tnImage.sizeX;
mean_disp = (mean_disp * strength + norm_ds[0][nTile] * norm_ds[1][nTile])/(strength + norm_ds[1][nTile]);
mean_disparity = (mean_disparity * strength + norm_ds[0][nTile] * norm_ds[1][nTile])/(strength + norm_ds[1][nTile]);
mean_x = (mean_x * strength + tileX * norm_ds[1][nTile])/(strength + norm_ds[1][nTile]);
mean_y = (mean_y * strength + tileY * norm_ds[1][nTile])/(strength + norm_ds[1][nTile]);
strength += norm_ds[1][nTile];
......@@ -125,8 +474,9 @@ public class PoleProcessor {
if (tiles.isEmpty() || ( tileY < bBox.y)) bBox.y = tileY;
if (tiles.isEmpty() || ((tileY - bBox.y) > (bBox.height -1))) bBox.height = tileY - bBox.y + 1;
tiles.add(nTile);
target_disparity = mean_disparity;
}
// remove would be trikier - will need to recalculate bBox
// remove would be trickier - will need to recalculate bBox
void removeTile(
double [][] norm_ds,
int nTile)
......@@ -139,7 +489,7 @@ public class PoleProcessor {
TileNeibs tnImage = biCamDSI.tnImage;
int tileX = nTile % tnImage.sizeX;
int tileY = nTile / tnImage.sizeX;
mean_disp = (mean_disp * strength - norm_ds[0][nTile] * norm_ds[1][nTile])/(strength - norm_ds[1][nTile]);
mean_disparity = (mean_disparity * strength - norm_ds[0][nTile] * norm_ds[1][nTile])/(strength - norm_ds[1][nTile]);
mean_x = (mean_x * strength - tileX * norm_ds[1][nTile])/(strength - norm_ds[1][nTile]);
mean_y = (mean_y * strength - tileY * norm_ds[1][nTile])/(strength - norm_ds[1][nTile]);
strength -= norm_ds[1][nTile];
......@@ -154,11 +504,411 @@ public class PoleProcessor {
if ((i==0) || ( tileY < bBox.y)) bBox.y = tileX;
if ((i==0) || ((tileY - bBox.y) > (bBox.height -1))) bBox.height = tileY - bBox.y + 1;
}
target_disparity = mean_disparity;
}
PoleCluster [] two_clusters = null;
// see if the cluster needs split. returns -1 if it does not, > 0 if all tiles with offset >= go to the second cluster
// returns null or a pair of new clusters instead of the current (does not trry to split in more than 2
public PoleCluster[] checkAndSplit(
double [][] norm_ds,
int min_dist, // minimal distance
boolean must_zero) {
if ((eBox.x == 61) && (eBox.y== 128)) {
System.out.println("Debugging "+this.toString());
}
if (eBox.width < 2) {
return null;
}
double [] histogram = new double [eBox.width];
for (DsPair dsp:ds_list) {
histogram [dsp.nTile % tilesX - eBox.x] += dsp.strength;
}
boolean [] is_max = new boolean[histogram.length];
boolean [] is_zero = new boolean[histogram.length];
int num_lmax = 0;
for (int i = 0; i < histogram.length;i++) {
is_zero[i] = histogram[i] == 0.0;
if (!is_zero[i]) {
is_max[i] = ((i == 0) || (histogram[i] >= histogram[i-1])) && ((i == (histogram.length - 1)) || (histogram[i] >= histogram[i+1]));
if (is_max[i]) num_lmax ++;
}
}
if (num_lmax < 2) {
return null;
}
int gs = 0, ge = 0;
boolean is_gap = false;
int cut = -1;
// find first (leftmost maximum) - skip all non-maximums
for (;gs < is_max.length-1;gs++) { //find start of the non-maximum run
if (is_max[gs]) break;
}
for (; gs < is_max.length-1;) {
for (;gs < is_max.length-1;gs++) { //find start of the non-maximum run
if (!is_max[gs]) break;
}
if (gs >= is_max.length-1) { // too late to have another maximum
break;
}
boolean has_zero = false;
ge = gs + 1;
for (;ge < is_max.length; ge++) { //find end of the non-maximum run (next maximum)
has_zero |= is_zero[ge];
is_gap = true;
if (is_max[ge]) break;
}
if ((ge < is_max.length) && is_gap && ((ge - gs) >= min_dist) && (!must_zero || has_zero)) { // gap valid for split - find the cut place
cut = gs;
for(int i = gs+1; i < ge; i++) {
if (histogram[i] < histogram[cut]) {
cut = i;
}
}
// if a long rung of 0 - find center
int cut_end = cut+1;
for (;cut_end < ge;cut_end++) {
if (histogram[cut_end] > histogram[cut]) break;
}
cut = (cut+cut_end+1)/2;
break;
}
is_gap = false; // that was a bad gap
gs = ge;
}
if (cut > 0) {
// split in 2 clusters, adjust eBox without normal gaps not to change layers
// will have to use wrong (old) mean_disparity as it was used for measurements
two_clusters = new PoleCluster [2];
two_clusters[0]= new PoleCluster();
two_clusters[1]= new PoleCluster();
for (int nTile: tiles) {
int dx = nTile % tilesX - eBox.x;
two_clusters[(dx < cut)? 0 : 1].addTile(norm_ds, nTile);
}
// overwrite mean disparity to that of the original
two_clusters[0].setTargetDisparity(getTargetDisparity());
two_clusters[1].setTargetDisparity(getTargetDisparity());
// manually set eBox-es, so they do not overlap on the same layer. OK, as for vertical objects there is no need for horizontal padding for measurements
two_clusters[0].eBox = new Rectangle(eBox.x, eBox.y, cut, eBox.height);
two_clusters[1].eBox = new Rectangle(eBox.x + cut, eBox.y, eBox.width - cut, eBox.height);
// set layer to that of the original one
two_clusters[0].layer = layer;
two_clusters[1].layer = layer;
// split ds_list (but not ds_selected - it should be calculated after that)
for (DsPair dsp:ds_list) {
int dx = dsp.nTile % tilesX - eBox.x;
two_clusters[(dx < cut)? 0 : 1].addDs(dsp);
}
System.out.println("Splitting cluster "+this.toString()+" into 2: "+two_clusters[0]+toString()+" and "+two_clusters[1].toString());
}
return two_clusters;
}
// approximate pole centerline as offset from eBox.x at eBox.y and slope (dx/dy)
public void calcPoleLine(
double max_diff,
double max_tilt,
double max_rms,
int min_tiles,
double damp_tilt,
int debugLevel)
{
double x0 = eBox.x + 0.5*eBox.width;
double y0 = eBox.y + 0.5*eBox.height;
{
double sw = 0.0, swx = 0.0;
for (DsPair dsp:ds_list) {
int x = dsp.nTile % tilesX;
double w = dsp.strength;
sw += w;
swx += w * x;
}
if (sw <=0) {
return;
}
xc_center = swx/sw - x0;
slope = 0.0;
}
// preliminary sort:
ds_selected = new ArrayList<DsPair>();
for (DsPair dsp: ds_list) {
double x = dsp.nTile % tilesX - x0; // relative to the eBox center
double y = dsp.nTile / tilesX - y0;
if (Math.abs(x - xc_center) <= (max_diff + Math.abs(y * max_tilt))) {
ds_selected.add(dsp);
}
}
if (ds_selected.size() < min_tiles) {
return; // use initial vertical line
}
for (int nstep = 0; nstep < ds_selected.size(); nstep++) { // should normally break;
// calculate rms, and worst tile
double worst_e2 = 0.0;
int worst_index = -1;
double sw = 0.0, swe2 = 0.0;
for (int indx = 0; indx < ds_selected.size(); indx++) {
DsPair dsp = ds_selected.get(indx);
double x = dsp.nTile % tilesX - x0; // relative to the eBox center
double y = dsp.nTile / tilesX - y0;
double w = dsp.strength;
double e = x - (xc_center + y* slope);
double e2 = e*e;
if (e2 > worst_e2) {
worst_e2 = e2;
worst_index = indx;
}
sw += w;
swe2 += w * e2;
}
double rms = Math.sqrt(swe2/sw);
if (nstep > 0){
if ((rms < max_rms) || (ds_selected.size() < min_tiles)){
return;
}
// remove worst
ds_selected.remove(worst_index);
}
// perform linear approximation, at least once
double [] approx_lin = fitLine(
damp_tilt, // double damp_tilt,
ds_selected, // ArrayList<DsPair> plist,
debugLevel); // int debugLevel);
if (approx_lin == null) {
if (debugLevel > -1){
System.out.println("fitLine(): can not find linear approximation (should not happen)");
}
return;
}
xc_center = approx_lin[0];
slope = approx_lin[1];
}
}
public double [] fitLine(
double damp_tilt,
ArrayList<DsPair> plist,
int debugLevel) {
PolynomialApproximation pa = new PolynomialApproximation();
// using 2d approximation and duplicate samples
final double [] damping = {damp_tilt, damp_tilt, 0.0}; // 0.0 will be applied to average value, tilt_cost - to both tilts
double [][][] mdata = new double [2 * plist.size()][3][];
int mindx = 0;
double x0 = eBox.x + 0.5*eBox.width;
double y0 = eBox.y + 0.5*eBox.height;
for (DsPair dsp: plist) {
int tileX = dsp.nTile % tilesX;
int tileY = dsp.nTile / tilesX;
double v = tileX - x0;
double w = dsp.strength;
double x = tileY - y0;
for (int y = 0; y < 2; y++) { //
mdata[mindx][0] = new double [2];
mdata[mindx][0][0] = x; // sx - smpl_radius;
mdata[mindx][0][1] = y; // sy - smpl_radius;
mdata[mindx][1] = new double [1];
mdata[mindx][1][0] = v; // smpl_d[indxs];
mdata[mindx][2] = new double [1];
mdata[mindx][2][0] = w; // smpl_w[indxs];
mindx ++;
}
}
double[][] approx2d = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // double [] damping,
THRESHOLD_LIN, // threshold ratio of matrix determinant to norm for linear approximation (det too low - fail)
THRESHOLD_QUAD, // threshold ratio of matrix determinant to norm for quadratic approximation (det too low - fail)
debugLevel);
if (approx2d == null){
if (debugLevel > -1){
System.out.println("fitLine(): can not find linear approximation");
}
return null;
}
double [] rslt = {approx2d[0][2], approx2d[0][0]};
return rslt;
}
public void copyNormDS(
double [][] norm_ds)
{
TileNeibs tnImage = biCamDSI.tnImage;
bg_ds = new double [eBox.width*eBox.height][];
int nTile = eBox.x + eBox.y * tilesX;
for (int dy = 0; dy< eBox.height; dy++) {
for (int dx = 0; dx < eBox.width; dx++) {
int nTile1 = tnImage.getNeibIndex(nTile, dx, dy);
if ((nTile1 >=0) && !Double.isNaN(norm_ds[0][nTile1]) && (norm_ds[1][nTile1] > 0.0)){
double [] ds = {norm_ds[0][nTile1], norm_ds[1][nTile1]};
bg_ds[dx + dy * eBox.width] = ds;
}
}
}
}
public void createPoleMask( // shoud run copyNormDS before
// double [][] norm_ds,
int min_neibs,
boolean use_seed,
double width,
double disp_aover,
double disp_rover,
int debugLevel)
{
boolean square_head = true; // only allow seed in the top square of eBox
int dbg_tileX = 99; // 221;// 128;
int dbg_tileY = 112; // 130; // 87;
if (debugLevel > -2) {
if (eBox.contains(new Point(dbg_tileX, dbg_tileY))) {
System.out.println("createPoleMask(): eBox={"+eBox.x+", "+eBox.y+", "+eBox.width+", "+eBox.height+"}");
}
}
double head_edge_strength = 0.1;
pole_selection = new boolean [eBox.width*eBox.height];
double disp_near = target_disparity + disp_aover + target_disparity * disp_rover;
if (bBox.width > 0) {
for (int dy = bBox.y - eBox.y; dy < eBox.height; dy++) {
double x = 0.5 * eBox.width +xc_center + slope * (dy - 0.5*eBox.height);
for (int dx = (int) Math.round(x - width); dx <= (int) Math.round(x + width); dx++) {
if ((dx >= 0) && (dx < eBox.width)) {
int indx = dx + eBox.width*dy;
if ((bg_ds[indx] == null) || !(bg_ds[indx][0] > disp_near)) {
pole_selection[indx] = true;
}
}
}
}
} else {
System.out.println("bBox is not initialized, eBox={"+eBox.x+","+eBox.y+","+eBox.width+","+eBox.height+"}");
System.out.println("bBox is not initialized");
}
TileNeibs tnEBox = new TileNeibs(eBox.width, eBox.height);
if (use_seed) { // will not be masked by nearer objects?
boolean [] seed_mask = new boolean [eBox.width*eBox.height];
for (int nTile:tiles){
int dy = nTile / tilesX - eBox.y;
if (!square_head || (dy < eBox.width)) {
int dx = nTile % tilesX - eBox.x;
int indx = dx + eBox.width*dy;
pole_selection[indx] = true;
seed_mask[indx] = true;
}
}
// add tiles 1 tile around seed if it was measured and passed filter
tnEBox.growSelection(
4, // 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
seed_mask, // boolean [] tiles,
null); // boolean [] prohibit)
for (DsPair dsp:ds_list) {
if (dsp.strength > head_edge_strength) {
int dx = dsp.nTile % tilesX - eBox.x;
int dy = dsp.nTile / tilesX - eBox.y;
int indx = dx + dy*eBox.width;
if (seed_mask[indx]) {
pole_selection[indx] = true;
}
}
}
}
if (min_neibs > 0) {
int num_removed = tnEBox.removeFewNeibs(
pole_selection, // boolean [] selection, // should be the same size
min_neibs); // int min_neibs)
if ((debugLevel > -2) && (num_removed > 0)) {
System.out.println("createPoleMask(): removed "+num_removed+" loosely connected/isolated tile (num_neibs < "+min_neibs+")");
}
}
}
public void filterByMask(
boolean trim_bottom)
{
if (trim_bottom) {
int indx;
for (indx = pole_selection.length-1; indx >=0; indx--) {
if (pole_selection[indx]) break;
}
int height = indx/eBox.width + 1;
if (height < eBox.height) {
System.out.println(String.format("filterByMask() Trimming {%3d, %3d, %2d, %2d}:%7.5f to height %2d",
eBox.x, eBox.y, eBox.width,eBox.height, target_disparity, height));
xc_center -= 0.5 * slope * (eBox.height - height);
eBox.height = height;
boolean [] mask = new boolean[eBox.width * eBox.height];
double [][] trimmed_ds = new double[eBox.width * eBox.height][];
for (int i = 0; i < mask.length; i++) {
mask[i] = pole_selection[i];
trimmed_ds[i] = bg_ds[i];
}
pole_selection = mask;
bg_ds = trimmed_ds;
}
}
for (int np = 0; np< ds_selected.size(); np++) {
DsPair dsp = ds_selected.get(np);
int dx = dsp.nTile % tilesX - eBox.x;
int dy = dsp.nTile / tilesX - eBox.y;
int indx = dx + dy*eBox.width;
if ((indx >= pole_selection.length) || !pole_selection[indx]) {
ds_selected.remove(np);
np--; // to compensate for loop
}
}
}
public double getTargetDiff() {
return target_disparity - target_previous;
}
public double applyMeasuredDisparity(
double disparity_scale, // target disparity to differential disparity scale (baseline ratio) (~0.2)
double diff_power, // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
double diff_offset, // add to measured differential disaprity before raising to specified power
int cut_bottom, // cut few tile rows from the very bottom - they may be influenced by ground objects
double keep_bottom, // do not cut more that this fraction of the bounding bow height
int debugLevel) // debug level
{
int cut = (int) Math.round(Math.min(eBox.height * keep_bottom, cut_bottom));
int height = eBox.height - cut;
double sw=0.0, swd = 0.0;
for (DsPair dsp:ds_selected) {
int dx = dsp.nTile % tilesX - eBox.x;
int dy = dsp.nTile / tilesX - eBox.y;
int indx = dx + dy*eBox.width;
if (pole_selection[indx] && (dy < height)) {
double w = dsp.strength;
double d = dsp.disparity;
if (diff_power > 0.0) {
double v=d + diff_offset;
if (v < 0.0) v = 0;
w *= Math.pow(v, diff_power);
}
sw += w;
swd += w * d;
}
}
swd /= sw;
double diff = disparity_scale * swd;
target_previous = target_disparity;
target_disparity += diff;
if (debugLevel > 0) {
System.out.println("applyMeasuredDisparity() for cluster "+this.toString()+" target disparity change: "+diff);
}
return diff;
}
} // end of PoleCluster class
public PoleProcessor (
......@@ -282,7 +1032,7 @@ public class PoleProcessor {
label_gotit: {
for (PoleCluster cluster: clusters) {
if (
!(Math.abs(cluster.getMeanDisp() - this_disp) > max_dd) && // to allow Double.NaN
!(Math.abs(cluster.getMeanDisparity() - this_disp) > max_dd) && // to allow Double.NaN
!(Math.abs(cluster.getMeanX() - tileX) > max_dx) &&
!(Math.abs(cluster.getMeanY() - tileY) > max_dy)) {
cluster.addTile(norm_ds, nTile);
......@@ -339,7 +1089,7 @@ public class PoleProcessor {
Rectangle bBox = cluster.getBBox(); // never extends beyond ?
Rectangle eBox = cluster.getEBox();
int nTile = eBox.x + tilesX * eBox.y;
double disp_marg = cluster.getMeanDisp() + pedestal_disp_over;
double disp_marg = cluster.getMeanDisparity() + pedestal_disp_over;
for (int dy = bBox.y - eBox.y + bBox.height; dy < eBox.height; dy++) {
// label
label_still_some_far_weak:{
......@@ -366,6 +1116,25 @@ public class PoleProcessor {
}
public int removeFilteredClusters(
ArrayList<PoleCluster> clusters,
int debugLevel)
{
int original_size = clusters.size();
for (int nClust = 0; nClust < clusters.size(); nClust++) {
PoleCluster cluster = clusters.get(nClust);
if (cluster.poleFilter() < 0) {
if (debugLevel > -2) {
System.out.println("Removing filtered out cluster: "+cluster.toString());
}
clusters.remove(nClust);
nClust--;
}
}
return original_size - clusters.size();
}
public int assignClustersToLayers(
ArrayList<PoleCluster> clusters)
......@@ -412,11 +1181,11 @@ public class PoleProcessor {
int num_merge = 0;
for (int nCluster = 0; nCluster < clusters.size(); nCluster++) {
PoleCluster cluster = clusters.get(nCluster);
double this_disp = cluster.getMeanDisp();
double this_disp = cluster.getMeanDisparity();
Rectangle this_box = extended?cluster.getEBox():cluster.getBBox();
for (int nOther = 0; nOther < nCluster; nOther++) {
PoleCluster other_cluster = clusters.get(nOther);
double other_disp = other_cluster.getMeanDisp();
double other_disp = other_cluster.getMeanDisparity();
Rectangle other_box = extended?other_cluster.getEBox():other_cluster.getBBox();
if (this_box.intersects(other_box) && (Math.abs(other_disp - this_disp) < disp_tolerance)) {
if (debugLevel > -2) {
......@@ -429,7 +1198,7 @@ public class PoleProcessor {
nCluster--; // so when incremented by for() it will stay the same
num_merge++;
if (debugLevel > -2) {
other_disp = other_cluster.getMeanDisp();
other_disp = other_cluster.getMeanDisparity();
other_box = extended ? other_cluster.getEBox(): other_cluster.getBBox();
System.out.println("Combined cluster: ("+other_box.x+","+other_box.y+","+other_box.width+","+other_box.height+") disp="+other_disp);
}
......@@ -440,13 +1209,59 @@ public class PoleProcessor {
return num_merge;
}
public int splitClusters(
double [][] norm_ds,
int min_dist, // minimal distance between local maximums
boolean must_zero, // zero histogram should exist between local maximum to split
ArrayList<PoleCluster> clusters,
int debugLevel)
{
int num_split = 0;
for (int nClust = 0; nClust < clusters.size(); nClust++){
PoleCluster cluster = clusters.get(nClust);
PoleCluster [] two_clusters = cluster.checkAndSplit(
norm_ds,
min_dist,
must_zero);
if (two_clusters != null) {
num_split++;
clusters.remove(nClust);
// do not add "headless" clusters, at least one of them will remain
if (two_clusters[1].getNumTiles() > 0) {
clusters.add(nClust,two_clusters[1]);
} else {
System.out.println("Cluster 1 "+two_clusters[1]+ " has no tiles - discarding");
}
if (two_clusters[0].getNumTiles() > 0) {
clusters.add(nClust,two_clusters[0]);
} else {
System.out.println("Cluster 0 "+two_clusters[0]+ " has no tiles - discarding");
}
System.out.println("Number of clusters: "+clusters.size());
}
}
return num_split;
}
double [][] dbgClusterLayers( // layer and eBox should be set
boolean show_bbox,
boolean show_ebox,
boolean show_lines,
boolean show_masks,
boolean selected_only,
boolean show_rdisparity,
boolean show_strength,
boolean filter,
ArrayList<PoleCluster> clusters)
{
final boolean headlessOK = false;
final TileNeibs tnImage = biCamDSI.tnImage;
int tilesX = tnImage.sizeX;
int tilesY = tnImage.sizeY;
......@@ -463,26 +1278,13 @@ public class PoleProcessor {
dbg_layers[i][j] = Double.NaN;
}
}
for (PoleCluster cluster: clusters){
for (PoleCluster cluster: clusters) if (headlessOK ||(cluster.getNumTiles() > 0)){ // skip headless clusters
int layer = cluster.getLayer();
double disp = cluster.getMeanDisp();
if (filter && (cluster.poleFilter() <0)) continue;
double disp = cluster.getTargetDisparity(); // getMeanDisparity();
Rectangle [] rectangles = {show_bbox?cluster.getBBox():null,show_ebox?cluster.getEBox():null};
for (int nbox = 0; nbox< rectangles.length; nbox++) {
for (int nbox = 0; nbox< rectangles.length; nbox++) if (rectangles[nbox] != null) {
Rectangle box = new Rectangle(rectangles[nbox]);
if (box.x < 0) {
box.width+=box.x;
box.x = 0;
}
if (box.y < 0) {
box.height +=box.y;
box.y = 0;
}
if ((box.x + box.width) >= tilesX) {
box.width = tilesX - box.x;
}
if ((box.y + box.height) >= tilesY) {
box.height = tilesY - box.y;
}
int nTile = box.y * tilesX + box.x;
for (int dx = 0; dx < box.width;dx++) {
if (tnImage.getNeibIndex(nTile, dx, 0) < 0 ) {
......@@ -510,7 +1312,82 @@ public class PoleProcessor {
}
}
}
return dbg_layers;
if (show_lines) {
int ends = 1;
for (PoleCluster cluster: clusters) if (headlessOK ||(cluster.getNumTiles() > 0)){
if (filter && (cluster.poleFilter() <0)) continue;
double [] pole_line = cluster.getPoleLine();
if (pole_line != null) {
int layer = cluster.getLayer();
double disp = cluster.getTargetDisparity(); // getMeanDisp();
Rectangle box = cluster.getEBox();
int nTile = box.y * tilesX + box.x;
double rx0 = 0.5*box.width;
double ry0 = 0.5*box.height;
for (int dy = -ends; dy < box.height + ends; dy++) {
int dx = (int) Math.round(rx0 + pole_line[0] + (dy - ry0)*pole_line[1]);
int nTile1 = tnImage.getNeibIndex(nTile, dx, dy);
if (nTile1 >= 0) {
dbg_layers[layer][tnImage.getNeibIndex(nTile, dx, dy)] = disp;
}
}
}
}
}
if (show_masks) {
for (PoleCluster cluster: clusters) if (headlessOK ||(cluster.getNumTiles() > 0)){
if (filter && (cluster.poleFilter() <0)) continue;
double [] pole_line = cluster.getPoleLine();
if (pole_line != null) {
int layer = cluster.getLayer();
double disp = cluster.getTargetDisparity(); // getMeanDisp();
Rectangle box = cluster.getEBox();
int nTile = box.y * tilesX + box.x;
for (int dy = 0; dy < box.height; dy++) {
for (int dx = 0; dx < box.width; dx++) {
if (cluster.getMask (dx, dy)) {
int nTile1 = tnImage.getNeibIndex(nTile, dx, dy);
if (nTile1 >= 0) {
dbg_layers[layer][tnImage.getNeibIndex(nTile, dx, dy)] = disp;
}
}
}
}
}
}
}
double [][] dbg_layers_2 = dbg_layers;
if (show_rdisparity && show_strength) {
dbg_layers_2= new double [2*num_layers][];
for (int i = 0; i < num_layers; i++) {
dbg_layers_2[i] = dbg_layers[i];
dbg_layers_2[i+num_layers] = dbg_layers[i].clone();
}
}
if (show_rdisparity || show_strength) {
int offs_strength = show_rdisparity ? num_layers:0;
for (PoleCluster cluster: clusters) if (headlessOK ||(cluster.getNumTiles() > 0)){
if (filter && (cluster.poleFilter() <0)) continue;
int layer = cluster.getLayer();
double [][] ds = cluster.getDsPairs(selected_only);
int [] nTiles = cluster.getDsTiles(selected_only);
for (int i = 0; i < nTiles.length; i++) {
int nTile = nTiles[i];
if (show_rdisparity) {
dbg_layers_2[layer][nTile] = ds[0][i];
}
if (show_strength) {
dbg_layers_2[layer+offs_strength][nTile] = ds[1][i];
}
}
}
}
return dbg_layers_2;
}
......@@ -535,22 +1412,8 @@ public class PoleProcessor {
}
for (PoleCluster cluster: clusters){
int layer = cluster.getLayer();
double disp = cluster.getMeanDisp();
double disp = cluster.getTargetDisparity(); // getMeanDisp();
Rectangle box = new Rectangle(cluster.getEBox());
if (box.x < 0) {
box.width+=box.x;
box.x = 0;
}
if (box.y < 0) {
box.height +=box.y;
box.y = 0;
}
if ((box.x + box.width) >= tilesX) {
box.width = tilesX - box.x;
}
if ((box.y + box.height) >= tilesY) {
box.height = tilesY - box.y;
}
int nTile = box.y * tilesX + box.x;
for (int dy = 0; dy < box.height; dy++) {
for (int dx = 0; dx < box.width;dx++) {
......@@ -562,6 +1425,268 @@ public class PoleProcessor {
}
public void addMeasuredTiles(
final boolean reset_data,
final double [][][] measured_layers,
final ArrayList<PoleCluster> clusters
) {
final TileNeibs tnImage = biCamDSI.tnImage;
// final int num_tiles = tnImage.sizeX * tnImage.sizeY;
final int tilesX = tnImage.sizeX;
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
Rectangle box = cluster.getEBox();
int layer = cluster.getLayer();
if (reset_data) {
cluster.resetDsPairs();
}
int nTile = box.x + tilesX * box.y;
for (int dy = 0; dy < box.height; dy++) {
for (int dx = 0; dx < box.width; dx++) {
int nTile1 = tnImage.getNeibIndex(nTile, dx, dy); // should always be inside image
if (!Double.isNaN(measured_layers[layer][0][nTile1])) {
cluster.addDs(nTile1, measured_layers[layer][0][nTile1], measured_layers[layer][1][nTile1]);
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
// returns number of clusters that need to be split
public int calcPoleLines(
final double max_diff,
final double max_tilt,
final double max_rms,
final int min_tiles,
final double damp_tilt,
final ArrayList<PoleCluster> clusters,
final int debugLevel)
{
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
cluster.calcPoleLine(
max_diff, // double max_diff,
max_tilt, // double max_tilt,
max_rms, // double max_rms,
min_tiles, // int min_tiles,
damp_tilt, // double damp_tilt,
debugLevel); // int debugLevel);
}
}
};
}
ImageDtt.startAndJoin(threads);
return 0;
}
public void createPoleMasks(
final double [][] norm_ds,
final int min_neibs,
final boolean use_seed,
final double width,
final double disp_aover,
final double disp_rover,
final ArrayList<PoleCluster> clusters,
final int debugLevel)
{
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
cluster.copyNormDS(norm_ds);
cluster.createPoleMask(
// norm_ds, // double [][] norm_ds,
min_neibs, // int min_neibs,
use_seed, // boolean use_seed,
width, // double width,
disp_aover, // double disp_aover,
disp_rover, // double disp_rover,
debugLevel); // int debugLevel)
}
}
};
}
ImageDtt.startAndJoin(threads);
}
public void filterByMask(
final boolean trim_bottoms,
final ArrayList<PoleCluster> clusters)
{
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
cluster.filterByMask(trim_bottoms);
}
}
};
}
ImageDtt.startAndJoin(threads);
}
public void filterByRightLeftSeparation(
final double min_strength,
final double disp_adiff,
final double disp_rdiff,
final double [][] norn_ds,
final ArrayList<PoleCluster> clusters)
{
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
boolean sep = cluster.checkRightLeft(
min_strength, // double min_strength,
disp_adiff, // double disp_adiff,
disp_rdiff, // double disp_rdiff,
norn_ds); // [][] norn_ds);
if (!sep) {
System.out.println(String.format("Disabling cluster {%d, %d, %d, %d} disp= %f as it is not separated on right or left",
cluster.eBox.x, cluster.eBox.y, cluster.eBox.width, cluster.eBox.height, cluster.target_disparity));
cluster.disabled = true;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
public double applyMeasuredDisparity(
final double disparity_scale, // target disparity to differential disparity scale (baseline ratio)
final double diff_power, // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
final double diff_offset, // add to measured differential disaprity before raising to specified power
final int cut_bottom, // cut few tile rows from the very bottom - they may be influenced by ground objects
final double keep_bottom, // do not cut more that this fraction of the bounding bow height
final ArrayList<PoleCluster> clusters,
final int debugLevel) // debug level
{
final Thread[] threads = ImageDtt.newThreadArray(biCamDSI.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final int num_clust = clusters.size();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
@Override
public void run() {
for (int nClust = ai.getAndIncrement(); nClust < num_clust; nClust = ai.getAndIncrement()) {
PoleCluster cluster = clusters.get(nClust);
double diff = cluster.applyMeasuredDisparity(
disparity_scale, // double disparity_scale, // target disparity to differential disparity scale (baseline ratio)
diff_power, // double diff_power, // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
diff_offset, // double diff_offset, // add to measured differential disaprity before raising to specified power
cut_bottom, // int cut_bottom, // cut few tile rows from the very bottom - they may be influenced by ground objects
keep_bottom, // double keep_bottom, // do not cut more that this fraction of the bounding bow height
debugLevel); // int debugLevel); // debug level
if (debugLevel > -2) {
System.out.println("applyMeasuredDisparity() for cluster "+cluster.toString()+" -> target disparity change: "+diff);
}
}
}
};
}
ImageDtt.startAndJoin(threads);
double max_diff = 0.0;
for (PoleCluster cluster: clusters) {
if (Math.abs(cluster.getTargetDiff()) > max_diff) {
max_diff = Math.abs(cluster.getTargetDiff());
}
}
return max_diff;
}
public void printClusterStats(
final ArrayList<PoleCluster> clusters)
{
int [][] real_poles = {
{232,109},
{233,109},
{226,109},
{219,108},
{208,106},
{191,101},
{ 99,112}, //{101,112},
{ 35,112}, //{37,112},
{123,112}, //{124,112},
{ 84,110},
{128,87}};
for (int nClust = 0; nClust < clusters.size(); nClust++) {
PoleCluster cluster = clusters.get(nClust);
if (cluster.poleFilter() >= 0) {
boolean real_pole = false;
for (int i = 0; i < real_poles.length; i++) {
if ((real_poles[i][0] == cluster.eBox.x) && (real_poles[i][1] == cluster.eBox.y)) {
real_pole = true;
break;
}
}
Rectangle box = cluster.eBox;
boolean unselected_only = true;
double bg_strength = cluster.getAverageBgStrength(unselected_only);
double meas_strength = cluster.getAverageStrength(true); // divided by measured tiles only
double mask_strength = cluster.getAverageStrength(false); // divided by a total number of pole tiles (by mask)
double rstrength = mask_strength/bg_strength;
System.out.println(String.format("%1s %1s: %3d {%3d,%3d,%3d,%3d} %6.4f %3d (%3d) frac=%6.4f str=%7.5f str_mask=%7.5f frac_height=%5.3f fract_box =%5.3f gap=%2d x0=%6.3f/%6.3f rms=%8.6f, rstrength=%7.4f",
((cluster.poleFilter() > 0)?"*":" "),
(real_pole?"#":"-"),
nClust, box.x, box.y, box.width, box.height, cluster.getTargetDisparity(), cluster.getSelectionSize(), cluster.getMaskSize(),cluster.getFractSelected(),
meas_strength, mask_strength,cluster.getFractHeight() , cluster.getFractBoxHeight(), cluster.getFreeStemGap(),
cluster.xc_center, cluster.slope, cluster.getMDisparityRMS(),
rstrength
));
}
}
}
public void sortClustersByDisparity(
ArrayList<PoleCluster> clusters)
{
......@@ -569,7 +1694,7 @@ public class PoleProcessor {
@Override
public int compare(PoleCluster lhs, PoleCluster rhs) {
// -1 - less than, 1 - greater than, 0 - equal
return (rhs.mean_disp > lhs.mean_disp) ? -1 : (rhs.mean_disp < lhs.mean_disp ) ? 1 : 0;
return (rhs.target_disparity > lhs.target_disparity) ? -1 : (rhs.target_disparity < lhs.target_disparity) ? 1 : 0;
}
});
......
......@@ -472,4 +472,44 @@ public class TileNeibs{
}
return mx;
}
public int removeFewNeibs(
boolean [] selection, // should be the same size
int min_neibs)
{
int num_total_removed=0;
int l = getLength();
while (true) {
int num_removed = 0;
boolean [] to_remove = new boolean[l];
for (int indx = 0; indx < l; indx++) if (selection[indx]){
int num_neibs = 0;
label_neibs: {
for (int dir = 0; dir < 8; dir++) {
int indx1 = getNeibIndex(indx, dir);
if ((indx1 >= 0) && selection[indx1]) {
num_neibs++;
if (num_neibs >= min_neibs) {
break label_neibs;
}
}
}
to_remove[indx] = true;
num_removed++;
}
}
if (num_removed > 0) {
for (int i = 0; i < l; i++) if (to_remove[i]) {
selection[i] = false;
}
num_total_removed += num_removed;
} else {
break;
}
}
return num_total_removed;
}
}
......@@ -1215,7 +1215,7 @@ if (debugLevel > -100) return true; // temporarily !
EyesisCorrectionParameters.CLTParameters clt_parameters,
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel)// throws Exception
final int debugLevel)
{
if ((quadCLT_main == null) ||
(quadCLT_main.tp == null) ||
......@@ -1229,7 +1229,9 @@ if (debugLevel > -100) return true; // temporarily !
}
int scan_index = biCamDSI_persistent.biScans.size()-1;
BiScan biScan = biCamDSI_persistent.biScans.get(scan_index);
if (debugLevel> -1) {
biScan.showScan(quadCLT_main.image_name+"LastBiScan-"+scan_index, null);
}
if (poleProcessor_persistent == null) {
poleProcessor_persistent = new PoleProcessor(
biCamDSI_persistent,
......@@ -1310,11 +1312,474 @@ if (debugLevel > -100) return true; // temporarily !
int num_layers = pp.assignClustersToLayers(
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] layers_to_measure = pp.getClusterLayers(
measurePoles(
quadCLT_main, // QuadCLT quadCLT_main, // tiles should be set
quadCLT_aux, // QuadCLT quadCLT_aux,
// BiCamDSI biCamDSI,
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
pole_clusters, // ArrayList<PoleProcessor.PoleCluster> pole_clusters,
threadsMax, // final int threadsMax, // maximal number of threads to launch
updateStatus, // final boolean updateStatus,
debugLevel); //final int debugLevel)
if (debugLevel > -2) {
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
for (int i = 0; i <dbg_layers.length; i++) {
dbg_layers1[i] = dbg_layers[i];
}
dbg_layers1[dbg_layers.length + 0] = norm_ds[0];
dbg_layers1[dbg_layers.length + 1] = norm_ds[1];
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers[0].length/quadCLT_main.tp.getTilesX(),
true,
"CLUSTER-BOX-MEAS");
}
// split clusters if measurements horizontal profile has multiple maximums
final int split_min_dist = 3; // minimal distance between local maximums to split
final boolean must_zero = false; // true; // false; // there should be complete zero-strength on the profile to enable split
for (int nTry = 0; nTry < 10; nTry++) {
int num_split = pp.splitClusters(
norm_ds, // double [][] norm_ds,
split_min_dist, // int min_dist, // minimal distance between local maximums
must_zero, // boolean must_zero, // zero histogram should exist between local maximum to split
pole_clusters, // ArrayList<PoleCluster> clusters,
debugLevel); // int debugLevel)
if (debugLevel > -2) {
System.out.println("splitClusters() -> "+num_split+" clusters split in two");
}
if (num_split == 0) { // one pass splits only in two, what if there are more (unlikely, but still)
break;
}
}
final double max_diff = 2.0;
final double max_tilt = 0.1;
final double max_rms = 1.0;
final int min_tiles = 3;
final double damp_tilt = 0.1;
pp.calcPoleLines(
max_diff, // final double max_diff,
max_tilt, // final double max_tilt,
max_rms, // final double max_rms,
min_tiles, // final int min_tiles,
damp_tilt, // final double damp_tilt,
pole_clusters, // final ArrayList<PoleCluster> clusters,
debugLevel); // final int debugLevel)
final boolean use_seed = true;
final double hwidth = 0.75;
final double disp_aover = 0.2;
final double disp_rover = 0.05;
final int min_neibs = 3; // minimal number of neighbors to keep tile in the pole mask
pp.createPoleMasks(
norm_ds, // final double [][] norm_ds,
min_neibs, // final int min_neibs,
use_seed, // final boolean use_seed,
hwidth, // final double width,
disp_aover, // final double disp_aover,
disp_rover, // final double disp_rover,
pole_clusters, // final ArrayList<PoleCluster> clusters,
debugLevel); // final int debugLevel)
final boolean trim_bottoms = true; // false; // true;
pp.filterByMask(
trim_bottoms, // final boolean trim_bottoms,
pole_clusters); // final ArrayList<PoleCluster> clusters)
final double sep_min_strength = 0.07;
final double sep_disp_adiff = 0.15;
final double sep_disp_rdiff = 0.05;
pp.filterByRightLeftSeparation(
sep_min_strength, // final double min_strength,
sep_disp_adiff, // final double disp_adiff,
sep_disp_rdiff, // final double disp_rdiff,
norm_ds, // final double [][] norn_ds,
pole_clusters); // final ArrayList<PoleCluster> clusters)
int num_removed =pp.removeFilteredClusters(
pole_clusters, // ArrayList<PoleCluster> clusters,
debugLevel); // int debugLevel)
if (debugLevel > -2) {
System.out.println("Removed "+num_removed+" filtered out pole clusters");
}
pp.sortClustersByDisparity(pole_clusters);
// re-assign layers
num_layers = pp.assignClustersToLayers(
pole_clusters); // ArrayList<PoleCluster> clusters)
if (debugLevel > -2) {
System.out.println("Reaasigned layers: "+num_layers);
}
final double disparity_scale = 0.3; // 2; // target disaprity to differential disparity scale (baseline ratio)
final double diff_power = 1.0; // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
final double diff_offset = 0.5; // add to measured differential disaprity before raising to specified power
final int cut_bottom = 2; // cut few tile rows from the very bottom - they may be influenced by ground objects
final double keep_bottom = 0.1; // do not cut more that this fraction of the bounding bow height
double max_target_diff = pp.applyMeasuredDisparity(
disparity_scale, // final double disparity_scale, // target disparity to differential disparity scale (baseline ratio)
diff_power, // final double diff_power, // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
diff_offset, // final double diff_offset, // add to measured differential disaprity before raising to specified power
cut_bottom, // final int cut_bottom, // cut few tile rows from the very bottom - they may be influenced by ground objects
keep_bottom, //final double keep_bottom, // do not cut more that this fraction of the bounding bow height
pole_clusters, // final ArrayList<PoleCluster> clusters,
debugLevel); // final int debugLevel) // debug level
if (debugLevel > -2) {
System.out.println("applyMeasuredDisparity() -> "+max_target_diff+" (max_diff)");
}
if (debugLevel > -2) {
if (debugLevel > -1) {
{
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
true, // boolean show_lines,
false, // boolean show_masks,
true, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
for (int i = 0; i <dbg_layers.length; i++) {
dbg_layers1[i] = dbg_layers[i];
}
dbg_layers1[dbg_layers.length + 0] = norm_ds[0];
dbg_layers1[dbg_layers.length + 1] = norm_ds[1];
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers[0].length/quadCLT_main.tp.getTilesX(),
true,
"LINES-BOX-MEAS");
}
{
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
true, // boolean show_lines,
false, // boolean show_masks,
true, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
for (int i = 0; i <dbg_layers.length; i++) {
dbg_layers1[i] = dbg_layers[i];
}
dbg_layers1[dbg_layers.length + 0] = norm_ds[0];
dbg_layers1[dbg_layers.length + 1] = norm_ds[1];
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers[0].length/quadCLT_main.tp.getTilesX(),
true,
"LINES-MEAS");
}
{
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
true, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
for (int i = 0; i <dbg_layers.length; i++) {
dbg_layers1[i] = dbg_layers[i];
}
dbg_layers1[dbg_layers.length + 0] = norm_ds[0];
dbg_layers1[dbg_layers.length + 1] = norm_ds[1];
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers[0].length/quadCLT_main.tp.getTilesX(),
true,
"MEAS-SEL");
}
}
{
boolean filter_poles = true;
double [][] dbg_layers_meas = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_selected = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
true, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_masks = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
true, // boolean show_masks,
false, // boolean selected_only,
false, // boolean show_rdisparity,
false, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_frames = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
true, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
false, // boolean show_rdisparity,
false, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [4 * dbg_layers_meas.length+2][];
String [] titles = new String [4 * dbg_layers_meas.length+2];
int nl = dbg_layers_meas.length/2;
for (int i = 0; i < dbg_layers_meas.length; i++) {
dbg_layers1[4* i + 0] = dbg_layers_meas[i];
dbg_layers1[4* i + 1] = dbg_layers_selected[i];
dbg_layers1[4* i + 2] = dbg_layers_masks [i % dbg_layers_frames.length];
dbg_layers1[4* i + 3] = dbg_layers_frames[i % dbg_layers_frames.length];
if ( i < nl) {
titles[4* i + 0] ="d_full-"+i;
titles[4* i + 1] ="d_sel-"+i;
titles[4* i + 2] ="sel-"+i;
titles[4* i + 3] ="frames-"+i;
} else {
titles[4* i + 0] ="s_full-"+ (i%nl);
titles[4* i + 1] ="s_sel-"+ (i%nl);
titles[4* i + 2] ="sel-"+ (i%nl);
titles[4* i + 3] ="frames-"+ (i%nl);
}
}
dbg_layers1[4 * dbg_layers_meas.length + 0] = norm_ds[0];
dbg_layers1[4 * dbg_layers_meas.length + 1] = norm_ds[1];
titles[4 * dbg_layers_meas.length + 0] = "disparity";
titles[4 * dbg_layers_meas.length + 1] = "str-norm";
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers1[0].length/quadCLT_main.tp.getTilesX(),
true,
"MEAS-COMBO",
titles);
pp.printClusterStats(pole_clusters);
}
}
final int max_refines = 20;
for (int nRefine = 0; nRefine < max_refines; nRefine ++) {
measurePoles(
quadCLT_main, // QuadCLT quadCLT_main, // tiles should be set
quadCLT_aux, // QuadCLT quadCLT_aux,
// BiCamDSI biCamDSI,
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
pole_clusters, // ArrayList<PoleProcessor.PoleCluster> pole_clusters,
threadsMax, // final int threadsMax, // maximal number of threads to launch
updateStatus, // final boolean updateStatus,
debugLevel + 0); //final int debugLevel)
max_target_diff = pp.applyMeasuredDisparity(
disparity_scale, // final double disparity_scale, // target disparity to differential disparity scale (baseline ratio)
diff_power, // final double diff_power, // bias towards higher disparities - (disparity+offset) is raised to this power and applied to weight
// if 0.0 - do not apply value to weight
diff_offset, // final double diff_offset, // add to measured differential disaprity before raising to specified power
cut_bottom, // final int cut_bottom, // cut few tile rows from the very bottom - they may be influenced by ground objects
keep_bottom, //final double keep_bottom, // do not cut more that this fraction of the bounding bow height
pole_clusters, // final ArrayList<PoleCluster> clusters,
debugLevel); // final int debugLevel) // debug level
if (debugLevel > -2) {
System.out.println("applyMeasuredDisparity() -> "+max_target_diff+" (max_diff)");
}
if (debugLevel > 0) {
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
for (int i = 0; i <dbg_layers.length; i++) {
dbg_layers1[i] = dbg_layers[i];
}
dbg_layers1[dbg_layers.length + 0] = norm_ds[0];
dbg_layers1[dbg_layers.length + 1] = norm_ds[1];
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers[0].length/quadCLT_main.tp.getTilesX(),
true,
"CLUSTER-BOX-MEAS-REFINE_"+nRefine);
// select measured:
pp.filterByMask(
false, // trim_bottoms, // final boolean trim_bottoms,
pole_clusters); // final ArrayList<PoleCluster> clusters)
}
if (debugLevel > -2){
boolean filter_poles = true;
double [][] dbg_layers_meas = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_selected = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
true, // boolean selected_only,
true, // boolean show_rdisparity,
true, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_masks = pp.dbgClusterLayers( // layer and eBox should be set
false, // boolean show_bbox,
false, // boolean show_ebox,
false, // boolean show_lines,
true, // boolean show_masks,
false, // boolean selected_only,
false, // boolean show_rdisparity,
false, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers_frames = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
true, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
false, // boolean show_rdisparity,
false, // boolean show_strength,
filter_poles, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [4 * dbg_layers_meas.length+2][];
String [] titles = new String [4 * dbg_layers_meas.length+2];
int nl = dbg_layers_meas.length/2;
for (int i = 0; i < dbg_layers_meas.length; i++) {
dbg_layers1[4* i + 0] = dbg_layers_meas[i];
dbg_layers1[4* i + 1] = dbg_layers_selected[i];
dbg_layers1[4* i + 2] = dbg_layers_masks [i % dbg_layers_frames.length];
dbg_layers1[4* i + 3] = dbg_layers_frames[i % dbg_layers_frames.length];
if ( i < nl) {
titles[4* i + 0] ="d_full-"+i;
titles[4* i + 1] ="d_sel-"+i;
titles[4* i + 2] ="sel-"+i;
titles[4* i + 3] ="frames-"+i;
} else {
titles[4* i + 0] ="s_full-"+ (i%nl);
titles[4* i + 1] ="s_sel-"+ (i%nl);
titles[4* i + 2] ="sel-"+ (i%nl);
titles[4* i + 3] ="frames-"+ (i%nl);
}
}
dbg_layers1[4 * dbg_layers_meas.length + 0] = norm_ds[0];
dbg_layers1[4 * dbg_layers_meas.length + 1] = norm_ds[1];
titles[4 * dbg_layers_meas.length + 0] = "disparity";
titles[4 * dbg_layers_meas.length + 1] = "str-norm";
(new showDoubleFloatArrays()).showArrays(
dbg_layers1,
quadCLT_main.tp.getTilesX(),
dbg_layers1[0].length/quadCLT_main.tp.getTilesX(),
true,
"MEAS-COMBO-REFINE_"+nRefine,
titles);
pp.printClusterStats(pole_clusters);
}
} // for (int nRefine = 0; nRefine < max_refines; nRefine ++) {
return true;
}
public void measurePoles(
QuadCLT quadCLT_main, // tiles should be set
QuadCLT quadCLT_aux,
// BiCamDSI biCamDSI,
EyesisCorrectionParameters.CLTParameters clt_parameters,
ArrayList<PoleProcessor.PoleCluster> pole_clusters,
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel)
{
PoleProcessor pp = poleProcessor_persistent;
double [][] layers_to_measure = pp.getClusterLayers(
pole_clusters); // ArrayList<PoleCluster> clusters)
/*
if (debugLevel > -1) {
double [][] disparity_strength = biScan.getDisparityStrength(
false, // only_strong,
false, // only_trusted,
......@@ -1338,7 +1803,7 @@ if (debugLevel > -100) return true; // temporarily !
}
for (int nclust = 0; nclust < pole_clusters.size(); nclust++) {
PoleProcessor.PoleCluster cluster = pole_clusters.get(nclust);
double d = cluster.getMeanDisp();
double d = cluster.getTargetDisparity();
ArrayList<Integer> tiles = cluster.getTiles();
for (int i: tiles) {
dbg_clust[0][i] = d;
......@@ -1356,6 +1821,12 @@ if (debugLevel > -100) return true; // temporarily !
double [][] dbg_layers = pp.dbgClusterLayers( // layer and eBox should be set
true, // boolean show_bbox,
true, // boolean show_ebox,
false, // boolean show_lines,
false, // boolean show_masks,
false, // boolean selected_only,
false, // boolean show_rdisparity,
false, // boolean show_strength,
false, // boolean filter,
pole_clusters); // ArrayList<PoleCluster> clusters)
double [][] dbg_layers1 = new double [dbg_layers.length+2][];
......@@ -1378,7 +1849,8 @@ if (debugLevel > -100) return true; // temporarily !
true,
"LAYERS_TO_MEASURE");
}
// Just measuring poles with different averaging
*/
// Just measuring poles with different averaging
double high_disp_tolerance = 0.3;
double low_disp_tolerance = 0.3;
double min_strength = 0.15;
......@@ -1399,10 +1871,12 @@ if (debugLevel > -100) return true; // temporarily !
low_disp_tolerance, //double low_disp_tolerance );
min_strength); //double min_strength);//
if (debugLevel > -1) {
showMeasuredPoles(
"MEASURED_POLES-CENTER", // String title,
quadCLT_main.tp.getTilesX(), // tilesX, // int tilesX,
measured_poles); // double [][][] measured_poles);
}
double [][][] measured_poles_strongest = poleAddStrongest(
measured_poles, // double [][][] ds_src,
null); // double [][][] ds_other)
......@@ -1436,13 +1910,15 @@ if (debugLevel > -100) return true; // temporarily !
measured_poles_fittest, // double [][][] ds_src,
measured_poles); // double [][][] ds_other)
if (debugLevel > 0) {
showMeasuredPoles(
"MEASURED_POLES-AVG"+num_avg, // String title,
quadCLT_main.tp.getTilesX(), // tilesX, // int tilesX,
measured_poles); // double [][][] measured_poles);
}
}
if (debugLevel > -1) {
showMeasuredPoles(
"MEASURED_POLES-STRONGEST", // String title,
quadCLT_main.tp.getTilesX(), // tilesX, // int tilesX,
......@@ -1451,10 +1927,18 @@ if (debugLevel > -100) return true; // temporarily !
"MEASURED_POLES-FITTEST", // String title,
quadCLT_main.tp.getTilesX(), // tilesX, // int tilesX,
measured_poles_fittest); // double [][][] measured_poles);
}
pp.addMeasuredTiles(
true, // final boolean reset_data,
measured_poles_fittest, // final double [][][] measured_layers,
pole_clusters); // final ArrayList<PoleCluster> clusters
return true;
}
public double [][][] removeDisparityMismatch(
double [][][] ds_src,
double high_disp_tolerance,
......
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