BiCamDSI.java 38.1 KB
Newer Older
1
package com.elphel.imagej.tileprocessor;
2 3 4 5 6 7 8
/**
 ** BiCamDSI - Building DSI using correlation between two quad cameras
 **
 ** Copyright (C) 2018 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
9
 **  BiCamDSI.java is free software: you can redistribute it and/or modify
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
 **  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/>.
 ** -----------------------------------------------------------------------------**
 **
 */

import java.util.ArrayList;

27
import com.elphel.imagej.common.ShowDoubleFloatArrays;
Andrey Filippov's avatar
Andrey Filippov committed
28

29 30

public class BiCamDSI {
31 32
	TileNeibs         tnImage; //  = new TileNeibs(tilesX, tilesY)
	int               threadsMax;
33
	public ArrayList<BiScan> biScans;
34 35 36 37 38 39 40



	public int addBiScan(
			double [] disparity,
			double [] strength,
			boolean [] trusted,
41 42
			boolean [] disabled,
			int        scan_type) {
43 44

		if (biScans == null) biScans = new ArrayList<BiScan>();
45
		biScans.add(new BiScan(this, biScans.size(), disparity, strength, trusted, disabled, scan_type));
46 47 48 49 50
		return biScans.size()-1;

	}

	public int addBiScan(
51 52 53
			double [][] disparity_bimap,
			int        scan_type
			) {
54 55 56 57
		return addBiScan(
				disparity_bimap[ImageDtt.BI_TARGET_INDEX], // double [] disparity,
				disparity_bimap[ImageDtt.BI_STR_CROSS_INDEX], // double [] strength,
				null, // boolean [] trusted,
58 59
				null, // boolean [] disabled)
				scan_type);
60 61
	}

62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80
	/**
	 * Get last BiScan of specific type
	 * @param scan_type -1 - any (returns last scan), >=0 must match type. If none of the type exists, returns null
	 * @return
	 */
	public BiScan getLastBiScan( int scan_type) {
		if (scan_type < 0) 	return getBiScan(-1);
		if (biScans.isEmpty()) return null;
		int last_index = -1;
		for (int i = 0; i < biScans.size(); i++) {
			if (biScans.get(i).scan_type == scan_type) {
				last_index = i;
			}
		}
		if (last_index >= 0) {
			return biScans.get(last_index);
		} else {
			return null;
		}
81 82 83 84 85 86 87 88 89 90 91
	}
	public BiScan getBiScan(int indx) {
		if (biScans.isEmpty()) return null;
		return biScans.get((indx>=0)? indx: (biScans.size() - 1));
	}

	public double [] getTargetDisparity(int indx) {
		BiScan biScan = getBiScan(indx);
		if (biScan == null) return null;
		return biScan.target_disparity;
	}
92 93 94

	public BiCamDSI(
			int tilesX,
95 96 97
			int tilesY,
			int threadsMax) {
		this.threadsMax = threadsMax;
98 99 100
		tnImage  = new TileNeibs(tilesX, tilesY);
	}

101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230
	private double [][] get9weightMasks(
			int        neib_dist, // >=1
			double     rsigma) {
		int sample_size = 2*neib_dist + 1;
		int center = neib_dist; //+1;
		int last = 2*neib_dist;

		double [][] weight_mask = new double [9][sample_size * sample_size];
		for (int row = 0; row <= neib_dist; row++) {
			int row1 = (last - row);
			for (int col = 0; col < sample_size; col++) {
				weight_mask[0][row *sample_size + col ] = 1.0;
				weight_mask[2][col *sample_size + row1] = 1.0;
				weight_mask[4][row1*sample_size + col ] = 1.0;
				weight_mask[6][col *sample_size + row ] = 1.0;
			}
		}
		for (int i = 0; i < sample_size; i++) {
			for (int j = 0; j <= i; j++) {
				int row0 =  j;
				int col0 = last - i + j;
				int row1 = (last - row0);
				int col1 = (last - col0);
				weight_mask[1][row0*sample_size + col0 ] = 1.0;
				weight_mask[3][col0*sample_size + row1 ] = 1.0;
				weight_mask[5][row1*sample_size + col1 ] = 1.0;
				weight_mask[7][col1*sample_size + row0 ] = 1.0;
			}
		}
		// Fill the center-symmetrical weights
		int k =0;
		double w4 = 0.0;
		double w8 = 0.0;
		int n8 = 0;
		int left_cells = sample_size * (neib_dist + 1); //number of cells in "halves"
		for (; ((2*k + 1)*(2*k + 1)) <= left_cells; k++); // seems that it never equals
		k--; // k is the largest square with odd side and area less than for 8 halves above
		left_cells -= (2*k + 1)*(2*k + 1);
		if (left_cells < 4) {
			w4 = left_cells/4.0;
		} else {
			w4 = 1.0;
			left_cells -=4;
			n8 = left_cells/8;
			left_cells -= 8*n8;
			if (n8 == k) {
				w8 = left_cells/4.0; // using very corners, so there are only 4, not 8 different cells for w8
			} else {
				w8 = left_cells/8.0;
			}


		}
		// Fill center square
		for (int row = neib_dist-k; row <= (neib_dist+k); row++) {
			for (int col = neib_dist-k; col <= (neib_dist+k); col++) {
				weight_mask[8][row*sample_size + col ] = 1.0;
			}
		}
		int low =  center - k - 1;
		int high = center + k + 1;
		weight_mask[8][center*sample_size + low   ] = w4;
		weight_mask[8][low   *sample_size + center] = w4;
		weight_mask[8][center*sample_size + high  ] = w4;
		weight_mask[8][high  *sample_size + center] = w4;
		if (w8 > 0.0) {
			n8++;
		} else {
			w8 = 1.0;
		}
		if (n8 > 0){
			for (int in8 = 1; in8 <= n8; in8++) {
				double w = (in8 == n8)? w8: 1.0;
				weight_mask[8][(center - in8)*sample_size +           low ] = w;
				weight_mask[8][(center + in8)*sample_size +           low ] = w;
				weight_mask[8][low           *sample_size + (center - in8)] = w;
				weight_mask[8][low           *sample_size + (center + in8)] = w;
				weight_mask[8][(center - in8)*sample_size +           high] = w;
				weight_mask[8][(center + in8)*sample_size +           high] = w;
				weight_mask[8][high          *sample_size + (center - in8)] = w;
				weight_mask[8][high          *sample_size + (center + in8)] = w;
			}
		}
		if (rsigma > 0) {
			double sigma = rsigma * center;
			double [] w1d = new double [center+1];
			for (int i = 0; i < w1d.length; i++) w1d[i] = Math.exp(-i*i/(2*sigma*sigma));
			for (int row = 0; row < sample_size; row++) {
				int ady = (row > center)? (row - center) : (center - row);
				for (int col = 0; col < sample_size; col++) {
					int adx = (col > center)? (col - center) : (center - col);
					for (int n = 0; n < weight_mask.length; n++) {
						weight_mask[n][row*sample_size + col] *= w1d[ady]*w1d[adx];
					}
				}
			}
		}
		return weight_mask;
	}

