Debugging OrientationSceneLMA - working, will clean up

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

@@ -78,6 +78,9 @@ public class OrthoAltitudeMatch {
         
         boolean do_not_save = true;
 
+//        OrientationSceneLMA.testGetPairErrQuaternion ();
+        OrientationSceneLMA.testGetPairPairScaleDirError();
+        
 		if (test_quat0) {
 			QuatUtils.testQuatAff();
 		}

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

@@ -6286,6 +6286,11 @@ public class OrthoMapsCollection  implements Serializable{
 	public boolean altutudeMatchPairs(
 			CLTParameters clt_parameters,
 			String        orthoMapsCollection_path) {
+        //OrientationSceneLMA.testGetPairErrQuaternion ();	
+
+
+        
+        
         // Create list of all pairs (after recreating all overlaps with updated affines)
 		ArrayList<Point> pairs_list = new ArrayList<Point>();
 		boolean dbg00 = false;

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

@@ -70,6 +70,7 @@ public final class QuatUtils { // static class
 	/**
 	 * Differential d_(p*q)/dp
 	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
+	 * Seems that (11) for []x is transposed!
 	 * @param p quaternion
 	 * @param q quaternion
 	 * @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 [] 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]}  
+/*
 			{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 [] 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
 	 * https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
+	 * Seems that (11) for []x is transposed!
 	 * @param p quaternion
 	 * @param q quaternion
 	 * @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 [] q) {
 		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[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) {
+		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
 	 * @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(
 			double [] q){
 		double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
@@ -135,6 +234,26 @@ public final class QuatUtils { // static class
 			{     -q3*q0 /l3,      -q3*q1 /l3,      -q3*q2 /l3, (l2 - q3*q3)/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(
 			double [] q){
 		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}; 
 	}
 	
+	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;
+	}
+	
 	
 	
 	/**