Working on IMU-to-camera calibration

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

@@ -361,6 +361,7 @@ public class ErsCorrection extends GeometryCorrection {
 	}
 	public void setPropertiesScenes(String prefix, Properties properties){
 		String [] timestamps = getScenes();
+		// TODO: use getScenes(first, last) as there may be stray entries in getScenes() caused by earlier runs
 		for (String k : timestamps) {
 			if (getScene(k) != null) {
 				String [] s_scenes = getScene(k).toStrings(); // null pointer

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

@@ -6201,6 +6201,7 @@ public class OpticalFlow {
 		if (debugLevel >-3) {
 			System.out.println("===== All adjustment cycles are done, considering one-time processing, such as lazy eye calibration. ====="); 
 		}
+		boolean     orient_save_details = true;
 		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
 			boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
 			QuadCLT.adjustImuOrient(
@@ -6210,6 +6211,7 @@ public class OpticalFlow {
 					ref_index, // int           ref_index,
 					earliest_scene, // int           earliest_scene,
 					last_index, // int           last_index,
+					orient_save_details, // boolean       save_details,
 					debugLevel); // 			    		int debugLevel
 			// Try both orient_combo/!orient_combo for the log!
 			QuadCLT.adjustImuOrient(
@@ -6219,6 +6221,7 @@ public class OpticalFlow {
 					ref_index, // int           ref_index,
 					earliest_scene, // int           earliest_scene,
 					last_index, // int           last_index,
+					orient_save_details, // boolean       save_details,
 					debugLevel); // 			    		int debugLevel
 			
 		}
@@ -11434,6 +11437,7 @@ public class OpticalFlow {
 		System.out.println("Scale = "+scale);
 		if (compensate_dsi) {
 			ErsCorrection  ers_reference = ref_scene.getErsCorrection();
+			// TODO: use getScenes(first_ts, last_ts) to remove stray data
 			String [] timestamps = ers_reference.getScenes();
 			for (String ts:timestamps) {
 				double [][] xyzatr = ers_reference.getSceneXYZATR(ts);

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

@@ -6137,10 +6137,7 @@ if (debugLevel < -100) {
 			Path x3d_path=Paths.get(correctionsParameters.x3dDirectory).resolve(scene_name).resolve(correctionsParameters.x3dModelVersion);
 			Path interframe_path = x3d_path.resolve(scene_name+INTERFRAME_SUFFIX+CONFIGURATION_EXTENSION); 
 			File interframe_file = new File(interframe_path.toString());
-			if (!interframe_file.exists()) {
-				if (debugLevel > -3) System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
-				return null;
-			}
+			if (interframe_file.exists()) { // only read if available
 				// read all parameters?  from //fish://elphel@192.168.0.137/home/elphel/lwir16-proc/NC/linked/linked_1763232117-1763234145-v88/1763232148_284331-index/v88/1763232148_284331-SETTINGS.corr-xml
 				// read interscene parameters and <entry key="EYESIS_DCT_AUX.refscenes_1763232144_433048"></entry>
 				quadCLT.restoreInterProperties(     // restore properties for interscene processing (extrinsics, ers, ...)
@@ -6151,10 +6148,29 @@ if (debugLevel < -100) {
 				// get directory with version from name and this
 				// read existing parameters and inter-intra
 				// all data should exist, inter-intra and ims
+
+				//			if (debugLevel > -3) System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
+				//			return null;
+			} else {
+				if (debugLevel > -2) {
+					System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
+				}
+			}
+			boolean always_read_ims = debugLevel>1000;
+			if (always_read_ims) {
+				final String ims_suffix = correctionsParameters.imsSuffix; // assuming common for all
+				Path ims_path = Paths.get(correctionsParameters.x3dDirectory).resolve(scene_name).resolve(scene_name+ims_suffix);
+				if (new File(ims_path.toString()).exists()) {
+					restoreIms(
+							clt_parameters, // // CLTParameters clt_parameters,
+							ims_path.toString(), // String ims_path,
+							true, // boolean create,
+							debugLevel-1); // debugLevelInner); // int debugLevel);
+				}
+			}
 			return quadCLT;
 		}
 	  
-	  
 		public QuadCLT spawnQuadCLT(
 				String              set_name,
 				CLTParameters       clt_parameters,

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

@@ -60,6 +60,7 @@ import java.util.concurrent.atomic.DoubleAccumulator;
 import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 
+import com.elphel.imagej.calibration.CalibrationFileManagement;
 import com.elphel.imagej.calibration.PixelMapping;
 import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.cameras.ColorProcParameters;
@@ -109,6 +110,8 @@ import loci.formats.FormatException;
 
 public class QuadCLTCPU {
 	public static final String    IMU_CALIB_LOGS_SUFFIX =    "-IMU_CALIB.log";
+	public static final String    IMU_CALIB_DETAILS_SUFFIX = "-IMU_CALIB_DETAILS.csv";
+	public static final String    IMU_CALIB_COMBO_SUFFIX =   "-IMU_CALIB_COMBO.csv";
 	public static final String    ORIENTATION_LOGS_SUFFIX =  "-ORIENTATION.log";
 	public static final String    INTERFRAME_SUFFIX =        "-INTERFRAME";
 	public static final String    FIELD_CALIBRATION_SUFFIX = "-FIELD_CALIBRATION";
@@ -838,8 +841,8 @@ public class QuadCLTCPU {
     		int           last_index,
 			double []     rms, // null or double[5];
 			double []     quaternion, // null or double[4]
-			int           debugLevel
-    		) {
+			String        suffix,
+			int           debugLevel) {
     	final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
     	boolean use_inv = false; //
     	double [] quat = getRotationFromXYZATRCameraIms(
@@ -883,6 +886,21 @@ public class QuadCLTCPU {
     		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
     			rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
     	}
+        if (suffix != null) {
+    	    Path orient_path = Paths.get(quadCLTs[ref_index].getX3dDirectory(true)).
+    	    		resolve(quadCLTs[ref_index].getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
+        	saveRotateImsDetails(
+        			rot,                     // Rotation      rot,
+        			early_index,             // int           early_index,
+        			last_index,              // int           last_index,
+        			xyzatr,                  // double [][][] xyzatr,
+        			ims_xyzatr,              // double [][][] ims_xyzatr,
+        			rotated_xyzatr,          // double [][][] rotated_xyzatr,
+        			orient_path.toString()); // String        path) {
+    		if (debugLevel > -3) {
+    			System.out.println("Orientation details saved to "+orient_path.toString());
+    		}
+        }
         if (debugLevel > -1) {
 			double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
 			double []  degrees = new double[3];
@@ -899,7 +917,7 @@ public class QuadCLTCPU {
     				"PIMU-X","PIMU-Y","PIMU-Z","PIMU-A","PIMU-T","PIMU-R",
     				"ROT-X","ROT-Y","ROT-Z","ROT-A","ROT-T","ROT-R"));
         	for (int nscene = early_index; nscene <= last_index; nscene++) {
-        		System.out.println(String.format("%3s"+
+        		System.out.println(String.format("%3d"+
         				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
         				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
         				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
@@ -915,6 +933,44 @@ public class QuadCLTCPU {
     	return rotated_xyzatr;
     }
     
+    public static void saveRotateImsDetails(
+    		Rotation      rot,
+    		int           early_index,
+    		int           last_index,
+    		double [][][] xyzatr,
+    		double [][][] ims_xyzatr,
+    		double [][][] rotated_xyzatr,
+    		String        path) {
+		double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+		double []  degrees = new double[3];
+		for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
+		StringBuffer sb = new StringBuffer();
+		sb.append("quat\t"+rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\n");
+		sb.append("ATR(rad)\t"+angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\n");
+		sb.append("ATR(deg)\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
+		sb.append(
+				"N\tX\tY\tZ\tA\tT\tR\t"+
+				"PIMU-X\tPIMU-Y\tPIMU-Z\tPIMU-A\tPIMU-T\tPIMU-R\t"+
+				"ROT-X\tROT-Y\tROT-Z\tROT-A\tROT-T\tROT-R\n");
+    	for (int nscene = early_index; nscene <= last_index; nscene++) {
+    		sb.append(String.format("%3d"+
+    				"\t%f\t%f\t%f\t%f\t%f\t%f"+
+    				"\t%f\t%f\t%f\t%f\t%f\t%f"+
+    				"\t%f\t%f\t%f\t%f\t%f\t%f\n",
+    				nscene,
+    				xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
+    				xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
+    				ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],
+    				ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],
+    				rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],
+    				rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));
+    	}
+		CalibrationFileManagement.saveStringToFile (
+				path,
+				sb.toString());
+    }
+    
+    
     public static void adjustImuOrient(
     		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
     		boolean       orient_combo,
@@ -922,8 +978,8 @@ public class QuadCLTCPU {
     		int           ref_index,
     		int           earliest_scene,
     		int           last_index,
-    		int           debugLevel
-    		) {
+    		boolean       save_details,
+    		int           debugLevel) {
 		boolean orient_by_move =    clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
 		boolean orient_by_rot =     clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
 		if (!orient_by_move && !orient_by_rot) {
@@ -965,6 +1021,9 @@ public class QuadCLTCPU {
     	} else if (!orient_by_rot) {
     		translation_weight = 1.0;
     	}
+    	// boolean       orient_combo,
+    	// boolean       save_details
+    	String        suffix = save_details ? (orient_combo ? IMU_CALIB_COMBO_SUFFIX: IMU_CALIB_DETAILS_SUFFIX) : null;
     	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
     			clt_parameters,     // CLTParameters clt_parameters,
     			quat_lma_mode,      // int quat_lma_mode,
@@ -976,8 +1035,9 @@ public class QuadCLTCPU {
     			ref_index,          // int           ref_index,
     			earliest_scene,     // int           early_index,
     			last_index,         // int           last_index,
-    			rms,                //double []     rms, // null or double[5];
+    			rms,                // double []     rms, // null or double[5];
     			quat,               // double []     quaternion, // null or double[4]
+    			suffix,             // String        suffix,
     			debug_lev);         // int           debugLevel
     	if (rotated_xyzatr != null) {
         	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
@@ -1589,6 +1649,9 @@ public class QuadCLTCPU {
        	if (quat_corr != null) {
        		quat_ims_cam=Imx5.applyQuaternionToQuaternion(quat_corr, quat_ims_cam, false);
        	} 
+       	if (did_ins_2 == null) {
+       		System.out.println("getDxyzatrIms(): did_ins_2=null");
+       	}
     	double [] double_uvw =  did_ins_2.getUvw();
     	if ((double_uvw[0] == 0.0) && (double_uvw[1] == 0.0) && (double_uvw[2] == 0.0)) {
     		double_uvw =  did_ins_1.getUvw();

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

@@ -8612,12 +8612,21 @@ if (debugLevel > -100) return true; // temporarily !
 //				if (keep_segments) {
 //					System.out.println("want to keep segments");
 //				}
-				boolean pre_series = true; // perform actions
-				int     pre_series_orient = 1; // it is 1 now
-				int     pre_series_accum =  2; // it is 2 now
+				int operation_mode = 0;
+				if (run_ly && (run_ly_mode > 0)  && (run_ly_mode <= 3) ) { // do not yet know if ity is needed
+					operation_mode =OpticalFlow.RESULTS_BUILD_SEQ_LY;
+				}
+				int initial_operation_mode = operation_mode;
+
+				boolean pre_series =        clt_parameters.imp.series_run; // perform actions
+				int     pre_series_orient = clt_parameters.imp.series_orient; //   1; // it is 1 now
+				int     pre_series_accum =  clt_parameters.imp.series_accum;  //   2; // it is 2 now
+				boolean series_only =       clt_parameters.imp.series_only;   //false;// Do not run per-segment after per-sequence
+
 				boolean run_pre_series = false;
 				QuadCLT [] ref_scenes = null;
-				if (keep_segments && (scene_names != null)) {
+				// do not try to do that for the LY calibration
+				if (keep_segments && (scene_names != null) && (operation_mode != OpticalFlow.RESULTS_BUILD_SEQ_LY)) {
 					// use last scene as index // ref_index
 					int index_index = ref_index;
 					if (index_index <0) index_index+=scene_names.length;
@@ -8659,29 +8668,50 @@ if (debugLevel > -100) return true; // temporarily !
 						}
 					}
 				}
-				boolean series_infinity = true;
+				boolean series_infinity =      clt_parameters.imp.wser_inf_en;    //  true; // Calculate per-series infinity adjustments based on visual-to-imu comparison
+				boolean series_orient =        clt_parameters.imp.wser_orient_en; // true;  // per-series orientation dcamera-to-imu adjust enable
+				boolean air_mode_en =          clt_parameters.imp.air_mode_en; // fast airplane mode
+				boolean save_series_infinity = clt_parameters.imp.wser_inf_save; // true;  // Save per-series infinity adjustments data as CSV file
+				boolean save_series_orient =   clt_parameters.imp.wser_orient_save; 
 				if (run_pre_series) {
 					if (debugLevel > -4) {
 						System.out.println("===== Conditions met for all segment of the scene series, will run per-series calibrations =====");
 					}
-					if (series_infinity) {
+					if (series_infinity && air_mode_en) { // non-arplane mode use other method of infinity calibration
+						double [][] infinity_rslt = save_series_infinity ? (new double [ref_scenes.length][]) : null;
 						double infinity_multi = SeriesInfinityCorrection.getInfinityMuliSeries(
 								clt_parameters,  // CLTParameters    clt_parameters,
 								ref_scenes,      // QuadCLT []       quadCLTs,
 								index_scenes[0], // QuadCLT          index_CLT, // normally - just the last in quadCLTs
+								infinity_rslt,   //	double [][]      infinity_rslt,
+								debugLevel);     // int              debugLevel)
+						if (infinity_rslt != null) {
+							SeriesInfinityCorrection.saveInfinitySeries(
+									ref_scenes,      // QuadCLT []       refCLTs,
+									index_scenes[0], // QuadCLT          index_CLT, // normally - just the last in quadCLTs
+									infinity_rslt,   // double [][]      infinity_rslt)
+									debugLevel + 1); // int              debugLevel)
+						}
+					}
+					if (series_orient) {
+						// TODO: implement
+						// for testing running adjustImuOrient for the last segment in this series
+						double [][] orient_rslt = save_series_orient ? (new double [ref_scenes.length][]) : null;
+						double [] infinity_multi = SeriesInfinityCorrection.adjustImuOrientMulti(
+								clt_parameters,     // CLTParameters    clt_parameters,
+								ref_scenes,         // QuadCLT []       quadCLTs,
+								index_scenes[0],    // QuadCLT          index_CLT, // normally - just the last in quadCLTs
+								save_series_orient, // boolean          save_series_orient, 
+								orient_rslt,        // double [][]      orient_rslt,
 								debugLevel);        // int              debugLevel)
+						if (orient_rslt != null) {
+						}						
 					}
-					
-					
 				}
 				
 //		    	boolean continue_ly = clt_parameters.imp.continue_ly; // calculate field calibration for this series even if it was calibrated earlier
-				int operation_mode = 0;
-				if (run_ly && (run_ly_mode > 0)  && (run_ly_mode <= 3) ) { // do not yet know if ity is needed
-					operation_mode =OpticalFlow.RESULTS_BUILD_SEQ_LY;
-				}
-				int initial_operation_mode = operation_mode;
 				boolean stop_now_asap = false; // to skip ground planes index generating if it did not finish the scene sequence
+				if (!series_only || (operation_mode == OpticalFlow.RESULTS_BUILD_SEQ_LY)) {
 					while ((ref_index < 0) || ((ref_index + 1) >= min_num_scenes)) {
 						int [] build_series_result = new int[1]; // 0, normal return  will not change it
 						String model_directory = null;
@@ -8739,7 +8769,7 @@ if (debugLevel > -100) return true; // temporarily !
 									break;
 								}
 							}
-					}
+						} // while true
 
 
 						if (model_directory == null) {
@@ -8758,6 +8788,7 @@ if (debugLevel > -100) return true; // temporarily !
 							}
 							ref_index = start_ref_pointers[0]; // continue from the same attached to the previous reference
 						}
+						/*
 					// put test here, temporarily:
 					if (index_scenes[0] != null) {
 						SeriesInfinityCorrection.getInfinityMuliSeries(
@@ -8766,6 +8797,7 @@ if (debugLevel > -100) return true; // temporarily !
 					    		index_scenes[0], // QuadCLT          index_CLT, // normally - just the last in quadCLTs
 					    		debugLevel);     // int              debugLevel)
 					}
+						 */
 
 						stop_now_asap = sync_command.stopNow(false);
 						if ((model_directory == null) && !stop_now_asap) {
@@ -8911,7 +8943,11 @@ if (debugLevel > -100) return true; // temporarily !
 								IJ.d2s(0.000000001*(System.nanoTime()-start_time_seq),3)+" sec ("+
 								IJ.d2s(0.000000001*(System.nanoTime()-start_time_all),3)+" sec from the overall start\n");
 					} // while ((ref_index < 0) || ((ref_index + 1) >= min_num_scenes)) {
-				
+				}  else { //if (!series_only) {
+					System.out.println("\n ----- PROCESSING SCENE SEQUENCE "+nseq+" (last is "+(num_seq-1)+") BYPASSED in "+
+							IJ.d2s(0.000000001*(System.nanoTime()-start_time_seq),3)+" sec ("+
+							IJ.d2s(0.000000001*(System.nanoTime()-start_time_all),3)+" sec from the overall start\n");
+				}
 				if ((initial_operation_mode == OpticalFlow.RESULTS_BUILD_SEQ_LY) && !continue_ly) {
 					System.out.println("Performed only LY adjustment for the series.");
 					continue;
@@ -8929,13 +8965,6 @@ if (debugLevel > -100) return true; // temporarily !
 					System.out.println("5. cuas_centers="+((cuas_centers != null)?("[\""+cuas_centers[0]+"\",\""+cuas_centers[1]+"\"]"):"null"));
 				}
 				// TODO: calculate infinity correction if needed using IMS
-				/*
-				SeriesInfinityCorrection.double getImgImsScale( // correctInfinityFromIMS(
-						QuadCLT []       quadCLTs,
-						QuadCLT          master_CLT,
-						int              earliest_scene,
-						int              latest_scene)
-				*/
 				// List ground planes in this series:
 				Map<String, GroundPlane> map_gp = GroundPlane.getGroundPlanes(
 						clt_parameters,      // CLTParameters        clt_parameters,
@@ -9687,7 +9716,9 @@ if (debugLevel > -100) return true; // temporarily !
 					threadsMax,                                // int            threadsMax,  // maximal number of threads to launch
 					updateStatus);
 			ErsCorrection ers_reference = ref_quadCLT.getErsCorrection();
-			String [] sts = ref_only ? (new String [0]) : ers_reference.getScenes(); // others, referenced by reference
+			String [] fl_ts = ref_quadCLT.getFirstLastTimestamps();
+			boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
+			String [] sts = ref_only ? (new String [0]) : (good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes()); // others, referenced by reference
 			// get list of all other scenes
 			int num_scenes = sts.length + 1; // including reference
 			//		int indx_ref = num_scenes - 1;