More refactoring: moving orientation-related static methods from

SeriesInfinityCorrection to XyzQLma class

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

@@ -8698,7 +8698,7 @@ if (debugLevel > -100) return true; // temporarily !
 						// 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 [] quatcorr_multi = SeriesInfinityCorrection.adjustImuOrientMulti(
+						double [] quatcorr_multi = XyzQLma.adjustImuOrientMulti(
 								clt_parameters,     // CLTParameters    clt_parameters,
 								ref_scenes,         // QuadCLT []       quadCLTs,
 								index_scenes[0],    // QuadCLT          index_CLT, // normally - just the last in quadCLTs

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

@@ -3,7 +3,11 @@ package com.elphel.imagej.tileprocessor;
 import java.nio.file.Path;
 import java.nio.file.Paths;
 import java.text.SimpleDateFormat;
+import java.util.Arrays;
 import java.util.Calendar;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.concurrent.atomic.AtomicInteger;
 
 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
@@ -1509,5 +1513,179 @@ public class XyzQLma {
 				sb.toString());
     }
  
+	public static double [] adjustImuOrientMulti(
+			CLTParameters    clt_parameters,
+			QuadCLT []       refCLTs,
+			QuadCLT          index_CLT, // normally - just the last in quadCLTs
+			boolean          save_sequence_orient,
+			boolean          process_segments,
+			double [][]      orient_rslt,
+			final int        debugLevel) {
+//		boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
+		boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
+		
+		for (QuadCLT ref_scene:refCLTs) {
+			ref_scene.restoreAnyDSI(debugLevel);
+		}
+		// get all scenes used in at least one segment.
+		HashSet<String> scene_set = new HashSet<String>();
+		HashMap<String,QuadCLT> ref_map = new HashMap<String,QuadCLT>(); 
+		for (QuadCLT ref_scene:refCLTs) {
+			ref_map.put(ref_scene.getImageName(), ref_scene);
+			ErsCorrection ers_reference = ref_scene.getErsCorrection();
+			String [] fl_ts = ref_scene.getFirstLastTimestamps();
+			boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
+			String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
+			for (String s:names)scene_set.add(s);
+		}
+		String [] all_scene_names = scene_set.toArray(new String[0]);
+		Arrays.sort(all_scene_names);
+		HashMap<String,QuadCLT> scene_map = new HashMap<String,QuadCLT>(); 
+		String models_dir = index_CLT.correctionsParameters.x3dDirectory;
+		final String suffix = index_CLT.correctionsParameters.imsSuffix; // assuming common for all
+		final QuadCLT[] scenes = new QuadCLT[all_scene_names.length];
+		final Thread[] threads = ImageDtt.newThreadArray();
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int iScene = ai.getAndIncrement(); iScene < scenes.length; iScene = ai.getAndIncrement()) {
+						String scene_name = all_scene_names[iScene];
+						QuadCLT rscene = ref_map.get(scene_name);
+						// create new if did not exist,
+						if (rscene == null) { 
+							scenes[iScene] = new QuadCLT(index_CLT,scene_name);
+						} else {
+							scenes[iScene] = rscene;
+						}
+						Path ims_path = Paths.get(models_dir).resolve(scene_name).resolve(scene_name+suffix);
+						scenes[iScene].restoreIms(
+								clt_parameters, // // CLTParameters clt_parameters,
+								ims_path.toString(), // String ims_path,
+								true, // boolean create,
+								debugLevel-1); // debugLevelInner); // int debugLevel);
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		for (int nscene = 0; nscene < all_scene_names.length; nscene++) { // scene_name:all_scene_names) {
+			scene_map.put(all_scene_names[nscene], scenes[nscene]);
+		}
+		// Now iterate through the reference scenes, using IMS data from the scene_map scenes.
+		// initially will use earliest and latest scenes only. Improve for the future non-linear flying multicopters
+		// and account for sync loss (far samples with random coordinates)
+		double [] img_scales =  new double[refCLTs.length];
+		double [] dist_visual = new double[refCLTs.length];
+		double [] dist_ims2 =   new double[refCLTs.length];
+		double [] disp_corrs=   new double[refCLTs.length];
+		int []    ref_indices = new int [refCLTs.length];
+		double [][] quats =     process_segments? (new double [refCLTs.length][]) : null;
+		QuadCLT[][] quadCLTss = new QuadCLT[refCLTs.length][];
+			for (int nref = 0; nref < refCLTs.length; nref++) {
+				QuadCLT ref_scene = refCLTs[nref];
+				ErsCorrection ers_reference = ref_scene.getErsCorrection();
+				String [] fl_ts = ref_scene.getFirstLastTimestamps();
+				boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
+				String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
+				String name_ref = ref_scene.getImageName();
+				QuadCLT[] quadCLTs = new QuadCLT[names.length];
+				int ref_index = -1;
+				for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
+					if (names[nscene].equals(name_ref)) {
+						ref_index = nscene;
+						quadCLTs[nscene] = ref_scene;
+					} else {
+						quadCLTs[nscene] = scene_map.get(names[nscene]); 
+					}
+				}
+				if (ref_index < 0) {
+					System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
+					continue;
+				}
+				ref_indices[nref] = ref_index;
+				quadCLTss[nref] =   quadCLTs;
+				if (ref_scene.getImageName().equals("1763233239_531146")) {
+					System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
+					System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
+				}
+				// just for testing - adjusting per segment
+				if (quats != null) {
+					quats [nref] = QuadCLTCPU.adjustImuOrient(
+							clt_parameters,      // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+							orient_combo,        // boolean       orient_combo,
+							quadCLTs,            // QuadCLT[]     quadCLTs,
+							ref_index,           // int           ref_index,
+							0,                   // int           earliest_scene,
+							quadCLTs.length - 1, // int           last_index,
+							save_sequence_orient,  // boolean       save_details,
+							debugLevel);         // int           debugLevel);
+				}
+				continue; // just to put a break point
+				
+			}
+		// adjusting for all series
+		double [] quat = adjustImuOrient(
+				clt_parameters,      // CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+				orient_combo,        // boolean       orient_combo,
+				quadCLTss,           // QuadCLT[][]   quadCLTss,
+				ref_indices,         // int []        ref_indices,
+				index_CLT,           // QuadCLT       index_scene, // where to put result
+				save_sequence_orient,  // boolean       save_details,
+				debugLevel+1);         // int           debugLevel)
+		
+		// save per-segment quaternion and angles, and the result one
+		if (save_sequence_orient && (quats != null)) {
+			saveQuternionCorrection(
+					quats,            // double [][]      quats,
+					quat,             // double []        quat,
+					refCLTs,          // QuadCLT []       refCLTs,
+					index_CLT,        // QuadCLT          index_CLT,
+					orient_combo,     // boolean          orient_combo,
+					(debugLevel>-3)); // boolean          debug)
+		}
+		return quat;
+	}
 	
