debugging disparity grid distortion calculation (2fab35c1) · Commits · Elphel / imagej-elphel

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

+117 −23

Original line number	Diff line number	Diff line
		@@ -83,7 +83,7 @@ public class GeometryCorrection {
		public double disparityRadius=150.0; // distance between cameras to normalize disparity units to. sqrt(2)*disparityRadius for quad camera (~=150mm)?

		private double [] rByRDist=null;
		private double stepR=0.001;
		private double stepR=0.0001; // 0.001;
		private double maxR=2.0; // calculate up to this*distortionRadius

		public CorrVector extrinsic_corr;
		@@ -2396,11 +2396,24 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		double pXci = rvi.get(0, 0) * norm_z;
		double pYci = rvi.get(1, 0) * norm_z;

		// debug
		double norm_z_dbg = fl_pix/vi.get(2, 0);
		double pXci_dbg = vi.get(0, 0) * norm_z_dbg;
		double pYci_dbg = vi.get(1, 0) * norm_z_dbg;




		// Re-apply distortion
		double rNDi = Math.sqrt(pXcipXci + pYcipYci); // in pixels
		// Rdist/R=A8R^7+A7R^6+A6R^5+A5R^4+AR^3+BR^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
		double ri = rNDi* ri_scale; // relative to distortion radius
		// double rD2rND = (1.0 - distortionA8 - distortionA7 - distortionA6 - distortionA5 - distortionA - distortionB - distortionC);

		double rNDi_dbg = Math.sqrt(pXci_dbgpXci_dbg + pYci_dbgpYci_dbg); // in pixels
		double ri_dbg = rNDi_dbg* ri_scale; // relative to distortion radius


		double rD2rND = 1.0;
		double rri = 1.0;
		for (int j = 0; j < rad_coeff.length; j++){
		@@ -2408,40 +2421,74 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		rD2rND += rad_coeff[j]*(rri - 1.0); // Fixed
		}

		double rD2rND_dbg = 1.0;
		double rri_dbg = 1.0;
		for (int j = 0; j < rad_coeff.length; j++){
		rri_dbg *= ri_dbg;
		rD2rND_dbg += rad_coeff[j]*(rri_dbg - 1.0); // Fixed
		}



		// Get port pixel coordinates by scaling the 2d vector with Rdistorted/Dnondistorted coefficient)
		double pXid = pXci * rD2rND;
		double pYid = pYci * rD2rND;

		double pXid_dbg = pXci_dbg * rD2rND_dbg; // Should be the same as pXcd
		double pYid_dbg = pYci_dbg * rD2rND_dbg; //


		pXY[i][0] = pXid + this.pXY0[i][0];
		pXY[i][1] = pYid + this.pXY0[i][1];

		// used when calculating derivatives, TODO: combine calculations !
		double drD2rND_dri = 0.0;
		if ((disp_dist != null) \|\| (pXYderiv != null)) {
		rri = 1.0;
		for (int j = 0; j < rad_coeff.length; j++){
		drD2rND_dri += rad_coeff[j] * (j+1) * rri;
		rri *= ri;
		}
		}

		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
		// Not clear - what should be in Z direction before rotation here?
		double [][] add0 = {
		{-disparity * rXY[i][0], disparity * rXY[i][1]},
		{-disparity * rXY[i][1], -disparity * rXY[i][0]}};
		{-rXY[i][0], rXY[i][1], 0.0},
		{-rXY[i][1], -rXY[i][0], 0.0},
		{ 0.0, 0.0, 0.0}}; // what is last element???
		/*
		{1.0, 0.0, 0.0},
		{0.0, 1.0, 0.0},
		{0.0, 0.0, 0.0}}; // what is last element???
		*/
		Matrix dd0 = new Matrix(add0);
		Matrix dd1 = rots[i].times(dd0).getMatrix(0, 1,0,1).times(norm_z); // get top left 2x2 sub-matrix
		//// Matrix dd1 = dd0.getMatrix(0, 1,0,1); // get top left 2x2 sub-matrix
		// now first column of 2x2 dd1 - x, y components of derivatives by disparity, second column - derivatives by ortho to disparity (~Y in 2d correlation)
		// unity vector in the direction of radius
		double c_dist = pXci/rNDi;
		double s_dist = pYci/rNDi;
		double c2_dist = c_dist * c_dist;
		double s2_dist = s_dist * s_dist;
		double cs_dist = c_dist * s_dist;

		double [][] adist_dcorr = {
		{rD2rND * c2_dist + s2_dist, (rD2rND - 1)* cs_dist},
		{(rD2rND - 1)* cs_dist, rD2rND * s2_dist + c2_dist}};
		Matrix dist_dcorr = new Matrix(adist_dcorr);
		Matrix dd2 = dist_dcorr.times(dd1);
		double [][] arot2= {
		{c_dist, s_dist},
		{-s_dist, c_dist}};
		Matrix rot2 = new Matrix(arot2); // convert from non-distorted X,Y to parallel and perpendicular (CCW) to the radius

		double [][] ascale_distort = {
		{rD2rND + ri* drD2rND_dri, 0 },
		{0, rD2rND}};
		Matrix scale_distort = new Matrix(ascale_distort); // scale component parallel to radius as distortion derivative, perpendicular - as distortion

		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);

		}


		@@ -2475,12 +2522,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		double dri_droll = ri_scale / rNDi* (pXci * dpXci_droll + pYci * dpYci_droll); // Not used anywhere ?
		// TODO: verify dri_droll == 0 and remove
		*/
		double drD2rND_dri = 0.0;
		rri = 1.0;
		for (int j = 0; j < rad_coeff.length; j++){
		drD2rND_dri += rad_coeff[j] * (j+1) * rri;
		rri *= ri;
		}
		// double drD2rND_dri = 0.0;
		// rri = 1.0;
		// for (int j = 0; j < rad_coeff.length; j++){
		// drD2rND_dri += rad_coeff[j] * (j+1) * rri;
		// rri *= ri;
		// }

		double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
		double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
		@@ -2924,8 +2971,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		*/
		public boolean calcReverseDistortionTable(){
		boolean debugThis=false; //true;
		double delta=1E-8;
		double minDerivative=0.1;
		double delta=1E-20; // 12; // 10; // -8;
		double minDerivative=0.01;
		int numIterations=1000;
		double drDistDr=1.0;
		// public double distortionA5=0.0; //r^5 (normalized to focal length or to sensor half width?)
		@@ -2956,15 +3003,19 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		drDistDr=(((5this.distortionA5r + 4this.distortionA)r+3this.distortionB)r+2this.distortionC)r+d;
		}
		double rD=r*k;
		if (drDistDr<minDerivative) {
		if (drDistDr<minDerivative) { // folds backwards !
		bailOut=true;
		break; // too high distortion
		}
		if (Math.abs(rD-rDist)<delta) break; // success
		if (Math.abs(rD-rDist)<delta) {
		System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
		break; // success
		}
		r+=(rDist-rD)/drDistDr;
		}
		if (bailOut) {
		if (debugThis) System.out.println("calcReverseDistortionTable() i="+i+" Bailing out, drDistDr="+drDistDr);
		// if (debugThis)
		System.out.println("calcReverseDistortionTable() i="+i+" Bailing out, drDistDr="+drDistDr);
		return false;
		}
		rPrev=r;
		@@ -3017,7 +3068,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		}


		public double getRByRDist(double rDist, boolean debug){
		public double getRByRDistlin(double rDist, boolean debug){
		// add exceptions;
		if (this.rByRDist==null) {
		calcReverseDistortionTable();
		@@ -3044,4 +3095,47 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
		return result;
		}


		public double getRByRDist(double rDist, boolean debug){
		// add exceptions;
		if (this.rByRDist==null) {
		calcReverseDistortionTable();
		if (debug)System.out.println("getRByRDist("+IJ.d2s(rDist,3)+"): this.rByRDist==null");
		// return Double.NaN;
		}
		if (rDist < 0) {
		if (debug)System.out.println("getRByRDist("+IJ.d2s(rDist,3)+"): rDist < 0");
		return Double.NaN;
		}
		double findex = rDist/this.stepR;
		int index=(int) Math.floor(findex);
		if (index>=(this.rByRDist.length-2)) {
		if (debug) System.out.println("getRByRDist("+IJ.d2s(rDist,3)+"): index="+index+">="+(this.rByRDist.length-2));
		return Double.NaN;
		}
		double mu = findex - index;
		double mu2 = mu * mu;
		double y0 = (index > 0)? this.rByRDist[index-1] : ( 2 * this.rByRDist[index] - this.rByRDist[index+1]);
		// use Catmull-Rom

		double a0 = -0.5 * y0 + 1.5 * this.rByRDist[index] - 1.5 * this.rByRDist[index+1] + 0.5 * this.rByRDist[index+2];
		double a1 = y0 - 2.5 * this.rByRDist[index] + 2 * this.rByRDist[index+1] - 0.5 * this.rByRDist[index+2];
		double a2 = -0.5 * y0 + 0.5 * this.rByRDist[index+1];
		double a3 = this.rByRDist[index];


		double result= a0mumu2+a1mu2+a2mu+a3;
		// double result=this.rByRDist[index] + (this.rByRDist[index+1]-this.rByRDist[index])*(rDist/this.stepR-index);
		// double result=this.rByRDist[index] + (this.rByRDist[index+1]-this.rByRDist[index])*mu;
		if (Double.isNaN(result)){
		if (debug) System.out.println("this.rByRDist["+index+"]="+this.rByRDist[index]);
		if (debug) System.out.println("this.rByRDist["+(index+1)+"]="+this.rByRDist[index+1]);
		if (debug) System.out.println("rDist="+rDist);
		if (debug) System.out.println("(rDist/this.stepR="+(rDist/this.stepR));

		}
		return result;
		}


		}

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

+17 −1

Original line number	Diff line number	Diff line
		@@ -1575,9 +1575,9 @@ public class ImageDtt {
		final int threadsMax, // maximal number of threads to launch
		final int globalDebugLevel)
		{
		final boolean debug_distort= true;
		final boolean macro_mode = macro_scale != 1; // correlate tile data instead of the pixel data
		final int quad = 4; // number of subcameras

		final int numcol = 3; // number of colors // keep the same, just do not use [0] and [1], [2] - green

		// final int numColors = image_data[0].length;
		@@ -1589,6 +1589,8 @@ public class ImageDtt {
		final Thread[] threads = newThreadArray(threadsMax);
		final AtomicInteger ai = new AtomicInteger(0);
		final double [] col_weights= new double [numcol]; // colors are RBG
		final double [][] dbg_distort = debug_distort? (new double [4quad][tilesXtilesY]) : null;


		// keep for now for mono, find out what do they mean for macro mode
		if (macro_mode) { // all the same as they now mean different
		@@ -1887,6 +1889,14 @@ public class ImageDtt {
		centersXY[ip][0] -= shiftXY[ip][0];
		centersXY[ip][1] -= shiftXY[ip][1];
		}
		// save disparity distortions for visualization:
		for (int cam = 0; cam <quad; cam++) {
		dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[ 2* cam + 0][0];
		dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[ 2* cam + 0][1];
		dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[ 2* cam + 1][0];
		dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[ 2* cam + 1][1];
		}

		// TODO: use correction after disparity applied (to work for large disparity values)
		if (fine_corr != null){

		@@ -2842,6 +2852,12 @@ public class ImageDtt {
		};
		}
		startAndJoin(threads);

		// final double [][] dbg_distort = debug_distort? (new double [4quad][tilesXtilesY]) : null;
		if (dbg_distort != null) {
		(new ShowDoubleFloatArrays()).showArrays(dbg_distort, tilesX, tilesY, true, "disparity_distortions"); // , dbg_titles);
		}

		/*
		if (dbg_ports_coords != null) {
		(new showDoubleFloatArrays()).showArrays(dbg_ports_coords, tilesX, tilesY, true, "ports_coordinates", dbg_titles);

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

+1 −1

Original line number	Diff line number	Diff line
		@@ -505,7 +505,7 @@ public class QuadCLT {
		pXY0);
		geometryCorrection.planeProjectLenses(); // project all lenses to the common plane

		// calcualte reverse distortion as a table to be linear intr4epolated
		// calcualte reverse distortion as a table to be linear interpolated (now cubic!)
		geometryCorrection.calcReverseDistortionTable();

		if (numSensors == 4){

Admin message