	/**
	 * Select near tiles (those that are not infinity)
	 * @param min_strength minimal tile strength (may be 0 as the tiles are already filtered)
	 * @param infinity_select tile, previously assigned to infinity (or null)
	 * @param selection existing selection (to add to) or null
	 * @param disparity array of tile disparity values (may have NaN-s)
	 * @param strength array of tile strength values
	 * @return new selection (new or added to existing)
	 */

	public boolean [] selectNearObjects(
			double     min_strength,
			boolean [] infinity_select,
			boolean [] selection,
			double []  disparity,
			double []  strength) {
		int num_tiles = disparity.length;
		if (infinity_select == null) {
			infinity_select = new boolean [num_tiles];
			for (int nTile = 0; nTile < num_tiles; nTile++) infinity_select[nTile] = true;
		}
		if (selection == null) selection = new boolean [num_tiles];
		for (int nTile =0; nTile <num_tiles; nTile++) {
			if (!infinity_select[nTile] && (strength[nTile] > 0.0) && (strength[nTile] >= min_strength)) {
				selection[nTile] = true;
			}
		}
		return selection;
	}

231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290
	/**
	 * Select small/thin near objects over infinity
	 * @param min_strength minimal strength of the tile and neighbors
	 * @param min_neibs minimal number of neighbors with close disparity
	 * @param min_disparity minimal disaparity
	 * @param disp_atolerance disparity absolute tolerance (to qualify for a similar neighbor)
	 * @param disp_rtolerance disparity relative tolerance (adds to absolute)
	 * @param infinity_select tile, previously assigned to infinity (or null)
	 * @param selection existing selection (to add to) or null
	 * @param disparity array of tile disparity values (may have NaN-s)
	 * @param strength array of tile strength values
	 * @return new selection (new or added to existing)
	 */
	public boolean [] selectNearOverInfinity(
			double     min_strength,
			int        min_neibs,
			double     min_disparity,
			double     disp_atolerance,
			double     disp_rtolerance,
			boolean [] infinity_select,
			boolean [] selection,
			double []  disparity,
			double []  strength) {
		int num_tiles = disparity.length;
		if (infinity_select == null) {
			infinity_select = new boolean [num_tiles];
			for (int nTile = 0; nTile < num_tiles; nTile++) infinity_select[nTile] = true;
		}
		if (selection == null) selection = new boolean [num_tiles];
		boolean [] new_sel = selection.clone();
		//tnImage
		for (int nTile =0; nTile <num_tiles; nTile++)
			if (     infinity_select[nTile] &&
					!selection[nTile] &&
					(strength[nTile] >=  min_strength ) &&
					(disparity[nTile] >= min_disparity)){
			int nn = 0;
			double tolerance = disp_atolerance + disparity[nTile] * disp_rtolerance;
			double min_d = Math.max(disparity[nTile] - tolerance, min_disparity);
			double max_d = disparity[nTile] + tolerance;
			for (int dir = 0; dir < 8; dir++) {
				int nTile1 = tnImage.getNeibIndex(nTile, dir);
				if (    (nTile1 >=0) &&
						((strength [nTile1] >=  min_strength ) || selection[nTile1]) && // strong or already selected
						(disparity[nTile1] >= min_d) &&
						(disparity[nTile1] <= max_d)){
					nn++;
					/// if (selection[nTile]) nn++; // count selected as 2 ?
				}
			}
			if (nn >= min_neibs) {
				new_sel[nTile] = true;
			}
		}
		for (int nTile =0; nTile <num_tiles; nTile++) {
			selection[nTile] |= new_sel[nTile];
		}
		return selection;
	}

291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322
	/**
	 * Select far tiles (even lone) over infinity areas that have sufficient disparity and strength
	 * @param min_far_strength minimal tile strength (may be 0 as the tiles are already filtered)
	 * @param min_far_disparity minimal tile disparity to accept lone strong tiles for far objects
	 * @param infinity_select tile, previously assigned to infinity (or null)
	 * @param selection existing selection (to add to) or null
	 * @param disparity array of tile disparity values (may have NaN-s)
	 * @param strength array of tile strength values
	 * @return new selection (new or added to existing)
	 */

