Debugging OrientationSceneLMA - working, will clean up (767ae031) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/orthomosaic/OrientationSceneLMA.java

+377 −15

Original line number	Original line	Diff line number	Diff line
	@@ -47,7 +47,10 @@ public class OrientationSceneLMA {
	private double [][] last_jt = null;		private double [][] last_jt = null;

	public boolean last3only = false; // true; // debug feature		public boolean last3only = false; // true; // debug feature
	public double delta= 1e-7;		public double delta= 1e-5; // 7;
			public boolean debug_deriv = true;
			public int debug_width = 12;
			public int debug_decimals = 9;


	public int prepareLMA(		public int prepareLMA(
	@@ -131,15 +134,38 @@ public class OrientationSceneLMA {
	double [] qscene1 = new double[4];		double [] qscene1 = new double[4];
	double [] qscene2 = new double[4];		double [] qscene2 = new double[4];
	double [][][] derivs = (jt!=null) ? (new double [2][][]): null;		double [][][] derivs = (jt!=null) ? (new double [2][][]): null;
			double [][][] qderivs = (jt != null) ? (new double [2][][]) : null;

	for (int npair = ai.getAndIncrement(); npair < num_pairs; npair = ai.getAndIncrement()) {		for (int npair = ai.getAndIncrement(); npair < num_pairs; npair = ai.getAndIncrement()) {
	int [] scene_ind = {4 * cpairs[npair][0], 4 * cpairs[npair][1]};		int [] scene_ind = {4 * cpairs[npair][0], 4 * cpairs[npair][1]};
	System.arraycopy(vector, scene_ind[0], qscene1, 0, 4);		System.arraycopy(vector, scene_ind[0], qscene1, 0, 4);
	System.arraycopy(vector, scene_ind[1], qscene2, 0, 4);		System.arraycopy(vector, scene_ind[1], qscene2, 0, 4);
	double [] fx_frag =getPairScaleDirError( //{scale-1, qn1,qn2,qn3}		double [] q2iq1q12;
			if (debug_deriv && (debug_level>10)) {
			q2iq1q12 = getPairErrQuaternion(
			qscene1, // double [] qscene1,
			qscene2, // double [] qscene2,
			qpairs[npair], // double [] qpair12,
			qderivs, // double [][][] qderivs)
			delta, // double delta,
			debug_width, // int fmt_width,
			debug_decimals, // int fmt_decimals,
			npair); // int npair);


			} else {
			q2iq1q12 = getPairErrQuaternion( // qderivs here tested
	qscene1, // double [] qscene1,		qscene1, // double [] qscene1,
	qscene2, // double [] qscene2,		qscene2, // double [] qscene2,
	qpairs[npair], // double [] qpair12,		qpairs[npair], // double [] qpair12,
	derivs); // double [][][] derivs)		qderivs); // double [][][] qderivs)
			}


			double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
			double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
			double [] fx_frag = {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};

	System.arraycopy(		System.arraycopy(
	fx_frag,		fx_frag,
	0,		0,
	@@ -147,14 +173,46 @@ public class OrientationSceneLMA {
	fx_frag.length*npair,		fx_frag.length*npair,
	fx_frag.length);		fx_frag.length);
	if (jt!=null) {		if (jt!=null) {
			double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
			qscene1, // double [] q1,
			qscene2, // double [] q2,
			qpairs[npair],
			delta); // double delta)
			// printDerivDelta("npair="+npair+" qderivs[0], delta="+delta, qderivs[0], qderivs_delta[0],debug_width,debug_decimals);
			// printDerivDelta("npair="+npair+" qderivs[1], delta="+delta, qderivs[1], qderivs_delta[1],debug_width,debug_decimals);


			double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
			double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
			if (debug_deriv) {
			double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
			printDerivDelta("npair="+npair+" dq2iq1q12_norm, delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
			double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
			printDerivDelta("npair="+npair+" dscale_dq, delta="+delta, dscale, dscale_delta,debug_width,debug_decimals);
			}
			dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
			if (debug_deriv) {
			double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
			double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
			dq2iq1q12_norm_delta[0] = dscale_delta; // replace first row (q_norm/dq) with dsca/e/dq
			printDerivDelta("npair="+npair+" dq2iq1q12_normD,delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
			}
			derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
			derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq2 (scene 2)

	for (int iscene = 0; iscene <2; iscene++) {		for (int iscene = 0; iscene <2; iscene++) {
	for (int npar = 0; npar <4; npar++) {		for (int npar = 0; npar <4; npar++) {
			for (int i = 0; i < 4; i++) {
			jt[scene_ind[iscene]+npar][fx_frag.length*npair+i] = derivs[iscene][i][npar];
			}
			/*
	System.arraycopy(		System.arraycopy(
	derivs[iscene][npar], // null		derivs[iscene][npar], // null
	0,		0,
	jt[scene_ind[iscene]+npar],		jt[scene_ind[iscene]+npar],
	fx_frag.length*npair,		fx_frag.length*npair,
	fx_frag.length);		fx_frag.length);
			*/
	}		}
	}		}
	}		}
	@@ -268,8 +326,9 @@ public class OrientationSceneLMA {
	errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);		errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
	}		}
	}		}
	for (int i = 0; i < vector.length; i++) {		System.out.print(String.format("%3s","err"));
	System.out.print("\t\t");		for (int i = 0; i < 2* vector.length; i++) {
			System.out.print(String.format("\t%12s",""));
	}		}
	for (int i = 0; i < vector.length; i++) {		for (int i = 0; i < vector.length; i++) {
	System.out.print(String.format("\t%12.9f",errors[i]));		System.out.print(String.format("\t%12.9f",errors[i]));
	@@ -647,7 +706,7 @@ public class OrientationSceneLMA {
	* @return		* @return
	*/		*/

	public static double [] getPairErrQuaternion(		public static double [] getPairErrQuaternion0(
	double [] qscene1,		double [] qscene1,
	double [] qscene2,		double [] qscene2,
	double [] qpair12,		double [] qpair12,
	@@ -668,11 +727,240 @@ public class OrientationSceneLMA {
	return q2iq1q12;		return q2iq1q12;
	}		}

			public static double [] getPairErrQuaternion(
			double [] q1,
			double [] q2,
			double [] q12,
			double [][][] qderivs) {
			double [] iq2 = QuatUtils.invert(q2);
			double [] iq2q1 = QuatUtils.multiply(iq2, q1);
			double [] iq2q1q12 = QuatUtils.multiply(iq2q1, q12);
			if (qderivs != null) {
			double [][] diq2_dq2 = QuatUtils.d_invert_dq(q2);

			double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
			double [][] diq2q1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq2);

			double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iq2);
			double [][] diq2q1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq1);
			qderivs[0] = diq2q1q12_dq1;
			qderivs[1] = diq2q1q12_dq2;
			}
			return iq2q1q12;
			}

			public static double [] getPairErrQuaternion( // test only
			double [] q1,
			double [] q2,
			double [][][] qderivs) {
			double [] iq2 = QuatUtils.invert(q2);
			double [] iq2q1 = QuatUtils.multiply(iq2, q1);
			if (qderivs != null) {
			double [][] diq2_dq2 = QuatUtils.d_invert_dq(q2);
			double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
			double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iq2);
			qderivs[0] = diq2q1_dq1;
			qderivs[1] = diq2q1_dq2;
			}
			return iq2q1;
			}

			public static double [][][] getPairErrQuaternionDelta( // test only
			double [] q1,
			double [] q2,
			double delta) {
			double [][][] qderivs = new double [2][4][4];
			for (int nscene = 0; nscene <2; nscene++) {
			for (int npar = 0; npar < 4; npar++) {
			double [][] vpm = new double [][] {q1.clone(),q2.clone()};
			vpm[nscene][npar] += 0.5*delta;
			double [] qd_p = getPairErrQuaternion(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			null); // double [][][] qderivs)
			vpm[nscene][npar]-= delta;
			double [] qd_m = getPairErrQuaternion(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			null); // double [][][] qderivs)
			for (int i = 0; i < 4; i++) {
			qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
			}
			// System.out.println("nscene="+nscene+", npar="+npar);
			}
			}
			return qderivs;
			}



			public static void testGetPairErrQuaternion () {
			double delta = 1e-5;
			int debug_width = 12,debug_decimals=9;
			double [] q1 = {0.9995, 0.01, 0.015, 0.02};
			double [] q2 = {0.999, 0.02, 0.01, 0.015};
			double [] q12 = {1.1, 0.1, 0.15, -0.2};
			double [][][] qderivs = new double [2][4][4];
			for (int n = 0; n < 10; n++) {
			getPairErrQuaternion( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			qderivs, // double [][][] qderivs)
			delta, // double delta,
			debug_width, // int fmt_width,
			debug_decimals, // int fmt_decimals,
			0); // int npair) { //
			double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			delta); // double delta)

			printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
			printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
			System.out.println();
			}
			}


			public static void testGetPairPairScaleDirError () {
			double delta = 1e-5;
			int debug_width = 12,debug_decimals=9;
			double [] q1 = {0.9995, 0.01, 0.015, 0.02};
			double [] q2 = {0.999, 0.02, 0.01, 0.015};
			double [] q12 = {1.1, 0.1, 0.15, -0.2};
			double [][][] qderivs = new double [2][4][4];
			for (int n = 0; n < 10; n++) {
			/*
			getPairErrQuaternion( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			qderivs); // double [][][] qderivs)
			*/
			/*
			getPairErrQuaternion( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			qderivs, // double [][][] qderivs)
			delta, // double delta,
			debug_width, // int fmt_width,
			debug_decimals, // int fmt_decimals,
			0); // int npair) { //
			*/
			getPairScaleDirError(
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			qderivs); // double [][][] qderivs)
			/*
			double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			delta); // double delta)
			*/
			double [][][] qderivs_delta = getPairScaleDirError( // test only
			q1, // double [] q1,
			q2, // double [] q2,
			q12,
			delta); // double delta)

			printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
			printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
			System.out.println();
			}
			}






			public static double [] getPairErrQuaternion(
			double [] qscene1,
			double [] qscene2,
			double [] qpair12,
			double [][][] qderivs,
			double delta,
			int fmt_width,
			int fmt_decimals,
			int npair) { //
			double [] iqscene2 = QuatUtils.invert(qscene2);
			double [] iq2q1 = QuatUtils.multiply(iqscene2, qscene1);
			double [] q2iq1q12 = QuatUtils.multiply(iq2q1, qpair12);
			if (qderivs != null) {
			double [][] diqscene2_dq2 = QuatUtils.d_invert_dq(qscene2);
			{
			double [][] diqscene2_dq2_delta=QuatUtils.d_invert_dq(qscene2,delta);
			printDerivDelta("npair="+npair+" diqscene2_dq2, delta="+delta, diqscene2_dq2, diqscene2_dq2_delta,fmt_width,fmt_decimals);
			}
			double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1), diqscene2_dq2);
			{
			double [][] diq2q1_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1, delta), diqscene2_dq2);
			printDerivDelta("npair="+npair+" diq2q1_dq2, delta="+delta, diq2q1_dq2, diq2q1_dq2_delta,fmt_width,fmt_decimals);
			}
			double [][] dq2iq1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq2);
			{
			double [][] dq2iq1q12_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq2);
			printDerivDelta("npair="+npair+" dq2iq1q12_dq2, delta="+delta, dq2iq1q12_dq2, dq2iq1q12_dq2_delta,fmt_width,fmt_decimals);
			}
			double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iqscene2,qscene1);
			{
			double [][] diq2q1_dq1_delta= QuatUtils.d_pq_dq(iqscene2,qscene1,delta);
			printDerivDelta("npair="+npair+" diq2q1_dq1, delta="+delta, diq2q1_dq1, diq2q1_dq1_delta,fmt_width,fmt_decimals);
			}
			double [][] dq2iq1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq1);
			{
			double [][] dq2iq1q12_dq1_dq1_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq1);
			printDerivDelta("npair="+npair+" dq2iq1q12_dq1, delta="+delta, dq2iq1q12_dq1, dq2iq1q12_dq1_dq1_delta,fmt_width,fmt_decimals);
			}
			qderivs[0] = dq2iq1q12_dq1;
			qderivs[1] = dq2iq1q12_dq2;
			}
			return q2iq1q12;
			}


			public static double [][][] getPairErrQuaternionDelta(
			double [] qscene1,
			double [] qscene2,
			double [] qpair12,
			double delta) {
			double [][][] qderivs = new double [2][4][4];
			for (int nscene = 0; nscene <2; nscene++) {
			for (int npar = 0; npar < 4; npar++) {
			double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
			vpm[nscene][npar] += 0.5*delta;
			double [] qd_p = getPairErrQuaternion(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			qpair12,// double [][][] qderivs),
			null); // double [][][] qderivs)
			vpm[nscene][npar]-= delta;
			double [] qd_m = getPairErrQuaternion(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			qpair12,// double [][][] qderivs),
			null); // double [][][] qderivs)
			for (int i = 0; i < 4; i++) {
			qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
			}
			}
			}
			return qderivs;
			}



	public static double [] getPairScaleDirError(		public static double [] getPairScaleDirError(
	double [] qscene1,		double [] qscene1,
	double [] qscene2,		double [] qscene2,
	double [] qpair12,		double [] qpair12,
	double [][][] derivs) {		double [][][] derivs) {
			boolean truncate = false; // true;
	double [][][] qderivs = (derivs != null) ? (new double [2][][]) : null;		double [][][] qderivs = (derivs != null) ? (new double [2][][]) : null;
	double [] q2iq1q12 = getPairErrQuaternion(		double [] q2iq1q12 = getPairErrQuaternion(
	qscene1, // double [] qscene1,		qscene1, // double [] qscene1,
	@@ -682,15 +970,89 @@ public class OrientationSceneLMA {
	double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)		double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
	double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;		double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
	if (derivs != null) {		if (derivs != null) {
	double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12_norm);		if (truncate) {
	double [] dscale = QuatUtils.dscale_dq(q2iq1q12_norm);		derivs[0] = qderivs[0];
			derivs[1] = qderivs[1];

			} else {
			double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
			double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
	dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq		dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
	derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)		derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
	derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq1 (scene 2)		derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq1 (scene 2)
	}		}
			}
			if (truncate) return q2iq1q12;
	return new double [] {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};		return new double [] {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};
	}		}




			public static double [][][] getPairScaleDirError(
			double [] qscene1,
			double [] qscene2,
			double [] qpair12,
			double delta) {
			double [][][] sd_derivs = new double [2][4][4];
			for (int nscene = 0; nscene <2; nscene++) {
			for (int npar = 0; npar < 4; npar++) {
			double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
			vpm[nscene][npar] += 0.5*delta;
			double [] qd_p = getPairScaleDirError(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			qpair12,// double [][][] qderivs),
			null); // double [][][] qderivs)
			vpm[nscene][npar]-= delta;
			double [] qd_m = getPairScaleDirError(
			vpm[0], // double [] qscene1,
			vpm[1], // double [] qscene2,
			qpair12,// double [][][] qderivs),
			null); // double [][][] qderivs)
			for (int i = 0; i < 4; i++) {
			sd_derivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
			}
			}
			}
			return sd_derivs;
			}




			public static void printDerivDelta(String name, double [] deriv, double [] delta, int width, int decimals) {
			double max_err = 0;
			double [] diff= new double [deriv.length];
			String fmt ="\t%"+width+"."+decimals+"f";
			for (int i = 0; i < diff.length; i++) {
			diff[i] = deriv[i]-delta[i];
			max_err = Math.max(max_err, Math.abs(diff[i]));
			}
			System.out.println (name+", max_err = "+max_err);
			System.out.print ("deriv ");
			for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, deriv[i]));
			System.out.println();
			System.out.print ("delta ");
			for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, delta[i]));
			System.out.println();
			System.out.print (" diff ");
			for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, diff[i]));
			System.out.println();
			}

			public static void printDerivDelta(String name, double [][] deriv2, double [][] delta2, int width, int decimals) {
			int l = deriv2[0].length;
			double [] deriv = new double [deriv2.length*l];
			double [] delta = new double [deriv.length];
			for (int i = 0; i < deriv2.length; i++) {
			for (int j = 0; j <l; j++) {
			deriv[i*l+j] = deriv2[i][j];
			delta[i*l+j] = delta2[i][j];
			}
			}
			printDerivDelta(name, deriv, delta, width, decimals);
			}


	}		}

