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/Interscene.java

@@ -614,9 +614,14 @@ public class Interscene {
 							" ("+getFailReason(fail_reason[0])+")");
 					if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
 						fpn_list.add(scene_index);
+						scenes_xyzatr[scene_index] = initial_pose;
+						// should not be modified for initial adjust, but just in case
+						quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+								QuadCLTCPU.scaleDtToErs(
+										clt_parameters,
+										scenes_dxyzatr[scene_index]));		
 						if (fpn_skip) {
 							System.out.println("fpn_skip is set, just using initial pose, num_fpn_mitigate="+fpn_list.size());
-							scenes_xyzatr[scene_index] = initial_pose;
 							break handle_failure; // to next scene
 						} else {
 							System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
@@ -745,8 +750,13 @@ public class Interscene {
     					clt_parameters.imp.debug_level); // int            debugLevel);
     			boolean adjust_OK = rel_xyzatr != null;
 				if (!adjust_OK) {
-					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][1]+
-							" referenced from scene "+fpn_pairs[ipair][0]+". Reason = "+fail_reason[0]+
+					// should not be modified for initial adjust, but just in case
+					quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+							QuadCLTCPU.scaleDtToErs(
+									clt_parameters,
+									scenes_dxyzatr[fpn_pairs[ipair][0]]));		
+					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
+							" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
 							" ("+getFailReason(fail_reason[0])+")");
 				} else {
 					scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
@@ -777,28 +787,6 @@ public class Interscene {
 		} else if (debugLevel > -4) {
 			System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
 		}
-        /*
-		// Set ERS data - now old way, change to IMS data (and apply to adjustPairsLMAInterscene())
-        double  half_run_range =    clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
-        double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
-        		quadCLTs,        // QuadCLT []     scenes, // ordered by increasing timestamps
-        		ref_index,
-        		earliest_scene,  // int            start_scene,
-        		ref_index,       // int            end1_scene,
-        		scenes_xyzatr,   // double [][][]  scenes_xyzatr,
-        		half_run_range); // double         half_run_range
-    	quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
-    			dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double []    ers_xyz_dt,
-    			dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
-		for (int scene_index =  ref_index; scene_index >= earliest_scene ; scene_index--) {
-			ers_reference.addScene(quadCLTs[scene_index].getImageName(),
-					scenes_xyzatr[scene_index][0],
-					scenes_xyzatr[scene_index][1],
-					dxyzatr_dt[scene_index][0], // ers_scene.getErsXYZ_dt(),		
-					dxyzatr_dt[scene_index][1] // ers_scene.getErsATR_dt()		
-					);
-		}		
-		*/
 		quadCLTs[ref_index].set_orient(1); // first orientation
 		quadCLTs[ref_index].set_accum(0);  // reset accumulations ("build_interscene") number
 		quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)  // null pointer
@@ -1326,12 +1314,72 @@ public class Interscene {
 		}
 	}	
 	