	public boolean [] selectLoneFar(
			double     min_far_strength,
			double     min_far_disparity,
			boolean [] infinity_select,
			boolean [] selection,
			double []  disparity,
			double []  strength) {
		int num_tiles = disparity.length;
		if (infinity_select == null) {
			infinity_select = new boolean [num_tiles];
			for (int nTile = 0; nTile < num_tiles; nTile++) infinity_select[nTile] = true;
		}
		if (selection == null) selection = new boolean [num_tiles];
		for (int nTile =0; nTile <num_tiles; nTile++) {
			if (infinity_select[nTile] && (strength[nTile] > 0.0) && (strength[nTile] >= min_far_strength) && (disparity[nTile] >= min_far_disparity)) {
				selection[nTile] = true;
			}
		}
		return selection;
	}

323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340
	public double [][] filterDisparityStrength(
			double  min_disparity,
			double [] disparity,
			double [] strength,
			boolean [] selected){
		int num_tiles = disparity.length;
		double [][] fds = new double [2][num_tiles];
		for (int nTile = 0; nTile < num_tiles; nTile++) {
			fds[0][nTile] = Double.NaN;
			if ((strength[nTile] > 0.0) && (disparity[nTile] >= min_disparity) && ((selected == null) || selected[nTile])) {
				fds[0][nTile] = disparity[nTile];
				fds[1][nTile] = strength[nTile];
			}
		}
		return fds;
	}


341
	/**
342
	 * Filter selection by expanding, then shrinking (fills small gaps) shrinking (combined with
343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436
	 * previous step) and expanding again (removes small clusters)
	 * @param pre_expand number of steps for initial expand (odd last is - only hor/vert, even - last step includes diagonals)
	 * @param shrink number of shrink steps normally equals to pre_expand+post_expand
	 * @param post_expand numaber of final expansion steps
	 * @param selection selection to be modified
	 * @param prohibit tiles to keep from expansion/shrinking (such as infinity selection to modify only near regions (may be null)
	 */
	public void expandShrinkExpandFilter(
			int        pre_expand,
			int        shrink,
			int        post_expand,
			boolean [] selection,
			boolean [] prohibit)
	{
		tnImage.growSelection(
				pre_expand,  // 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
				selection,   // boolean [] tiles,
				prohibit);   // boolean [] prohibit)
		tnImage.shrinkSelection(
				shrink,      // int        shrink,
				selection,   // boolean [] tiles,
				prohibit);   // boolean [] prohibit)
		tnImage.growSelection(
				post_expand,  // 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
				selection,   // boolean [] tiles,
				prohibit);   // boolean [] prohibit)
	}
	/**
	 * Combine (OR) two selections
	 * @param selection1 first selection
	 * @param selection2 second selection
	 * @param enable only use second selection if corresponding element in enable is true. May be null
	 * @param invert_enable make 'enable' 'disable'
	 * @return OR-ed selections
	 */
	public boolean [] combineSelections(
			boolean [] selection1,
			boolean [] selection2,
			boolean [] enable,
			boolean    invert_enable) {
		boolean [] selection = selection1.clone();
		if (enable == null) {
			for (int i = 0; i < selection.length; i++) selection[i] |= selection2[i];
		} else if (invert_enable) {
			for (int i = 0; i < selection.length; i++) selection[i] |= (selection2[i] && !enable[i]);
		} else {
			for (int i = 0; i < selection.length; i++) selection[i] |= (selection2[i] && enable[i]);
		}
		return selection;
	}



