Changed disp_dist format to [quad][4], implemented and connected new LMA (1e0474a8) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/gpu/GPUTileProcessor.java

+2 −2

Original line number	Diff line number	Diff line
		@@ -733,8 +733,8 @@ public class GPUTileProcessor {
		// TODO: move port coordinates out of color channel loop
		double [][] centersXY_main = null;
		double [][] centersXY_aux = null;
		double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
		double [][] disp_dist_aux = new double[2 * quad_aux][]; // used to correct 3D correlations
		double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
		double [][] disp_dist_aux = new double[quad_aux][]; // used to correct 3D correlations

		if (geometryCorrection_main != null) {
		centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(

src/main/java/com/elphel/imagej/tileprocessor/AlignmentCorrection.java

+1 −1

Original line number	Diff line number	Diff line
		@@ -3091,7 +3091,7 @@ B = \|+dy0 -dy1 -2*dy3 \|
		double [] pXY = mm.getPXY();
		double [][] deriv = new double [2 * NUM_SENSORS][];
		int dbg_index =dbg_index (pXY, dbg_decimate);
		double [][] disp_dist = new double[dbg_titles_xy.length][]; // used to correct 3D correlations
		double [][] disp_dist = new double[NUM_SENSORS][]; // used to correct 3D correlations
		geometryCorrection.getPortsCoordinatesAndDerivatives(
		geometryCorrection, // GeometryCorrection gc_main,
		false, // boolean use_rig_offsets,

src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java

+10 −10

Original line number	Diff line number	Diff line
		@@ -114,7 +114,7 @@ public class Corr2dLMA {
		private int ncam = 0; // number of used cameras
		private int npairs=0; // number of used pairs
		private int last_cam; // index of the last camera (special treatment for disparity correction)
		private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
		// private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
		private final Matrix [][] m_pairs = new Matrix[NUM_CAMS][NUM_CAMS];
		private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
		private final int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
		@@ -206,13 +206,13 @@ public class Corr2dLMA {
		}
		}
		public void initVector( // USED in lwir
		boolean adjust_width, // adjust width of the maximum
		boolean adjust_scales, // adjust 2D correlation scales
		boolean adjust_ellipse, // allow non-circular correlation maximums
		boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities
		boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
		boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
		boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
		boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities lma_adjust_wxy
		double disp0, // initial value of disparity
		double half_width, // A=1/(half_widh)^2
		double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity
		double half_width, // A=1/(half_widh)^2 lma_half_width
		double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity lma_cost_wy
		) {
		// int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
		for (int f = 0; f < NUM_CAMS; f++) {
		@@ -231,6 +231,7 @@ public class Corr2dLMA {
		for (Sample s:samples) { // ignore zero-weight samples
		used_cameras[s.fcam]=true;
		used_cameras[s.scam]=true;
		/*
		if (s.fcam > last_cam) {
		second_last = false;
		last_cam = s.fcam;
		@@ -239,6 +240,7 @@ public class Corr2dLMA {
		second_last = true;
		last_cam = s.scam;
		}
		*/
		used_pairs[pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s
		used_pairs_dir[s.fcam][s.scam] = true;
		}
		@@ -335,8 +337,6 @@ public class Corr2dLMA {
		}

		public void initMatrices() { // should be called after initVector and after setMatrices
		// private final Matrix [][] m_pairs = new Matrix[NUM_CAMS][NUM_CAMS];
		// private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
		for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) {
		m_pairs[f][s] = null;
		m_pairs_last[f][s] = null;
		@@ -389,7 +389,7 @@ public class Corr2dLMA {

		int num_samples = samples.size();
		double [] fx= new double [num_samples + 2 * NUM_CAMS];
		double sqrt2 = Math.sqrt(2.0);
		// double sqrt2 = Math.sqrt(2.0);
		double A = av[A_INDEX];
		double B = av[B_INDEX];
		double C = A + av[CMA_INDEX];

src/main/java/com/elphel/imagej/tileprocessor/Correlation2d.java

+98 −2

Original line number	Diff line number	Diff line
		package com.elphel.imagej.tileprocessor;
		import java.util.ArrayList;

		import com.elphel.imagej.common.DoubleGaussianBlur;
		import com.elphel.imagej.common.PolynomialApproximation;
		import com.elphel.imagej.common.ShowDoubleFloatArrays;

		@@ -1751,7 +1752,103 @@ public class Correlation2d {
		return rslt;
		}

		public Corr2dLMA corrLMA2( // USED in lwir
		ImageDttParameters imgdtt_params,
		double [][] corrs,
		double [][] disp_dist, // per camera disparity matrix as a 1d (linescan order)
		int pair_mask, // which pairs to process
		boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
		double sigma, // low-pass sigma to find maximum (and convex too
		double xcenter, // preliminary center x in pixels for largest baseline
		double vasw_pwr, // value as weight to this power,
		int debug_level,
		int tileX, // just for debug output
		int tileY
		)
		{
		// corrs are organized as PAIRS, some are null if not used
		// for each enabled and available pair find a maximum, filter convex and create sample list
		DoubleGaussianBlur gb = null;
		if (sigma > 0) gb = new DoubleGaussianBlur();
		int center = transform_size - 1;
		int corr_size = 2 * transform_size - 1;
		Corr2dLMA lma = new Corr2dLMA(transform_size);
		for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (((pair_mask >> npair) & 1) !=0)){
		double[] corr = corrs[npair].clone();
		if (sigma > 0) {
		gb.blurDouble(corr, corr_size, corr_size, sigma, sigma, 0.01);
		}
		int imx = 0;
		for (int i = 1; i < corr.length; i++) if (corr[i] > corr[imx]) imx = i;
		// filter convex
		int ix0 = (imx % corr_size) - center; // signed, around center to match filterConvex
		int iy0 = (imx / corr_size) - center; // signed, around center to match filterConvex
		double [] filtWeight = filterConvex(
		corr, // double [] corr_data,
		imgdtt_params.cnvx_hwnd_size, // int hwin,
		ix0, // int x0,
		iy0, // int y0,
		imgdtt_params.cnvx_add3x3, // boolean add3x3,
		imgdtt_params.cnvx_weight, // double nc_cost,
		(debug_level > 2)); // boolean debug);
		int fcam = PAIRS[npair][0];
		int scam = PAIRS[npair][1];
		for (int i = 1; i < filtWeight.length; i++) if (filtWeight[i] > 0.0) {
		int ix = i % corr_size; // >=0
		int iy = i / corr_size; // >=0
		double v = corrs[npair][i]; // not blurred
		double w = filtWeight[i];
		if (vasw_pwr != 0) {
		w *= Math.pow(Math.abs(v), vasw_pwr);
		}
		lma.addSample( // x = 0, y=0 - center
		fcam, // int fcam, // first camera index
		scam, // int scam, // second camera index
		ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
		iy, // int y, // y coordinate (0 - disparity axis)
		v, // double v, // correlation value at that point
		w); //double w){ // sample weight
		}
		}
		lma.initVector( // USED in lwir
		imgdtt_params.lma_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
		imgdtt_params.lma_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
		imgdtt_params.lma_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
		imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities lma_adjust_wxy
		xcenter, // double disp0, // initial value of disparity
		imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
		imgdtt_params.lma_cost_wy // double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity lma_cost_wy
		);
		lma.setMatrices(disp_dist);
		lma.initMatrices(); // should be called after initVector and after setMatrices
		boolean lmaSuccess = false;
		if (debug_level > 1) {
		System.out.println("Input data:");
		lma.printInputDataFx(false);
		}

		lmaSuccess = lma.runLma(
		imgdtt_params.lma_lambda_initial, // double lambda, // 0.1
		imgdtt_params.lma_lambda_scale_good, // double lambda_scale_good,// 0.5
		imgdtt_params.lma_lambda_scale_bad, // double lambda_scale_bad, // 8.0
		imgdtt_params.lma_lambda_max, // double lambda_max, // 100
		imgdtt_params.lma_rms_diff, // double rms_diff, // 0.001
		imgdtt_params.lma_num_iter, // int num_iter, // 20
		debug_level); // int debug_level)

		lma.updateFromVector();
		double [] rms = lma.getRMS();
		if (debug_level > 0) {
		System.out.println("LMA ->"+lmaSuccess+" RMS="+rms[0]+", pure RMS="+rms[1]);
		lma.printParams();
		}

		if (debug_level > 1) {
		System.out.println("Input data and approximation:");
		lma.printInputDataFx(true);
		}
		return lmaSuccess? lma: null;
		}

		public Correlations2dLMA corrLMA( // USED in lwir
		ImageDttParameters imgdtt_params,
		@@ -1824,7 +1921,6 @@ public class Correlation2d {
		lma, // Correlations2dLMA lma,
		imgdtt_params.cnvx_add3x3, // boolean add3x3,
		imgdtt_params.cnvx_weight, // double nc_cost,

		debug_level); // int debug_level
		boolean lmaSuccess;
		if (run_poly_instead) { // not used in lwir

src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java

+7 −8

Original line number	Diff line number	Diff line
		@@ -2336,7 +2336,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		* @param rots misalignment correction (now includes zoom in addition to rotations
		* @param deriv_rots derivatives by d_az, f_elev, d_rot, d_zoom
		* @param pXYderiv - null or double[2 * number_of_cameras][] array to accommodate derivatives of px, py by each of the parameters
		* @param disp_dist - null or double[2 * number_of_cameras][] array to accommodate X,Y derivatives by disp and CCW90 of disp
		* @param disp_dist - null or double[number_of_cameras][4] array to accommodate X,Y (rows) derivatives by disp and CCW90 of disp (cols)
		* @param px pixel X coordinate
		* @param py pixel Y coordinate
		* @param disparity disparity (for non-distorted image space)
		@@ -2452,8 +2452,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		}

		if (disp_dist != null) {
		disp_dist[2 * i] = new double [2]; // dx/d_disp, dx_d_ccw_disp
		disp_dist[2 * i+1] = new double [2]; // dy/d_disp, dy_d_ccw_disp
		disp_dist[i] = new double [4]; // dx/d_disp, dx_d_ccw_disp
		// Not clear - what should be in Z direction before rotation here?
		double [][] add0 = {
		{-rXY[i][0], rXY[i][1], 0.0},
		@@ -2484,10 +2483,10 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.

		Matrix dd2 = rot2.transpose().times(scale_distort).times(rot2).times(dd1);

		disp_dist[2 * i ][0] = dd2.get(0, 0);
		disp_dist[2 * i ][1] = dd2.get(0, 1);
		disp_dist[2 * i+1][0] = dd2.get(1, 0);
		disp_dist[2 * i+1][1] = dd2.get(1, 1);
		disp_dist[i][0] = dd2.get(0, 0);
		disp_dist[i][1] = dd2.get(0, 1);
		disp_dist[i][2] = dd2.get(1, 0);
		disp_dist[i][3] = dd2.get(1, 1);

		}

		@@ -3008,7 +3007,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		break; // too high distortion
		}
		if (Math.abs(rD-rDist)<delta) {
		System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
		if (debugThis) System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
		break; // success
		}
		r+=(rDist-rD)/drDistDr;

Admin message