-
+//    		double        avg_rlen,// >= 1.0
+	/**
+	 * Re-adjust camera poses with LMA after initial poses are already known.
+	 * Fitting can be done for "reliable_ref" tiles only, they may be filtered
+	 * by SfM gain external to this method).
+	 * Fitting can be performed with ERS for Azimuth and Tilt Rotations (other
+	 * ERS components are preserved intact).
+	 * By default X,Y,Z and Roll are adjusted (with or without 2 angular velocities).
+	 * Optionally one of the 2 refinement methods is used:
+	 *   a) lpf_xy true:
+	 *   previous X and Y are low-pass filtered, by running-average with cosine window
+	 *   of avg_rlen half-width     
+	 *   b) readjust_xy_ims && !lpf_xy:
+	 *   X and Y are refined with IMS linear velocities shifted (by adding ax+b to
+	 *   coordinates) to best fit previously determined coordinates (which in turn
+	 *   depended on IMS angles that are more accurate than coordinates). The result
+	 *   still has absolute AGL accuracy determined by the intrascene measurements
+	 *   that are good to ~1% at 100m if properly field-calibrated
+	 *   In both a) and b) all of X,Y,Z,A,T, and R are adjusted (with optional dA/dt
+	 *   and dT/dt as ERS), and X and Y are pulled to new calculated values (by
+	 *   averaging or IMS) with strength reg_weight_xy, other parameters (Z,A,T,R) are
+	 *   not regularized.
+	 *   Special cases:
+	 *   c) (reg_weight_xy == 0.0) && readjust_xy_ims - freeze X,Y, freely adjust
+	 *   Z,A,T,R (optional subsequent runs after X,Y are "pulled" regardless of IMU or
+	 *   not
+	 *   d) (reg_weight_xy == 0.0) && lpf_xy - freely adjust X,Y,Z, A,T,R (typical mode
+	 *   for the forward-looking applications
+	 * @param clt_parameters   configuration parameters
+	 * @param mb_max_gain      maximal motion blur gain
+	 * @param disable_ers      disable adjustment of the angular velocities through ERS.
+	 *                         ERS may be disabled inside adjustDiffPairsLMAInterscene()
+	 *                         if not all 4 image quadrants have enough "reliable" tiles.
+	 * @param configured_lma   Ignore calculated LMA parameter selection and regularization
+	 *                         weights, use ones from the configuration parameters
+	 * @param lpf_xy           Low pass filter X and Y "pull" values (derived from the
+	 *                         current ones). Overrides readjust_xy_ims as they are mutually
+	 *                         exclusive.
+	 * @param avg_rlen         half-width of the LPF window, used with lpf_xy==true
+	 * @param readjust_xy_ims  readjust X and Y from current values and IMS linear velocities
+	 * @param reg_weight_xy    regularization weights for X and Y, valid with any of lpf_xy or
+	 *                         readjust_xy_ims.
+	 * @param reliable_ref     tile mask in line-scan order. LMA fitting ignores data for
+	 *                         unselected tiles.  
+	 * @param quadCLTs         array of scene instances. Only scenes between range[0] and
+	 *                         range[1] (inclusive) will be used.
+	 * @param ref_index        index of the reference frame in the scenes array
+	 * @param range            {earliest_scene_index, latest_scene_index};
+	 * @param ers_mode         0 - keep, 1 - set from velocity, 2 - set from IMS. Now always 0
+	 * @param test_motion_blur motion blur debugging and images
+	 * @param debugLevel       debug level
+	 * @return earliest scene - may be higher than range[0] if LMA failed (should not happen at
+	 *         this pass, should fail during initial orientation.
+	 */
 	public static int  reAdjustPairsLMAInterscene( // after combo dgi is available and preliminary poses are known
 			CLTParameters  clt_parameters,
 			double         mb_max_gain,
 			boolean        disable_ers,
-			boolean []     reliable_ref, // null or bitmask of reliable reference tiles			
+			boolean        configured_lma,
+			boolean        lpf_xy,       // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables  
+			double         avg_rlen,
+			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
+			double         reg_weight_xy,    // regularization weight for X and Y when lpf_xy or readjust_xy_ims
+			boolean []     reliable_ref,     // null or bitmask of reliable reference tiles			
     		QuadCLT []     quadCLTs,
     		int            ref_index,
     		int     []     range,
@@ -1340,6 +1388,15 @@ public class Interscene {
 			int            debugLevel)
 	{ 
 		System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
+		boolean freeze_xy_pull = true; // debugging freezing xy to xy_pull
+		final boolean[]    param_select =     configured_lma? clt_parameters.ilp.ilma_lma_select :
+			ErsCorrection.getParamSelect(
+				!freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
+				readjust_xy_ims || lpf_xy,  // boolean use_AT,
+				!disable_ers); //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
+		final double []    param_regweights =  configured_lma? clt_parameters.ilp.ilma_regularization_weights :
+			ErsCorrection.getParamRegWeights(
+					(freeze_xy_pull? 0.0: reg_weight_xy)); // double  reg_weight,
 		boolean fail_on_zoom_roll=false; // it should fail on initial adjustment
     	boolean fmg_reorient_en =    clt_parameters.imp.fmg_reorient_en; // enable IMS-based FPN mitigation for readjustmnet orientation
     	double  fmg_distance =       clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
@@ -1347,7 +1404,7 @@ public class Interscene {
     	                                           // offset is too small
     	boolean fmg_rectilinear =    clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation 
 		
-		int     avg_len	=                clt_parameters.imp.avg_len;
+//		int     avg_len	=                clt_parameters.imp.avg_len;
         // Set up velocities from known coordinates, use averaging
         double  half_run_range =    clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
 		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
@@ -1411,7 +1468,6 @@ public class Interscene {
         	}
         }
         
-        double [][] ref_pXpYD = null;
         double [][] dbg_mb_img = null;
         double [] mb_ref_disparity =null;
         //        if (test_motion_blur) {
@@ -1419,121 +1475,152 @@ public class Interscene {
         if (mb_ref_disparity == null) {
         	mb_ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
         }
-        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
-        ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
-        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
-        		mb_ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
-        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
-        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
-        		quadCLTs[ref_index],     // final QuadCLT     scene_QuadClt,
-        		quadCLTs[ref_index]);    // final QuadCLT     reference_QuadClt)
+        
         if (test_motion_blur) {
         	dbg_mb_img = new double[quadCLTs.length][];
         }
         
-        
 		ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
-		double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
-        double [][][] dxyzatr_dt =    new double [quadCLTs.length][][];
+		double [][][] scenes_xyzatr =      new double [quadCLTs.length][][];
+		double [][][] scenes_xyzatr_pull = new double [quadCLTs.length][][];
+        double [][][] dxyzatr_dt =         new double [quadCLTs.length][][];
 
 		scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
 		// should have at least next or previous non-null
 		int debug_scene = 69; // -68; // -8;
 		double maximal_series_rms = 0.0;
-///		double [][] mb_vectors_ref = null;
-///		TpTask[][] tp_tasks_ref = null;
     	if (debug_ers) {
     		System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
     	}
     	
         for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
-			if (nscene == debug_scene) {
-				System.out.println("nscene = "+nscene);
-				System.out.println("nscene = "+nscene);
-			}
 			// just checking it is not isolated
         	if (quadCLTs[nscene] == null) {
         		earliest_scene = nscene + 1;
         		break;
         	}
-        	if (nscene != ref_index) {
-        		String ts = quadCLTs[nscene].getImageName();
-        		if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
-        			earliest_scene = nscene + 1;
-        			break;
-        		}
-        		scenes_xyzatr[nscene] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
+        	// now reference scene should also be in ers_reference.getSceneXYZ(ts)
+        	String ts = quadCLTs[nscene].getImageName();
+        	if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
+        		earliest_scene = nscene + 1;
+        		break;
         	}
+        	scenes_xyzatr[nscene] = ers_reference.getSceneXYZATR(ts);
+        	scenes_xyzatr_pull[nscene] = scenes_xyzatr[nscene].clone();
         	dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
-        	if (debug_ers) {
+        }
+        switch (ers_mode) {
+        case 1 : 
+        	dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
+        			quadCLTs,        // QuadCLT []     scenes, // ordered by increasing timestamps
+        			ref_index,
+        			earliest_scene,  // int            start_scene,
+        			last_scene,       // int            end1_scene,
+        			scenes_xyzatr,   // double [][][]  scenes_xyzatr,
+        			half_run_range); // double         half_run_range
+        	break;
+        default: // do nothing - already read
+        }
+
+        // optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
+        // double         avg_rlen
+        if (lpf_xy && (reg_weight_xy > 0.0)) {
+        	scenes_xyzatr_pull = QuadCLT.refineFromLPF(
+        			clt_parameters, // CLTParameters clt_parameters,
+        			quadCLTs,       // QuadCLT[]     quadCLTs,
+        			scenes_xyzatr,  // double [][][] xyzatr,
+        			avg_rlen,       // double        avg_rlen,// >= 1.0
+            		false,          // boolean       filter_rot, // false - translation. Do twice for both
+            		true,           // boolean       keep_rz, // keep Z and Roll
+            		ref_index,      // int           ref_index,
+            		earliest_scene, // int           early_index,
+            		last_scene);    // int           last_index)
+        } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
+        	scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
+        			clt_parameters, // CLTParameters clt_parameters,
+        			quadCLTs,       // QuadCLT[]     quadCLTs,
+        			scenes_xyzatr,  // double [][][] xyzatr,
+        			dxyzatr_dt,     // double [][][] dxyzatr,
+            		ref_index,      // int           ref_index,
+            		earliest_scene, // int           early_index,
+            		last_scene);    // int           last_index)
+        }
+        if (freeze_xy_pull) {
+        	System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
+            for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
+            	if (scenes_xyzatr_pull[nscene] != null) {
+            		// only X,Y, keep Z.
+            		scenes_xyzatr[nscene][0] = scenes_xyzatr_pull[nscene][0];
+            		scenes_xyzatr[nscene][1] = scenes_xyzatr_pull[nscene][1];
+            	}
+            }
+        }
+        if (debug_ers) {
+    		System.out.println(String.format(
+    				"%3s,%9s,%9s,%9s,,%9s,%9s,%9s,,%9s,%9s,%9s,,%s,%9s,%9s,,%9s,%9s,%9s,%9s",
+    				"N","X","Y","Z","A","T","R","X-pull","Y-pull","Z-pull","A-pull","T-pull","R-pull",
+    				"cent-dist","quad-dist","cent-rect","quad-rect"));
+        	for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
         		// All 4 variants - just for testing. Normally should try center first and use
         		// quad if there is no significant movement
         		double est_center = quadCLTs[ref_index].estimateAverageShift(
         				scenes_xyzatr[ref_index], // double [][] xyzatr0,
         				scenes_xyzatr[nscene],    // double [][] xyzatr1,
-        	    		avg_z,                    // double average_z,
-        	    		false,                    // boolean use_rot,
-        	    		false);                   // boolean rectilinear)
+        				avg_z,                    // double average_z,
+        				false,                    // boolean use_rot,
+        				false);                   // boolean rectilinear)
         		double est_quad = quadCLTs[ref_index].estimateAverageShift(
         				scenes_xyzatr[ref_index], // double [][] xyzatr0,
         				scenes_xyzatr[nscene],    // double [][] xyzatr1,
-        	    		avg_z,                    // double average_z,
-        	    		true,                    // boolean use_rot,
-        	    		false);                   // boolean rectilinear)
+        				avg_z,                    // double average_z,
+        				true,                    // boolean use_rot,
+        				false);                   // boolean rectilinear)
         		double est_center_rectilinear = quadCLTs[ref_index].estimateAverageShift(
         				scenes_xyzatr[ref_index], // double [][] xyzatr0,
         				scenes_xyzatr[nscene],    // double [][] xyzatr1,
-        	    		avg_z,                    // double average_z,
-        	    		false,                    // boolean use_rot,
-        	    		true);                   // boolean rectilinear)
+        				avg_z,                    // double average_z,
+        				false,                    // boolean use_rot,
+        				true);                   // boolean rectilinear)
         		double est_quad_rectilinear = quadCLTs[ref_index].estimateAverageShift(
         				scenes_xyzatr[ref_index], // double [][] xyzatr0,
         				scenes_xyzatr[nscene],    // double [][] xyzatr1,
-        	    		avg_z,                    // double average_z,
-        	    		true,                    // boolean use_rot,
-        	    		true);                   // boolean rectilinear)
+        				avg_z,                    // double average_z,
+        				true,                    // boolean use_rot,
+        				true);                   // boolean rectilinear)
+        		/*
         		System.out.println(String.format("%3d xyz:    %9.5f %9.5f %9.5f   atr:    %9.5f %9.5f %9.5f est_pix: %9.4f %9.4f %9.4f %9.4f",
         				nscene,
         				scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
         				scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2],
         				est_center, est_quad, est_center_rectilinear, est_quad_rectilinear));
+        				*/
+        		System.out.println(String.format("%3d,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.4f, %9.4f, %9.4f, %9.4f",
+        				nscene,
+        				scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
+        				scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2],
+        				scenes_xyzatr_pull[nscene][0][0], scenes_xyzatr_pull[nscene][0][1], scenes_xyzatr_pull[nscene][0][2],
+        				scenes_xyzatr_pull[nscene][1][0], scenes_xyzatr_pull[nscene][1][1], scenes_xyzatr_pull[nscene][1][2],
+        				est_center, est_quad, est_center_rectilinear, est_quad_rectilinear));
+        	}
+        	System.out.println();
+        	for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
+        		System.out.println(String.format("%3d xyz_dt: %9.5f %9.5f %9.5f   atr_dt: %9.5f %9.5f %9.5f",
+        				nscene,
+        				dxyzatr_dt[nscene][0][0], dxyzatr_dt[nscene][0][1], dxyzatr_dt[nscene][0][2],
+        				dxyzatr_dt[nscene][1][0], dxyzatr_dt[nscene][1][1], dxyzatr_dt[nscene][1][2]));
         	}