src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java

+3 −0

Original line number	Original line	Diff line number	Diff line
	@@ -78,6 +78,9 @@ public class OrthoAltitudeMatch {

	boolean do_not_save = true;		boolean do_not_save = true;

			// OrientationSceneLMA.testGetPairErrQuaternion ();
			OrientationSceneLMA.testGetPairPairScaleDirError();

	if (test_quat0) {		if (test_quat0) {
	QuatUtils.testQuatAff();		QuatUtils.testQuatAff();
	}		}

src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java

+6 −1

Original line number	Original line	Diff line number	Diff line
	@@ -6286,6 +6286,11 @@ public class OrthoMapsCollection implements Serializable{
	public boolean altutudeMatchPairs(		public boolean altutudeMatchPairs(
	CLTParameters clt_parameters,		CLTParameters clt_parameters,
	String orthoMapsCollection_path) {		String orthoMapsCollection_path) {
			//OrientationSceneLMA.testGetPairErrQuaternion ();




	// Create list of all pairs (after recreating all overlaps with updated affines)		// Create list of all pairs (after recreating all overlaps with updated affines)
	ArrayList<Point> pairs_list = new ArrayList<Point>();		ArrayList<Point> pairs_list = new ArrayList<Point>();
	boolean dbg00 = false;		boolean dbg00 = false;

src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java

+134 −0

Original line number	Original line	Diff line number	Diff line
	@@ -70,6 +70,7 @@ public final class QuatUtils { // static class
	/**		/**
	* Differential d_(p*q)/dp		* Differential d_(p*q)/dp
	* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf		* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
			* Seems that (11) for []x is transposed!
	* @param p quaternion		* @param p quaternion
	* @param q quaternion		* @param q quaternion
	* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,		* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,
	@@ -79,16 +80,57 @@ public final class QuatUtils { // static class
	double [] p,		double [] p,
	double [] q) {		double [] q) {
	return new double[][] {		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]}
			/*
	{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 [][] d_pq_dp(
			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]}
	};		};
	}		}



			public static double [][] d_pq_dp(
			double [] p,
			double [] q,
			double delta) {
			double [][] d_pq_dp_delta=new double[4][4];
			for (int npar = 0; npar < 4; npar++) {
			double [] vpm = p.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = multiply(vpm, q);
			vpm[npar] -= delta;
			double [] qd_m = multiply(vpm, q);
			for (int i = 0; i < 4; i++) {
			d_pq_dp_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
			}
			return d_pq_dp_delta;
			}



	/**		/**
	* Differential d_(p*q)/dq		* Differential d_(p*q)/dq
	* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf		* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
			* Seems that (11) for []x is transposed!
	* @param p quaternion		* @param p quaternion
	* @param q quaternion		* @param q quaternion
	* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,		* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,
	@@ -98,13 +140,51 @@ public final class QuatUtils { // static class
	double [] p,		double [] p,
	double [] q) {		double [] q) {
	return new double[][] {		return new double[][] {
			{p[0],-p[1],-p[2], -p[3]},
			{p[1], p[0], p[3], -p[2]},
			{p[2],-p[3], p[0], p[1]},
			{p[3], p[2],-p[1], p[0]}
			/*
	{p[0],-p[1],-p[2], -p[3]},		{p[0],-p[1],-p[2], -p[3]},
	{p[1], p[0],-p[3], p[2]},		{p[1], p[0],-p[3], p[2]},
	{p[2], p[3], p[0], -p[1]},		{p[2], p[3], p[0], -p[1]},
	{p[3],-p[2], p[1], p[0]}		{p[3],-p[2], p[1], p[0]}

			*/
	};		};
	}		}

			public static double [][] d_pq_dq(
			double [] p) {
			return new double[][] {
			{p[0],-p[1],-p[2], -p[3]},
			{p[1], p[0], p[3], -p[2]},
			{p[2],-p[3], p[0], p[1]},
			{p[3], p[2],-p[1], p[0]} };
			}



			public static double [][] d_pq_dq(
			double [] p,
			double [] q,
			double delta) {
			double [][] d_pq_dq_delta=new double[4][4];
			for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = multiply(p, vpm);
			vpm[npar] -= delta;
			double [] qd_m = multiply(p, vpm);
			for (int i = 0; i < 4; i++) {
			d_pq_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
			}
			return d_pq_dq_delta;
			}



	/**		/**
	* Derivative of inverse quanternion		* Derivative of inverse quanternion
	* @param q quternion		* @param q quternion
	@@ -123,6 +203,25 @@ public final class QuatUtils { // static class
	};		};
	}		}

			public static double [][] d_invert_dq(
			double [] q,
			double delta) {
			double [][] d_invert_dq_delta=new double[4][4];
			for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = invert(vpm);
			vpm[npar] -= delta;
			double [] qd_m = invert(vpm);
			for (int i = 0; i < 4; i++) {
			d_invert_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
			}
			return d_invert_dq_delta;
			}



	public static double [][] dnormalize_dq(		public static double [][] dnormalize_dq(
	double [] q){		double [] q){
	double q0= q[0],q1=q[1],q2=q[2],q3=q[3];		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
	@@ -135,6 +234,26 @@ public final class QuatUtils { // static class
	{ -q3q0 /l3, -q3q1 /l3, -q3q2 /l3, (l2 - q3q3)/l3}		{ -q3q0 /l3, -q3q1 /l3, -q3q2 /l3, (l2 - q3q3)/l3}
	};		};
	}		}

			public static double [][] dnormalize_dq(
			double [] q,
			double delta) {
			double [][] dnormalize_dq_delta=new double[4][4];
			for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] += 0.5*delta;
			double [] qd_p = normalize(vpm);
			vpm[npar] -= delta;
			double [] qd_m = normalize(vpm);
			for (int i = 0; i < 4; i++) {
			dnormalize_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
			}
			}
			return dnormalize_dq_delta;
			}



	public static double [] dscale_dq(		public static double [] dscale_dq(
	double [] q){		double [] q){
	double q0= q[0],q1=q[1],q2=q[2],q3=q[3];		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
	@@ -142,6 +261,21 @@ public final class QuatUtils { // static class
	return new double [] {q0/l, q1/l, q2/l,q3/l};		return new double [] {q0/l, q1/l, q2/l,q3/l};
	}		}

			public static double [] dscale_dq(
			double [] q,
			double delta) {
			double [] dscale_dq_delta=new double[4];
			for (int npar = 0; npar < 4; npar++) {
			double [] vpm = q.clone();
			vpm[npar] += 0.5*delta;
			double qd_p = norm(vpm);
			vpm[npar] -= delta;
			double qd_m = norm(vpm);
			dscale_dq_delta[npar] = (qd_p-qd_m)/delta;
			}
			return dscale_dq_delta;
			}



	/**		/**

Admin message