Initial ERS from IMS

src/main/java/com/elphel/imagej/ims/Imx5.java

@@ -186,26 +186,12 @@ public class Imx5 {
 		}
 		return rslt;
 	}
-	
-	public static double [] quaternionImsToCam(double[]quat) {
-		double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
-		Rotation cam_to_ims0 = new Rotation(ort, 1E-8);
-		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
-		Rotation cam_quat = cam_to_ims0.applyTo(ims_rot);
-		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
-	}
 
-	/**
-	 * Get quaternion from IMS data, fixed ortho rotation for IMU to camera and small
-	 * additional correction of IMU mount to the camera using camera axes (Y- up, X - right,
-	 * z - back from the target)
-	 * @param quat    IMU-measured quaternion
-	 * @param ims_atr azimuth, tilt, roll of the IMU mount relative to the camera axes
-	 * @return quaternion to apply to NED to get camera-referenced coordinates
-	 */
-	public static double [] quaternionImsToCam(double[]quat, double [] ims_atr) {
-		double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
-		Rotation ims_to_mount_ortho = new Rotation(ort, 1E-8);
+	public static double [] quaternionImsToCam(
+			double [] quat,      // ims_to_ned
+			double [] ims_atr, // -> mount_to_cam
+			double [] quat_ort) { // -> ims_to_mount_ortho
+		Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
 		Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
 				ims_atr[0],    ims_atr[1],    ims_atr[2]);
@@ -213,76 +199,25 @@ public class Imx5 {
 		Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
 		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
 	}
-	
+
+	// from ims frame to camera frame
 	public static double [] quaternionImsToCam(
-			double[]quat,
-			double [] ims_atr,
-			double [] quat_ort) {
+			double [] ims_atr, // -> mount_to_cam
+			double [] quat_ort) { // -> ims_to_mount_ortho
 		Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
-		Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
 				ims_atr[0],    ims_atr[1],    ims_atr[2]);
-		Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
-		Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
+		Rotation cam_quat = mount_to_cam.applyTo(ims_to_mount_ortho);
 		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
 	}
 	
+	
+	
 	public static double [] quatToCamAtr(double[]quat) {
 		Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);		
 	}
 	
-	public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
-		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
-//		RotationOrder ro =  zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
-		RotationOrder ro = RotationOrder.XYZ;
-		switch (ord) {
-		case 0: ro = RotationOrder.XYZ; break;
-		case 1: ro = RotationOrder.XZY; break;
-		case 2: ro = RotationOrder.YXZ; break;
-		case 3: ro = RotationOrder.YZX; break;
-		case 4: ro = RotationOrder.ZXY; break;
-		case 5: ro = RotationOrder.ZYX; break;
-		}
-		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
-		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
-//		Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
-		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc,  -Math.PI/2, 0,  -Math.PI/2)):(new Rotation(ro, rc,  Math.PI/2, 0,  Math.PI/2));
-		double [] angles1 = cam_to_ims.getAngles(ro1, rc);
-		Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
-				//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
-		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
-		double [] angles = comb_rot.getAngles(ro1, rc);
-		return angles;
-	}
-	
-	public static double [] imsQToCamRotations(double [] ims_qn2b, int ord, boolean rev_order, boolean rev_matrix ) {
-		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
-//		RotationOrder ro =  zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
-		RotationOrder ro = RotationOrder.XYZ;
-		switch (ord) {
-		case 0: ro = RotationOrder.XYZ; break;
-		case 1: ro = RotationOrder.XZY; break;
-		case 2: ro = RotationOrder.YXZ; break;
-		case 3: ro = RotationOrder.YZX; break;
-		case 4: ro = RotationOrder.ZXY; break;
-		case 5: ro = RotationOrder.ZYX; break;
-		}
-		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
-		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
-//		Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
-		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc,  -Math.PI/2, 0,  -Math.PI/2)):(new Rotation(ro, rc,  Math.PI/2, 0,  Math.PI/2));
-		double [] angles1 = cam_to_ims.getAngles(ro1, rc);
-		
-//		Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
-//  public Rotation(double q0, double q1, double q2, double q3,  boolean needsNormalization) {
-		Rotation ims_rot = new Rotation(ims_qn2b[0],ims_qn2b[1],ims_qn2b[2],ims_qn2b[3],true);
-		//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
-		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
-		double [] angles = comb_rot.getAngles(ro1, rc);
-		return angles;
-	}
-
 	public static double [] nedFromLla (double [] lla, double [] lla_ref) {
 		double [] ned = new double[] { 
 				EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,

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

@@ -260,6 +260,13 @@ public class ErsCorrection extends GeometryCorrection {
 		this.ers_wxyz_center_dt = ers_xyz_dt;
 		this.ers_watr_center_dt = ers_atr_dt;
 	}
+	
+	public void setErsDt(
+			double [][]    ers_xyzatr_dt) {
+		this.ers_wxyz_center_dt = ers_xyzatr_dt[0];
+		this.ers_watr_center_dt = ers_xyzatr_dt[1];
+	}
+	
 	public void setErsDt_test(
 			double []    ers_xyz_dt,
 			double []    ers_atr_dt) {

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

@@ -277,6 +277,7 @@ public class Interscene {
 		return earliest_scene2;
 	}
 	
+	
 	public static int setInitialOrientationsIms(
 			final CLTParameters          clt_parameters,
 			int                          min_num_scenes,
@@ -290,6 +291,7 @@ public class Interscene {
 			final int                    threadsMax,  // int               threadsMax,
 			final boolean                updateStatus,
 			final int                    debugLevel) {
+//		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
 		double maximal_series_rms = 0.0;
 		double  min_ref_str =        clt_parameters.imp.min_ref_str;
 		boolean ref_need_lma =       clt_parameters.imp.ref_need_lma;
@@ -300,7 +302,10 @@ public class Interscene {
     	boolean ims_use =         clt_parameters.imp.ims_use;
 		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
 		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
-		double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz;    // not yet used
+//		double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz;    // not yet used
+//		double [] quat_ims_cam = Imx5.quaternionImsToCam(
+//				ims_mount_atr, // new double[] {0, 0.13, 0},
+//				ims_ortho);
     	
     	double boost_max_short = 2.0; // 
     	double boost_zoom_short = 1.5; // 
@@ -380,7 +385,7 @@ public class Interscene {
 		boolean after_spiral = false;
 		boolean got_spiral = false;
 //		int    search_rad =        clt_parameters.imp.search_rad;    // 10;
-		
+		boolean scale_ims_velocities = true;
 		Did_ins_2   d2_ref = quadCLTs[ref_index].did_ins_2;
 		// apply correction to orientation
 		double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
@@ -389,11 +394,19 @@ public class Interscene {
 		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};
+		double [][] cam_dxyzatr_ref = quadCLTs[ref_index].getDxyzatrIms(
+				clt_parameters,
+				scale_ims_velocities); // scale IMS velocities
 		ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
 				scenes_xyzatr[ref_index][0],
 				scenes_xyzatr[ref_index][1],
-				ZERO3, // ers_scene.getErsXYZ_dt(),		
-				ZERO3); // ers_scene.getErsATR_dt()		
+				cam_dxyzatr_ref[0],  // ZERO3, // ers_scene.getErsXYZ_dt(),		
+				cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt()	
+		// Will be used in prepareLMA()
+		quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+				QuadCLTCPU.scaleDtToErs(
+						clt_parameters,
+						cam_dxyzatr_ref));		
 		for (int scene_index =  ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
 			if ((ref_index - scene_index) >= max_num_scenes){
 				earliest_scene = scene_index + 1;
@@ -462,6 +475,15 @@ public class Interscene {
 							" pixels");
 				}
 			}
+			double [][] cam_dxyzatr = quadCLTs[scene_index].getDxyzatrIms(
+					clt_parameters,
+					scale_ims_velocities); // scale IMS velocities
+			// Will be used in prepareLMA()
+			quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+					QuadCLTCPU.scaleDtToErs(
+							clt_parameters,
+							cam_dxyzatr));		
+			
 			// Does not use motion blur for reference scene here!
 			scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
 					clt_parameters,      // CLTParameters  clt_parameters,
@@ -562,12 +584,15 @@ public class Interscene {
 				}
 			}
 			
-			// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes
+			double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
+					clt_parameters,
+					quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
+							));
 			ers_reference.addScene(scene_QuadClt.getImageName(),
 					scenes_xyzatr[scene_index][0],
 					scenes_xyzatr[scene_index][1],
-					ZERO3, // ers_scene.getErsXYZ_dt(),		
-					ZERO3 // ers_scene.getErsATR_dt()		
+					adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),		
+					adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()		
 					);
 		    if (lma_rms[0] > maximal_series_rms) {
 		        maximal_series_rms = lma_rms[0];
@@ -1103,7 +1128,8 @@ public class Interscene {
         
 		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][][];
+
 		scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
 		// should have at least next or previous non-null
 		int debug_scene = 68; // -8;
@@ -1132,6 +1158,7 @@ public class Interscene {
         		}
         		scenes_xyzatr[nscene] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
         	}
