Commit 468688b4 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

fixed derivatives at infinity

parent aa3c316e

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

+27 −21
Original line number Diff line number Diff line
@@ -960,11 +960,11 @@ public class ErsCorrection extends GeometryCorrection { 
//		double [] scene_xyz =     new double [] {-.3,-0.2,  2.0}; // scene center in world coordinates

//		double [] scene_atr =     new double [] {0.05, -0.03,  0.05}; // scene orientation relative to world frame



//		double [] reference_xyz = new double [] {0.5, 0.2,-3.0}; // reference center in world coordinates

		double [] reference_xyz = new double [3]; // reference center in world coordinates

		double [] reference_xyz = new double [] {0.5, 0.2,-3.0}; // reference center in world coordinates

//		double [] reference_xyz = new double [3]; // reference center in world coordinates

		double [] reference_atr = new double [] {0.1, 0.05,   0.15}; // reference orientation relative to world frame

		double [] scene_xyz =     reference_xyz.clone(); //new double[3]; // 

		double [] scene_atr =     new double[3]; // reference_atr.clone();

		double [] scene_xyz =     new double [] {0.2, -0.1,  -2.0}; // reference_xyz.clone(); //new double[3]; // 

		double [] scene_atr =     new double [] {0.05, 0.15, -0.05}; // new double[3]; // reference_atr.clone();

		System.out.println(String.format("reference_xyz= {%8f, %8f, %8f}",reference_xyz[0],reference_xyz[1],reference_xyz[2]));

		System.out.println(String.format("reference_atr= {%8f, %8f, %8f}",reference_atr[0],reference_atr[1],reference_atr[2]));

		System.out.println(String.format("scene_xyz=     {%8f, %8f, %8f}",scene_xyz[0],scene_xyz[1],scene_xyz[2]));
@@ -986,10 +986,10 @@ public class ErsCorrection extends GeometryCorrection { 
			System.out.println(String.format("Scene     ers_watr_center_dt= {%8f, %8f, %8f}",scene_ers.ers_watr_center_dt[0],scene_ers.ers_watr_center_dt[1],scene_ers.ers_watr_center_dt[2]));

		}

		if (fake_linear_ers) {

//			this.ers_wxyz_center_dt=      new double[] { 1.0, 0.25, -2.0}; // this.ers_watr_center_dt.clone(); // new double[3];

//			scene_ers.ers_wxyz_center_dt= new double[] { 0.5, 0.3,  -1.5}; // new double[3];

			this.ers_wxyz_center_dt=      new double[3];// { 1.0, 0.25, -2.0}; // this.ers_watr_center_dt.clone(); // new double[3];

			scene_ers.ers_wxyz_center_dt= new double[3];// { 0.5, 0.3,  -1.5}; // new double[3];

			this.ers_wxyz_center_dt=      new double[] { 1.0, 0.25, -2.0}; // this.ers_watr_center_dt.clone(); // new double[3];

			scene_ers.ers_wxyz_center_dt= new double[] { 0.5, 0.3,  -1.5}; // new double[3];

//			this.ers_wxyz_center_dt=      new double[3];// { 1.0, 0.25, -2.0}; // this.ers_watr_center_dt.clone(); // new double[3];

//			scene_ers.ers_wxyz_center_dt= new double[3];// { 0.5, 0.3,  -1.5}; // new double[3];

			System.out.println("Updated2:");

			System.out.println(String.format("Reference ers_wxyz_center_dt= {%8f, %8f, %8f}",this.ers_wxyz_center_dt[0],this.ers_wxyz_center_dt[1],this.ers_wxyz_center_dt[2]));

			System.out.println(String.format("Reference ers_watr_center_dt= {%8f, %8f, %8f}",this.ers_watr_center_dt[0],this.ers_watr_center_dt[1],this.ers_watr_center_dt[2]));
@@ -1974,10 +1974,16 @@ public class ErsCorrection extends GeometryCorrection { 
			scene_vectors[DW_DPX + 1].toArray(),

			scene_vectors[DW_DPY + 1].toArray(),

			scene_vectors[DW_DD +  1].toArray()}).transpose();

		double x = xyz4[0];

		double y = xyz4[1];

		double z = xyz4[2];

		double z2 = z * z; 

		// next variables are {x,y,z} * disparity

		double xref = xyz4[0];

		double yref = xyz4[1];

		double zref = xyz4[2];

		double z2ref = zref * zref;

		double xscene = scene_vectors[0].getX();

		double yscene = scene_vectors[0].getY();

		double zscene = scene_vectors[0].getZ();

		double z2scene = zscene * zscene;

		

		if (is_infinity) {

			// dividing X, Y by Z, getting 2x2 Matrix

			/*
@@ -1989,10 +1995,10 @@ public class ErsCorrection extends GeometryCorrection { 
			 * dV/dpX = 1/z * (dy/dpX) - y/z^2 * (dz/dpX)

			 * dV/dpY = 1/z * (dy/dpY) - y/z^2 * (dz/dpY)  

			 */

			double dU_dpX = dx_dpscene.get(0, 0) / z - x * dx_dpscene.get(2, 0) / z2;

			double dU_dpY = dx_dpscene.get(0, 1) / z - x * dx_dpscene.get(2, 1) / z2;

			double dV_dpX = dx_dpscene.get(1, 0) / z - y * dx_dpscene.get(2, 0) / z2;

			double dV_dpY = dx_dpscene.get(1, 1) / z - y * dx_dpscene.get(2, 1) / z2;  

			double dU_dpX = dx_dpscene.get(0, 0) / zscene - xscene * dx_dpscene.get(2, 0) / z2scene;

			double dU_dpY = dx_dpscene.get(0, 1) / zscene - xscene * dx_dpscene.get(2, 1) / z2scene;

			double dV_dpX = dx_dpscene.get(1, 0) / zscene - yscene * dx_dpscene.get(2, 0) / z2scene;

			double dV_dpY = dx_dpscene.get(1, 1) / zscene - yscene * dx_dpscene.get(2, 1) / z2scene;  

			dx_dpscene = new Matrix (new double[][] {

				{ dU_dpX, dU_dpY, 0.0},

				{ dV_dpX, dV_dpY, 0.0},
@@ -2012,8 +2018,8 @@ public class ErsCorrection extends GeometryCorrection { 
			int vindx = indx+1;

			if (is_infinity) {

				Vector3D dw_dp = new Vector3D(

						reference_vectors[vindx].getX()/z - x*reference_vectors[vindx].getZ()/z2,

						reference_vectors[vindx].getY()/z - y*reference_vectors[vindx].getZ()/z2,

						reference_vectors[vindx].getX()/zref - xref * reference_vectors[vindx].getZ() / z2ref,

						reference_vectors[vindx].getY()/zref - yref * reference_vectors[vindx].getZ() / z2ref,

						0.0);

				derivatives[vindx] = matrixTimesVector(dpscene_dxyz, dw_dp).toArray();

			} else {
@@ -2025,8 +2031,8 @@ public class ErsCorrection extends GeometryCorrection { 
			int indx_in =  indx_out - DP_DSVAZ + DW_DVAZ; // 4, 5, ...

			if (is_infinity) {

				Vector3D dw_dp = new Vector3D(

						scene_vectors[indx_in].getX()/z - x*scene_vectors[indx_in].getZ()/z2,

						scene_vectors[indx_in].getY()/z - y*scene_vectors[indx_in].getZ()/z2,

						scene_vectors[indx_in].getX()/zscene - xscene * scene_vectors[indx_in].getZ() / z2scene,

						scene_vectors[indx_in].getY()/zscene - yscene * scene_vectors[indx_in].getZ() / z2scene,

						0.0);

				derivatives[indx_out] = matrixTimesVector(dpscene_dxyz_minus, dw_dp).toArray();

			} else {