+        	System.out.println();
         }
-        switch (ers_mode) {
-        case 1 : 
-        	dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
-        			quadCLTs,        // QuadCLT []     scenes, // ordered by increasing timestamps
-        			ref_index,
-        			earliest_scene,  // int            start_scene,
-        			last_scene,       // int            end1_scene,
-        			scenes_xyzatr,   // double [][][]  scenes_xyzatr,
-        			half_run_range); // double         half_run_range
-        	break;
-        default: // do nothing - already read
-
+        double max_z_change = Double.NaN; // only applicable for drone images
+        if (max_zoom_diff > 0) { // ignore if set to
+        	max_z_change = avg_z * max_zoom_diff;
+        	if (fail_on_zoom_roll) {
+        		if (debugLevel > -3) {
+        			System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
+        		}
+        	}
         }
-    	if (debug_ers) {
-    		System.out.println();
-    		for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
-    		System.out.println(String.format("%3d xyz_dt: %9.5f %9.5f %9.5f   atr_dt: %9.5f %9.5f %9.5f",
-    				nscene,
-    				dxyzatr_dt[nscene][0][0], dxyzatr_dt[nscene][0][1], dxyzatr_dt[nscene][0][2],
-    				dxyzatr_dt[nscene][1][0], dxyzatr_dt[nscene][1][1], dxyzatr_dt[nscene][1][2]));
-    		}
-    		System.out.println();
-    	}
-		double max_z_change = Double.NaN; // only applicable for drone images
-		if (max_zoom_diff > 0) { // ignore if set to
-			max_z_change = avg_z * max_zoom_diff;
-			if (fail_on_zoom_roll) {
-				if (debugLevel > -3) {
-					System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
-				}
-			}
-		}
     	
