ObjectLocation.java

package com.elphel.imagej.orthomosaic;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;

import com.elphel.imagej.common.DoubleFHT;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.tileprocessor.Correlation2d;
import com.elphel.imagej.tileprocessor.TileNeibs;

import Jama.EigenvalueDecomposition;
import Jama.Matrix;
import ij.ImagePlus;
import ij.gui.PointRoi;

public class ObjectLocation {
	String name;
	double [] xy_meters;
	double [] xy_pixels;
	
	public ObjectLocation (
			String name,
			double [] xy_meters) {
		this.name = name;
		this.xy_meters = xy_meters;
	}
	public String getName() {
		return name;
	}
	public double [] getMetric() {
		return xy_meters;
	}
	public int [] getPixels() {
		return new int[] {
				(int) Math.round(xy_pixels[0]),
				(int) Math.round(xy_pixels[1])};
	};
	
	public void setPixels(double [] xy) {
		xy_pixels = xy;
	}
	
	public double [] extractObjectImage(
			OrthoMapsCollection maps_collection,
			int size) {
		int nmap =maps_collection.getIndex(getName());
		OrthoMap [] ortho_maps=maps_collection.getMaps();
		final int width =     ortho_maps[nmap].getImageData().width;
		final int height =    ortho_maps[nmap].getImageData().height;
		final float [] src_img =  ortho_maps[nmap].getImageData().readFData();
		double [] dcrop = new double [size*size];
		Arrays.fill(dcrop, Double.NaN);
		int hsize = size/2;
		int [] xy = getPixels();
		boolean has_NaN=false;		
		for (int y = 0; y < size; y++) {
			int src_y = y - hsize + xy[1];
			if ((src_y >= 0) && (y < height)) {
				for (int x = 0; x < size; x++) {
					int src_x = x - hsize + xy[0];
					if ((src_x >= 0) && (x < width)) {
						double d = src_img[src_x + src_y * width];
						dcrop[x + size * y] = d;
						has_NaN |= Double.isNaN(d);
					} else {
						has_NaN=true;
					}
				}
			} else {
				has_NaN=true;
			}
		}
		if (has_NaN) {
			TileNeibs tn =  new TileNeibs(size,size);
			OrthoMap.fillNaNs(
					dcrop, // double [] data,
					tn, // TileNeibs tn,
					3); // int min_neibs)
		}
		return dcrop;
	}

	public static double [] extractObjectImage(
			double [] center_xy,
			int       size,
			double [] src_img,
			int width) {
		int height =   src_img.length / width;
		double [] dcrop = new double [size*size];
		Arrays.fill(dcrop, Double.NaN);
		int hsize = size/2;
		int [] xy = {(int) Math.round(center_xy[0]), (int) Math.round(center_xy[1])};
		boolean has_NaN=false;		
		for (int y = 0; y < size; y++) {
			int src_y = y - hsize + xy[1];
			if ((src_y >= 0) && (y < height)) {
				for (int x = 0; x < size; x++) {
					int src_x = x - hsize + xy[0];
					if ((src_x >= 0) && (x < width)) {
						double d = src_img[src_x + src_y * width];
						dcrop[x + size * y] = d;
						has_NaN |= Double.isNaN(d);
					} else {
						has_NaN=true;
					}
				}
			} else {
				has_NaN=true;
			}
		}
		if (has_NaN) {
			TileNeibs tn =  new TileNeibs(size,size);
			OrthoMap.fillNaNs(
					dcrop, // double [] data,
					tn, // TileNeibs tn,
					3); // int min_neibs)
		}
		return dcrop;
	}
	