+        	dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
         	if (debug_ers) {
         		System.out.println(String.format("%3d xyz:    %9.5f %9.5f %9.5f   atr:    %9.5f %9.5f %9.5f",
         				nscene,
@@ -1139,7 +1166,6 @@ public class Interscene {
         				scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2]));
         	}
         }
-        double [][][] dxyzatr_dt;
         switch (ers_mode) {
         case 1 : 
         	dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
@@ -1150,13 +1176,13 @@ public class Interscene {
         			scenes_xyzatr,   // double [][][]  scenes_xyzatr,
         			half_run_range); // double         half_run_range
         	break;
-        case 2: // read from ims
-        default:
-        	dxyzatr_dt = new double [quadCLTs.length][][];
-        	for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
-        		// read from map (ts)
-        		// divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
-        	}        		
+//        case 2: // read from ims
+        default: // do nothing - already read
+//        	dxyzatr_dt = new double [quadCLTs.length][][];
+//        	for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
+//        		// read from map (ts)
+//        		// divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
+//        	}        		
 
         }
     	if (debug_ers) {
@@ -1214,7 +1240,7 @@ public class Interscene {
 			double []   scene_xyz_pre = ZERO3;
 			double []   scene_atr_pre = ZERO3;
 			// find correct signs when setting. dxyzatr_dt[][] is also used for motion blur (correctly)
-			
+			/*
 			double [][] scaled_dxyzatr_dt = new double[2][3];
 			for (int i = 0; i < scaled_dxyzatr_dt.length; i++) {
 				for (int j = 0; j < scaled_dxyzatr_dt[i].length; j++) {
@@ -1226,6 +1252,12 @@ public class Interscene {
 			quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
 					scaled_dxyzatr_dt[0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double []    ers_xyz_dt,
 					scaled_dxyzatr_dt[1]); // dxyzatr_dt[nscene][1]); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
+			
+			quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
+					QuadCLTCPU.scaleDtToErs(
+							clt_parameters,
+							dxyzatr_dt[nscene]));		
+			*/
 			if (dbg_mb_img != null) {
 				boolean show_corrected = false;
 				if (nscene == debug_scene) {
@@ -1304,15 +1336,14 @@ public class Interscene {
 //					double [][] dxyzatr_dt_ref = getVelocities( // was already set
 //							quadCLTs, // QuadCLT []     quadCLTs,
 //							ref_index);  // int            nscene)
-					double [][] dxyzatr_dt_ref = dxyzatr_dt[nscene];
 					mb_vectors_ref = OpticalFlow.getMotionBlur(
 							quadCLTs[ref_index],   // QuadCLT        ref_scene,
 							quadCLTs[ref_index],   // QuadCLT        scene,         // can be the same as ref_scene
 							ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
 							ZERO3,                 // double []      camera_xyz,
 							ZERO3,                 // double []      camera_atr,
-							dxyzatr_dt_ref[0],     // double []      camera_xyz_dt,
-							dxyzatr_dt_ref[1],     // double []      camera_atr_dt,
+							dxyzatr_dt[nscene][0],     // double []      camera_xyz_dt,
+							dxyzatr_dt[nscene][1],     // double []      camera_atr_dt,
 							0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
 							debugLevel);           // int            debug_level)
 				}
@@ -1477,12 +1508,24 @@ public class Interscene {
 					}
 				}
 				// overwrite old data
+				// undo velocities->ers scaling
+				double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
+						clt_parameters,
+						quadCLTs[nscene].getErsCorrection().getErsXYZATR_dt(
+								));
+//				ers_reference.addScene(ts,
+//						scenes_xyzatr[nscene][0],
+//						scenes_xyzatr[nscene][1],
+//						quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(),	// same as dxyzatr_dt[nscene][0], just keep for future adjustments?	
+//						quadCLTs[nscene].getErsCorrection().getErsATR_dt()	// same as dxyzatr_dt[nscene][1], just keep for future adjustments?		
+//						);
 				ers_reference.addScene(ts,
 						scenes_xyzatr[nscene][0],
 						scenes_xyzatr[nscene][1],
-						quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(),	// same as dxyzatr_dt[nscene][0], just keep for future adjustments?	
-						quadCLTs[nscene].getErsCorrection().getErsATR_dt()	// same as dxyzatr_dt[nscene][1], just keep for future adjustments?		
+						adjusted_xyzatr_dt[0],	
+						adjusted_xyzatr_dt[1]		
 						);
+
 				if (lma_rms[0] > maximal_series_rms) {
 					maximal_series_rms = lma_rms[0];
 				}
@@ -1717,12 +1760,15 @@ public class Interscene {
 				for (int i:ErsCorrection.DP_ERS_INDICES) {
 					param_select_mod[i] = false;
 				}
+				// TODO Removed on 11/23/2023 for use with IMS. Set reasonable data if IMS is not used
+				/*
 				// now just copy ers from the reference 
 				ErsCorrection ers_ref =   reference_QuadClt.getErsCorrection();
 				ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
 				// when disabled - scene ers from the same as reference one
 				ers_scene.ers_watr_center_dt = ers_ref.ers_watr_center_dt.clone();
 				ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
+				*/
 			}
 			// TODO: save ers_scene.ers_watr_center_dt and ers_scene.ers_wxyz_center_dt
 			intersceneLma.prepareLMA(
@@ -3728,340 +3774,7 @@ public class Interscene {
 		ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
 		return tp_tasks_ref;
     }
-	@Deprecated
-	public static void generateEgomotionTable0(
-			CLTParameters  clt_parameters,
-			QuadCLT []     quadCLTs,
-			int            ref_index,
-			int            earliest_scene,
-			String         path,
-			String         comment) {
-		boolean use_euler = true;
-		boolean use_qn2b = true;
-		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
-		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
-//    	double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
-		String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
-			"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
-		String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
-			"\tlat\tlong\talt\tned0\tned1\tned2";
-		String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
-		String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
-				"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
-				"\tabs_A\tabs_T\tabs_R\trel_A\trel_T\trel_R"+
-				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
-		
-		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
-		
-		String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
-		String header_cam001= "\ttheta0-001\ttheta1-001\ttheta2-001";
-		String header_cam010= "\ttheta0-010\ttheta1-010\ttheta2-010";
-		String header_cam011= "\ttheta0-011\ttheta1-011\ttheta2-011";
-		String header_cam100= "\ttheta0-100\ttheta1-100\ttheta2-100";
-		String header_cam101= "\ttheta0-101\ttheta1-101\ttheta2-101";
-		String header_cam110= "\ttheta0-110\ttheta1-110\ttheta2-110";
-		String header_cam111= "\ttheta0-111\ttheta1-111\ttheta2-111";
-		
-		String header_cam200= "\ttheta0-200\ttheta1-200\ttheta2-200";
-		String header_cam201= "\ttheta0-201\ttheta1-201\ttheta2-201";
-		String header_cam210= "\ttheta0-210\ttheta1-210\ttheta2-210";
-		String header_cam211= "\ttheta0-211\ttheta1-211\ttheta2-211";
-		String header_cam300= "\ttheta0-300\ttheta1-300\ttheta2-300";
-		String header_cam301= "\ttheta0-301\ttheta1-301\ttheta2-301";
-		String header_cam310= "\ttheta0-310\ttheta1-310\ttheta2-310";
-		String header_cam311= "\ttheta0-311\ttheta1-311\ttheta2-311";
-		
-		String header_cam400= "\ttheta0-400\ttheta1-400\ttheta2-400";
-		String header_cam401= "\ttheta0-401\ttheta1-401\ttheta2-401";
-		String header_cam410= "\ttheta0-410\ttheta1-410\ttheta2-410";
-		String header_cam411= "\ttheta0-411\ttheta1-411\ttheta2-411";
-		String header_cam500= "\ttheta0-500\ttheta1-500\ttheta2-500";
-		String header_cam501= "\ttheta0-501\ttheta1-501\ttheta2-501";
-		String header_cam510= "\ttheta0-510\ttheta1-510\ttheta2-510";
-		String header_cam511= "\ttheta0-511\ttheta1-511\ttheta2-511";
-		
-		String header_qn2b000= "\tqn2b0-000\tqn2b1-000\tqn2b2-000";
-		String header_qn2b001= "\tqn2b0-001\tqn2b1-001\tqn2b2-001";
-		String header_qn2b010= "\tqn2b0-010\tqn2b1-010\tqn2b2-010";
-		String header_qn2b011= "\tqn2b0-011\tqn2b1-011\tqn2b2-011";
-		String header_qn2b100= "\tqn2b0-100\tqn2b1-100\tqn2b2-100";
-		String header_qn2b101= "\tqn2b0-101\tqn2b1-101\tqn2b2-101";
-		String header_qn2b110= "\tqn2b0-110\tqn2b1-110\tqn2b2-110";
-		String header_qn2b111= "\tqn2b0-111\tqn2b1-111\tqn2b2-111";
-
-		String header_qn2b200= "\tqn2b0-200\tqn2b1-200\tqn2b2-200";
-		String header_qn2b201= "\tqn2b0-201\tqn2b1-201\tqn2b2-201";
-		String header_qn2b210= "\tqn2b0-210\tqn2b1-210\tqn2b2-210";
-		String header_qn2b211= "\tqn2b0-211\tqn2b1-211\tqn2b2-211";
-		String header_qn2b300= "\tqn2b0-300\tqn2b1-300\tqn2b2-300";
-		String header_qn2b301= "\tqn2b0-301\tqn2b1-301\tqn2b2-301";
-		String header_qn2b310= "\tqn2b0-310\tqn2b1-310\tqn2b2-310";
-		String header_qn2b311= "\tqn2b0-311\tqn2b1-311\tqn2b2-311";
-
-		String header_qn2b400= "\tqn2b0-400\tqn2b1-400\tqn2b2-400";
-		String header_qn2b401= "\tqn2b0-401\tqn2b1-401\tqn2b2-401";
-		String header_qn2b410= "\tqn2b0-410\tqn2b1-410\tqn2b2-410";
-		String header_qn2b411= "\tqn2b0-411\tqn2b1-411\tqn2b2-411";
-		String header_qn2b500= "\tqn2b0-500\tqn2b1-500\tqn2b2-500";
-		String header_qn2b501= "\tqn2b0-501\tqn2b1-501\tqn2b2-501";
-		String header_qn2b510= "\tqn2b0-510\tqn2b1-510\tqn2b2-510";
-		String header_qn2b511= "\tqn2b0-511\tqn2b1-511\tqn2b2-511";
-		
-		String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
-		if (use_qn2b) {
-			header+=
-					header_qn2b000 + header_qn2b001 + header_qn2b010 + header_qn2b011 +
-					header_qn2b100 + header_qn2b101 + header_qn2b110 + header_qn2b111 +
-					header_qn2b200 + header_qn2b201 + header_qn2b210 + header_qn2b211 +
-					header_qn2b300 + header_qn2b301 + header_qn2b310 + header_qn2b311 +
-					header_qn2b400 + header_qn2b401 + header_qn2b410 + header_qn2b411 +
-					header_qn2b500 + header_qn2b501 + header_qn2b510 + header_qn2b511;
-		}
-		if (use_euler) {
-			header+=
-				header_cam000 + header_cam001 + header_cam010 + header_cam011 +
-				header_cam100 + header_cam101 + header_cam110 + header_cam111 +
-				header_cam200 + header_cam201 + header_cam210 + header_cam211 +
-				header_cam300 + header_cam301 + header_cam310 + header_cam311 +
-				header_cam400 + header_cam401 + header_cam410 + header_cam411 +
-				header_cam500 + header_cam501 + header_cam510 + header_cam511;
-		}
-		StringBuffer sb = new StringBuffer();
-		QuadCLT ref_scene = quadCLTs[ref_index];
-		ErsCorrection ers_reference = ref_scene.getErsCorrection();
-		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
-		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
-			QuadCLT scene = quadCLTs[nscene];
-			if (nscene == ref_index) {
-				scenes_xyzatr[nscene] = new double[2][3];
-			} else {
-				String ts = scene.getImageName();
-				scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
-			}
-		}
-		double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
-        		quadCLTs,          // QuadCLT []     scenes, // ordered by increasing timestamps
-        		ref_index,
-        		earliest_scene,    // int            start_scene,
-        		quadCLTs.length-1, // int            end1_scene,
-        		scenes_xyzatr,     // double [][][]  scenes_xyzatr,
-        		clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
-		Did_ins_2   d2_ref = quadCLTs[ref_index].did_ins_2;
-		double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
-				ims_mount_atr, // new double[] {0, 0.13, 0},
-				ims_ortho);
-		double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref);
-		double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
-		
-		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
-			QuadCLT scene = quadCLTs[nscene];
-			if (scene == null) {
-				for (int i = 0; i < 26; i++) {
-					sb.append("\t---");
-				}
-				sb.append("\n");
-				continue;
-			}
-			double timestamp = scene.getTimeStamp();
-			Did_ins_1   d1 = scene.did_ins_1;
-			Did_ins_2   d2 = scene.did_ins_2;
-			Did_pimu    d3 = scene.did_pimu;
-			
-			sb.append(nscene+"\t"+timestamp);
-			// TODO: try saved, not calculated velocities!
-			sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
-			sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
-			
-			sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
-			sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
-			
-//			String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
-//					"\tlong\tlat\talt\tned0\tned1\tned2";
-			sb.append("\t"+d1.timeOfWeek);
-			sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[2]);
-			sb.append("\t"+d1.uvw[0]+  "\t"+d1.uvw[1]+  "\t"+d1.uvw[2]);
-			sb.append("\t"+d1.lla[0]+  "\t"+d1.lla[1]+  "\t"+d1.lla[2]);
-			sb.append("\t"+d1.ned[0]+  "\t"+d1.ned[1]+  "\t"+d1.ned[2]);
-//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
-			sb.append("\t"+d2.timeOfWeek);
-			sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
-			sb.append("\t"+d2.uvw[0]+  "\t"+d2.uvw[1]+  "\t"+d2.uvw[2]);
-			sb.append("\t"+d2.lla[0]+  "\t"+d2.lla[1]+  "\t"+d2.lla[2]);
-			double [] double_theta = d1.getTheta();
-//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
-			double [] double_qn2b = d2.getQn2b();
-			double [] double_uvw =  d2.getUvw();
-			double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
-			double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
-//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp] 			
-			
-			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);			
-			double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
-//			String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
-//					"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
-			
-			sb.append("\t"+ned[0]+  "\t"+ned[1]+  "\t"+ned[2]);             // global axes
-			sb.append("\t"+ims_xyz[0]+  "\t"+ims_xyz[1]+  "\t"+ims_xyz[2]); // imu axes
-			
-			double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
-			double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
-					ims_mount_atr, // new double[] {0, 0.13, 0},
-					ims_ortho);
-			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
-			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
-			
-			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
-			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //
-			
-			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
-			double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
-			double [] scene_rel_atr=ErsCorrection.combineXYZATR(
-					ims_scene_xyzatr,
-					ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
-			
-			sb.append("\t"+scene_abs_atr[0]+  "\t"+scene_abs_atr[1]+  "\t"+scene_abs_atr[2]); // 
-			sb.append("\t"+scene_rel_atr[0]+  "\t"+scene_rel_atr[1]+  "\t"+scene_rel_atr[2]); //
-			
-			
-			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
-			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
-			
-//				String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
-			sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
-			sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
-// 		String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
-			if (use_qn2b) {
-				double [] cam_qn2b_000 = Imx5.imsQToCamRotations(double_qn2b, 0, false, false);		
-				double [] cam_qn2b_001 = Imx5.imsQToCamRotations(double_qn2b, 0, false, true);		
-				double [] cam_qn2b_010 = Imx5.imsQToCamRotations(double_qn2b, 0, true,  false);		
-				double [] cam_qn2b_011 = Imx5.imsQToCamRotations(double_qn2b, 0, true,  true);		
-				double [] cam_qn2b_100 = Imx5.imsQToCamRotations(double_qn2b, 1, false, false);		
-				double [] cam_qn2b_101 = Imx5.imsQToCamRotations(double_qn2b, 1, false, true);		
-				double [] cam_qn2b_110 = Imx5.imsQToCamRotations(double_qn2b, 1, true,  false);		
-				double [] cam_qn2b_111 = Imx5.imsQToCamRotations(double_qn2b, 1, true,  true);
-
-				double [] cam_qn2b_200 = Imx5.imsQToCamRotations(double_qn2b, 2, false, false);		
-				double [] cam_qn2b_201 = Imx5.imsQToCamRotations(double_qn2b, 2, false, true);		
-				double [] cam_qn2b_210 = Imx5.imsQToCamRotations(double_qn2b, 2, true,  false);		
-				double [] cam_qn2b_211 = Imx5.imsQToCamRotations(double_qn2b, 2, true,  true);		
-				double [] cam_qn2b_300 = Imx5.imsQToCamRotations(double_qn2b, 3, false, false);		
-				double [] cam_qn2b_301 = Imx5.imsQToCamRotations(double_qn2b, 3, false, true);		
-				double [] cam_qn2b_310 = Imx5.imsQToCamRotations(double_qn2b, 3, true,  false);		
-				double [] cam_qn2b_311 = Imx5.imsQToCamRotations(double_qn2b, 3, true,  true);
-
-				double [] cam_qn2b_400 = Imx5.imsQToCamRotations(double_qn2b, 4, false, false);		
-				double [] cam_qn2b_401 = Imx5.imsQToCamRotations(double_qn2b, 4, false, true);		
-				double [] cam_qn2b_410 = Imx5.imsQToCamRotations(double_qn2b, 4, true,  false);		
-				double [] cam_qn2b_411 = Imx5.imsQToCamRotations(double_qn2b, 4, true,  true);		
-				double [] cam_qn2b_500 = Imx5.imsQToCamRotations(double_qn2b, 5, false, false);		
-				double [] cam_qn2b_501 = Imx5.imsQToCamRotations(double_qn2b, 5, false, true);		
-				double [] cam_qn2b_510 = Imx5.imsQToCamRotations(double_qn2b, 5, true,  false);		
-				double [] cam_qn2b_511 = Imx5.imsQToCamRotations(double_qn2b, 5, true,  true);
-
-				sb.append("\t"+cam_qn2b_000[0]+"\t"+cam_qn2b_000[1]+"\t"+cam_qn2b_000[2]);
-				sb.append("\t"+cam_qn2b_001[0]+"\t"+cam_qn2b_001[1]+"\t"+cam_qn2b_001[2]);
-				sb.append("\t"+cam_qn2b_010[0]+"\t"+cam_qn2b_010[1]+"\t"+cam_qn2b_010[2]);
-				sb.append("\t"+cam_qn2b_011[0]+"\t"+cam_qn2b_011[1]+"\t"+cam_qn2b_011[2]);
-				sb.append("\t"+cam_qn2b_100[0]+"\t"+cam_qn2b_100[1]+"\t"+cam_qn2b_100[2]);
-				sb.append("\t"+cam_qn2b_101[0]+"\t"+cam_qn2b_101[1]+"\t"+cam_qn2b_101[2]);
-				sb.append("\t"+cam_qn2b_110[0]+"\t"+cam_qn2b_110[1]+"\t"+cam_qn2b_110[2]);
-				sb.append("\t"+cam_qn2b_111[0]+"\t"+cam_qn2b_111[1]+"\t"+cam_qn2b_111[2]);
-
-				sb.append("\t"+cam_qn2b_200[0]+"\t"+cam_qn2b_200[1]+"\t"+cam_qn2b_200[2]);
-				sb.append("\t"+cam_qn2b_201[0]+"\t"+cam_qn2b_201[1]+"\t"+cam_qn2b_201[2]);
-				sb.append("\t"+cam_qn2b_210[0]+"\t"+cam_qn2b_210[1]+"\t"+cam_qn2b_210[2]);
-				sb.append("\t"+cam_qn2b_211[0]+"\t"+cam_qn2b_211[1]+"\t"+cam_qn2b_211[2]);
-				sb.append("\t"+cam_qn2b_300[0]+"\t"+cam_qn2b_300[1]+"\t"+cam_qn2b_300[2]);
-				sb.append("\t"+cam_qn2b_301[0]+"\t"+cam_qn2b_301[1]+"\t"+cam_qn2b_301[2]);
-				sb.append("\t"+cam_qn2b_310[0]+"\t"+cam_qn2b_310[1]+"\t"+cam_qn2b_310[2]);
-				sb.append("\t"+cam_qn2b_311[0]+"\t"+cam_qn2b_311[1]+"\t"+cam_qn2b_311[2]);
-
-				sb.append("\t"+cam_qn2b_400[0]+"\t"+cam_qn2b_400[1]+"\t"+cam_qn2b_400[2]);
-				sb.append("\t"+cam_qn2b_401[0]+"\t"+cam_qn2b_401[1]+"\t"+cam_qn2b_401[2]);
-				sb.append("\t"+cam_qn2b_410[0]+"\t"+cam_qn2b_410[1]+"\t"+cam_qn2b_410[2]);
-				sb.append("\t"+cam_qn2b_411[0]+"\t"+cam_qn2b_411[1]+"\t"+cam_qn2b_411[2]);
-				sb.append("\t"+cam_qn2b_500[0]+"\t"+cam_qn2b_500[1]+"\t"+cam_qn2b_500[2]);
-				sb.append("\t"+cam_qn2b_501[0]+"\t"+cam_qn2b_501[1]+"\t"+cam_qn2b_501[2]);
-				sb.append("\t"+cam_qn2b_510[0]+"\t"+cam_qn2b_510[1]+"\t"+cam_qn2b_510[2]);
-				sb.append("\t"+cam_qn2b_511[0]+"\t"+cam_qn2b_511[1]+"\t"+cam_qn2b_511[2]);
-			}
-
-			if (use_euler) {
-				double [] cam_theta_000 = Imx5.imsToCamRotations(double_theta, 0, false, false);		
-				double [] cam_theta_001 = Imx5.imsToCamRotations(double_theta, 0, false, true);		
-				double [] cam_theta_010 = Imx5.imsToCamRotations(double_theta, 0, true,  false);		
-				double [] cam_theta_011 = Imx5.imsToCamRotations(double_theta, 0, true,  true);		
-				double [] cam_theta_100 = Imx5.imsToCamRotations(double_theta, 1, false, false);		
-				double [] cam_theta_101 = Imx5.imsToCamRotations(double_theta, 1, false, true);		
-				double [] cam_theta_110 = Imx5.imsToCamRotations(double_theta, 1, true,  false);		
-				double [] cam_theta_111 = Imx5.imsToCamRotations(double_theta, 1, true,  true);
-
-				double [] cam_theta_200 = Imx5.imsToCamRotations(double_theta, 2, false, false);		
-				double [] cam_theta_201 = Imx5.imsToCamRotations(double_theta, 2, false, true);		
-				double [] cam_theta_210 = Imx5.imsToCamRotations(double_theta, 2, true,  false);		
-				double [] cam_theta_211 = Imx5.imsToCamRotations(double_theta, 2, true,  true);		
-				double [] cam_theta_300 = Imx5.imsToCamRotations(double_theta, 3, false, false);		
-				double [] cam_theta_301 = Imx5.imsToCamRotations(double_theta, 3, false, true);		
-				double [] cam_theta_310 = Imx5.imsToCamRotations(double_theta, 3, true,  false);		
-				double [] cam_theta_311 = Imx5.imsToCamRotations(double_theta, 3, true,  true);
-
-				double [] cam_theta_400 = Imx5.imsToCamRotations(double_theta, 4, false, false);		
-				double [] cam_theta_401 = Imx5.imsToCamRotations(double_theta, 4, false, true);		
-				double [] cam_theta_410 = Imx5.imsToCamRotations(double_theta, 4, true,  false);		
-				double [] cam_theta_411 = Imx5.imsToCamRotations(double_theta, 4, true,  true);		
-				double [] cam_theta_500 = Imx5.imsToCamRotations(double_theta, 5, false, false);		
-				double [] cam_theta_501 = Imx5.imsToCamRotations(double_theta, 5, false, true);		
-				double [] cam_theta_510 = Imx5.imsToCamRotations(double_theta, 5, true,  false);		
-				double [] cam_theta_511 = Imx5.imsToCamRotations(double_theta, 5, true,  true);
-
-				sb.append("\t"+cam_theta_000[0]+"\t"+cam_theta_000[1]+"\t"+cam_theta_000[2]);
-				sb.append("\t"+cam_theta_001[0]+"\t"+cam_theta_001[1]+"\t"+cam_theta_001[2]);
-				sb.append("\t"+cam_theta_010[0]+"\t"+cam_theta_010[1]+"\t"+cam_theta_010[2]);
-				sb.append("\t"+cam_theta_011[0]+"\t"+cam_theta_011[1]+"\t"+cam_theta_011[2]);
-				sb.append("\t"+cam_theta_100[0]+"\t"+cam_theta_100[1]+"\t"+cam_theta_100[2]);
-				sb.append("\t"+cam_theta_101[0]+"\t"+cam_theta_101[1]+"\t"+cam_theta_101[2]);
-				sb.append("\t"+cam_theta_110[0]+"\t"+cam_theta_110[1]+"\t"+cam_theta_110[2]);
-				sb.append("\t"+cam_theta_111[0]+"\t"+cam_theta_111[1]+"\t"+cam_theta_111[2]);
-
-				sb.append("\t"+cam_theta_200[0]+"\t"+cam_theta_200[1]+"\t"+cam_theta_200[2]);
-				sb.append("\t"+cam_theta_201[0]+"\t"+cam_theta_201[1]+"\t"+cam_theta_201[2]);
-				sb.append("\t"+cam_theta_210[0]+"\t"+cam_theta_210[1]+"\t"+cam_theta_210[2]);
-				sb.append("\t"+cam_theta_211[0]+"\t"+cam_theta_211[1]+"\t"+cam_theta_211[2]);
-				sb.append("\t"+cam_theta_300[0]+"\t"+cam_theta_300[1]+"\t"+cam_theta_300[2]);
-				sb.append("\t"+cam_theta_301[0]+"\t"+cam_theta_301[1]+"\t"+cam_theta_301[2]);
-				sb.append("\t"+cam_theta_310[0]+"\t"+cam_theta_310[1]+"\t"+cam_theta_310[2]);
-				sb.append("\t"+cam_theta_311[0]+"\t"+cam_theta_311[1]+"\t"+cam_theta_311[2]);
-
-				sb.append("\t"+cam_theta_400[0]+"\t"+cam_theta_400[1]+"\t"+cam_theta_400[2]);
-				sb.append("\t"+cam_theta_401[0]+"\t"+cam_theta_401[1]+"\t"+cam_theta_401[2]);
-				sb.append("\t"+cam_theta_410[0]+"\t"+cam_theta_410[1]+"\t"+cam_theta_410[2]);
-				sb.append("\t"+cam_theta_411[0]+"\t"+cam_theta_411[1]+"\t"+cam_theta_411[2]);
-				sb.append("\t"+cam_theta_500[0]+"\t"+cam_theta_500[1]+"\t"+cam_theta_500[2]);
-				sb.append("\t"+cam_theta_501[0]+"\t"+cam_theta_501[1]+"\t"+cam_theta_501[2]);
-				sb.append("\t"+cam_theta_510[0]+"\t"+cam_theta_510[1]+"\t"+cam_theta_510[2]);
-				sb.append("\t"+cam_theta_511[0]+"\t"+cam_theta_511[1]+"\t"+cam_theta_511[2]);
-			}
-			/*
-			sb.append("\t"+
-					cam_theta[0]*180/Math.PI+"\t"+
-					cam_theta[1]*180/Math.PI+"\t"+
-					cam_theta[2]*180/Math.PI);
-			*/
-			sb.append("\n");
-		}
-		
-		if (path!=null) {
-			String footer=(comment != null) ? ("Comment: "+comment): "";
-			CalibrationFileManagement.saveStringToFile (
-					path,
-					header+"\n"+sb.toString()+"\n"+footer);
-		}else{
-			new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
-		}
-		
-    	
-    }
-
+	
     public static void generateEgomotionTable(
 			CLTParameters  clt_parameters,
 			QuadCLT []     quadCLTs,
@@ -4110,8 +3823,10 @@ public class Interscene {
 		
 		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
 		
+		String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
 		
-		String header = header_ts+(use_processed?header_img:"")+header_ins1+header_ins2+header_ins2_extra+header_pimu;
+		String header = header_ts+(use_processed?header_img:"")+header_ins1+
+				header_ins2+header_ins2_extra+header_pimu+header_camv;
 		
 		StringBuffer sb = new StringBuffer();
 		double [][][] dxyzatr_dt = null;
@@ -4124,6 +3839,11 @@ public class Interscene {
 					scenes_xyzatr,     // double [][][]  scenes_xyzatr,
 					clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
 		}
+		
+		double [] quat_ims_cam = Imx5.quaternionImsToCam(
+				ims_mount_atr, // new double[] {0, 0.13, 0},
+				ims_ortho);
+				
 		Did_ins_2   d2_ref = quadCLTs[ref_index].did_ins_2;
 		double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
 				ims_mount_atr, // new double[] {0, 0.13, 0},
@@ -4212,11 +3932,20 @@ public class Interscene {
 					d2_ref.getQn2b(), // double[]  quat_ned,
 					ims_mount_atr, // double [] ims_mount_atr,
 					ims_ortho); //double [] ims_ortho)
-			double [] cam_xyz_enu = test_xyz_enu(
+			/*
+			double [] cam_xyz_enu0 = test_xyz_enu(
 					Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
 					d2_ref.getQEnu(), // double[]  quat_enu,
 					ims_mount_atr, // double [] ims_mount_atr,
 					ims_ortho); //double [] ims_ortho)
+		    */
+			double [] cam_xyz_enu = Imx5.applyQuternionTo(
+					Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
+							ims_mount_atr,
+							ims_ortho),
+					enu,
+					false);
+			
 			
 			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
 			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //
@@ -4246,9 +3975,22 @@ public class Interscene {
 			
 			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
 			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
-//				String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
 			sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
 			sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
+			/*
+			double [] omegas=new double[] {d3.theta[0]/d3.dt, d3.theta[1]/d3.dt, d3.theta[2]/d3.dt};
+			
+			double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
+			double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
+			sb.append("\t"+cam_dxyz[0]+  "\t"+cam_dxyz[1]+  "\t"+cam_dxyz[2]);
+			// a (around Y),t (around X), r (around Z)
+			sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]);
+			*/
+			double [][] cam_dxyzatr = scene.getDxyzatrIms(clt_parameters, false); // raw, do not scale 
+			sb.append("\t"+cam_dxyzatr[0][0]+ "\t"+cam_dxyzatr[0][1]+ "\t"+cam_dxyzatr[0][2]);
+			sb.append("\t"+cam_dxyzatr[1][0]+ "\t"+cam_dxyzatr[1][1]+ "\t"+cam_dxyzatr[1][2]);
+			
+			// add lpf 
 			sb.append("\n");
 		}
 		
@@ -4285,73 +4027,6 @@ public class Interscene {
 				ims_ortho);
 		return Imx5.applyQuternionTo(cam_quat2, enu, false);
     }
-    
-    
-    static double [] test_ned0(
-    		double [] ned,
-			double[]  quat,
-			double [] ims_mount_atr,
-			double [] ims_ortho) {
-		double [][] enu_ned = {{0,1,0},{1, 0, 0},{0, 0, -1}};
-		Rotation rot_enu_ned = new Rotation(enu_ned, 1E-8);
-//			double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
-		Rotation quat_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
-    	double [] enu = new double [] {ned[1], ned[0], -ned[2]}; 
-		double [] rslt = new double[3]; // same as ned
-		rot_enu_ned.applyTo(enu, rslt);
-		
-		double [] rslt1 = new double[3];
-		double [] rslt2 = new double[3];
-		quat_rot.applyTo(ned,rslt1);
-//		Rotation qr = rot_enu_ned.applyTo(quat_rot);
-		Rotation qr = quat_rot.applyTo(rot_enu_ned);
-		qr.applyTo(enu,rslt2);
-    	
-		double [] cam_quat2 =Imx5.quaternionImsToCam(quat,
-				ims_mount_atr,
-				ims_ortho);
-		double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
-		
-		double [][] reord = new double [16][4];
-		for (int i = 0; i < reord.length; i++) {
-			for (int j = 0; j<4; j++) {
-				boolean neg = ((i >> j) & 1) > 0;
-				reord[i][j] = 0.5 * (neg? -1 : 1);
-			}
-		}
-		System.out.println ("cam_xyz2: "+
-		cam_xyz2[0]+", "+ cam_xyz2[1]+", "+ cam_xyz2[2]);
-		Rotation rot_reord;
-		Rotation quat_enu_rot;
-		double [] quat_enu;
-		double [] cam_quats;
-		for (int i = 0; i < reord.length; i++) {
-			rot_reord = new Rotation(
-					reord[i][0],
-					reord[i][1],
-					reord[i][2],
-					reord[i][3],true);
-//			quat_enu_rot = rot_reord.applyTo(quat_rot);
-			quat_enu_rot = quat_rot.applyTo(rot_reord);
-			quat_enu = new double[] {
-					quat_enu_rot.getQ0(),
-					quat_enu_rot.getQ1(),
-					quat_enu_rot.getQ2(),
-					quat_enu_rot.getQ3()};
-			
-			cam_quats =Imx5.quaternionImsToCam(
-					quat_enu,
-					ims_mount_atr,
-					ims_ortho);
-			double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
-			System.out.println (i+": "+
-			cam_xyzs[0]+", "+ cam_xyzs[1]+", "+ cam_xyzs[2]);
-		}
-		
-    	return cam_xyz2;
-    	
-    	
-    }
-    
+   
     
 }

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