+	public static void saveQuternionCorrection(
+			double [][]      quats,
+			double []        quat,
+			QuadCLT []       refCLTs,
+			QuadCLT          index_CLT,
+			boolean          orient_combo,
+			boolean          debug) {
+		double [][] quats_all = new double[quats.length+1][];
+		QuadCLT []  allCLTs = new QuadCLT [quats_all.length]; 
+		allCLTs[0] = index_CLT;
+		quats_all[0] = quat;
+		for (int i = 0; i < quats.length; i++) {
+			allCLTs[i+1] = refCLTs[i];
+			quats_all[i+1] = quats[i];
+		}
+		StringBuffer sb = new StringBuffer();
+		sb.append("timestamp\tQ0\tQ1\tQ2\tQ3\tscale\tA\tT\tR\tA(deg)\tT(deg)\tR(deg)\n");
+		for (int nscene = 0; nscene < quats_all.length; nscene++) {
+			double [] q = quats_all[nscene];
+        	Rotation rot = new Rotation(q[0],q[1],q[2],q[3], false); // no normalization - see if can be scaled
+			double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+			double []  degrees = new double[3];
+			double quat_scale = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+			for (int i = 0; i < 3; i++) degrees[i] = angles[i]*180/Math.PI;
+			sb.append(allCLTs[nscene].getImageName()+"\t");
+			sb.append(rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\t");
+			sb.append(quat_scale+"\t");
+			sb.append(angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\t");
+			sb.append(degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
+		}
+	   	String        suffix = orient_combo ? QuadCLTCPU.IMU_CALIB_SUM_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_SUMMARY_SUFFIX;
+	    Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
+		CalibrationFileManagement.saveStringToFile (
+				path.toString(),
+				sb.toString());
+		if (debug) {
+			System.out.println("Summary orientation correction data saved to "+path.toString());
+		}
+	}
+	
+		
 }