	public static double [] getPatternCenter(
			double [] data_in,
			double [] pattern,
			double [] kernel,
			int       zoomout,
			double    phaseCoeff,
			double    min_corr,
			int       radius_search,
			int       radius_centroid,
			double [] corr_ret, // null or double[corr_size*corr_size]
			String    dbg_prefix,
			int       debugLevel){
		int refine = 1;
		int extr_size =    (int) Math.sqrt(data_in.length);
		int corr_size = 1;
		for (; corr_size <= extr_size; corr_size*=2);
		corr_size /=2;
		double [] data = data_in;
		if (corr_size < extr_size) {
			data = new double [corr_size*corr_size];
			int offs = extr_size/2 - corr_size/2;
			for (int i = 0; i < corr_size; i++) {
				System.arraycopy(
						data_in,
						offs * (extr_size + 1) + i * extr_size,
						data,
						i * corr_size,
						corr_size);
			}
		}
		
		int pattern_size = (int) Math.sqrt(pattern.length);
		/*
		// Already done while extracting
		boolean has_NaN=false;
		for (int i = 0; i < data.length; i++) {
			if (Double.isNaN(data[i])) {
				has_NaN=true;
				break;
			}
		}
		if (has_NaN) {
			TileNeibs tn =  new TileNeibs(corr_size,corr_size);
			OrthoMap.fillNaNs(
					data, // double [] data,
					tn, // TileNeibs tn,
					3); // int min_neibs)
		}
        */
		if (radius_search > (corr_size/2 - 1)) {
			radius_search = corr_size/2 - 1;
			System.out.println("getPatterCenter(): limiting radius_search to "+radius_search);
		}
		if (radius_centroid > radius_search) {
			radius_centroid = radius_search;
			System.out.println("getPatterCenter(): limiting radius_centroid to "+radius_centroid);
		}
		double [] corr_pattern = OrthoMap.patternZoomCropPad(
				pattern,      // double [] pattern,
				pattern_size, // int       pattern_size,
				corr_size,    // int       size,
				zoomout,      // int       zoomout,
				false); // true);        // out_normalize); // boolean   normalize) 
		if (kernel != null) {
			corr_pattern = OrthoMap.convolveWithKernel(
					corr_pattern,   // final double [] data,
					kernel,           // final double [] kernel,
					corr_size);       // final int width)
		}
		double [] corr_out= OrthoMap.correlateWithPattern(
				data,             // final double [] data,
				corr_size,        // final int       width,
				corr_size,        // final int       psize,      // power of 2, such as 64
				corr_pattern,     // final double [] pattern,    // [psize*psize]
				false,            // final boolean   convolve,   // convolve, not correlate
				phaseCoeff,       // final double    phaseCoeff,
				0,                // final double    lpf_sigma, // 0 - do not filter
				debugLevel);      // final int       debugLevel)
		if (corr_ret != null) {
			System.arraycopy(corr_out, 0, corr_ret, 0, corr_out.length);
		}
		int data_width = 2 *radius_search + 1;
		int offs = corr_size/2 - radius_search;
		
		double [] data_max = new double [data_width*data_width];
		for (int i = 0; i < data_width; i++) {
			System.arraycopy(
					corr_out,
					offs * (corr_size + 1) + i * corr_size,
					data_max,
					i * data_width,
					data_width);
		}
		// TODO: use min_corr and clusterize! Or use fixed fraction (50%) of the maximum ?
		double [] xys = Correlation2d.getMaxXYCm( // last, average
				data_max,        // corrs.length-1], // double [] data,
				data_width,      // int       data_width,      //  = 2 * transform_size - 1;
				radius_centroid, // double    radius, // 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
				refine,          // int       refine, //  re-center window around new maximum. 0 -no refines (single-pass)
				null,            // boolean [] fpn_mask,
				false,           // boolean    ignore_border, // only if fpn_mask != null - ignore tile if maximum touches fpn_mask
				false);          // boolean   debug)

		
		if (dbg_prefix != null) {
			String []   dbg_titles = {"corr", "orig", "pattern"};
			double [][] dbg_img = {corr_out, data, corr_pattern};
			ImagePlus imp_corrs = ShowDoubleFloatArrays.makeArrays(
					dbg_img,
					corr_size,
					corr_size,
					dbg_prefix+"_pc"+phaseCoeff+"_rad"+radius_centroid,				
					dbg_titles); // test_titles,
			PointRoi roi = new PointRoi();
			roi.setOptions("label");
			roi.addPoint(xys[0]+corr_size/2, xys[1]+corr_size/2); // ,1);
			imp_corrs.setRoi(roi);
			imp_corrs.show();
		}
		return xys;
	}
    
