Updated initial orientation, started refining position

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

@@ -4975,12 +4975,14 @@ public class OpticalFlow {
 		boolean ims_use =      center_reference || clt_parameters.imp.ims_use; // center works with IMS only
 ///		int center_index=ref_index;
 		int ref_index = last_index; // old versions
+		boolean compensate_ims_rotation = false;
 		if (force_initial_orientations && !reuse_video) {
 			boolean OK = false;
 			int es1 = -1;
 			if (center_reference) {
 				es1 = Interscene.setInitialOrientationsCenterIms(
 						clt_parameters,      // final CLTParameters          clt_parameters,
+						compensate_ims_rotation, // final boolean                                compensate_ims_rotation,			
 						min_num_scenes,      // int                          min_num_scenes,
 						colorProcParameters, // final ColorProcParameters    colorProcParameters,
 						debayerParameters,	 // EyesisCorrectionParameters.DebayerParameters debayerParameters,			
@@ -5007,6 +5009,7 @@ public class OpticalFlow {
 			} else if (ims_use) {	
 				earliest_scene =	Interscene.setInitialOrientationsIms(
 						clt_parameters,      // final CLTParameters          clt_parameters,
+						compensate_ims_rotation, // final boolean                compensate_ims_rotation,
 						min_num_scenes,      // int                          min_num_scenes,
 						colorProcParameters, // final ColorProcParameters    colorProcParameters,
 						quadCLTs,            //  final QuadCLT[]              quadCLTs, //

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

@@ -125,6 +125,7 @@ public class QuadCLTCPU {
 	
 	public static final boolean   USE_PRE_2021 = false; // temporary
 	public static final int       THREADS_MAX =       100;
+	public static final double [] ZERO3 = {0.0,0.0,0.0};
 
 //	public GPUTileProcessor.GpuQuad gpuQuad =      null;
 
@@ -205,7 +206,90 @@ public class QuadCLTCPU {
     public void setQuatCorr(double[] quat) {
     	quat_corr = quat;
     }
-	public double [][] getDxyzatrIms(
+    
+    /**
+     * Refine scene poses (now only linear) from currently adjusted poses
+     * (adjusted using IMS orientations) and IMS velocities (currently
+     * only linear are used). Refined scene poses may be used as pull targets
+     * with free orientations (and angular velocities for ERS).
+     * Result poses are {0,0,0} for the reference frame.
+     * 
+     * Assuming correct IMU angular velocities and offset linear ones.   
+     * 
+     * @param clt_parameters configuration parameters
+     * @param quadCLTs       scenes sequence (for timestamps)
+     * @param xyzatr         current scenes [[x,y,z],[a,t,r]] in reference scene frame
+     * @param dxyzatr        IMU-provided linear and angular velocities in reference
+     *                       scene frame
+     * @param ref_index      reference scene index 
+     * @param early_index    earliest (lowest) scene index to use
+     * @param last_index     last (highest) scene index to use
+     * @return refined array of scene poses [[x,y,z],[a,t,r]] (in reference scene frame),
+     *                       same indices as input 
+     */
+	public static double [][][] refineFromImsVelocities(
+			CLTParameters clt_parameters,
+			QuadCLT[]     quadCLTs,
+			double [][][] xyzatr,
+			double [][][] dxyzatr,
+			int           ref_index,
+			int           early_index,
+			int           last_index
+			) {
+		    double [][] xyz_integ = new double [quadCLTs.length][3];
+		    double [] tim = new double [quadCLTs.length];
+		    double t00= quadCLTs[early_index].getTimeStamp();
+		    double t0 = t00;
+		    double s0 = 0, sx = 0, sx2 = 0;
+		    double [] sxy = new double[3], sy = new double[3]; 
+		    for (int nscene = early_index; nscene <= last_index; nscene++) {
+		    	double t = quadCLTs[nscene].getTimeStamp();
+		    	double x = t - t00; // from early_index
+		    	tim[nscene] = x;
+		    	s0 +=1.0;
+		    	sx += x;
+		    	sx2 += x*x;
+		    	double dt = (t-t0)/2;  // half from previous
+		    	t0 = t;
+		    	for (int i = 0; i < 3; i++) {
+		    		double y = 0;
+		    		if (nscene > early_index) {
+		    			xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene-1][0][i]);
+		    			y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
+		    		}
+		    		sy[i] += y;
+		    		sxy[i] += x*y;
+		    	}
+		    }
+		    double denom = sx2 * s0 - sx * sx;
+		    double [] a = new double[3], b = new double[3], ref_offs = new double[3];
+		    for (int i = 0; i < 3; i++) {
+		    	a[i] = (sxy[i] * s0 - sx * sy[i])/denom;
+		    	b[i] = (sy[i] * sx2 - sx * sxy[i])/denom;
+		    	ref_offs[i] = b[i] + a[i] *  tim[ref_index] + xyz_integ[ref_index][i];
+		    }
+		    double [][][] xyzatr_out = new double [quadCLTs.length][][];
+		    for (int nscene = early_index; nscene <= last_index; nscene++) {
+		    	xyzatr_out[nscene] = new double[3][2];
+		    	for (int i = 0; i < 3; i++) {
+		    		xyzatr_out[nscene][0][i] = 	b[i] + a[i] *  tim[nscene] + xyz_integ[nscene][i] - ref_offs[i];
+		    		xyzatr_out[nscene][1][i] = xyzatr[nscene][1][i]; // for now - just copy old, maybe will add smth.
+		    	}
+		    }
+		
+		return xyzatr_out;
+	}
+
+    
+    /**
+     * Get linear and angular velocities (camera frame) from
+     * this scene IMS data
+     * @param clt_parameters configuration parameters (including
+     *        scales from IMS to linear for linear and angular velocities
+     * @param scale if true, multiply by scales from he configuration parameters)
+     * @return linear and angular velocities in camera frame
+     */
+	protected double [][] getDxyzatrIms(
 			CLTParameters clt_parameters,
 			boolean       scale) {
 		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
@@ -219,16 +303,106 @@ public class QuadCLTCPU {
 		double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false);
 		double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
 		// a (around Y),t (around X), r (around Z)
-		double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
+		double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
 		if (scale) {
 			for (int i = 0; i < 3; i++) {
-				dxyzatyr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
-				dxyzatyr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
+				dxyzatr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
+				dxyzatr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
 			}
 		}
-		return dxyzatyr;
+		return dxyzatr;
 	}
 
+	/**
+    * Get linear and angular velocities (camera frame) from
+     * scenes' IMS data
+     * @param clt_parameters configuration parameters (including
+     *        scales from IMS to linear for linear and angular velocities
+	 * @param quadCLTs array of scenes with IMS data loaded
+     * @param scale if true, multiply by scales from he configuration parameters)
+     * @return linear and angular velocities in camera frame for each scene that has IMS data
+	 */
+	public static double [][][] getDxyzatrIms(
+			CLTParameters clt_parameters,
+			QuadCLT[]     quadCLTs,
+			boolean       scale) {
+		double [][][] dxyzatr = new double [quadCLTs.length][][];
+		for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
+			dxyzatr[i] = quadCLTs[i].getDxyzatrIms(
+					clt_parameters,
+					scale);
+		}
+		return dxyzatr;
+	}
+	
+	
+	/**
+	 * Get offset and rotation of each scene from the sequence relative to this
+	 * scene using IMS data. Linear offsets may be too big for the fix, so they
+	 * need running correction from the last matched scene, scenes should start from
+	 * the closest to this one and move gradually farther away.
+	 * @param clt_parameters Configuration parameters, including IMS to camera rotation
+	 * @param quadCLTs   array of scenes
+	 * @param quat_corr  optional quaternion correcting IMS orientation (when known) or null
+	 * @param debugLevel debug level
+	 * @return per-scene array of [[x,y,z],[a,t,r]] in camera frame for initial fitting.
+	 * Orientation may be later used to pull LMA. Result for reference (this) scene may
+	 * be small values, but not exactly zeros.
+	 */
+	public double [][][] getXyzatrIms(CLTParameters clt_parameters,
+			QuadCLT[]  quadCLTs,
+			double []  quat_corr, // only applies to rotations - verify!
+			int        debugLevel){
+		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
+		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
+//		scenes_xyzatr[ref_index] =         new double[2][3]; // all zeros
+
+		Did_ins_2   d2_ref = did_ins_2;
+		// apply correction to orientation - better not to prevent extra loops.
+		// And it only applies to rotations
+		double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
+				ims_mount_atr,
+				ims_ortho);
+		if (quat_corr != null) {
+			cam_quat_ref_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_ref_enu, false);
+	    	if (debugLevel > -3) {
+	    		System.out.println("getXyzatrIms(): Applying attitude correction from quaternion ["+
+	    				quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
+	    	}
+		}
+		double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
+		double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
+		double [][] last_corr_xyzatr = {ZERO3,ZERO3};
+		for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
+			QuadCLT scene = quadCLTs[nscene];
+			Did_ins_2 d2 = scene.did_ins_2;
+			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up
+			double [] cam_quat_enu =Imx5.quaternionImsToCam(
+					d2.getQEnu(),
+					ims_mount_atr,
+					ims_ortho);
+			if (quat_corr != null) {
+				cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
+			}			
+			double [] cam_xyz_enu = Imx5.applyQuaternionTo(
+					// Offset in the reference scene frame, not in the current scene
+					cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
+					enu, // absolute offset (East, North, Up) in meters
+					false);
+			double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
+			double [][] ims_scene_xyzatr_enu = {cam_xyz_enu, scene_abs_atr_enu }; // try with xyz?
+			// set initial approximation from IMS, subtract reference XYZATR
+			// predicted by IMU from the reference scene
+			double [][] pose_ims = ErsCorrection.combineXYZATR(
+					ims_scene_xyzatr_enu,
+					ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu));
+			scenes_xyzatr[nscene] = pose_ims;
+		}
+		return scenes_xyzatr;
+	}
+	
+	
 	public double [] getLla() {
 		return did_ins_2.lla;
 	}
@@ -355,10 +529,6 @@ public class QuadCLTCPU {
     			scene1.getImageName());
     }
     
-    //QuadCLTCPU [] scenes
-    // TODO: Use dsi[] instead
-//    @Deprecated
-//    public boolean[] blue_sky =                            null;
     public void inc_orient()        {num_orient++;}
     public void inc_accum()         {num_accum++;}
     public void set_orient(int num) {num_orient = num;}
@@ -417,30 +587,20 @@ public class QuadCLTCPU {
     /**
      * Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
      * should have DSI to estimate average distance 
+     * Use estimateAverageShift() instead - much more accurate. This one underestimates
+     * up to twice (not understood)
+     * 
      * @param xyzatr0 - first  scene [[x,y,z],[a,t,r]] 
      * @param xyzatr1 - second scene [[x,y,z],[a,t,r]]
      * @param use_lma
      * @return
      */
+    @Deprecated
     public double estimatePixelShift(
     		double [][] xyzatr0,
     		double [][] xyzatr1,
     		boolean use_lma
     		) {
-        return estimatePixelShift(
-        		xyzatr0,
-        		xyzatr1,
-        		use_lma,
-        		0);
-    }
-    public double estimatePixelShift(
-    		double [][] xyzatr0,
-    		double [][] xyzatr1,
-    		boolean use_lma,
-    		int     mode
-    		) {
-    	boolean sign_x = (mode & 1) != 0; 
-    	boolean sign_y = (mode & 2) != 0; 
     	double z = getAverageZ(use_lma);
     	if (Double.isNaN(z)) {
     		System.out.println("estimatePixelShift(): failed estimating distance - no DSI or too few DSI tiles.");
@@ -452,8 +612,8 @@ public class QuadCLTCPU {
     	for (int i = 0; i<2;i++) for (int j=0; j < 3; j++) {
     		dxyzatr[i][j] = xyzatr1[i][j]-xyzatr0[i][j];
     	}
-    	double eff_az = dxyzatr[1][0] + (sign_x?-1:1) * dxyzatr[0][0] / z;
-    	double eff_tl = dxyzatr[1][1] + (sign_y?-1:1) * dxyzatr[0][1] / z;
+    	double eff_az = dxyzatr[1][0] + dxyzatr[0][0] / z;
+    	double eff_tl = dxyzatr[1][1] + dxyzatr[0][1] / z;
     	double pix_x = -eff_az*aztl_pix_per_rad;
     	double pix_y =  eff_tl*aztl_pix_per_rad;
     	double pix_roll = dxyzatr[1][2] * roll_pix_per_rad;
@@ -462,6 +622,20 @@ public class QuadCLTCPU {
     	return est_pix;
     }
 
+    /**
+     * Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
+     * should have DSI to estimate average distance.
+     * Difference with rectilinear is small so both are OK.
+     * @param xyzatr0 first scene pose (double[3][2]) for reference scene
+     * @param xyzatr1 second scene pose
+     * @param average_z average distance (altitude AGL for downward images) in meters
+     * @param use_rot when false - compare only the center, when true - average 4 samples
+     *                at 25% of width/height. May be needed for pure rotation (no center
+     *                shift), but may fail if interscene shift exceeds 25%. So try first
+     *                for center and use 4 samples only if center does not move.
+     * @param rectilinear Ignore lens distortions
+     * @return interscene shift in pixels
+     */
     public double estimateAverageShift(
     		double [][] xyzatr0,
     		double [][] xyzatr1,