	public boolean [] selectFarObjects(
			double     strength_floor,
			double     min_disparity,
			double     min_mean,
			double     max_disparity,
			double     max_mean,
			double     min_disp_to_rms,
			double     min_strength,
			int        neib_dist, // >=1
			double     rsigma,
			double     tile_frac,
			boolean [] infinity_select,
			double []  disparity,
			double []  strength,
			int        tilesX,
			int        debugLevel) {
		int num_tiles = disparity.length;
		if (infinity_select == null) {
			infinity_select = new boolean [num_tiles];
			for (int nTile = 0; nTile < num_tiles; nTile++) infinity_select[nTile] = true;
		}
		if (Double.isNaN(strength_floor)) {
			strength_floor = Double.NaN;
			for (int nTile = 0; nTile < num_tiles; nTile++) if (
					infinity_select[nTile] &&
					!Double.isNaN(disparity[nTile])&&
					(strength[nTile] > 0) &&
					(Double.isNaN(strength_floor) || (strength[nTile] < strength_floor))){
				strength_floor = strength[nTile];
			}
		}
		if (debugLevel >-2) {
			System.out.println("selectFarObjects(): strength_floor = "+strength_floor);
		}
		if (debugLevel > 0) {
			double [] rsigmas = {0.0, 1.0, 0.5};
			for (double rs:rsigmas) {
				for (int nd = 1; nd < 7; nd++) {
					double [][] weight_mask =  get9weightMasks(
							nd, // >=1int        neib_dist, // >=1
							rs);
437
					(new ShowDoubleFloatArrays()).showArrays(
438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567
							weight_mask,
							2*nd+1,
							2*nd+1,
							true,
							"w9m_"+rs);

				}
			}
		}

		double [][] weight_mask =  get9weightMasks(
				neib_dist, // >=1int        neib_dist, // >=1
				rsigma); // int debugLevel
		int [] min_tiles = new int [weight_mask.length];
		for (int n = 0; n< min_tiles.length; n++) {
			int numnz = 0;
			for (int i = 0; i < weight_mask[n].length; i++) {
				if (weight_mask[n][i] >0.0) numnz++;

			}
			min_tiles[n] = (int) Math.round(numnz * tile_frac);

		}
		int sample_size = 2*neib_dist + 1;
//		int last = 2*neib_dist; tile_frac

		double [][] mean_to_rms = (debugLevel >-2) ?(new double [4][num_tiles]): null;
		double [][] dbg_dirs =    (debugLevel > 0) ?(new double [3*weight_mask.length][num_tiles]): null;
		if (dbg_dirs != null) {
			for (int n = 0; n < dbg_dirs.length; n++) {
				for (int nTile =0; nTile <num_tiles; nTile++) {
					dbg_dirs[n][nTile] = Double.NaN;
				}
			}
		}
		boolean [] selection = new boolean [num_tiles];
		if (mean_to_rms != null) for (int nTile =0; nTile <num_tiles; nTile++) for (int i = 0; i < mean_to_rms.length; i++) mean_to_rms[i][nTile] = Double.NaN; // only needed for debug?
		int    [] num_best = new int [weight_mask.length]; //debug feature

		for (int nTile =0; nTile <num_tiles; nTile++) {
			// disparity[nTile] may be Double.NaN
			if (infinity_select[nTile] && (disparity[nTile] >= min_disparity) && (disparity[nTile] <= max_disparity) && (strength[nTile] >= min_strength)) {
				double d0 = disparity[nTile];
				double [] s0 =  new double[weight_mask.length];
				double [] s0a = new double[weight_mask.length];
				double [] s1 =  new double[weight_mask.length];
				double [] s2 =  new double[weight_mask.length];
				int [] num_nz = new int[weight_mask.length];
				// calculate weights, means, and rms for each of 9 windows
				for (int dy = -neib_dist; dy <= neib_dist; dy++) {
					for (int dx = -neib_dist; dx <= neib_dist; dx++) {
						int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
						if (    (nTile1 >=0) &&
								!Double.isNaN(disparity[nTile1])) {
							double w = strength[nTile1] - strength_floor;
							if (w > 0.0) {
								int tindx = (dy + neib_dist) * sample_size + (dx + neib_dist);
								double d = disparity[nTile1];
								for (int n = 0; n < weight_mask.length; n++) {
									double ww = w * weight_mask[n][tindx];
									s0[n] += ww;
									s1[n] += ww* d;
									if ((dy!=0) || (dx!=0)){
										double dd = (d - d0);
										s0a[n] +=ww;
										s2[n] += ww*dd*dd;
									}

									if (ww >0.0) {
										num_nz[n]++;
									}
								}
							}
						}
					}
				}
				//min_tiles[n]
				double rm = 0.0;
				int best_dir = -1;
				for (int n = 0; n < weight_mask.length; n++) {
					if (s0[n] > 0) {
						s1[n] /= s0[n]; // mean
					}
					if (num_nz[n] < min_tiles[n]) {
						s1[n] =0.0;
					}
					if (s0a[n] > 0) {
						s2[n] = Math.sqrt(s2[n]/s0a[n]);
						double r = s1[n]/s2[n]; // denominator - rms (not including itself
						if (r > rm) {
							rm = r;
							best_dir = n;
						}
					}
				}
				if (dbg_dirs != null) {
					for (int n = 0; n < weight_mask.length; n++) {
						dbg_dirs[n + 0 * weight_mask.length][nTile] = s1[n]/s2[n];
						dbg_dirs[n + 1 * weight_mask.length][nTile] = s1[n];
						dbg_dirs[n + 2 * weight_mask.length][nTile] = s2[n];
					}
				}

//		double [][] dbg_dirs =    (debugLevel >-2) ?(new double [3*weight_mask.length][num_tiles]): null;

				if ((rm > min_disp_to_rms) && (s1[best_dir] >= min_mean ) && (s1[best_dir] <= max_mean )){
					num_best[best_dir]++;
					if (mean_to_rms != null) {
						mean_to_rms[0][nTile] = rm;
						if (best_dir >= 0) {
							mean_to_rms[1][nTile] = s1[best_dir];
							mean_to_rms[2][nTile] = s2[best_dir];
							mean_to_rms[3][nTile] = 0.1 * best_dir;
						}
					}
					selection[nTile] = true;
				}
			}
		}
//			double     min_mean,
//		double     max_disparity,
//		double     max_mean,
		if (debugLevel>-2) {
			System.out.println("selectFarObjects(): number of wins:");
			for (int i = 0; i < num_best.length; i++) {
				System.out.println(i+": "+num_best[i]);
			}

		}
		if (debugLevel > 0) {
568
			(new ShowDoubleFloatArrays()).showArrays(
569 570 571 572 573 574 575 576 577 578
					weight_mask,
					2*neib_dist+1,
					2*neib_dist+1,
					true,
					"weight_mask");
		}

		if (mean_to_rms != null) {
			double [][] dbg_img = {disparity, strength, mean_to_rms[0], mean_to_rms[1], mean_to_rms[2], mean_to_rms[3]};
			String [] titles = {"disparity", "strength", "mean_to_rms","mean","rms", "dir/10"};
579
			(new ShowDoubleFloatArrays()).showArrays(
580 581 582 583 584 585 586 587 588 589 590 591
					dbg_img,
					tilesX,
					mean_to_rms[0].length/tilesX,
					true,
					"mean_to_rms_"+neib_dist,
					titles);
		}
		if (dbg_dirs != null) {
			String [] titles = {
					"dr0","dr1","dr2","dr3","dr4","dr5","dr6","dr7","dr8",
					"m0","m1","m2","m3","m4","m5","m6","m7","m8",
					"r0","r1","r2","r3","r4","r5","r6","r7","r8"};
592
			(new ShowDoubleFloatArrays()).showArrays(
593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655
					dbg_dirs,
					tilesX,
					mean_to_rms[0].length/tilesX,
					true,
					"mean_and_rms_"+neib_dist,
					titles);
		}
		return selection;
	}



