First working version of IMU/LWIR positioning fusion

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

@@ -27,6 +27,9 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.concurrent.atomic.AtomicInteger;
 
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
+
 import com.elphel.imagej.calibration.CalibrationFileManagement;
 import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.cameras.ColorProcParameters;
@@ -2068,7 +2071,7 @@ public class Interscene {
 
         // optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
         // double         avg_rlen
-        readjust_xy_ims = false;
+//        readjust_xy_ims = false;
         
         if (lpf_xy && (reg_weight_xy > 0.0)) {
         	scenes_xyzatr_pull = QuadCLT.refineFromLPF(
@@ -2082,6 +2085,62 @@ public class Interscene {
             		earliest_scene, // int           early_index,
             		last_scene);    // int           last_index)
         } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
+			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
+					clt_parameters, // final CLTParameters clt_parameters,
+					quadCLTs,       // final QuadCLT[]     quadCLTs,
+					ref_index,      // final int           ref_index,
+					null,           // double [][][]       dxyzatr,
+					earliest_scene, // final int           early_index,
+					last_scene      //(quadCLTs.length -1) // int           last_index,
+		    		);
+	    	double [] rms = new double[5];
+	    	double [] quat = new double[4];
+	    	int quat_lma_mode = 2; // 1; // 2;
+	    	int debug_lev = debugLevel; // 3;
+//	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
+	    	double  translation_weight =   1.0 / (avg_z + 1.0);
+			
+			//scenes_xyzatr
+        	
+	    	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
+	    			clt_parameters,     // CLTParameters clt_parameters,
+	    			quat_lma_mode,      // int quat_lma_mode,
+	    			avg_z,              // double        avg_height,
+	    			translation_weight, // double        translation_weight,
+	    			quadCLTs,           // QuadCLT[]     quadCLTs,
+	    			scenes_xyzatr,      // double [][][] xyzatr,
+	    			pimu_xyzatr,        // double [][][] ims_xyzatr,
+	    			ref_index,          // int           ref_index,
+	    			earliest_scene,     // int           early_index,
+	    			last_scene,         // int           last_index,
+	    			rms,                // double []     rms, // null or double[5];
+	    			quat,               // double []     quaternion, // null or double[4]       
+	    			debug_lev);         // int           debugLevel
+	    	if (rotated_xyzatr == null) {
+	    		System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() failed");
+	    	}  else {
+	    		System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() finished in "+((int) rms[4])+ "iterations.");
+	    		System.out.println("rms="+rms[0]+" ("+rms[2]+"), pure rms="+rms[1]+" ("+rms[3]+")");
+	    		if (debug_lev > -3) {
+	    	    	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
+	    			double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+	    			double []  degrees = new double[3];
+	    			for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
+	    			System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
+	    			System.out.println("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
+	    			System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
+	    		}
+	    		
+	    		// TODO: Use quat[] to update corrections of the IMU relative to the camera?
+	    		for (int nscene = earliest_scene; nscene <= last_scene; nscene++) if (scenes_xyzatr[nscene] != null) {
+	    			scenes_xyzatr_pull[nscene] =  modifyATRtoXYZ(
+	    					scenes_xyzatr[nscene],     // double [][] cur_xyzatr, // careful with Z - using the new one
+	    					rotated_xyzatr[nscene][0], // double []   new_xyz,
+	    					avg_z);                    // double      avg_z
+	    		}
+	    	}
+	    	// old version
+        	/*
         	scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
         			clt_parameters, // CLTParameters clt_parameters,
         			quadCLTs,       // QuadCLT[]     quadCLTs,
@@ -2090,6 +2149,7 @@ public class Interscene {
             		ref_index,      // int           ref_index,
             		earliest_scene, // int           early_index,
             		last_scene);    // int           last_index)
+            		*/
         }
         if (copy_pull_current) { // freeze_xy_pull) {
         	System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
@@ -2560,6 +2620,30 @@ public class Interscene {
 		return earliest_scene;
 	}
 	
+	/**
+	 * Modify initial ATR to match change of the XYZ from the already adjusted pose.
+	 * Uses the fact that movement in X is approximate equivalent to Azimuth rotation
+	 * (positive X has the same effect as negative A multiplied by Z) and Y-movement
+	 * has the same sign to Tilt.
+	 * @param cur_xyzatr current (already adjusted by the image-based LMA) {{x,y,z},{a,t,r}
+	 * @param new_xyz new (target from the IMS) {x,y,z} 
+	 * @param avg_z average altitude (Z)
+	 * @return new pose {{x,y,z},{a,t,r} expected to have a close match between scenes
+	 */
+	public static double [][] modifyATRtoXYZ(
+			double [][] cur_xyzatr, // careful with Z - using the new one
+			double []   new_xyz,
+			double      avg_z
+			){
+		double dx = new_xyz[0] - cur_xyzatr[0][0];
+		double dy = new_xyz[1] - cur_xyzatr[0][1];
+//		System.out.println(String.format("modifyATRtoXYZ(): dx=%7.3f, dy=%7.3f", dx, dy));
+		double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
+		double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr); 
+		return new_xyzatr;
+	}
+	
+	
 	// Make it the only entry point
 	// uses *_dt from
 	// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()
