Commit 598a2074 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

Implemented mode 4 for IMS-to-camera adjustment

parent 1d564879

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

+1 −1
Original line number Original line Diff line number Diff line
@@ -5437,7 +5437,7 @@ public class OpticalFlow { 
	    	}

	    	}

	    	double [] rms = new double[5];

	    	double [] rms = new double[5];

	    	double [] quat = new double[4];

	    	double [] quat = new double[4];

	    	int quat_lma_mode = 3; //  2; // 1;

	    	int quat_lma_mode = 4; // 3; //  2; // 1;

	    	int debug_lev = 3;

	    	int debug_lev = 3;

	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		

	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		

	    	double  translation_weight =   1.0 / (avg_z + 1.0);

	    	double  translation_weight =   1.0 / (avg_z + 1.0);

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

+1 −1
Original line number Original line Diff line number Diff line
@@ -266,7 +266,7 @@ public class QuadCLTCPU { 
		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;

		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;

		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity

		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity

		QuaternionLma quaternionLma = new QuaternionLma();

		QuaternionLma quaternionLma = new QuaternionLma();

		if (quat_lma_mode == 2) {

		if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {

			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR

			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR

			double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR

			double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR

			for (int nscene = early_index; nscene <= last_index; nscene++) {

			for (int nscene = early_index; nscene <= last_index; nscene++) {

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

+222 −314
Original line number Original line Diff line number Diff line
@@ -289,6 +289,19 @@ public class QuaternionLma { 
			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

			double []     quat0,

			double []     quat0,

			final int     debug_level) {

			final int     debug_level) {

		if (mode != 2) {

			prepareLMAMode4(

					mode,       // int           mode,

					avg_height, // double        avg_height,

					vect_x,     // double [][][] vect_x, // []{{x,y,z},{a,t,r}}

					vect_y,     // double [][][] vect_y, // []{{x,y,z},{a,t,r}}

					vect_w,     // double []     vect_w, // one per scene

					reg_w,              // double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

					quat0,              // double []     quat0,

					debug_level);       // final int     debug_level);

			return;

		}

		

		N = vect_x.length;

		N = vect_x.length;

		this.mode = mode; // 2;

		this.mode = mode; // 2;

		samples = 4;

		samples = 4;
@@ -350,307 +363,88 @@ public class QuaternionLma { 
		}

		}

	}

	}





	private double [] getFxDerivsOld(

	public void prepareLMAMode4(

			double []         vector,

			int           mode,

			final double [][] jt, // should be null or initialized with [vector.length][]

			double        avg_height,         // 

			double [][][] vect_x, // []{{x,y,z},{a,t,r}}

			double [][][] vect_y, // []{{x,y,z},{a,t,r}}

			double []     vect_w, // one per scene

			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

			double []     quat0,

			final int     debug_level) {

			final int     debug_level) {

		switch (mode) {

		case 1:return  getFxDerivs6DofOld(

				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

		case 2:return  getFxDerivsVisualOld( // fill change

				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

		}

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

				jt[i] = new double [weights.length];

				jt[i][3 * N] = 2 * vector[i];

			}

		}

		fx[3 * N] = q0*q0 + q1*q1 + q2 * q2 + q3*q3;

		for (int i = 0; i < N; i++) {

			int i3 = 3 * i;

			final double x = x_vector[i3 + 0];

			final double y = x_vector[i3 + 1];

			final double z = x_vector[i3 + 2];

			final double s = q1 * x + q2 * y + q3 * z;

			fx[i3 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;

			fx[i3 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;

			fx[i3 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;

			if (jt != null) {

				jt[0][i3 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;

				jt[1][i3 + 0] = 2*s +    2*q1*x;

				jt[2][i3 + 0] = 2*z*q0 + 2*q1*y;

				jt[3][i3 + 0] = 2*y*q0 + 2*q1*z;

		

		

				jt[0][i3 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1;

		N = vect_x.length;

				jt[1][i3 + 1] = 2*z*q0 + 2*x*q2; 

		this.mode = mode; // 2;

				jt[2][i3 + 1] = 2*s +    2*y*q2;

		samples = 4;

				jt[3][i3 + 1] =-2*x*q0+  2*z*q2;

		samples_x = 7;



		height = avg_height;

				jt[0][i3 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;

		pure_weight = 1.0 - reg_w;

				jt[1][i3 + 2] =-2*y*q0 + 2*x*q3;

		x_vector = new double [samples_x * N];

				jt[2][i3 + 2] = 2*x*q0 + 2*y*q3;

		y_vector = new double [samples *   N + REGLEN];

				jt[3][i3 + 2] = 2*s    + 2*z*q3;

		y_inv_vector = new double [samples_x * N + REGLEN]; 		

			}

		weights =  new double [samples *   N + REGLEN];

		}

		parameters_vector = quat0.clone();

		return fx;

		double sw = 0;

	}

	private double [] getFxDerivs6DofOld(

			double []         vector,

			final double [][] jt, // should be null or initialized with [vector.length][]

			final int         debug_level) {

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

				jt[i] = new double [weights.length];

				jt[i][samples * N] = 2 * vector[i];

			}

		}

		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;

		for (int i = 0; i < N; i++) {

		for (int i = 0; i < N; i++) {

			int i7 = samples * i;

			if ((vect_x[i]== null) || (vect_y[i]== null)) {

			// translations

				for (int j = 0; j < samples; j++) {

			final double x = x_vector[i7 + 0];

					weights [samples * i + j] = 0.0;

			final double y = x_vector[i7 + 1];

			final double z = x_vector[i7 + 2];

			final double s = q1 * x + q2 * y + q3 * z;

			fx[i7 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;

			fx[i7 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;

			fx[i7 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;

			if (jt != null) {

				jt[0][i7 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;

				jt[1][i7 + 0] = 2*s +    2*q1*x;

				jt[2][i7 + 0] = 2*z*q0 + 2*q1*y;

				jt[3][i7 + 0] = 2*y*q0 + 2*q1*z;



				jt[0][i7 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1;

				jt[1][i7 + 1] = 2*z*q0 + 2*x*q2; 

				jt[2][i7 + 1] = 2*s +    2*y*q2;

				jt[3][i7 + 1] =-2*x*q0+  2*z*q2;



				jt[0][i7 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;

				jt[1][i7 + 2] =-2*y*q0 + 2*x*q3;

				jt[2][i7 + 2] = 2*x*q0 + 2*y*q3;

				jt[3][i7 + 2] = 2*s    + 2*z*q3;

			}

			// rotations

			final double r0 = x_vector[i7 + 3];

			final double r1 = x_vector[i7 + 4];

			final double r2 = x_vector[i7 + 5];

			final double r3 = x_vector[i7 + 6];

			fx[i7 + 3] = r0 * q0 - (r1 * q1 +  r2 * q2 + r3 * q3);

			fx[i7 + 4] = r1 * q0 +  r0 * q1 + (r2 * q3 - r3 * q2);

			fx[i7 + 5] = r2 * q0 +  r0 * q2 + (r3 * q1 - r1 * q3);

			fx[i7 + 6] = r3 * q0 +  r0 * q3 + (r1 * q2 - r2 * q1);

			if (jt != null) {

				jt[0][i7 + 3] =  r0;

				jt[1][i7 + 3] = -r1;

				jt[2][i7 + 3] = -r2;

				jt[3][i7 + 3] = -r3;

 

				jt[0][i7 + 4] =  r1;

				jt[1][i7 + 4] =  r0;

				jt[2][i7 + 4] = -r3;

				jt[3][i7 + 4] =  r2;



				jt[0][i7 + 5] =  r2;

				jt[1][i7 + 5] =  r3;

				jt[2][i7 + 5] =  r0;

				jt[3][i7 + 5] = -r1;



				jt[0][i7 + 6] =  r3;

				jt[1][i7 + 6] = -r2;

				jt[2][i7 + 6] =  r1;

				jt[3][i7 + 6] =  r0;

			}			

		}

		return fx;

	}

	private double [] getFxDerivsVisualOld(

			double []         vector,

			final double [][] jt, // should be null or initialized with [vector.length][]

			final int         debug_level) {

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

				jt[i] = new double [weights.length];

				jt[i][samples * N] = 2 * vector[i];

				}

				}

				for (int j = 0; j < samples_x; j++) {

					x_vector    [samples_x * i + j] = 0.0;

					y_inv_vector[samples_x * i + j] = 0.0;

				}				

				}				

		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;

		for (int i = 0; i < N; i++) {

			int i4 = samples * i;

			int i7 = samples_x * i;

			// translations

			final double x = x_vector[i7 + 0];

			final double y = x_vector[i7 + 1];

			final double z = x_vector[i7 + 2];

			final double s = q1 * x + q2 * y + q3 * z;

			final double fx_x = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;

			final double fx_y = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;

			final double fx_z = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;

			// rotations

			final double r0 = x_vector[i7 + 3];

			final double r1 = x_vector[i7 + 4];

			final double r2 = x_vector[i7 + 5];

			final double r3 = x_vector[i7 + 6];

			/* // unused

			final double fx_q0 = r0 * q0 - (r1 * q1 +  r2 * q2 + r3 * q3);

			*/

			final double fx_q1 = r1 * q0 +  r0 * q1 + (r2 * q3 - r3 * q2);

			final double fx_q2 = r2 * q0 +  r0 * q2 + (r3 * q1 - r1 * q3);

			final double fx_q3 = r3 * q0 +  r0 * q3 + (r1 * q2 - r2 * q1);

			// combined samples

			fx[i4 + 0] = fx_z / height; // Z

			fx[i4 + 1] = 2 * fx_q3;        // 2 * Q3

			fx[i4 + 2] = 2 * fx_q2 - fx_x / height; // 2 * Q2 - X / height

			fx[i4 + 3] = 2 * fx_q1 + fx_y / height; // 2 * Q1 + Y / height

				

				

			if (jt != null) {

			} else {

				final double jt_0_x = 4*x*q0 - 2*z*q2 + 2*y*q3;

				// mode=4

				final double jt_1_x = 2*s +    2*q1*x;

				double w = ((vect_w != null)?  vect_w[i] : 1.0);

				final double jt_2_x = 2*z*q0 + 2*q1*y;

				// quaternions

				final double jt_3_x = 2*y*q0 + 2*q1*z;

				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,



						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);

				final double jt_0_y = 4*y*q0 - 2*x*q3 + 2*z*q1;

				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

				final double jt_1_y = 2*z*q0 + 2*x*q2; 

						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);

				final double jt_2_y = 2*s +    2*y*q2;

				x_vector[samples_x * i + 0] = vect_x[i][0][0]; // X

				final double jt_3_y =-2*x*q0+  2*z*q2;

				x_vector[samples_x * i + 1] = vect_x[i][0][1]; // Y



				x_vector[samples_x * i + 2] = vect_x[i][0][2]; // Z

				final double jt_0_z = 4*z*q0 - 2*y*q1 + 2*x*q2;

				x_vector[samples_x * i + 3] = rot_x.getQ0();   // Q0

				final double jt_1_z =-2*y*q0 + 2*x*q3;

				x_vector[samples_x * i + 4] = rot_x.getQ1();   // Q1

				final double jt_2_z = 2*x*q0 + 2*y*q3;

				x_vector[samples_x * i + 5] = rot_x.getQ2();   // Q2

				final double jt_3_z = 2*s    + 2*z*q3;

				x_vector[samples_x * i + 6] = rot_x.getQ3();   // Q3

				/* // unused

				double [] xyz_y = new double [] {

				final double jt_0_q0 =  r0;

						vect_y[i][0][0],

				final double jt_1_q0 = -r1;

						vect_y[i][0][1],

				final double jt_2_q0 = -r2;

						vect_y[i][0][2]}; 

				final double jt_3_q0 = -r3;

				double [] quat_y = new double [] {

                */

						rot_y.getQ0(),

				final double jt_0_q1 =  r1;

						rot_y.getQ1(),

				final double jt_1_q1 =  r0;

						rot_y.getQ2(),

				final double jt_2_q1 = -r3;

						rot_y.getQ3()};

				final double jt_3_q1 =  r2;

				double [][] inv_y = invertTransRot(



						xyz_y,   // double [] xyz_src,     // transformation to apply to (was reference_xyz)

				final double jt_0_q2 =  r2;

						quat_y); // double [] quat_src);

				final double jt_1_q2 =  r3;

				System.arraycopy(inv_y[0], 0, y_inv_vector, samples_x * i,      3);

				final double jt_2_q2 =  r0;

				System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3,  4);

				final double jt_3_q2 = -r1;

				// y_vector remains all 0



				

				final double jt_0_q3 =  r3;

				for (int j = 0; j < samples; j++) {

				final double jt_1_q3 = -r2;

					weights[samples * i + j] = w;

				final double jt_2_q3 =  r1;

					sw += w;

				final double jt_3_q3 =  r0;

				// Z

				jt[0][i4 + 0] = jt_0_z / height;

				jt[1][i4 + 0] = jt_1_z / height;

				jt[2][i4 + 0] = jt_2_z / height;

				jt[3][i4 + 0] = jt_3_z / height;

				// 2 * Q3

				jt[0][i4 + 1] = 2 * jt_0_q3;

				jt[1][i4 + 1] = 2 * jt_1_q3; 

				jt[2][i4 + 1] = 2 * jt_2_q3;

				jt[3][i4 + 1] = 2 * jt_3_q3;

				// 2 * Q2 - X / height                

				jt[0][i4 + 2] = 2 * jt_0_q2 - jt_0_x / height;

				jt[1][i4 + 2] = 2 * jt_1_q2 - jt_1_x / height;

				jt[2][i4 + 2] = 2 * jt_2_q2 - jt_2_x / height;

				jt[3][i4 + 2] = 2 * jt_3_q2 - jt_3_x / height;

				// 2 * Q1 + Y / height                

				jt[0][i4 + 3] = 2 * jt_0_q1 + jt_0_y / height;

				jt[1][i4 + 3] = 2 * jt_1_q1 + jt_1_y / height;

				jt[2][i4 + 3] = 2 * jt_2_q1 + jt_2_y / height;

				jt[3][i4 + 3] = 2 * jt_3_q1 + jt_3_y / height;

			}			

		}

		return fx;

	}

	private double [] getFxDerivsVisualWrong(

			double []         vector,

			final double [][] jt, // should be null or initialized with [vector.length][]

			final int         debug_level) {

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

				jt[i] = new double [weights.length];

				jt[i][samples * N] = 2 * vector[i];

				}

				}

			}

			}

		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;

		double [] xyz_rot;

		double [] quat_rot;

		double [][] xyz_dq;

		double [][] quat_dq;

		

		for (int i = 0; i < N; i++) {

			int i4 = samples * i;

			int i7 = samples_x * i;

			// translations

			final double x = x_vector[i7 + 0];

			final double y = x_vector[i7 + 1];

			final double z = x_vector[i7 + 2];

			final double s = q1 * x + q2 * y + q3 * z;

			

			final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};

			xyz_rot = applyTo(vector, xyz);

			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};

			quat_rot = compose(vector, quat_r);

			// combined samples

			fx[i4 + 0] = xyz_rot[2] / height;                   // Z

			fx[i4 + 1] = 2 * quat_rot[3];                       // 2 * Q3

			fx[i4 + 2] = 2 * quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height

			fx[i4 + 3] = 2 * quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height

			if (jt != null) {

				xyz_dq =  applyToDQ(vector, xyz);

				quat_dq = composeDQ(quat_r);

				// Z

				jt[0][i4 + 0] = xyz_dq[0][2] / height;

				jt[1][i4 + 0] = xyz_dq[1][2] / height;

				jt[2][i4 + 0] = xyz_dq[2][2] / height;

				jt[3][i4 + 0] = xyz_dq[3][2] / height;

				// 2 * Q3

				jt[0][i4 + 1] = 2 * quat_dq[0][3];

				jt[1][i4 + 1] = 2 * quat_dq[1][3]; 

				jt[2][i4 + 1] = 2 * quat_dq[2][3];

				jt[3][i4 + 1] = 2 * quat_dq[3][3];

				// 2 * Q2 - X / height                

				jt[0][i4 + 2] = 2 * quat_dq[0][2] - xyz_dq[0][0] / height;

				jt[1][i4 + 2] = 2 * quat_dq[1][2] - xyz_dq[1][0] / height;

				jt[2][i4 + 2] = 2 * quat_dq[2][2] - xyz_dq[2][0] / height;

				jt[3][i4 + 2] = 2 * quat_dq[3][2] - xyz_dq[3][0] / height;

				// 2 * Q1 + Y / height                

				jt[0][i4 + 3] = 2 * quat_dq[0][1] + xyz_dq[0][1] / height;

				jt[1][i4 + 3] = 2 * quat_dq[1][1] + xyz_dq[1][1] / height;

				jt[2][i4 + 3] = 2 * quat_dq[2][1] + xyz_dq[2][1] / height;

				jt[3][i4 + 3] = 2 * quat_dq[3][1] + xyz_dq[3][1] / height;

		}

		}

		double k = (pure_weight)/sw;

		for (int i = 0; i < weights.length; i++) weights[i] *= k;

		weights [samples * N] = 1.0 - pure_weight;

		y_vector[samples * N] = 1.0;

		last_jt = new double [parameters_vector.length][];		

		if (debug_level > 0) {

			 debugYfX ( "Y-INV-",   // String pfx,

					 y_inv_vector); // double [] data)

			 debugYfX ( "PIMU-",   // String pfx,

						x_vector); // double [] data)

		}

		}

		return fx;

	}

	}

	

	

	

	

	// TODO: Consider adding differences between x and y for regularization (or it won't work)

	// TODO: Consider adding differences between x and y for regularization (or it won't work)

	// goal - to minimize "unneeded" rotation along the common axis

	// goal - to minimize "unneeded" rotation along the common axis

	private double [] getFxDerivs(

	private double [] getFxDerivs(
@@ -670,7 +464,12 @@ public class QuaternionLma { 
				 vector,       // double []         vector,

				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

				 debug_level); // final int         debug_level)

		case 4:return  getFxDerivsVisualMode4( // fill change

				 vector,       // double []         vector,

				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]

				 debug_level); // final int         debug_level)

		}

		}

		// remains here for mode 0

		double [] fx = new double [weights.length];

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q1 = vector[1];
@@ -820,23 +619,26 @@ public class QuaternionLma { 
				for (int j = 0; j < 4; j++) {

				for (int j = 0; j < 4; j++) {

					System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);

					System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);

				}

				}

				

				quat_dq = composeQR_QdQ(vector_r,quat_r);

				quat_dq = composeQR_QdQ(vector_r,quat_r);

				// 2 alternative ways with the same result

				if (debug_level < 1000) {

					double [][] invy_mat = qMat(inv_y[1]);

					double [][] invy_mat = qMat(inv_y[1]);

				double [][] quat_dq_local = mulMat(invy_mat, quat_dq);

					double [][] quat_dq_local = mulMat(quat_dq, invy_mat);

				

					for (int j = 0; j < 4; j++) {

						System.arraycopy(quat_dq_local[j], 0, jt[j], i7+3, 4);

					}

				} else {

					double [][] dcomp_dsecond = composeDR(inv_y[1]);

					double [][] dcomp_dsecond = composeDR(inv_y[1]);

					double [][] quat_dq_local1 = new double [4][];

					double [][] quat_dq_local1 = new double [4][];

					for (int j = 0; j < 4; j++) {

					for (int j = 0; j < 4; j++) {

//					quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);

						quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);

					quat_dq_local1[j] = mulMat(invy_mat, quat_dq[j]);

					}

					}

				

					for (int j = 0; j < 4; j++) {

					for (int j = 0; j < 4; j++) {

						System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4);

						System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4);

					}

					}

				}

				}

			}			

			}			

		}

		return fx;

		return fx;

	}

	}
