debugging longer series

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

@@ -225,11 +225,13 @@ public class ErsCorrection extends GeometryCorrection {
 			boolean use_ERS,
 			boolean use_ERS_tilt) {
 		boolean [] param_select = new boolean[DP_NUM_PARS];
+		boolean use_ERS_roll = use_ERS && use_ERS_tilt;
 		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) param_select[DP_DSVAZ] = true;
+		if (use_ERS)                 param_select[DP_DSVAZ] = true;
 		if (use_ERS && use_ERS_tilt) param_select[DP_DSVTL] = true;
+		if (use_ERS_roll)            param_select[DP_DSVRL] = true;
 		return param_select;
 	}
 	

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

@@ -328,10 +328,12 @@ public class Interscene {
 				clt_parameters, // final CLTParameters          clt_parameters,
 				batch_mode,     // final boolean                batch_mode,
 				readjust,       // final boolean                readjust,
-				quadCLTs,       //final QuadCLT[]              quadCLTs, //
-				ref_index,      // final int                    ref_index,
-				cent_index,     // final int                    cent_index,
-				earliest_scene2, // int                earliest_index//
+				false,          // final int set_self,   // set record for the new reference to new reference
+				quadCLTs,       // final QuadCLT[]              quadCLTs, //
+				ref_index,      // final int                    last_index,  // last to process (normally old reference)
+				cent_index,     // final int                    ref_index,  // new reference index (center)
+				ref_index,      // final int                    ref_old,    // original ref_index (normally == last_index)				
+				earliest_scene2, //final int                    earliest_index// first to process
 				debugLevel);    // final int                    debugLevel
 		String cent_ts = quadCLTs[cent_index].getImageName();
 		// write config for both ref_index and cent_index scenes