	public int removeFalseMatches(
			double [][] disparity_bimap,
			double      trusted_strength, // =       0.2;    // strength sufficient without neighbors
			double      stray_rstrength, // = 2.0;    // Relative to trusted strength - trust above that
			double      strength_rfloor, // = 0.28;    // Relative to trusted strength
			int         stray_dist,      // = 2;      // How far to look (and erase) around a potentially falsely matched tile
			double      stray_over,      // = 2.0;    // Stray tile should be this stronger than the strongest neighbor to be recognized
			int         debugLevel)
	{
			double max_stray_strength = trusted_strength * stray_rstrength;
			double strength_floor =     trusted_strength * strength_rfloor;
			double max_under = 1.0/stray_over;
			ArrayList<Integer> stray_list = new ArrayList<Integer>();
			final double [] disparity = disparity_bimap[ImageDtt.BI_TARGET_INDEX];
			final double [] strength =  disparity_bimap[ImageDtt.BI_STR_CROSS_INDEX];
			for (int nTile=0; nTile < strength.length; nTile++) {
				if (!Double.isNaN(disparity[nTile]) && (strength[nTile] >= trusted_strength) && (strength[nTile] < max_stray_strength)){
					double max_stength = 0.0;
					for (int dy = -stray_dist; dy <= stray_dist; dy++) {
						for (int dx = -stray_dist; dx <= stray_dist; dx++)  if ((dy!=0) || (dx!=0)){
							int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
							if (    (nTile1 >=0) &&
									!Double.isNaN(disparity[nTile1]) &&
									(strength[nTile1] > max_stength)) {
								max_stength = strength[nTile1];
							}
						}
					}
					if (((max_stength - strength_floor)/(strength[nTile] - strength_floor) <= max_under) &&
							(max_stength < trusted_strength)){
						stray_list.add(nTile);
						if (debugLevel > -2) {
							System.out.println(String.format("removeFalseMatches(): adding tile %d (%d/%d) ",nTile, nTile%tnImage.sizeX, nTile/tnImage.sizeX));
						}
					}
				}
			}
			// now erase around tiles in stray_list
			for (int nTile: stray_list) {
				for (int dy = -stray_dist; dy <= stray_dist; dy++) {
					for (int dx = -stray_dist; dx <= stray_dist; dx++) {
						int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
						disparity[nTile1] = Double.NaN;
						strength[nTile1] = 0.0;
					}
				}
			}
			return stray_list.size();

	}

656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686
	public boolean [] getLTTrusted(
			double [][] disparity_bimap,
			double      min_disparity, //  =         0.0;    // apply low texture to near objects
			double      trusted_strength, // =       0.2;    // strength sufficient without neighbors
			double      need_friends, // =           0.4;    // strength sufficient with neighbors support, fraction of lt_trusted_strength
			double      friends_diff, // =           0.15;   // pix difference to neighbors to be considered a match (TODO: use tilted)
			double      friends_rdiff, // =          0.04;   // additional relative pix per pixel of disparity
			int         min_friends_any, // =        2;      // minimal number of even weak friends
			int         min_friends_trusted, // =    2;      // minimal number of trusted (strong or already confirmed)
			int         friends_dist, // =           3;      // how far to look for friends
			int         debugLevel
			) {
		final double cond_strength = trusted_strength * need_friends;
		final double [] disparity = disparity_bimap[ImageDtt.BI_TARGET_INDEX];
		final double [] strength =  disparity_bimap[ImageDtt.BI_STR_CROSS_INDEX];
		final boolean [] trusted = new boolean[strength.length];
		final boolean [] cond_trusted = trusted.clone();
		for (int nTile = 0; nTile < trusted.length; nTile ++) if (strength[nTile] >= cond_strength){
			cond_trusted[nTile] = true;
			trusted[nTile] = strength[nTile] >= trusted_strength;
		}
		for (int new_tiles = 0;  ; new_tiles = 0) {
			for (int nTile = 0; nTile < trusted.length; nTile ++) {
				if (cond_trusted[nTile] && !trusted[nTile]) {
					int num_trusted = 0;
					int num_friends = 0;
					double low_friend =  disparity[nTile] - friends_diff -  friends_rdiff*disparity[nTile];
					double high_friend = disparity[nTile] + friends_diff +  friends_rdiff*disparity[nTile];
					label_tile:
					{
						for (int dy = -friends_dist; dy <= friends_dist; dy++) {
687
							for (int dx = -friends_dist; dx <= friends_dist; dx++) if ((dy!=0) || (dx!=0)){
688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710
								int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
								if ((nTile1 >= 0) && (disparity[nTile1] >= low_friend)  && (disparity[nTile1] <= high_friend)){ // disparity[nTile1] may be NaN!
									if (cond_trusted[nTile1]) {
										num_friends++;
										if (num_friends >= min_friends_any){
											trusted[nTile] = true;
											new_tiles++;
											break label_tile;
										} else if (trusted[nTile1]) {
											num_trusted++;
											if (num_trusted >= min_friends_trusted){
												trusted[nTile] = true;
												new_tiles++;
												break label_tile;
											}
										}
									}
								}
							}
						}
					}
				}
			}
711
			if (debugLevel > -2) System.out.print ("new tiles = "+new_tiles); // find out why second pass always returns 0
712 713
			if (new_tiles == 0) break;
		}
714
		if (debugLevel > -2) System.out.println();
715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746
		return trusted;
	}