@@ -2683,7 +2767,15 @@ public class Interscene {
     				true);                   // boolean rectilinear)
 			if (debugLevel > -3) {
 				System.out.println("adjustDiffPairsLMAInterscene(): pull_offset="+pull_offset);
+				double dx = scene1_xyzatr_pull[0][0]-scene1_xyzatr[0][0];
+				double dy = scene1_xyzatr_pull[0][1]-scene1_xyzatr[0][1];
+				double da = (scene1_xyzatr_pull[1][0]-scene1_xyzatr[1][0])*average_z;
+				double dt = (scene1_xyzatr_pull[1][1]-scene1_xyzatr[1][1])*average_z;
+//				System.out.println(String.format("modifyATRtoXYZ(): dx=%8.4f, dy=%8.4f"+
+//						 ", hda=%8.4f, hdt=%8.4f, dx+hda=%8.4f, dy-hdt=%8.4f"
+//						, dx, dy, da, dt, dx+da, dy-dt));				
 			}
+			
 			while (pull_offset > clt_parameters.imp.max_pull_jump) {
 				// so far assuming only X and Y to be modified. Z - won't harm, angles are not used now, 
 				// but if yes - quaternions are needed. Note, that angles are updated and will not now match pull

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

@@ -5437,7 +5437,7 @@ public class OpticalFlow {
 	    	}
 	    	double [] rms = new double[5];
 	    	double [] quat = new double[4];
-	    	int quat_lma_mode = 1; // 2;
+	    	int quat_lma_mode = 2; // 1;
 	    	int debug_lev = 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/QuadCLTCPU.java

@@ -1098,6 +1098,8 @@ public class QuadCLTCPU {
     		boolean use_rot,
     		boolean rectilinear
     		) {
+    	double [][] xyzatr0_a = new double [][] {xyzatr0[0].clone(),xyzatr0[1].clone()}; // debugging
+    	double [][] xyzatr1_a = new double [][] {xyzatr1[0].clone(),xyzatr1[1].clone()};
     	ErsCorrection ers = getErsCorrection();
     	double disp_avg = ers.getDisparityFromZ(average_z);
     	int [] wh = ers.getSensorWH();
@@ -1117,17 +1119,19 @@ public class QuadCLTCPU {
     				xy[1], // double py,                // pixel coordinate Y in the reference view
     				disp_avg, // double disparity,         // this reference disparity 
     				!rectilinear, // boolean distortedView,    // This camera view is distorted (diff.rect), false - rectilinear
-    				xyzatr0[0], // double [] reference_xyz,  // this view position in world coordinates (typically zero3)
-    				xyzatr0[1], // double [] reference_atr,  // this view orientation relative to world frame  (typically zero3)
+    				xyzatr0_a[0], // double [] reference_xyz,  // this view position in world coordinates (typically zero3)
+    				xyzatr0_a[1], // double [] reference_atr,  // this view orientation relative to world frame  (typically zero3)
     				!rectilinear, // boolean distortedCamera,  // camera view is distorted (false - rectilinear)
-    				xyzatr1[0], // double [] camera_xyz,     // camera center in world coordinates
-    				xyzatr1[1], // double [] camera_atr,     // camera orientation relative to world frame
+    				xyzatr1_a[0], // double [] camera_xyz,     // camera center in world coordinates
+    				xyzatr1_a[1], // double [] camera_atr,     // camera orientation relative to world frame
     				OpticalFlow.LINE_ERR); // double    line_err);       // threshold error in scan lines (1.0)
     		if (pXpYD != null) {
     			double dx = pXpYD[0]-xy[0]; // null pointer
     			double dy = pXpYD[1]-xy[1];
     			s2 += dx*dx+dy*dy;
     			s0+=1.0;
+//				System.out.println(String.format("estimateAverageShift(): dx=%8.4f, dy=%8.4f",dx, dy));				
+
     		} else {
     			continue;
     		}

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

@@ -599,7 +599,8 @@ public class QuaternionLma {
 		final double q1 = vector[1]; 
 		final double q2 = vector[2]; 
 		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(new double[] {-q0,q1,q2,q3}); // seems better with reversal
+		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
 		if (jt != null) {
 			for (int i = 0; i < vector.length; i++) {
 				jt[i] = new double [weights.length];
@@ -1231,7 +1232,9 @@ public class QuaternionLma {
 			double [] q,
 			double [] r) {
 		return new double [][] {
+			// for inverted {-q0,q1,q2,q3}
 			//  t=s*q' d/dQ0
+			/*
 			{ 2*r[0]*q[0],
 			  2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
 			  2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
@@ -1251,8 +1254,8 @@ public class QuaternionLma {
 			 -2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
 			 -2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
 			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
-			
-			/*
+			*/ 
+			// for non-inverted {q0,q1,q2,q3}
 			//  t=s*q' d/dQ0    
 			{ 2*r[0]*q[0],
 			  2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
@@ -1273,7 +1276,6 @@ public class QuaternionLma {
 			  2*r[3]*q[1] + 2*r[2]*q[0] - 2*r[1]*q[3],
 			  2*r[3]*q[2] - 2*r[1]*q[0] - 2*r[2]*q[3],
 			  2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3]}
-			  */
 		};
 	}
 	public static double [][] composeQR_QdQ_(