	/**
	 * Invert kernel in frequency domain, then condition it knowing it is radial. Conditioning
	 * involves converting to 1d radial (linear interpolation with optionally increased radial
	 * resolution and simultaneous cosine blurring. Blurring may increase for the outer areas
	 * of the result kernel
	 * @param kernel    direct radial kernel from extracted objects, centered at size/2,size/2
	 * @param fat_zero  fat zero for frequency-domain inversion
	 * @param lpf_sigma low-pass sigma after frequency-domain inversion
	 * @param scale_radial resolution
	 * @param lim_rad     limit inverted kernel by radius
	 * @param trans_width transition width for radial limiting (cosine)
	 * @param blur_center spread data in +/- blur_radius (cosine) among radial bins
	 * @param blur_radius radius of the constant radial blurring, outside increase blur
	 * @param blur_rate   rate of blur increase (pix/pix) in the peripheral areas
	 * @param debugLevel  debug level
	 * @return inverted kernel, same dimension. Normalize to have convolution of derect and inverted  be 1.0
	 * in the center?
	 */
	public static double [] invertRadialKernel(
			double [] kernel,
			double    fat_zero,
			double    lpf_sigma,
			int       scale_radial, // scale up radial resolution 
			double    lim_rad,     // outside all 0
			double    trans_width, // lim_rad-trans_width - start reducing
			double    blur_center, // cosine blur (in original pixels) in the center (<blur_radius) area
			double    blur_radius, // constant blur radius
			double    blur_rate,   // blur increase outside of blur_radius
			// just for testing - source images and corresponding centers to convolve
			double [][] centers,
			double [][] src_cuts,
			String    dbg_prefix,
			int       debugLevel
			) {
		int size = (int) Math.sqrt(kernel.length);
		double [] kernel0 = kernel.clone();
		double [] inverted0 = (new DoubleFHT()).invert (
				kernel0,   // double [] data,
				fat_zero, // double fat_zero,
				0, // double highPassSigma,
				lpf_sigma); // double lowPassSigma);
		int irad = (int) Math.ceil(scale_radial*lim_rad) +2;
		double [] sw = new double [irad], swd = new double[irad];
//		double maxrad2 = irad*irad;
		double xc = size/2, yc=size/2;
		for (int y = 0; y < size; y++) {
			double y2 = (y - yc)*(y-yc);
			for (int x = 0; x < size; x++) {
				double r = Math.sqrt(y2+ (x-xc)*(x-xc));
				double blur = (r < blur_radius) ? blur_center : ((r - blur_center) * blur_rate);
				double low_r =  (r - blur) * scale_radial;
				if (((int) low_r) < sw.length) {
					double d = inverted0[y*size + x];
					double high_r = (r + blur) * scale_radial;
					double s = Math.PI/(high_r-low_r);
					int ilr = Math.max(0,  (int) Math.ceil(low_r));
					int ihr = Math.min(irad-1, (int) Math.floor(high_r));
					for (int i = ilr; i <= ihr; i++) {
						double w = Math.sin(s * (i - low_r));
						sw[i] +=  w;
						swd[i] += w * d;
					}
				}
			}
		}
		for (int i = 0; i < sw.length; i++) {
			if (sw[i] > 0) {
				swd[i] /=  sw[i];
			}
		}
		
		// limit by lim_rad, trans_width
		if (trans_width > 0) {
			double lim_rad_scaled = lim_rad * scale_radial;
			double trans_width_scaled = trans_width * scale_radial;
			for (int i = (int) (lim_rad_scaled-trans_width_scaled); i < sw.length; i++) {
				if (i < lim_rad_scaled) {
					swd[i] *= 0.5*(1.0 + Math.cos(Math.PI*(i - (lim_rad_scaled-trans_width_scaled))/trans_width_scaled));
				} else {
					swd[i] = 0;
				}
			}
		}
		double [] inverted = new double [size*size];
		double lr2 = lim_rad * lim_rad;
		// generate output pattern
		int oc = size/2;
		for (int y = 0; y < size; y++) {
			double dy = y - oc;
			double y2 = dy*dy;
			if (y2 <= lr2) {
				for (int x = 0; x < size; x++) {
					double dx = x - oc;
					double r2 = y2 + dx*dx;
					if (r2 <= lr2) {
						double sr = Math.sqrt(r2)* scale_radial;
						int isr = (int) Math.floor(sr);
						double w1 = sr-isr;
						double w0 = 1.0 - w1;
						inverted[x + size * y] = w0 * swd[isr] + w1 * swd[isr + 1];
					}
				}
			}
		}
		double [] convolved =  (new DoubleFHT()).convolve(
				kernel.clone(), // double[] first,
				inverted.clone()); // double[] second)
		double center_data = convolved[size*(size+1)/2];
		
		double [] normalized = inverted.clone();
		double k = 1.0/center_data;
		for (int i = 0; i < normalized.length; i++) {
			normalized[i]*=k;
		}
		if (debugLevel > -4) {
			System.out.println("invertRadialKernel(): direct*inverted = "+center_data);
			if (debugLevel >0) {
				String dbg_title = dbg_prefix + "_invertRK_fz"+fat_zero+
						"_lpf"+lpf_sigma+"_sr"+scale_radial+"_lr"+lim_rad+
						"_tw"+trans_width+"_bc"+blur_center+"_br"+blur_radius+
						"_brt"+blur_rate;
				String [] dbg_titles= {"direct","inverted","radial","convolved","normalized"};
				double [][] dbg_img = {kernel,inverted0,inverted,convolved,normalized};
				ImagePlus imp_dbg = ShowDoubleFloatArrays.makeArrays(
						dbg_img,
						size,
						size,
						dbg_title,				
						dbg_titles); // test_titles,
//				PointRoi roi = new PointRoi();
//				roi.setOptions("label");
				imp_dbg.show();
			}
		}

		/*
	public double[] convolve(double[] first, double[] second) {
		return convolve(first, second, null);
	}
		 */
	// testing by convolving with the source images
		if (debugLevel>0) {
			double [] wnd1d = new double[size];
			boolean sq= false;
			for (int i= 0; i < size; i++) {
				wnd1d[i] = Math.sin(Math.PI*i/size);
				if (sq) {
					wnd1d[i]*=wnd1d[i];
				}
			}
			double [] wnd = new double[size*size];
			for (int i = 0; i < size; i++) {
				for (int j = 0; j < size; j++) {
					wnd[i*size+j] = wnd1d[i]*wnd1d[j];
				}
			}
			
			int extr_size = (int) Math.sqrt(src_cuts[0].length);
			double [][] test_src = new double [src_cuts.length][size*size];
			for (int i = 0; i < src_cuts.length; i++) {
				double xcent = extr_size/2+centers[i][0]; 
				double ycent = extr_size/2+centers[i][1]; 
				int x0 = (int) Math.round(xcent - size/2);
				int y0 = (int) Math.round(ycent - size/2);
				for (int y = 0; y < size; y++) {
					int ys = y0+y;
					if ((ys >= 0) && (ys < extr_size)) {
						for (int x = 0; x < size; x++) {
							int xs = x0+x;
							if ((xs >= 0) && (xs < extr_size)) {
								int indx = y*size + x;
								test_src[i][indx] = src_cuts[i][ys * extr_size + xs];
							}
						}					
					}
				}
				OrthoMap.removeDC(test_src[i]);
				for (int j = 0; j < test_src[i].length; j++) {
					test_src[i][j] *= wnd[j];
				}
			}
			double [][] test_conv = new double [src_cuts.length][];
			for (int i = 0; i < src_cuts.length; i++) {
				test_conv[i] = (new DoubleFHT()).convolve(
						test_src[i].clone(), // double[] first,
						normalized.clone()); // double[] second)
			}
			
			String dbg_title = dbg_prefix + "_conv_src_+fz"+fat_zero+
					"_lpf"+lpf_sigma+"_sr"+scale_radial+"_lr"+lim_rad+
					"_tw"+trans_width+"_bc"+blur_center+"_br"+blur_radius+
					"_brt"+blur_rate;
			String [] dbg_titles= new String[src_cuts.length] ; // {"direct","inverted","radial","convolved","normalized"};
			for (int i = 0; i < src_cuts.length; i++) {
				dbg_titles[i] = "img-"+i;
			}			
			// double [][] dbg_img = {kernel,inverted0,inverted,convolved,normalized};
			ImagePlus imp_dbg = ShowDoubleFloatArrays.makeArrays(
					test_conv,
					size,
					size,
					dbg_title,				
					dbg_titles); // test_titles,
			//				PointRoi roi = new PointRoi();
			//				roi.setOptions("label");
			imp_dbg.show();
			ImagePlus imp_dbg_src = ShowDoubleFloatArrays.makeArrays(
					test_src,
					size,
					size,
					dbg_prefix+"-src_cut",				
					dbg_titles); // test_titles,
			//				PointRoi roi = new PointRoi();
			//				roi.setOptions("label");
			imp_dbg_src.show();
			
		}

		return normalized;
	}
	
	
	// Needs improvement. Probably look at neighbors when deleting outliers. Multipass and pull to neighbors? 
	public static double [] getRadialPattern(
			final double [] data,
			double []  xy_offs,
			int        corr_size, 
			double     lim_rad,     // outside all 0
			double     trans_width, // lim_rad-trans_width - start reducing
			double     frac_outliers,
			double     reversal_rad,
			boolean [] good,       // null or same size as data
			int        debugLevel) {
		if (good != null) {
			Arrays.fill(good, false);
		}
		double min_abs_weight = 10.0; // Do not touch very center?
		int extr_size = (int) Math.sqrt(data.length);
		double [] rad_patt = new double [corr_size*corr_size];
		double xc = extr_size/2+xy_offs[0]; 
		double yc = extr_size/2+xy_offs[1]; 
		int size_1d = (int) Math.ceil(lim_rad);
		double lr2 = size_1d * size_1d; 
		double [] sw =  new double [size_1d + 2];
		double [] swd = new double [sw.length];
		double [] swd2 = new double [sw.length];
//		boolean [] good = new boolean [data.length];
		double [] distance = new double [data.length];
		ArrayList<ArrayList<Integer>> rad_lists = new ArrayList<ArrayList<Integer>>();
		for (int i = 0; i < sw.length; i++) {
			rad_lists.add(new ArrayList<Integer>());
		}
		for (int y = 0; y < extr_size; y++) {
			double dy = y - yc;
			double y2 = dy*dy;
			if (y2 <= lr2) {
				for (int x = 0; x < extr_size; x++) {
					double dx = x - xc;
					double r2 = y2 + dx*dx;
					if (r2 <= lr2) {
						int indx = x + extr_size * y;
						double r = Math.sqrt(r2);
						int ir = (int) Math.floor(r);
						distance[indx] = r;
						rad_lists.get(ir).add(indx);
						rad_lists.get(ir+1).add(indx);
						double w1 = r-ir;
						double w0 = 1.0 - w1;
						double d = data[indx];
						sw[ir] +=   w0;
						swd[ir] +=  w0 * d;
						swd2[ir] += w0 * d * d;
						sw[ir+1] +=  w1;
						swd[ir+1] += w1 * d;
						swd2[ir+1] += w1 * d * d;
						if (good != null) {
							good[indx] = true;
						}
					}
				}
			}
		}
		//size_1d
		for (int i = 0; i < sw.length; i++) {
			if (sw[i] > 0) {
				swd[i] /=  sw[i];
				swd2[i] /= sw[i];
			}
		}
		double [] sw0 = sw.clone(); // to watch how many were removed (frac_outliers)
		// now sort each list and later remove from either
		for (ArrayList<Integer> rad_list:rad_lists) {
			Collections.sort(rad_list, new Comparator<Integer>() {
				@Override
				public int compare(Integer lhs, Integer rhs) {
					double rhsd = data[rhs];
					double lhsd = data[lhs];
					return (rhsd > lhsd) ? -1 : (rhsd < lhsd) ? 1 : 0;
				}
			});
		}
		
		// min_weight = 10.0
		// Starting from very center remove farthest (first or last) pixel,
		// until remaining weight drops below min_weight or sw0[]*(1-frac_outliers).
		// weight of the previous radius may fall also, but likely not too much
		for (int nr = 1; nr < sw.length; nr++) {
			double min_weight = Math.max(min_abs_weight, sw0[nr] * (1-frac_outliers));
			ArrayList<Integer> rad_list = rad_lists.get(nr);
			ArrayList<Integer> rad_list_prev = (nr > 0)? rad_lists.get(nr-1): null;
			ArrayList<Integer> rad_list_next = (nr < (sw.length -1))? rad_lists.get(nr+1): null;
			while (sw[nr] > min_weight) { // will break trying to remove last
				if (rad_list.size()==0) {
					System.out.println("BUG! - empty list for nr="+nr);
					break;
				}
				int indx_first = rad_list.get(0);
				int indx_last = rad_list.get(rad_list.size()-1);
				boolean remove_first = Math.abs(data[indx_first] - swd[nr]) > Math.abs(data[indx_last] - swd[nr]);
				int indx_worst = remove_first? indx_first : indx_last;
				double d = data[indx_worst];
				double dist = distance[indx_worst];
				boolean other_next = dist >= nr;
				double removed_w = other_next ? ( 1 - (dist - nr)) : (1 + (dist - nr));
				double new_w = sw[nr] - removed_w;
				if (new_w < min_weight) {
					break;
				}
				swd[nr] =  (swd[nr] *  sw[nr] - removed_w *  d) / new_w;
				swd2[nr] = (swd2[nr] * sw[nr] - removed_w *  d * d) / new_w;
				sw[nr] = new_w;
				rad_list.remove(remove_first ? 0 : (rad_list.size()-1));
				if (good != null) {
					good[indx_worst] = false; // seems not used - just for debug/display
				}
				// now remove from previous/next list
				if (dist == nr) {
					break;
				}
				int nr_other = other_next? (nr+1): (nr-1);
				ArrayList<Integer> rad_list_other = other_next? rad_list_next : rad_list_prev;
				// check for consistency:
				int indx_in_other = rad_list_other.indexOf(indx_worst);
				if (indx_in_other < 0) {
					System.out.println("BUG in getRadialPattern(), pixel "+indx_worst+" from nr="+nr+" is missing in "+nr_other);
					break;
				}
				double removed_w_other = 1.0 - removed_w;
				double new_w_other = sw[nr_other] - removed_w_other; // no check for remaining weight here
				swd[nr_other] =  (swd[nr_other] *  sw[nr_other] - removed_w_other *  d) / new_w_other;
				swd2[nr_other] = (swd2[nr_other] * sw[nr_other] - removed_w_other *  d * d) / new_w_other;
				sw[nr_other]= new_w_other;
				rad_list_other.remove(indx_in_other);
			}
		}
		
		int lr = size_1d;
		if (reversal_rad > 0) {
			int irr = (int) reversal_rad;
			boolean is_growing = swd[irr+1] > swd[irr];
			for (lr = irr+1; lr <= size_1d; lr++) {
				if (is_growing) {
					if (swd[lr+1] < swd[lr]) {
						break;
					}
				} else {
					if (swd[lr+1] > swd[lr]) {
						break;
					}
				}
			}
		}
		
		
		for (int i = 0; i < lr; i++) {
			swd[i] -= swd[lr];
		}
		for (int i = lr; i <swd.length; i++) {
			swd[i] = 0;
		}
		
		// TODO: add outlier removal here
		// apply window
		if (trans_width > 0) {
			for (int i = (int) (lim_rad-trans_width); i < sw.length; i++) {
				if (i < lim_rad) {
					swd[i] *= 0.5*(1.0 + Math.cos(Math.PI*(i - (lim_rad-trans_width))/trans_width));
				} else {
					swd[i] = 0;
				}
			}
		}
		// generate output pattern
		int oc = corr_size/2;
		for (int y = 0; y < corr_size; y++) {
			double dy = y - oc;
			double y2 = dy*dy;
			if (y2 <= lr2) {
				for (int x = 0; x < corr_size; x++) {
					double dx = x - oc;
					double r2 = y2 + dx*dx;
					if (r2 <= lr2) {
						double r = Math.sqrt(r2);
						int ir = (int) Math.floor(r);
						double w1 = r-ir;
						double w0 = 1.0 - w1;
						rad_patt[x + corr_size * y] = w0 * swd[ir] + w1 * swd[ir + 1];
					}
				}
			}
		}
		return rad_patt;
	}
	
	
	
	
}