	public int removeLTUntrusted(
			double [][] disparity_bimap,
			double      min_disparity, //  =         0.0;    // apply low texture to near objects
			double      trusted_strength, // =       0.2;    // strength sufficient without neighbors
			double      need_friends, // =           0.4;    // strength sufficient with neighbors support, fraction of lt_trusted_strength
			double      friends_diff, // =           0.15;   // pix difference to neighbors to be considered a match (TODO: use tilted)
			double      friends_rdiff, // =          0.04;   // additional relative pix per pixel of disparity
			int         min_friends_any, // =        2;      // minimal number of even weak friends
			int         min_friends_trusted, // =    2;      // minimal number of trusted (strong or already confirmed)
			int         friends_dist, // =           3;      // how far to look for friends
			int         debugLevel
			) {
		boolean [] trusted = 	getLTTrusted(
				disparity_bimap,     // double [][] disparity_bimap,
				min_disparity,       //double      min_disparity, //  =         0.0;    // apply low texture to near objects
				trusted_strength,    //double      trusted_strength, // =       0.2;    // strength sufficient without neighbors
				need_friends,        //double      need_friends, // =           0.4;    // strength sufficient with neighbors support, fraction of lt_trusted_strength
				friends_diff,        //double      friends_diff, // =           0.15;   // pix difference to neighbors to be considered a match (TODO: use tilted)
				friends_rdiff,       //double      friends_rdiff, // =          0.04;   // additional relative pix per pixel of disparity
				min_friends_any,     //int         min_friends_any, // =        2;      // minimal number of even weak friends
				min_friends_trusted, //int         min_friends_trusted, // =    2;      // minimal number of trusted (strong or already confirmed)
				friends_dist,        //int         friends_dist, // =           3;      // how far to look for friends
				debugLevel);         //int         debugLevel
		int num_trusted = 0;
		for (int nTile = 0; nTile < trusted.length; nTile++) {
			if (trusted[nTile]) {
				num_trusted++;
			} else {
				disparity_bimap[ImageDtt.BI_TARGET_INDEX][nTile] = Double.NaN;
747
				disparity_bimap[ImageDtt.BI_STR_CROSS_INDEX][nTile] = 0.0;
748 749 750 751 752 753
			}
		}
		return num_trusted;
	}


754 755


756 757 758 759 760
	public double [] suggestLTTiles(
			double [][] disparity_bimap,
			boolean []  trusted,       // may be null if disparity is alreasdy NaN-ed
			double      min_disparity, //  =         0.0;    // apply low texture to near objects
			double      trusted_strength, // =       0.2;    // strength sufficient without neighbors
761
			double      strength_rfloor,  // =       0.28;   // strength floor relative to trusted_strength
762 763 764 765 766 767 768 769 770 771 772 773
			double      need_friends, // =           0.4;    // strength sufficient with neighbors support, fraction of lt_trusted_strength
			int         extend_dist, // =            3;      // how far to extend around known tiles (probably should increase this value up to?
			// dealing with neighbors variance
			double      wsigma,     //  = 1.0; // influence of far neighbors diminish as a Gaussian with this sigma
			double      max_asigma, // =             .15;     // Maximal acceptable standard deviation of the neighbors (remove, then add)
			double      max_rsigma, // =             .05;     // Maximal acceptable standard deviation of the neighbors (remove, then add)
			int         debugLevel
			) {
		final double rsigma = max_rsigma; //pix/pix
		final double asigma = max_asigma; // 1.0;

		final double [][] weights = new double [extend_dist+1][extend_dist+1];
774 775
		//final double cond_strength = trusted_strength * need_friends;
		final double strength_floor = strength_rfloor * trusted_strength;
776 777 778 779 780 781 782 783 784 785 786 787
		for (int i = 0; i <weights.length; i++) {
			for (int j = i; j <weights[i].length; j++) {
				weights[i][j]=Math.exp(-(i*i+j*j)/(2*wsigma*wsigma));
				weights[j][i] = weights[i][j];
			}
		}
		final double [] disparity = disparity_bimap[ImageDtt.BI_TARGET_INDEX];
		final double [] strength =  disparity_bimap[ImageDtt.BI_STR_CROSS_INDEX];
		if (trusted == null) {
			trusted = new boolean[disparity.length];
			for (int nTile = 0; nTile < trusted.length; nTile++) trusted[nTile] =  !Double.isNaN(disparity[nTile]);
		}
788

789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942
		double sigma = asigma;
		double sigma2 = sigma*sigma;
		final double [] to_measure = new double [disparity.length];
		for (int nTile = 0; nTile < to_measure.length; nTile++) {
			to_measure[nTile] = Double.NaN;
		}
		final boolean [] candidates = new boolean[strength.length];
		// can be done faster if jump on empty by square side
		for (int nTile = 0; nTile < candidates.length; nTile++) if (!trusted[nTile]){
			label_tile1:
			for (int dy = -extend_dist; dy <= extend_dist; dy++) {
				for (int dx = -extend_dist; dx <= extend_dist; dx++) if ((dy!=0) ||(dx !=0)){
					int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
					if ((nTile1 >= 0) && trusted[nTile1]){
						candidates[nTile] = true;
						break label_tile1;
					}
				}
			}
		}

		int num_sigma = 0;
		for (int nTile = 0; nTile < candidates.length; nTile++) if (candidates[nTile]){
			ArrayList<Integer> sample_list = new ArrayList<Integer>();
			double s0 = 0.0, s1 = 0.0, s2 = 0.0;
			for (int dy = -extend_dist; dy <= extend_dist; dy++) {
				for (int dx = -extend_dist; dx <= extend_dist; dx++) if ((dy!=0) ||(dx !=0)){
					int nTile1 =  tnImage.getNeibIndex(nTile, dx, dy);
					if ((nTile1 >= 0) && trusted[nTile1]){
						double w = (strength[nTile1] - strength_floor) * weights[(dy>0)?dy:-dy][(dx>0)?dx:-dx];
						s0 += w;
						s1 += w * disparity[nTile1];
						s2 += w * disparity[nTile1] * disparity[nTile1];
						sample_list.add(nTile1);
					}
				}
			}
			if (s0 <= 0) {
				System.out.println("suggestLTTiles() BUG? nTile = "+nTile+" s0 ="+s0);
				continue;
			}
			double s_mean = s1/s0;
			double smpl_var = s2/s0 -s_mean*s_mean;
			sigma = asigma + rsigma * s_mean;
			sigma2 = sigma*sigma;
// FIXME: use tilted planes
			if (smpl_var > sigma2) {
				if (debugLevel > -2) {
					System.out.print (String.format("%3d/%3d mean=%8f sigma2=%f var=%8f tiles=%3d ",nTile%tnImage.sizeX, nTile/tnImage.sizeX, s_mean, sigma2, smpl_var, sample_list.size()));
				}
				num_sigma++;
				ArrayList<Integer> rejected_list = new ArrayList<Integer>();
				int [] xy0 = tnImage.getXY(nTile);
				while (smpl_var > sigma2) {
					// find worst sample
					int worst_indx = -1;
					double worst_diff = 0;
					double d;
					for (int i = 0; i < sample_list.size(); i++) {
						int nTile1 = sample_list.get(i);
						d = Math.abs(disparity[nTile1] - s_mean);
						if (d > worst_diff) {
							worst_diff = d;
							worst_indx = i;
						}
					}
					// remove worst sample, add to reject list
					int nTile1 = sample_list.get(worst_indx);
					rejected_list.add(nTile1);
					sample_list.remove(worst_indx);
					// recalculate statistics
					int [] xy = tnImage.getXY(nTile1);
					int dx =xy[0] - xy0[0];
					int dy =xy[1] - xy0[1];
					double w = (strength[nTile1] - strength_floor) * weights[(dy>0)?dy:-dy][(dx>0)?dx:-dx];
					s0 -= w;
					s1 -= w * disparity[nTile1];
					s2 -= w * disparity[nTile1] * disparity[nTile1];
					s_mean = s1/s0;
					smpl_var = s2/s0 -s_mean*s_mean;
				}
				if (debugLevel > -2) {
//					System.out.print (" -> s_mean = "+s_mean+", smpl_var = "+smpl_var+" ... "+ " ntiles="+(sample_list.size()));
					System.out.print (String.format(" (-)-> mean=%8f var=%8f tiles=%3d ",s_mean, smpl_var, sample_list.size()));

				}

				// Try to add best of the rejected back (trying to deal with 2-maximums histogram)
				while (smpl_var < sigma2) { // then remove last added
					// find best rejected sample
					int best_indx = -1;
					double best_diff = 0;
					double d;
					for (int i = 0; i < rejected_list.size(); i++) {
						int nTile1 = rejected_list.get(i);
						d = Math.abs(disparity[nTile1] - s_mean);
						if ((best_indx <0) || (d < best_diff)) {
							best_diff = d;
							best_indx = i;
						}
					}
					// restore best rejected sample
					int nTile1 = rejected_list.remove(best_indx);
					sample_list.add(nTile1); // best_indx); // will be last - easy to remove
					// recalculate statistics
					int [] xy = tnImage.getXY(nTile1);
					int dx =xy[0] - xy0[0];
					int dy =xy[1] - xy0[1];
					double w = (strength[nTile1] - strength_floor) * weights[(dy>0)?dy:-dy][(dx>0)?dx:-dx];
					s0 += w;
					s1 += w * disparity[nTile1];
					s2 += w * disparity[nTile1] * disparity[nTile1];
					s_mean = s1/s0;
					smpl_var = s2/s0 -s_mean*s_mean;
				}
				if (debugLevel > -2) {
					System.out.print (String.format(" (+)-> mean=%8f var=%8f tiles=%3d ",s_mean, smpl_var, sample_list.size()));
				}
				// remove last added sample
				// remove worst sample, add to reject list
				int nTile1 = sample_list.get(sample_list.size()-1); // last added, no need to actually remove
				// recalculate statistics
				int [] xy = tnImage.getXY(nTile1);
				int dx =xy[0] - xy0[0];
				int dy =xy[1] - xy0[1];
				double w = (strength[nTile1] - strength_floor) * weights[(dy>0)?dy:-dy][(dx>0)?dx:-dx];
				s0 -= w;
				s1 -= w * disparity[nTile1];
				s2 -= w * disparity[nTile1] * disparity[nTile1];
				s_mean = s1/s0;
				smpl_var = s2/s0 -s_mean*s_mean;

				if (debugLevel > -2) {
//					System.out.println (" s_mean = "+s_mean+", smpl_var = "+smpl_var+ " ntiles="+(sample_list.size()-1) );
					System.out.println (String.format(" => mean=%8f var=%8f tiles=%3d ", s_mean, smpl_var, sample_list.size()-1));
				}

			} // if (smpl_var > sigma2) {
			to_measure[nTile] = s_mean;
			if (debugLevel > -2) {
				disparity_bimap[ImageDtt.BI_DBG2_INDEX][nTile] = s_mean;
				disparity_bimap[ImageDtt.BI_DBG3_INDEX][nTile] = Math.sqrt(smpl_var);
			}
		}

		if (debugLevel > -2) {
			int num_to_measure =0;
			for (int nTile = 0; nTile < to_measure.length; nTile++) {
				if (!Double.isNaN(to_measure[nTile]))num_to_measure++;
			}

			System.out.println ("Updated sigma in tiles:"+num_sigma+" (sigma = "+sigma+", sigma2 = "+sigma2);
			System.out.println ("Tiles to meaure:"+num_to_measure);
			disparity_bimap[ImageDtt.BI_DBG3_INDEX] = to_measure; // overwrites old debug data
943
			(new ShowDoubleFloatArrays()).showArrays(
944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986
					disparity_bimap,
					tnImage.sizeX,
					tnImage.sizeY,
					true,
					"BiCamDSI-2",
					ImageDtt.BIDISPARITY_TITLES);
		}
		return to_measure;
	}
/*
 * sigma2
	  static int  BI_DISP_FULL_INDEX =            0;  // 0 - disparity for all directions of the main camera
	  static int  BI_DISP_HOR_INDEX =             1;  // 1 - disparity for 2 horizontal pairs of the main camera
	  static int  BI_DISP_VERT_INDEX =            2;  // 2 - disparity for 2 vertical pairs of the main camera
	  static int  BI_DISP_DIAGM_INDEX =           3;  // 3 - disparity for main diagonal pair of the main camera
	  static int  BI_DISP_DIAGO_INDEX =           4;  // 4 - disparity for main diagonal pair of the main camera
	  static int  BI_ADISP_FULL_INDEX =           5;  // 5 - disparity for all directions of the aux camera
	  static int  BI_ADISP_HOR_INDEX =            6;  // 6 - disparity for 2 horizontal pairs of the aux camera
	  static int  BI_ADISP_VERT_INDEX =           7;  // 7 - disparity for 2 vertical pairs of the aux camera
	  static int  BI_ADISP_DIAGM_INDEX =          8;  // 8 - disparity for main diagonal pair of the aux camera
	  static int  BI_ADISP_DIAGO_INDEX =          9;  // 9 - disparity for main diagonal pair of the aux camera
	  static int  BI_DISP_CROSS_INDEX =          10;  //10 - disparity between the main the aux camera
	  static int  BI_DISP_CROSS_DX_INDEX =       11;  //11 - delta disparity between the main the aux camera (horizontal)
	  static int  BI_DISP_CROSS_DY_INDEX =       12;  //12 - delta disparity between the main the aux camera (vertical)
	  static int  BI_STR_FULL_INDEX =            13;  //13 - strength for all directions of the main camera
	  static int  BI_STR_HOR_INDEX =             14;  //14 - strength for 2 horizontal pairs of the main camera
	  static int  BI_STR_VERT_INDEX =            15;  //15 - strength for 2 vertical pairs of the main camera
	  static int  BI_STR_DIAGM_INDEX =           16;  //16 - strength for main diagonal pair of the main camera
	  static int  BI_STR_DIAGO_INDEX =           17;  //17 - strength for main diagonal pair of the main camera
	  static int  BI_ASTR_FULL_INDEX =           18;  //18 - strength for all directions of the aux camera
	  static int  BI_ASTR_HOR_INDEX =            19;  //19 - strength for 2 horizontal pairs of the aux camera
	  static int  BI_ASTR_VERT_INDEX =           20;  //20 - strength for 2 vertical pairs of the aux camera
	  static int  BI_ASTR_DIAGM_INDEX =          21;  //21 - strength for main diagonal pair of the aux camera
	  static int  BI_ASTR_DIAGO_INDEX =          22;  //22 - strength for main diagonal pair of the aux camera
	  static int  BI_STR_CROSS_INDEX =           23;  //23 - strength between the main the aux camera
	  static int  BI_STR_ALL_INDEX =             24;  //23 - average strength (product of strengths to 1/3 power), TODO: strength at cross disparity
	  static int  BI_TARGET_INDEX =              25;  //24 - target disparity
	  static int  BI_DBG1_INDEX =                26;  //26 - debug layer 1
	  static int  BI_DBG2_INDEX =                27;  //27 - debug layer 2

 */

}