@@ -383,37 +385,46 @@ public class Interscene {
 	public static int invertInitialOrientation(
 			final CLTParameters          clt_parameters,
 			final boolean                batch_mode,
-			final boolean                readjust, 
+			final boolean                readjust,
+			final boolean                set_self,   // set record for the new reference to new reference
 			final QuadCLT[]              quadCLTs,   //
 			final int                    last_index,  // original ref_index (last)
 			final int                    ref_index,  // new reference index (center)
-			final int                    earliest_index,
+			final int                    ref_old,    // original ref_index (normally == last_index)
+			final int                    earliest_index, // only used with readjust
 			final int                    debugLevel
 			) {
-		ErsCorrection ers_old_reference = quadCLTs[last_index].getErsCorrection();
 		
+		if (debugLevel > -3) {
+			System.out.println("\ninvertInitialOrientation(): earliest_index "+earliest_index+", ref_index="+
+					ref_index+", last_index="+last_index+". readjust="+readjust+"\n");
+		}
+		ErsCorrection ers_old_reference = quadCLTs[ref_old].getErsCorrection();
+		int offs = set_self ? 0 : 1;
 		ErsCorrection ers_reference =    quadCLTs[ref_index].getErsCorrection();
 		String cent_ts = quadCLTs[ref_index].getImageName();
 		double [][] center_xyzatr = new double [][] {ers_old_reference.getSceneXYZ(cent_ts), ers_old_reference.getSceneATR(cent_ts)};
 		double [][] inv_cent_xyzatr = ErsCorrection.invertXYZATR(center_xyzatr);
-		// Invert half-sequence to reference cent_index
-		for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
-			double [][] scene_xyzatr,dxyzatr_dt;
-			if (scene_index == last_index) {
-				scene_xyzatr = new double [2][3];
-				dxyzatr_dt = ers_old_reference.getErsXYZATR_dt();
-			} else {
-				String ts = quadCLTs[scene_index].getImageName();
-				scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
-				dxyzatr_dt =   ers_old_reference.getSceneErsXYZATR_dt(ts);
+		if (!readjust) {
+			// Invert half-sequence to reference cent_index
+			for (int scene_index = ref_index + offs; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
+				double [][] scene_xyzatr,dxyzatr_dt;
+				if (scene_index == ref_old) {
+					scene_xyzatr = new double [2][3];
+					dxyzatr_dt = ers_old_reference.getErsXYZATR_dt();
+				} else {
+					String ts = quadCLTs[scene_index].getImageName();
+					scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
+					dxyzatr_dt =   ers_old_reference.getSceneErsXYZATR_dt(ts);
+				}
+				double [][] scene_cent_xyzatr = ErsCorrection.combineXYZATR(scene_xyzatr, inv_cent_xyzatr);
+				ers_reference.addScene(quadCLTs[scene_index].getImageName(),
+						scene_cent_xyzatr,
+						dxyzatr_dt  // ers_scene.getErsXYZATR_dt(),		
+						);
 			}
-			double [][] scene_cent_xyzatr = ErsCorrection.combineXYZATR(scene_xyzatr, inv_cent_xyzatr);
-			ers_reference.addScene(quadCLTs[scene_index].getImageName(),
-					scene_cent_xyzatr,
-					dxyzatr_dt  // ers_scene.getErsXYZATR_dt(),		
-					);
-		}
-		if (readjust) {
+			return 0;
+		} else { //		if (readjust) {
 			boolean[]    param_select = 
 					ErsCorrection.getParamSelect( // ZR - always
 						true,   // boolean use_XY
@@ -429,8 +440,12 @@ public class Interscene {
 	        	dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
 				
 			}
-			
-			double [] min_max = {0, 10000, 0.0} ; // {min, max, actual rms)
+			int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
+			int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
+	        int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
+			double min_offset =         clt_parameters.imp.min_offset; // 0.0; // 
+			double max_offset =         clt_parameters.imp.max_rel_offset * tilesX * tile_size;
+			double [] min_max = {min_offset, 2*max_offset, 0.0} ; // {min, max, actual rms)
 			// set reference
 			double  min_ref_str =        clt_parameters.imp.min_ref_str;
 			boolean ref_need_lma =       clt_parameters.imp.ref_need_lma;
@@ -439,9 +454,6 @@ public class Interscene {
 			double  sfm_minmax =         clt_parameters.imp.sfm_minmax;  //10.0;   // minimal value of the SfM gain maximum to consider available
 			double  sfm_fracmax =        clt_parameters.imp.sfm_fracmax; // 0.75;  // minimal fraction of the SfM maximal gain
 			double  sfm_fracall =        clt_parameters.imp.sfm_fracall; // 0.3;   // minimal relative area of the SfM-e		
-			int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
-			int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
-	        int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
 	     	boolean fmg_initial_en =     clt_parameters.imp.fmg_initial_en; // enable IMS-based FPN mitigation for initial orientation
 	    	double  fmg_distance =       clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
 	    	double  fmg_max_quad =       clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
@@ -492,9 +504,9 @@ public class Interscene {
 	    	ArrayList<Integer> fpn_list = new ArrayList<Integer>();
 			double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
 			int []    fail_reason = new int[1];   // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
-			boolean fpn_skip = true;
+//			boolean fpn_skip = true;
 			double [][] last_corr_xyzatr = {ZERO3,ZERO3};
-			for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
+			for (int scene_index = ref_index + offs; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
 				double [][] scene_xyzatr_old; // ,dxyzatr_dt;
 				if (scene_index == last_index) {
 					scene_xyzatr_old = new double [2][3];
@@ -623,7 +635,7 @@ public class Interscene {
 						quadCLTs,          // QuadCLT []         quadCLTs,
 						scenes_xyzatr,     // double [][][]      scenes_xyzatr,
 						avg_z,             // double             avg_z,
-						ref_index,         // int                ref_index, // >= earliest_scene 
+						last_index, // ref_index,         // int                ref_index, // >= earliest_scene 
 						earliest_index);   // int                earliest_scene)
 				for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
 					if (debugLevel > -4) {
@@ -1827,8 +1839,8 @@ public class Interscene {
 		boolean restore_imu = false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
 		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,
+				true, // !freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
+				true, // readjust_xy_ims || lpf_xy,  // boolean use_AT,
 				!disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
 				!disable_ers_y); // boolean use_ERS_tilt);
 		final double []    param_regweights =  configured_lma? clt_parameters.ilp.ilma_regularization_weights :
@@ -1992,6 +2004,8 @@ public class Interscene {
 
         // optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
         // double         avg_rlen
+        readjust_xy_ims = false;
+        
         if (lpf_xy && (reg_weight_xy > 0.0)) {
         	scenes_xyzatr_pull = QuadCLT.refineFromLPF(
         			clt_parameters, // CLTParameters clt_parameters,
@@ -5079,7 +5093,7 @@ public class Interscene {
 				quadCLTs,       // final QuadCLT[]     quadCLTs,
 				ref_index,      // final int           ref_index,
 				null,           // double [][][]       dxyzatr,
-	    		0,              // final int           early_index,
+				earliest_scene, // final int           early_index,
 	    		(quadCLTs.length -1) // int           last_index,
 	    		);
 		int cent_index =        earliest_scene + (quadCLTs.length - earliest_scene) / 2;

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

@@ -5032,6 +5032,32 @@ public class OpticalFlow {
 					if (debugLevel> -3) {
 						System.out.println("Egomotion table saved to "+ego_path);
 					}
+					// reference to earliest
+					String  ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
+							quadCLTs[ref_index].getImageName()+
+							"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv"; 
+					Interscene.invertInitialOrientation(
+							clt_parameters,   // final CLTParameters          clt_parameters,
+							batch_mode,       // final boolean                batch_mode,
+							false,            // final boolean                readjust,
+							true,             // final int set_self,   // set record for the new reference to new reference
+							quadCLTs,         // final QuadCLT[]              quadCLTs, //
+							quadCLTs.length-1,// final int                    last_index,  // last to process (normally old reference)
+							earliest_scene,   // final int                    ref_index,  // new reference index (center)
+							ref_index,        // final int                    ref_old,    // original ref_index (normally == last_index)				
+							earliest_scene,   // final int                    earliest_index// first to process
+							debugLevel);      // final int                    debugLevel
+				    Interscene.generateEgomotionTable(
+				    		clt_parameters, // CLTParameters  clt_parameters,
+							quadCLTs, // QuadCLT []     quadCLTs,
+							earliest_scene,//            ref_indx,
+							earliest_scene, // int            earliest_scene,
+							ego_path_early, // String         path,
+							ego_comment); // String         comment);
+					if (debugLevel> -3) {
+						System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
+					}
+				    
 				}				
 			} else if (ims_use) {	
 				earliest_scene =	Interscene.setInitialOrientationsIms(
@@ -5267,7 +5293,7 @@ public class OpticalFlow {
 				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; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
+				double         reg_weight_xy =   0; // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
 				int            mb_ers_index =   clt_parameters.imp.mb_ers_index;
 				int            mb_ers_y_index = clt_parameters.imp.mb_ers_y_index;
 				int            mb_gain_index_pose =  clt_parameters.imp.mb_gain_index_pose; // pose readjust pass to switch to full mb_max_gain from mb_max_gain_inter
@@ -5346,6 +5372,33 @@ public class OpticalFlow {
 					if (debugLevel> -3) {
 						System.out.println("Egomotion table saved to "+ego_path);
 					}
+					
+					// reference to earliest
+					String  ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
+							quadCLTs[ref_index].getImageName()+
+							"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv"; 
+					Interscene.invertInitialOrientation(
+							clt_parameters,   // final CLTParameters          clt_parameters,
+							batch_mode,       // final boolean                batch_mode,
+							false,            // final boolean                readjust,
+							true,             // final int set_self,   // set record for the new reference to new reference
+							quadCLTs,         // final QuadCLT[]              quadCLTs, //
+							quadCLTs.length-1,// final int                    last_index,  // last to process (normally old reference)
+							earliest_scene,   // final int                    ref_index,  // new reference index (center)
+							ref_index,        // final int                    ref_old,    // original ref_index (normally == last_index)				
+							earliest_scene,   // final int                    earliest_index// first to process
+							debugLevel);      // final int                    debugLevel
+				    Interscene.generateEgomotionTable(
+				    		clt_parameters, // CLTParameters  clt_parameters,
+							quadCLTs, // QuadCLT []     quadCLTs,
+							earliest_scene,//            ref_indx,
+							earliest_scene, // int            earliest_scene,
+							ego_path_early, // String         path,
+							ego_comment); // String         comment);
+					if (debugLevel> -3) {
+						System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
+					}
+					
 				}				
 			}
 		}
@@ -8653,14 +8706,16 @@ public class OpticalFlow {
 		boolean generate_outlines = false; // true; // TODO: move to configs
 		System.out.println("intersceneExport(), scene timestamp="+ref_scene.getImageName());
 //		int num_scenes = scenes.length;
-		String [] combo_dsn_titles_full = COMBO_DSN_TITLES.clone();
+		String [] combo_dsn_titles_full = COMBO_DSN_TITLES.clone(); // keep all titles and slices
 		String [] combo_dsn_titles = new String [COMBO_DSN_INDX_DISP_BG];
+		/*
 		for (int i = 0; i < combo_dsn_titles.length; i++) {
 			combo_dsn_titles[i] = combo_dsn_titles_full[i]; 
 		}
 		if (clt_parameters.rig.mll_max_refines_bg <= 0) {
 			combo_dsn_titles_full = combo_dsn_titles;
 		}
+		*/
 		
 		double min_disp_change = clt_parameters.rig.mll_min_disp_change_pre; // 0.001; // stop re-measure when difference is below
 		final int max_refines_presplit =
@@ -8783,9 +8838,13 @@ public class OpticalFlow {
 		double [] target_disparity = combo_dsn_change[COMBO_DSN_INDX_DISP].clone();
 		double [] target_disparity_orig = target_disparity.clone(); // will just use NaN/not NaN to restore tasks before second pass with LMA
 //		double [][] combo_dsn_final = new double [combo_dsn_titles.length][combo_dsn[0].length];
+//		double [][] combo_dsn_final = 
+//				new double [(clt_parameters.rig.mll_max_refines_bg > 0)?
+//						combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
 		double [][] combo_dsn_final = 
-				new double [(clt_parameters.rig.mll_max_refines_bg > 0)?
-						combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
+				new double [COMBO_DSN_TITLES.length][combo_dsn[0].length];
+		
+		//COMBO_DSN_INDX_SFM_GAIN
 		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) &&
@@ -9244,7 +9303,7 @@ public class OpticalFlow {
 		} //for (int nrefine = 0; nrefine < max_refines; nrefine++) {
 		
 		// Add duplicate of FG disparity and FG+BG disparity (FG where no BG) for visual comparison
-		if (clt_parameters.rig.mll_max_refines_bg > 0) { 
+		if (true) { // clt_parameters.rig.mll_max_refines_bg > 0) { 
 			for (int nTile =0; nTile < combo_dsn_change[0].length; nTile++) {
 				combo_dsn_final[COMBO_DSN_INDX_DISP_FG][nTile] = combo_dsn_final[COMBO_DSN_INDX_DISP][nTile];
 				if (Double.isNaN(combo_dsn_final[COMBO_DSN_INDX_DISP_BG][nTile])) {

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

@@ -256,8 +256,16 @@ public class QuadCLTCPU {
     	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]; 
+    	double [] sxy = new double[3], sy = new double[3];
+    	double [] vel_ref = new double[3], vel_ref_prev = null;
     	for (int nscene = early_index; nscene <= last_index; nscene++) {
+    		// calculate inertial (reference frame) velocities
+    		Rotation rot = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
+    				xyzatr[nscene][1][0],    xyzatr[nscene][1][1],    xyzatr[nscene][1][2]);
+    		rot.applyTo(dxyzatr[nscene][0], vel_ref);
+    		if (vel_ref_prev == null) {
+    			vel_ref_prev = vel_ref;
+    		}
     		double t = quadCLTs[nscene].getTimeStamp();
     		double x = t - t00; // from early_index
     		tim[nscene] = x;
@@ -270,12 +278,15 @@ public class QuadCLTCPU {
     			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]);
+//    				xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene][0][i]);
+    				xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (vel_ref_prev[i] + vel_ref[i]);
+    				
     			}
     			double y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
     			sy[i] += y;
     			sxy[i] += x*y;
     		}
+    		vel_ref_prev = vel_ref;
     	}
     	
     	double denom = sx2 * s0 - sx * sx;
@@ -475,7 +486,19 @@ public class QuadCLTCPU {
     	}
     	return dxyzatr;
     }
-
+    
+    /**
+     * Integrate position and orientation of the camera relative to the
+     * (inertial) reference frame using IMU-derived local angular and 
+     * linera velocities
+     * @param clt_parameters configuration parameters
+     * @param quadCLTs       scenes sequence
+     * @param ref_index      reference scene index 
+     * @param dxyzatr        local linear and angular velocities
+     * @param early_index    earliest (lowest) scene index in quadCLTs to use
+     * @param last_index     last (highest) scene index to use
+     * @return  per-scene {{X,Y,Z},{A,T,R}} relative to the reference frame
+     */
     public static double [][][] integratePIMU(
     		final CLTParameters clt_parameters,
     		final QuadCLT[]     quadCLTs,