just a working snapshot

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

@@ -111,7 +111,6 @@ public class OrientationSceneLMA {
 		return weights.length;
 	}
 	
-	
 	private double [] getFxDerivs(
 			final double []         vector,
 			final double [][] jt, // should be null or initialized with [vector.length][]
@@ -124,6 +123,117 @@ public class OrientationSceneLMA {
 			}
 		}
 
+		final Thread[] threads = ImageDtt.newThreadArray();
+		final AtomicInteger ai = new AtomicInteger(0);
+		final AtomicInteger ati = new AtomicInteger(0);
+		// first cycle - minimize per-pair errors (differences between singular values)
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					double [] qscene1 = new double[4];
+					double [] qscene2 = new double[4];
+					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()) {
+						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[1], qscene2, 0, 4);
+						double [] q2iq1q12;
+						q2iq1q12 = getPairErrQuaternion( // qderivs here tested
+								qscene1, // double [] qscene1,
+								qscene2, // double [] qscene2,
+								qpairs[npair], // double [] qpair12,
+								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(
+								fx_frag,
+								0,
+								fX,
+								fx_frag.length*npair,
+								fx_frag.length);
+						if (jt!=null) {
+							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
+							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 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];
+									}
+								}
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		ai.set(0);
+		// second cycle - regularization (average scale diffs, q1,q2,q3}
+		double [][]   pull_threads = new double [threads.length][4];
+		final int pull_index = num_pairs*4; // start of pull sums in fx
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					int nthread =  ati.getAndIncrement();
+					double []   qscene = new double[4];
+					for (int nscene = ai.getAndIncrement(); nscene < num_scenes; nscene = ai.getAndIncrement()) {
+						System.arraycopy(vector, nscene*4, qscene, 0, 4);
+						double [] qscene_norm=QuatUtils.normalize(qscene); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
+						double scale_diff = QuatUtils.norm(qscene)-1.0;
+						qscene_norm[0] = scale_diff; // {scale_diff, qn1, qn2, qn3};
+						for (int i = 0; i < 4; i++) {
+							pull_threads[nthread][i] += qscene_norm[i];
+						}
+						if (jt != null) {
+							double [][] dnq_dq = QuatUtils.dnormalize_dq(qscene);
+							double []   dscale = QuatUtils.dscale_dq(qscene);
+							dnq_dq[0] = dscale;
+							for (int npar = 0; npar <4; npar++) {
+								System.arraycopy(
+										dnq_dq[npar],
+										0 ,
+										jt[nscene*4+npar],
+										pull_index,
+										4);
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		// sum partial sums from threads
+		for (int nthread = 0; nthread < pull_threads.length; nthread++ ) {
+			for (int i = 0; i < 4; i++) {
+				fX[pull_index+i] += pull_threads[nthread][i];
+			}
+		}
+		return fX;
+	}
+
+	
+	public double [] getFxDerivs_debug(
+			final double []         vector,
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level)
+	{
+		double [] fX = new double [weights.length]; // num_pairs + vector.length];
+		if (jt != null) {
+			for (int i = 0; i < jt.length; i++) {
+				jt[i] = new double [weights.length]; // weights.length];
+			}
+		}
+
 		final Thread[] threads = ImageDtt.newThreadArray();
 		final AtomicInteger ai = new AtomicInteger(0);
 		final AtomicInteger ati = new AtomicInteger(0);
@@ -264,6 +374,9 @@ public class OrientationSceneLMA {
 		}
 		return fX;
 	}
+
+	
+	
 	
 	private double [][] getFxDerivsDelta(
 			double []         vector,

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

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

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

@@ -133,6 +133,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	public transient int                        num_scenes = -1;;   // number of scenes that made up this image
 	public transient double                     sfm_gain = Double.NaN;     // maximal SfM gain of this map
 	public transient double []                  equalize = {1,0}; // rectified value = equalize[0]*source_value+equalize[1]
+	public transient double []                  qorient = null; // quternion representing orientation+scale of the scene
 	private void writeObject(ObjectOutputStream oos) throws IOException {
 		// temporary fix:
 //		double [][] affine_clone = {affine[0].clone(), affine[1].clone()};
@@ -191,6 +192,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 //		pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
 	}
 
+	public double [] getQOrinet() {
+		return qorient;
+	}
+	public void setQOrient(double [] q) {
+		qorient = q;
+	}
 	public double [][] getERSAffine(){
 		return ers_affine;
 	}

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

@@ -41,6 +41,7 @@ public class PairwiseOrthoMatch implements Serializable {
 	public transient double []   alt_data = null;
 	public transient double []   equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]
 	public transient double []   quat = null; // relative orientation of the second scene (ERS affine removed). Will be eventually saved 
+	public transient double [][] qaffine = null; // affine relative to rotated scenes 
 	// below - not saved/restored
 	public transient boolean     ok = false; // not saved/restored
 	public transient int []      nxy = null; // not saved, just to communicate for logging
@@ -52,6 +53,13 @@ public class PairwiseOrthoMatch implements Serializable {
 		this.quat = quat;
 	}
 	
+	public double [][] getQAffine() {
+		return qaffine;
+	}
+	public void setQAffine(double [][] qaffine) {
+		this.qaffine = qaffine;
+	}
+	
 //	public PairwiseOrthoMatch() {}
 	public double getOverlap() {
 		return overlap;
@@ -120,6 +128,8 @@ public class PairwiseOrthoMatch implements Serializable {
 		pom.equalize1to0 = this.equalize1to0.clone();
 		pom.ok = this.ok;
 		pom.alt_data = this.alt_data.clone();
+		pom.quat = (quat!=null)? quat.clone() : null;
+		pom.qaffine = (qaffine!=null) ? (new double [][] {qaffine[0].clone(),qaffine[1].clone()}):null;
 		return pom;
 	}
 	/**