Debugged QuaternionLma.MODE_COMBO_LOCAL (3f84a242) · Commits · Elphel / imagej-elphel

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

+34 −16

Original line number	Diff line number	Diff line
		@@ -257,6 +257,7 @@ public class QuadCLTCPU {
		double [] rms, // null or double[5];
		int debugLevel
		) {
		boolean use_scale_y = false;
		// final boolean use3 = (quat_lma_mode == 3); // true;
		double lambda = clt_parameters.imp.quat_lambda; // 0.1;
		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
		@@ -276,20 +277,44 @@ public class QuadCLTCPU {
		vect_x[nscene] = ims_xyzatr[nscene];
		vect_y[nscene] = xyzatr[nscene];
		}
		double [][][] vect_y_scaled = QuaternionLma.scaleXYZ(
		// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
		double [][][] vect_y_scaled;
		if (use_scale_y) {
		vect_y_scaled= QuaternionLma.scaleXYZ(
		vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
		vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
		new int [] {early_index, last_index}); // int [] first_last){
		if ((quat_lma_mode == quaternionLma.MODE_COMBO) \|\| (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
		} else {
		vect_y_scaled = vect_y;
		}
		if ((quat_lma_mode == QuaternionLma.MODE_COMBO) \|\| (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		avg_height, // double avg_height,
		vect_x, // double [][][] vect_x,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [] vect_w, all same weight
		translation_weight, // double translation_weight, // 0.0 ... 1.0;
		0.0, // reg_w, // double reg_w, // regularization weight [0..1)
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
		if (mn_mx_diag[0]quat_max_ratioquat_max_ratio < mn_mx_diag[1]) {
		double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
		reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
		if (debugLevel > -1) {
		System.out.println("==== Calculated reg_w = "+reg_w);
		}
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		avg_height, // double avg_height,
		vect_x, // double [][][] vect_x,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [] vect_w, all same weight
		reg_w, // double reg_w, // regularization weight [0..1)
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		}

		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
		@@ -320,14 +345,6 @@ public class QuadCLTCPU {
		debugLevel); // int debug_level)
		}
		} else {
		/*
		double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
		double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		vect_x[nscene] = ims_xyzatr[nscene][0];
		vect_y[nscene] = xyzatr[nscene][0];
		}
		*/
		quaternionLma.prepareLMA(
		vect_x, // quat_lma_xyz, // double [][] vect_x,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		@@ -493,7 +510,8 @@ public class QuadCLTCPU {
		}
		double [] rms = new double[5];
		double [] quat = new double[4];
		int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
		// int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
		int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
		int debug_lev = debugLevel; // 3;
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
		double translation_weight = 1.0 / (avg_z + 1.0);

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

+183 −24

Original line number	Diff line number	Diff line
		@@ -479,7 +479,7 @@ public class QuaternionLma {
		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 reg_w, // regularization weight [0..1)
		double [] quat0,
		final int debug_level) {
		if (mode != MODE_COMBO) {
		@@ -501,9 +501,10 @@ public class QuaternionLma {
		samples_x = 7;
		height = avg_height;
		pure_weight = 1.0 - reg_w;
		int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
		x_vector = new double [samples_x * N];
		y_vector = new double [samples * N + REGLEN];
		weights = new double [samples * N + REGLEN];
		y_vector = new double [samples * N + extra_samples];
		weights = new double [samples * N + extra_samples];
		parameters_vector = quat0.clone();
		double sw = 0;
		for (int i = 0; i < N; i++) {
		@@ -545,8 +546,15 @@ public class QuaternionLma {
		}
		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;
		if (extra_samples > 0) {
		double w = (1.0 - pure_weight)/parameters_vector.length;
		for (int i = 0; i < parameters_vector.length; i++) {
		weights [samples * N + i] = w;
		y_vector[samples * N] = 0.0; // or target value
		}
		}
		// 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 ( "", // String pfx,
		@@ -556,7 +564,7 @@ public class QuaternionLma {
		}
		}

		public void prepareLMAMode4(
		public void prepareLMAMode4( // MODE_COMBO_LOCAL
		int mode,
		double avg_height, //
		double [][][] vect_x, // []{{x,y,z},{a,t,r}}
		@@ -572,10 +580,11 @@ public class QuaternionLma {
		samples_x = 7;
		height = avg_height;
		pure_weight = 1.0 - reg_w;
		int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
		x_vector = new double [samples_x * N];
		y_vector = new double [samples * N + REGLEN];
		y_inv_vector = new double [samples_x * N + REGLEN];
		weights = new double [samples * N + REGLEN];
		y_vector = new double [samples * N + extra_samples];
		y_inv_vector = new double [samples_x * N + extra_samples];
		weights = new double [samples * N + extra_samples];
		parameters_vector = quat0.clone();
		double sw = 0;
		for (int i = 0; i < N; i++) {
		@@ -627,8 +636,15 @@ public class QuaternionLma {
		}
		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;
		if (extra_samples > 0) {
		double w = (1.0 - pure_weight)/parameters_vector.length;
		for (int i = 0; i < parameters_vector.length; i++) {
		weights [samples * N + i] = w;
		y_vector[samples * N] = 0.0; // or target value
		}
		}
		// 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,
		@@ -640,6 +656,7 @@ public class QuaternionLma {

		// TODO: Consider adding differences between x and y for regularization (or it won't work)
		// goal - to minimize "unneeded" rotation along the common axis
		// updated for MODE_XYZQ/3, MODE_COMBO_LOCAL/3 ( /3 - only 3 quaternion components
		private double [] getFxDerivs(
		double [] vector,
		final double [][] jt, // should be null or initialized with [vector.length][]
		@@ -678,10 +695,17 @@ public class QuaternionLma {
		debug_level); // final int debug_level)
		}
		case MODE_COMBO_LOCAL:
		if (parameters_vector.length < 4) { // updated for [3]
		return getFxDerivsVisualMode43( // fill change
		vector, // double [] vector,
		jt, // final double [][] jt, // should be null or initialized with [vector.length][]
		debug_level); // final int debug_level)
		} else {
		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)
		}
		case MODE_COMPASS:return getFxDerivsCompass( // fill change
		vector, // double [] vector,
		jt, // final double [][] jt, // should be null or initialized with [vector.length][]
		@@ -1307,23 +1331,23 @@ public class QuaternionLma {
		return fx;
		}

		private double [] getFxDerivsVisualMode4(
		private double [] getFxDerivsVisualMode4( // seems rotation is opposite sign
		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 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];
		// jt[i][samples * N] = 2 * vector[i];
		}
		}
		fx[samples * N] = q0q0 + q1q1 + q2q2 + q3q3;
		// fx[samples * N] = q0q0 + q1q1 + q2q2 + q3q3;
		double [] xyz_rot;
		double [] quat_rot;
		double [][] xyz_dq;
		@@ -1393,9 +1417,144 @@ public class QuaternionLma {
		jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
		}
		}
		if (weights.length > N*samples) {
		for (int i = 0; i < vector.length; i++) {
		fx[samples*N + i] = vector[i];
		if (jt != null) {
		jt[i][samples*N + i] = 1.0;
		}
		}
		}
		return fx;
		}

		private double [] getFxDerivsVisualMode43(
		double [] vector3,
		final double [][] jt, // should be null or initialized with [vector.length][]
		final int debug_level) {
		double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
		double [] fx = new double [weights.length];
		double [] qn = new double[4];
		final double [] vector_r = normSign(vector).clone(); // should be already q0>0
		qNorm (vector_r, qn); // calculates qn // normalized
		final double [][] dQn_dQ123 = dQ_dQ123(vector3);
		if (jt != null) {
		for (int i = 0; i < jt.length; i++) {
		jt[i] = new double [weights.length];
		}
		}
		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
		/*
		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, false); // true);
		double [][] invy_mat = qMat(inv_y[1]);
		double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
		double [][] d_dQn = new double[4][4];

		// Z
		d_dQn[0][0] = xyz_dq_local[0][2] / height;
		d_dQn[1][0] = xyz_dq_local[1][2] / height;
		d_dQn[2][0] = xyz_dq_local[2][2] / height;
		d_dQn[3][0] = xyz_dq_local[3][2] / height;
		/*
		// why all "-2"?
		// 2 * Q3
		d_dQn[0][1] = -2 * quat_dq_local[0][3];
		d_dQn[1][1] = -2 * quat_dq_local[1][3];
		d_dQn[2][1] = -2 * quat_dq_local[2][3];
		d_dQn[3][1] = -2 * quat_dq_local[3][3];
		// 2 * Q2 - X / height
		d_dQn[0][2] = -2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
		d_dQn[1][2] = -2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
		d_dQn[2][2] = -2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
		d_dQn[3][2] = -2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
		// 2 * Q1 + Y / height
		d_dQn[0][3] = -2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
		d_dQn[1][3] = -2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
		d_dQn[2][3] = -2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
		d_dQn[3][3] = -2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
		*/

		// 2 * Q3
		d_dQn[0][1] = 2 * quat_dq_local[0][3];
		d_dQn[1][1] = 2 * quat_dq_local[1][3];
		d_dQn[2][1] = 2 * quat_dq_local[2][3];
		d_dQn[3][1] = 2 * quat_dq_local[3][3];
		// 2 * Q2 - X / height
		d_dQn[0][2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
		d_dQn[1][2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
		d_dQn[2][2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
		d_dQn[3][2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
		// 2 * Q1 + Y / height
		d_dQn[0][3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
		d_dQn[1][3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
		d_dQn[2][3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
		d_dQn[3][3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;

		//dQn_dQ123
		double [][] d_dq = mulMat(dQn_dQ123, d_dQn);
		for (int j = 0; j < d_dq.length; j++) { // 3
		System.arraycopy(d_dq[j], 0, jt[j], i4, samples);
		}
		}
		}
		if (weights.length > N*samples) {
		for (int i = 0; i < vector3.length; i++) {
		fx[samples*N + i] = vector3[i];
		if (jt != null) {
		jt[i][samples*N + i] = 1.0;
		}
		}
		}

		return fx;
		}

		private double [] getYminusFxWeighted(
		final double [] fx,

Admin message