Testing translation filtering by IMS velocities

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

@@ -187,7 +187,8 @@ public class ErsCorrection extends GeometryCorrection {
 				DP_DVX,   DP_DVY,   DP_DVZ,
 				DP_DSVAZ, DP_DSVTL, DP_DSVRL,
 				DP_DSVX,  DP_DSVY,  DP_DSVZ};
-	public static final int [] DP_XYZR_INDICES =    {DP_DSX,DP_DSY,DP_DSZ,DP_DSRL};
+	public static final int [] DP_XY_INDICES =      {DP_DSX,DP_DSY};
+	public static final int [] DP_ZR_INDICES =      {DP_DSZ,DP_DSRL};
 	public static final int [] DP_AT_INDICES =      {DP_DSAZ,DP_DSTL};
 	public static final int [] DP_ATT_ERS_INDICES = {DP_DSVAZ,DP_DSVTL};
 
@@ -234,17 +235,19 @@ public class ErsCorrection extends GeometryCorrection {
 	*/
 	
 	public static boolean [] getParamSelect(
+			boolean use_XY,
 			boolean use_AT,
 			boolean use_ERS) {
 		boolean [] param_select = new boolean[DP_NUM_PARS];
-		for (int i:DP_XYZR_INDICES)                 param_select[i] = true;
+		for              (int i:DP_ZR_INDICES)      param_select[i] = true;
+		if (use_XY) for  (int i:DP_XY_INDICES)      param_select[i] = true;
 		if (use_AT) for  (int i:DP_AT_INDICES)      param_select[i] = true;
 		if (use_ERS) for (int i:DP_ATT_ERS_INDICES) param_select[i] = true;
 		return param_select;
 	}
 	public static double [] getParamRegWeights(
-			double  reg_weight,
-			boolean use_AT) {
+			double  reg_weight) {
+//			boolean use_AT) {
 		double [] reg_weights = new double[DP_NUM_PARS];
 		reg_weights[DP_DSX] = reg_weight;
 		reg_weights[DP_DSY] = reg_weight;

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

@@ -5265,6 +5265,13 @@ public class OpticalFlow {
 				if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
 					mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
 				}
+				boolean        configured_lma = false;
+				boolean        lpf_xy = false;       // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables  
+				double         avg_rlen = clt_parameters.imp.avg_len; //  3.0;
+				boolean        readjust_xy_ims = true; // false;
+				double         reg_weight_xy = 10.0; // 0.05; // TODO: find out reasonable values
+				
+				
 				boolean        disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
 				boolean ers_from_ims = true; // false; // change later
 //				int            ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
@@ -5283,6 +5290,13 @@ public class OpticalFlow {
 						clt_parameters, // CLTParameters  clt_parameters,
 						mb_max_gain,    // double         mb_max_gain,
 						disable_ers,    // boolean        disable_ers,
+						configured_lma, // boolean        configured_lma,
+						lpf_xy,         // boolean        lpf_xy,       // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables  
+						 avg_rlen,      // double         avg_rlen,
+						readjust_xy_ims,// boolean        readjust_xy_ims,  // readjust X,Y fromIMS linear velocities and full X,Y movement
+                        // from the previous adjustment. Adjust A,T,R,Z (and optionally
+                        // angular velocities) freely
+						reg_weight_xy,  // double         reg_weight_xy, // regularization weight for X and Y
 						reliable_ref,   // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
 						quadCLTs,       // QuadCLT []     quadCLTs,
 						ref_index,      // int            ref_index,
@@ -8748,7 +8762,9 @@ public class OpticalFlow {
 						combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
 		combo_dsn_final[COMBO_DSN_INDX_DISP]= combo_dsn[COMBO_DSN_INDX_DISP].clone();
 		for (int i = 1; i < combo_dsn_final.length; i++) {
-			if (i != COMBO_DSN_INDX_SFM_GAIN) {
+			if (    (i != COMBO_DSN_INDX_SFM_GAIN) &&
+					(i != COMBO_DSN_INDX_STRENGTH) &&
+					(i != COMBO_DSN_INDX_STRENGTH_BG)){
 				Arrays.fill(combo_dsn_final[i], Double.NaN);
 			}
 		}
@@ -9336,6 +9352,21 @@ public class OpticalFlow {
 				tilesY);             // int         height)
 		rslt_suffix = "-INTER-INTRA";
 		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
+		// fixing NaN in strengths. It is uses to return RMS in Not needed - NaN was from Arrays.fill(combo_dsn_final[i], Double.NaN);
+// ImageDtt.clt_process_tl_correlations( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
+//		if (use_rms) nan_slices.add(DISPARITY_STRENGTH_INDEX); // will be used for RMS if LMA succeeded
+// And NaN in strength cause (at least) NaN for double avg_z = quadCLTs[ref_index].getAverageZ(true);		
+//		public static int COMBO_DSN_INDX_STRENGTH =    1; // strength, FG
+//		public static int COMBO_DSN_INDX_STRENGTH_BG = 6; // background strength
+		for (int slice:new int[] {COMBO_DSN_INDX_STRENGTH,COMBO_DSN_INDX_STRENGTH_BG}) {
+			if (combo_dsn_final[slice] != null) {
+				for (int i = 0; i <combo_dsn_final[slice].length; i++) {
+					if (Double.isNaN(combo_dsn_final[slice][i])) {
+						combo_dsn_final[slice][i] = 0.0;
+					}
+				}
+			}
+		}
 
 		ref_scene.saveDoubleArrayInModelDirectory( // error
 				rslt_suffix,           // String      suffix,

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

@@ -25,6 +25,10 @@ package com.elphel.imagej.tileprocessor;
  */
 
 //import java.awt.Polygon;
+
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
 import java.awt.Rectangle;
 import java.io.File;
 import java.io.FileInputStream;
@@ -236,6 +240,7 @@ public class QuadCLTCPU {
     		int           early_index,
     		int           last_index
     		) {
+    	boolean debug = true;
     	double [][] xyz_integ = new double [quadCLTs.length][3];
     	double [] tim = new double [quadCLTs.length];
     	double t00= quadCLTs[early_index].getTimeStamp();
@@ -252,15 +257,17 @@ public class QuadCLTCPU {
     		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];
+    			if (nscene == early_index) {
+    				xyz_integ[nscene][i] = 0;
+    			} else {
+    				xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene][0][i]);
     			}
+    			double 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++) {
@@ -270,16 +277,139 @@ public class QuadCLTCPU {
     	}
     	double [][][] xyzatr_out = new double [quadCLTs.length][][];
     	for (int nscene = early_index; nscene <= last_index; nscene++) {
-    		xyzatr_out[nscene] = new double[3][2];
+    		xyzatr_out[nscene] = new double[2][3];
     		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][0][i] = 	b[i] + a[i] *  tim[nscene] + xyz_integ[nscene][i] - ref_offs[i]; // 2 of 2
     			xyzatr_out[nscene][1][i] = xyzatr[nscene][1][i]; // for now - just copy old, maybe will add smth.
     		}
     	}
+    	if (debug) {
+    		System.out.println("refineFromImsVelocities():");
+    		System.out.println("a= ["+a[0]+", "+a[1]+", "+a[2]+"]");
+    		System.out.println("b= ["+b[0]+", "+b[1]+", "+b[2]+"]");
+    		System.out.println("ref_offs=["+ref_offs[0]+", "+ref_offs[1]+", "+ref_offs[2]+"]");
+    		System.out.println(String.format(
+    				"%3s\t%9s\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%s\t%9s\t%9s\t\t%9s\t%9s\t%9s",
+    				"N","T", "X","Y","Z","X-INT","Y-INT","Z-INT","X-err","Y-err","Z-err","X-lin","Y-lin","Z-lin",
+    				"X-corr","Y-corr","Z-corr"));
+        	for (int nscene = early_index; nscene <= last_index; nscene++) {
+        		System.out.println(String.format(
+        				"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.4f\t%9.4f\t%9.4f",
+        				nscene,tim[nscene],
+        				xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
+        				xyz_integ[nscene][0],xyz_integ[nscene][1],xyz_integ[nscene][2],
+        				xyzatr[nscene][0][0]-xyz_integ[nscene][0],xyzatr[nscene][0][1]-xyz_integ[nscene][1],xyzatr[nscene][0][2]-xyz_integ[nscene][2],
+//        				b[0]+a[0]*(tim[nscene]-tim[ref_index]), b[1]+a[1]*(tim[nscene]-tim[ref_index]), b[2]+a[2]*(tim[nscene]-tim[ref_index]),
+        				b[0]+a[0]*tim[nscene], b[1]+a[1]*tim[nscene], b[2]+a[2]*tim[nscene],
+        				xyzatr_out[nscene][0][0]-xyzatr[nscene][0][0],
+        				xyzatr_out[nscene][0][1]-xyzatr[nscene][0][1],
+        				xyzatr_out[nscene][0][2]-xyzatr[nscene][0][2]
+        				));
+        	}    	
+    	}
 
     	return xyzatr_out;
     }
 
+    /**
+     * Refining scene poses by LPF filtering. Filters either translations or rotations
+     * so it may use different widths (split clt_parameters.imp.avg_len)
+     * repeat twice (feeding result to xyzatr) if both translation and rotation filtering
+     * is needed.
+     * @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 avg_rlen       half-radius of averaging (cosine window)
+     * @param filter_rot     false - filter translations (xyz), true - filter rotations (atr)
+     * @param keep_rz        do not filter 3-rd component (Z or Roll) as they can be unambiguously
+     *                       fitted 
+     * @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 [][][] refineFromLPF(
+    		CLTParameters clt_parameters,
+    		QuadCLT[]     quadCLTs,
+    		double [][][] xyzatr,
+    		double        avg_rlen,// >= 1.0
+    		boolean       filter_rot, // false - translation. Do twice for both
+    		boolean       keep_rz,
+    		int           ref_index,
+    		int           early_index,
+    		int           last_index
+    		) {
+//		int     avg_len	=                clt_parameters.imp.avg_len;
+		int     avg_len	= (int) Math.floor(avg_rlen); //                clt_parameters.imp.avg_len;
+    	double [][][] scenes_xyzatr_pull = new double [quadCLTs.length][][];
+    	double [] avg_weights = new double [2*avg_len+1];
+    	for (int i = 0; i < avg_weights.length; i++) {
+    		avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_rlen)); //  + 1));
+    	}
+    	// it is incorrect for rotations - need to use quaternions!
+		int num_comp = filter_rot ? 4: 3; 
+    	for (int nscene = early_index; nscene <= last_index; nscene++) {
+			double [][] inv_scene = ErsCorrection.invertXYZATR(xyzatr[nscene]);
+			int n_pre =  nscene + avg_len;
+			int n_post = nscene - avg_len;
+			if (n_post < early_index) n_post = early_index;
+			if (n_pre > last_index) n_pre = last_index;
+			double s0=0, sx=0, sx2=0;
+			double [] sy =  new double[num_comp], sxy = new double [num_comp];
+			for (int n_other = n_post; n_other <= n_pre; n_other++) {
+				int ix = n_other - nscene; //+/-i
+				int i = ix + avg_len; // >=0
+				double [][] other_xyzatr = xyzatr[n_other]; 
+				double [][] other_rel = ErsCorrection.combineXYZATR(other_xyzatr,inv_scene);
+				double [] other; 
+				if (filter_rot) {
+					other = other_rel[0];
+				} else {
+					Rotation rot = new Rotation(
+		    				RotationOrder.YXZ,
+		    				ErsCorrection.ROT_CONV,
+		    				other_rel[1][0],
+		    				other_rel[1][1],
+		    				other_rel[1][2]);
+					other = new double[] {
+		    				rot.getQ0(),
+		    				rot.getQ1(),
+		    				rot.getQ2(),
+		    				rot.getQ3()};
+				}
+				double w = avg_weights[i];
+				s0 += w;
+				sx += w * ix;
+				sx2 += w * ix * ix;
+				// averaging translations or rotations (quaternions)
+				for (int k = 0; k < other.length; k++) {
+					sy[k] += w * other[k];
+					sxy[k] += w * other[k] * ix;
+				}
+			}
+			double [] diff = new double[num_comp];
+			double [][] diff_pull = new double[2][3];
+			for (int k = 0; k < diff.length; k++) {
+				diff[k]=(sy[k]*sx2 - sxy[k]*sx) / (s0*sx2 - sx*sx);
+			}
+			if (filter_rot) {
+				Rotation rot = new Rotation(diff[0],diff[1],diff[2],diff[3],true);
+				diff_pull[1]=rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+				// diff_pull[0] is {0,0,0}
+			} else {
+				diff_pull[0]=diff; // diff_pull[1] is {0,0,0}
+			}
+			// Only one component is non-zero 
+			scenes_xyzatr_pull[nscene] = ErsCorrection.combineXYZATR(xyzatr[nscene],diff_pull);
+			if (keep_rz) {
+				scenes_xyzatr_pull[nscene][0][2] = 	xyzatr[nscene][0][2];
+				scenes_xyzatr_pull[nscene][1][2] = 	xyzatr[nscene][1][2];
+			}
+    	}
+    	return scenes_xyzatr_pull;
+    }    
+    
     
     /**
      * Get linear and angular velocities (camera frame) from
@@ -2424,6 +2554,7 @@ public class QuadCLTCPU {
     	boolean silent = false;
     	if (use_combo) {
     		readComboDSI (silent);
+    		main_dsi = this.dsi;
     		needs_lma = needs_lma_combo;
     	} else {
     		main_dsi =  readDsiMain();