@@ -910,7 +712,6 @@ public class QuaternionLma { 
		final double q1 = vector[1]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		final double q3 = vector[3];

///		final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal

		final double [] vector_r = normSign(vector);

		final double [] vector_r = normSign(vector);

		if (jt != null) {

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

			for (int i = 0; i < vector.length; i++) {
@@ -977,6 +778,94 @@ public class QuaternionLma { 
		return fx;

		return fx;

	}

	}

	

	

	private double [] getFxDerivsVisualMode4(

			double []         vector,

			final double [][] jt, // should be null or initialized with [vector.length][]

			final int         debug_level) {

		double [] fx = new double [weights.length];

		final double q0 = vector[0]; 

		final double q1 = vector[1]; 

		final double q2 = vector[2]; 

		final double q3 = vector[3];

		final double [] vector_r = normSign(vector);

		if (jt != null) {

			for (int i = 0; i < vector.length; i++) {

				jt[i] = new double [weights.length];

				jt[i][samples * N] = 2 * vector[i];

			}

		}

		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;

		double [] xyz_rot;

		double [] quat_rot;

		double [][] xyz_dq;

		double [][] quat_dq;

		double [][] inv_y = new double [][] {new double[3],new double[4]}; 			

		for (int i = 0; i < N; i++) {

			int i4 = samples * i;

			int i7 = samples_x * i;

			has_data:{

				for (int j = 0; j < samples; j++) {

					if (weights[i4+j] > 0) {

						break has_data;

					}

				}

				continue; // nothing to process for this scene 

			}

			// translations

			final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};

			// rotations

			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};

			xyz_rot = applyTo(vector, xyz);

			quat_rot = composeQR_Q(vector_r, quat_r);

			System.arraycopy(y_inv_vector, i7,   inv_y[0], 0, 3);

			System.arraycopy(y_inv_vector, i7+3, inv_y[1], 0, 4);

		    double [][] comb_y = combineTransRot( // 

		            inv_y[0],   // double [] xyz_src,     // transformation to apply to (was reference_xyz)

		            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)

		            xyz_rot,    // double [] xyz_target,  // to which is applied (was scene_xyz)

		            quat_rot);  // double [] quat_target  // to which is applied (was scene_atr)

			fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height;                   // Z

			fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3];                       // 2 * Q3

			fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height

			fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height

			

			if (jt != null) {

				xyz_dq =  applyToDQ(vector, xyz);

				double [][] xyz_dq_local = new double [xyz_dq.length][];

				for (int j = 0; j < xyz_dq.length; j++) {

					xyz_dq_local[j] =combineTransRot(

				            null,       // double [] xyz_src,     // transformation to apply to (was reference_xyz)

				            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)

				            xyz_dq[j],  // double [] xyz_target,  // to which is applied (was scene_xyz)

				            null)[0];   // double [] quat_target  // to which is applied (was scene_atr)		

				}

				quat_dq = composeQR_QdQ(vector_r,quat_r);

				double [][] invy_mat = qMat(inv_y[1]);

				double [][] quat_dq_local = mulMat(quat_dq, invy_mat);

				// Z

				jt[0][i4 + 0] = xyz_dq_local[0][2] / height;

				jt[1][i4 + 0] = xyz_dq_local[1][2] / height;

				jt[2][i4 + 0] = xyz_dq_local[2][2] / height;

				jt[3][i4 + 0] = xyz_dq_local[3][2] / height;

				// 2 * Q3

				jt[0][i4 + 1] = 2 * quat_dq_local[0][3];

				jt[1][i4 + 1] = 2 * quat_dq_local[1][3]; 

				jt[2][i4 + 1] = 2 * quat_dq_local[2][3];

				jt[3][i4 + 1] = 2 * quat_dq_local[3][3];

				// 2 * Q2 - X / height                

				jt[0][i4 + 2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;

				jt[1][i4 + 2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;

				jt[2][i4 + 2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;

				jt[3][i4 + 2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;

				// 2 * Q1 + Y / height                

				jt[0][i4 + 3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;

				jt[1][i4 + 3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;

				jt[2][i4 + 3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;

				jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;

			}			

		}

		return fx;

	}

	

	

	

	

	private double [] getYminusFxWeighted(

	private double [] getYminusFxWeighted(
@@ -1339,7 +1228,7 @@ public class QuaternionLma { 
						angles[0],angles[1],angles[2]));

						angles[0],angles[1],angles[2]));

			}

			}

			System.out.println();

			System.out.println();

		} else if (mode == 2) {

		} else { // if (mode == 2) {

			System.out.println(String.format("%3s"+

			System.out.println(String.format("%3s"+

					"\t%9s\t%9s\t%9s\t%9s", // Z, 2*Q3, 2*Q2-X, 2*Q1+Y

					"\t%9s\t%9s\t%9s\t%9s", // Z, 2*Q3, 2*Q2-X, 2*Q1+Y

					"N",pfx+"Z",pfx+"2*Q3",pfx+"2*Q2-X",pfx+"2*Q1+Y"));

					"N",pfx+"Z",pfx+"2*Q3",pfx+"2*Q2-X",pfx+"2*Q1+Y"));
@@ -1551,6 +1440,8 @@ public class QuaternionLma { 
	 * Get derivatives of the composed quaternion (compose(q,r)) by the

	 * Get derivatives of the composed quaternion (compose(q,r)) by the

	 * components of the second one (r). These derivatives do not depend

	 * components of the second one (r). These derivatives do not depend

	 * on the second quaternion, so it is not in the input.

	 * on the second quaternion, so it is not in the input.

	 * Also can be used to convert quaternion to a matrix for post-multiplying

	 * derivatives. 

	 * @param q  4 components (scalar, vector) of the quaternion being

	 * @param q  4 components (scalar, vector) of the quaternion being

	 *           applied to the second quaternion. 

	 *           applied to the second quaternion. 

	 * @return 4x4 array, where columns correspond to composition components

	 * @return 4x4 array, where columns correspond to composition components
@@ -1559,19 +1450,18 @@ public class QuaternionLma { 
	 */

	 */

	public static double [][] composeDR( // not used

	public static double [][] composeDR( // not used

			double [] q) {

			double [] q) {

		/*

		return new double [][] {                

		return new double [][] {                

			{ q[0], -q[1], -q[2], -q[3]},				                

			{ q[0], -q[1], -q[2], -q[3]},				                

			{ q[1],  q[0],  q[3], -q[2]},

			{ q[1],  q[0],  q[3], -q[2]},

			{ q[2], -q[3],  q[0],  q[1]},

			{ q[2], -q[3],  q[0],  q[1]},

			{ q[3],  q[2], -q[1],  q[0]}};

			{ q[3],  q[2], -q[1],  q[0]}};

		*/

/*		

		

		return new double [][] {                

		return new double [][] {                

			{ q[0], q[1], q[2], q[3]}, 

			{ q[0], q[1], q[2], q[3]}, 

			{-q[1], q[0],-q[3], q[2]}, 

			{-q[1], q[0],-q[3], q[2]}, 

			{-q[2], q[3], q[0],-q[1]},

			{-q[2], q[3], q[0],-q[1]},

			{-q[3],-q[2], q[1], q[0]}};

			{-q[3],-q[2], q[1], q[0]}};

*/			

	}

	}





	public static double [] addTo(

	public static double [] addTo(
@@ -1662,11 +1552,29 @@ public class QuaternionLma { 
	

	

	public static double [][] qMat(

	public static double [][] qMat(

			double [] q){

			double [] q){

		return new double [][] {                

			{ q[0], q[1], q[2], q[3]}, 

			{-q[1], q[0],-q[3], q[2]}, 

			{-q[2], q[3], q[0],-q[1]},

			{-q[3],-q[2], q[1], q[0]}};

		/*

		return new double [][] {

		return new double [][] {

			{q[0],-q[1],-q[2],-q[3]},

			{q[0],-q[1],-q[2],-q[3]},

			{q[1], q[0], q[3],-q[2]},

			{q[1], q[0], q[3],-q[2]},

			{q[2],-q[3], q[0], q[1]},

			{q[2],-q[3], q[0], q[1]},

			{q[3], q[2],-q[1], q[0]}};

			{q[3], q[2],-q[1], q[0]}};

			*/

	}



	public static double [][] transpose(

			double [][] mat){

		double [][] tmat = new double[mat[0].length][mat.length];

		for (int i = 0; i < mat.length; i++) {

			for (int j = 0; j < mat[0].length; j++) {

				tmat[j][i] = mat[i][j];

			}

		}

		return tmat;

	}

	}





}
 
}