-    	boolean [] failed_scenes = new boolean[quadCLTs.length];
-    	int num_failed =0;
     	int [] scene_seq = new int [last_scene-earliest_scene+1];
     	{
     		int indx=0;
@@ -1549,10 +1636,6 @@ public class Interscene {
 		double [][]        pXpYD_ref = new double [tilesX*tilesY][];
     	ArrayList<Integer> fpn_list = new ArrayList<Integer>();
 		boolean            fpn_disable = false; // estimate they are too close
-		double [][][]      scenes_pull =        new double [quadCLTs.length][][];
-		boolean [][]       scenes_param_select = new boolean[quadCLTs.length][];
-		final boolean[]    param_select =     clt_parameters.ilp.ilma_lma_select;
-		final double []    param_regweights = clt_parameters.ilp.ilma_regularization_weights; 
 		final double       max_rms =          clt_parameters.imp.max_rms;
 		
     	for (int nscene:scene_seq) {
@@ -1587,12 +1670,11 @@ public class Interscene {
         		break;
         	}
 			String ts = quadCLTs[nscene].getImageName();
-			double []   scene_xyz_pre = ZERO3;
-			double []   scene_atr_pre = ZERO3;
+			// it will be modified inside LMA, so needs to be backuped/restored to undo
 			quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
 					QuadCLTCPU.scaleDtToErs(
 							clt_parameters,
-							dxyzatr_dt[nscene]));		
+							dxyzatr_dt[nscene]));	
 			if (dbg_mb_img != null) { // fill in 1 slice of dbg_mb_img, show all at once later
 				readjustShowCorrected(
 						clt_parameters,     // CLTParameters  clt_parameters,
@@ -1606,60 +1688,19 @@ public class Interscene {
 						debug_scene,        //  = 68 int            debug_scene, //  = 68			
 						debugLevel);        // int            debugLevel);				
 			}
-//			final boolean[]   param_select =     clt_parameters.ilp.ilma_lma_select;
-//			final double []   param_regweights = clt_parameters.ilp.ilma_regularization_weights; 
-//			final double      max_rms =          clt_parameters.imp.max_rms;
 
 			if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes
 				ers_reference.addScene(ts,
-						scenes_xyzatr[nscene][0], // ref_index
-						scenes_xyzatr[nscene][1],
-						dxyzatr_dt[nscene][0],
-						dxyzatr_dt[nscene][1]
+						scenes_xyzatr[nscene], // ref_index
+						dxyzatr_dt[nscene]
 						);
 			} else { // if (nscene == ref_index)
-				scene_xyz_pre = ers_reference.getSceneXYZ(ts);
-				scene_atr_pre = ers_reference.getSceneATR(ts);
-				double [][] scene_xyzatr = new double[][] {scene_xyz_pre, scene_atr_pre};
-				double [][] inv_scene = ErsCorrection.invertXYZATR(scene_xyzatr);
-				double [] avg_weights = new double [2*avg_len+1];
-				int n_pre =  nscene + avg_len;
-				int n_post = nscene - avg_len;
-				if (n_post < earliest_scene) n_post = earliest_scene;
-				if (n_pre > last_scene) n_pre = last_scene;
-				double s0=0, sx=0, sx2=0;
-				double [][] sy = new double[2][3], sxy = new double [2][3];
-				for (int n_other = n_post; n_other <= n_pre; n_other++) {
-					int ix = n_other - nscene; //+/-i
-					int i = ix + avg_len; // >=0
-					avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_len + 1));
-					double [][] other_xyzatr = ers_reference.getSceneXYZATR(quadCLTs[n_other].getImageName()); 
-					double [][] other_rel = (n_other != ref_index)?ErsCorrection.combineXYZATR(other_xyzatr,inv_scene):
-						(new double[2][3]);
-					double w = avg_weights[i];
-					s0 += w;
-					sx += w * ix;
-					sx2 += w * ix * ix;
-					for (int j = 0; j < other_rel.length; j++) {
-						for (int k = 0; k < other_rel[j].length; k++) {
-							sy [j][k] += w * other_rel[j][k];
-							sxy[j][k] += w * other_rel[j][k] * ix;
-						}
-					}
-				}
-				//ers_reference
-				double [][] diff_pull = new double [2][3]; 
-				for (int j = 0; j < diff_pull.length; j++) {
-					for (int k = 0; k < diff_pull[j].length; k++) {
-						diff_pull[j][k]=(sy[j][k]*sx2 - sxy[j][k]*sx) / (s0*sx2 - sx*sx);
-					}
-				}
-				double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull);
 				double []      lma_rms = new double[2];