@@ -41,7 +41,10 @@ public class IntersceneMatchParameters {
 	public  double  [] ims_ortho =       {0.5, -0.5, 0.5, -0.5}; // approximate (90-deg) IMS to camera
 	public  double  [] ims_mount_atr =   {0, 0, 0}; // IMS mount fine correction (A,T,R around camera axes)
 	public  double  [] ims_mount_xyz =   {0, 0, 0}; // IMS center in camera coordinates
+	public  double  [] ims_scale_xyz =   {1.1, 1.1, 1.1};
+	public  double  [] ims_scale_atr =   {1.1, 1.1, 1.1};
 		
+	
 	public  boolean sfm_use =            true;   // use SfM to improve depth map
 	public  double  sfm_min_base =       2.0;    // use SfM if baseline exceeds this
 	public  double  sfm_min_gain =       5.0;    // Minimal SfM gain to apply SfM to the depth map
@@ -462,6 +465,11 @@ public class IntersceneMatchParameters {
 		gd.addStringField ("IMS mount XYZ correction (m)",  IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
 				"MS center (X,Y,Z m) in camera coordinates.");
 
+		gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
+				"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
+		gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
+				"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
+		
 		gd.addTab         ("SfM", "Structure from Motion to improve depth map for the lateral views");
 		gd.addCheckbox ("Use SfM",                                   this.sfm_use,
 				"Use SfM for the depth map enhancement for laterally moving camera.");
@@ -1186,6 +1194,8 @@ public class IntersceneMatchParameters {
 		this.ims_ortho =     IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 4);
 		this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
+		this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
+		this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
 		this.sfm_use =                        gd.getNextBoolean();
 		this.sfm_min_base =                   gd.getNextNumber();
 		this.sfm_min_gain =                   gd.getNextNumber();
@@ -1566,6 +1576,8 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"ims_ortho",                     IntersceneMatchParameters.doublesToString(this.ims_ortho));		
 		properties.setProperty(prefix+"ims_mount_atr",                 IntersceneMatchParameters.doublesToString(this.ims_mount_atr));		
 		properties.setProperty(prefix+"ims_mount_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
+		properties.setProperty(prefix+"ims_scale_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
+		properties.setProperty(prefix+"ims_scale_atr",                 IntersceneMatchParameters.doublesToString(this.ims_scale_atr));		
 		properties.setProperty(prefix+"sfm_use",                       this.sfm_use + "");                     // boolean
 		properties.setProperty(prefix+"sfm_min_base",                  this.sfm_min_base+"");                  // double
 		properties.setProperty(prefix+"sfm_min_gain",                  this.sfm_min_gain+"");                  // double
@@ -1906,6 +1918,8 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"ims_ortho")!=null) 	       this.ims_ortho= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_ortho"),4);
 		if (properties.getProperty(prefix+"ims_mount_atr")!=null) 	   this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
 		if (properties.getProperty(prefix+"ims_mount_xyz")!=null) 	   this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
+		if (properties.getProperty(prefix+"ims_scale_xyz")!=null) 	   this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
+		if (properties.getProperty(prefix+"ims_scale_atr")!=null) 	   this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
 		if (properties.getProperty(prefix+"sfm_use")!=null)            this.sfm_use=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_use"));		
 		if (properties.getProperty(prefix+"sfm_min_base")!=null)       this.sfm_min_base=Double.parseDouble(properties.getProperty(prefix+"sfm_min_base"));
 		if (properties.getProperty(prefix+"sfm_min_gain")!=null)       this.sfm_min_gain=Double.parseDouble(properties.getProperty(prefix+"sfm_min_gain"));
@@ -2273,6 +2287,8 @@ public class IntersceneMatchParameters {
  		imp.ims_ortho                     = this.ims_ortho.clone();
  		imp.ims_mount_atr                 = this.ims_mount_atr.clone();
  		imp.ims_mount_xyz                 = this.ims_mount_xyz.clone();
+ 		imp.ims_scale_xyz                 = this.ims_scale_xyz.clone();
+ 		imp.ims_scale_atr                 = this.ims_scale_atr.clone();
  		imp.sfm_use                       = this.sfm_use;
  		imp.sfm_min_base                  = this.sfm_min_base;
  		imp.sfm_min_gain                  = this.sfm_min_gain;

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

@@ -4773,6 +4773,20 @@ public class OpticalFlow {
 				int center_index =quadCLTs[last_index].getReferenceIndex(null);
 				if (center_index == -1) {
 					build_ref_dsi = true;
+				} else {
+					QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnQuadCLT( // will conditionImageSet
+							quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
+							clt_parameters,
+							colorProcParameters, //
+							threadsMax,
+							debugLevel-2);
+					if ((try_ref_scene == null) || (try_ref_scene.getDLS() == null)) {
+						if (debugLevel >-2) {
+							System.out.println("DSI data for scene "+quadCLTs[last_index].getReferenceTimestamp()+
+									" does not exist, forcing initial orientation.");
+						}
+						force_initial_orientations = true;
+					}
 				}
 			} else {
 				if ((quadCLTs[last_index] == null) || !quadCLTs[last_index].dsiExists()) {
@@ -4995,7 +5009,7 @@ public class OpticalFlow {
 				ref_index =quadCLTs[last_index].getReferenceIndex(quadCLTs);
 				quadCLTs[ref_index].restoreInterProperties(null, false, debugLevel); //null
 				int [] first_last_index = quadCLTs[ref_index].getFirstLastIndex(quadCLTs);
-				earliest_scene = first_last_index[0];
+				earliest_scene = first_last_index[0]; // null pointer if does not exist
 				last_index =  first_last_index[1];
 			}
 			// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
@@ -5188,8 +5202,12 @@ public class OpticalFlow {
 				}
 				boolean        disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
 				boolean ers_from_ims = false; // change later
-				int            ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
-				ers_mode = 1; // TODO: remove later!
+//				int            ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
+				int ers_mode = 0; // keep
+				// with IMS it is already set during initial orientation. In non-IMS mode
+				if (!ers_from_ims && (quadCLTs[ref_index].getNumOrient() < 2)) {
+					ers_mode = 1; // calculate velocity
+				}
 				earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
 						clt_parameters, // CLTParameters  clt_parameters,
 						mb_max_gain,    // double         mb_max_gain,

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

@@ -196,6 +196,57 @@ public class QuadCLTCPU {
     	return timestamp_reference;
     }
     
+	public double [][] getDxyzatrIms(
+			CLTParameters clt_parameters,
+			boolean       scale) {
+		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
+		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+		double [] quat_ims_cam = Imx5.quaternionImsToCam(
+				ims_mount_atr, // new double[] {0, 0.13, 0},
+				ims_ortho);
+		double [] double_uvw =  did_ins_2.getUvw();
+		double [] omegas=new double[] {did_pimu.theta[0]/did_pimu.dt,
+				did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt};
+		double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
+		double [] cam_datr = Imx5.applyQuternionTo(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]}};
+		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];
+			}
+		}
+		return dxyzatyr;
+	}
+
+	public static double [][] scaleDtToErs(
+			CLTParameters clt_parameters,
+			double [][] cam_dxyzatr){
+		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
+		double [][] scaled_dxyzatr = new double[2][3];
+		for (int i = 0; i < scaled_dxyzatr.length; i++) {
+			for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
+				scaled_dxyzatr[i][j] = cam_dxyzatr[i][j]* dbg_scale_dt[i][j];
+			}
+		}
+		return scaled_dxyzatr;
+	}
+
+	public static double [][] scaleDtFromErs(
+			CLTParameters clt_parameters,
+			double [][] scaled_dxyzatr){
+		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
+		double [][] cam_dxyzatr = new double[2][3];
+		for (int i = 0; i < scaled_dxyzatr.length; i++) {
+			for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
+				cam_dxyzatr[i][j] = scaled_dxyzatr[i][j] / dbg_scale_dt[i][j];
+			}
+		}
+		return cam_dxyzatr;
+	}
+	
+	
     /**
      * If this scene has a reference to one of the scenes array, return its index in the array
      * otherwise return -1 if there is no pointer to reference scene and -2 if pointer is not