+				double [][] initial_pose = scenes_xyzatr[nscene].clone();
 	    		boolean adjust_OK = false;
 	    		if (est_shift < min_max[0]) {
 	    			fail_reason[0]=FAIL_REASON_MIN;
-	    		} else {	
+	    		} else {
 	    			scenes_xyzatr[nscene] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
 	    					clt_parameters,          //CLTParameters  clt_parameters,
 	    					use_lma_dsi,             //,boolean        use_lma_dsi,
@@ -1678,8 +1719,8 @@ public class Interscene {
 	    					interscene_ref_disparity,// double []      ref_disparity, // null or alternative reference disparity
 	    					reliable_ref,            // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
 	    					scenes_xyzatr[ref_index],// double [][]    scene0_xyzatr,
-	    					scene_xyzatr,            // double [][]    scene1_xyzatr,
-	    					scene_pull,              // double [][]    scene1_xyzatr_pull,
+	    					scenes_xyzatr[nscene],   // double [][]    scene1_xyzatr,
+	    					scenes_xyzatr_pull[nscene], // double [][]    scene1_xyzatr_pull,
 	    					param_select,            // boolean[]      param_select,
 	    					param_regweights,        // double []      param_regweights,
 	    					lma_rms,                 // double []      rms_out, // null or double [2]
@@ -1705,21 +1746,18 @@ public class Interscene {
 				// FAIL_REASON_ROLL
 				handle_failure: {
 					if (!adjust_OK) {
+						scenes_xyzatr[nscene] = initial_pose.clone(); // restore pre-adjust
 						boolean OK = false;
 						System.out.println("LMA failed at nscene = "+nscene+". Reason = "+fail_reason[0]+
 								" ("+getFailReason(fail_reason[0])+")");
 						if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA))) {
 							fpn_list.add(nscene);
-							scenes_pull[nscene] = scene_pull; // need to clone()?
-							scenes_param_select[nscene] = param_select; // now not needed?
+							quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+									QuadCLTCPU.scaleDtToErs(
+											clt_parameters,
+											dxyzatr_dt[nscene]));		
 							if (fpn_skip) {
 								System.out.println("fpn_skip is set, just skipping this scene");
-								failed_scenes[nscene] = true;
-								num_failed++;
-								scenes_xyzatr[nscene] = new double[][] {
-									scene_xyz_pre, // for now restore original, pre-adjustment
-									scene_atr_pre  // for now restore original, pre-adjustment
-								};
 								break handle_failure;
 							} else {
 								System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
@@ -1807,9 +1845,9 @@ public class Interscene {
 						reliable_ref,            // boolean []     reliable_ref, // null or bitmask of reliable reference tiles
 						scenes_xyzatr[fpn_pairs[ipair][1]],// double [][]    scene0_xyzatr,
 						scenes_xyzatr[fpn_pairs[ipair][0]],            // double [][]    scene1_xyzatr,
-						scenes_pull  [fpn_pairs[ipair][0]],              // double [][]    scene1_xyzatr_pull,
-						scenes_param_select[fpn_pairs[ipair][0]],            // boolean[]      param_select,
-						param_regweights,        // double []      param_regweights,
+						scenes_xyzatr_pull [fpn_pairs[ipair][0]],              // double [][]    scene1_xyzatr_pull,
+    					param_select,            // boolean[]      param_select,
+    					param_regweights,        // double []      param_regweights,
 						lma_rms,                 // double []      rms_out, // null or double [2]
 						max_rms,                 // double         max_rms,
 						mb_en,                   // boolean        mb_en,
@@ -1818,9 +1856,13 @@ public class Interscene {
 						clt_parameters.imp.debug_level); // int            debugLevel)
 				boolean adjust_OK = rel_xyzatr != null;
 				if (!adjust_OK) {
-					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][1]+
-							" referenced from scene "+fpn_pairs[ipair][0]+". Reason = "+fail_reason[0]+
+					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
+							" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
 							" ("+getFailReason(fail_reason[0])+")");
+					quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+							QuadCLTCPU.scaleDtToErs(
+									clt_parameters,
+									dxyzatr_dt[fpn_pairs[ipair][0]]));		
 				} else {
 					scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
 					double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(

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,17 +277,140 @@ 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
      * this scene IMS data
@@ -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();