Added vertical averaging

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

@@ -118,6 +118,7 @@ import com.elphel.imagej.gpu.JCuda_ImageJ_Example_Plugin;
 import com.elphel.imagej.ims.DjiSrt;
 import com.elphel.imagej.ims.DjiSrtReader;
 import com.elphel.imagej.ims.EventLogger;
+import com.elphel.imagej.ims.QuatVertLMA;
 import com.elphel.imagej.ims.UasLogReader;
 //import com.elphel.imagej.ims.Imx5;
 import com.elphel.imagej.jp4.JP46_Reader_camera;
@@ -6164,11 +6165,24 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 				System.out.println("Failed to create eventLogger instance.");
 				return;
 			}
+			double [][] quats_all = eventLogger.getQuats(
+					CLT_PARAMETERS.imp.imsv_ts0, //  1763232236.0, // double ts_start, or NaN
+					CLT_PARAMETERS.imp.imsv_ts1); // 1763234000.0); // double ts_end)
+			double [] vert_xyz = null;
+			if (quats_all != null) {
+				vert_xyz = QuatVertLMA.getVertAndRms(
+						CLT_PARAMETERS, // CLTParameters   clt_parameters,
+						quats_all,      // double [][] quats,
+						DEBUG_LEVEL);   // int             debugLevel)
+			}
+			
 			eventLogger.print2Pps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"rec_1pps");
 			eventLogger.printStrobeInTime(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_strobe_in_time");
 			eventLogger.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");
 			eventLogger.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);
-			eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1");
+			eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1",
+					vert_xyz); // may be null. 4-element - ignore last (it is RMS)
+			
 			eventLogger.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");
  /*
 			if (EVENT_LOGGER == null) {

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

@@ -87,7 +87,7 @@ public class EventLogger {
 		
 	}
 	
-	public void printDidIns1(String out_did1_path) {
+	public void printDidIns1(String out_did1_path, double [] vert_xyz) {
 		out_did1_path = Paths.get(out_did1_path).normalize().toString();
 		Path op_did1 = Paths.get(out_did1_path);
 		op_did1=op_did1.normalize();
@@ -97,7 +97,7 @@ public class EventLogger {
 			if (logger_files[nf].hasIns(false)) {
 				System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
 				try {
-					logger_files[nf].listDidIns1(out_path,false);
+					logger_files[nf].listDidIns1(out_path,false, vert_xyz);
 				} catch (IOException e) {
 					// TODO Auto-generated catch block
 					e.printStackTrace();
@@ -131,6 +131,60 @@ public class EventLogger {
 		}
 	}
 	
+	public double [][] getQuats(
+			double ts_start,
+			double ts_end){
+		if (ts_start < 0) ts_start=Double.NaN;
+		if (ts_end   < 0) ts_end=Double.NaN;
+		double [][] quats_all = null;
+		double [][] quats =    null;
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			quats = null;
+			if (logger_files[nf].hasIns(true)) {
+				System.out.println("Processing all DID_INS_2 data in "+logger_files[nf].abs_path);
+				try {
+					quats = logger_files[nf].getQuats(
+							ts_start, // double ts_start, // or NaN
+							ts_end,   // double ts_end, // or NaN
+							false);   // boolean keep) throws IOException {
+
+				} catch (IOException e) {
+					// TODO Auto-generated catch block
+					e.printStackTrace();
+				}
+			} else {
+				System.out.println("DID_INS_2 data is not available in "+logger_files[nf].abs_path);
+			}
+			if ((quats == null) || (quats.length ==0)) {
+				System.out.println("Processing all DID_INS_1 data in "+logger_files[nf].abs_path);
+				try {
+					quats = logger_files[nf].getQuatsIns1(
+							ts_start, // double ts_start, // or NaN
+							ts_end,   // double ts_end, // or NaN
+							false);   // boolean keep) throws IOException {
+
+				} catch (IOException e) {
+					// TODO Auto-generated catch block
+					e.printStackTrace();
+				}
+			}
+			if ((quats != null) && (quats.length > 0)) {
+				if (quats_all == null) {
+					quats_all = quats;
+				} else {
+					double [][] quats0 = quats_all;
+					quats_all = new double[quats0.length + quats.length][];
+					System.arraycopy(quats0, 0, quats_all, 0,             quats0.length);
+					System.arraycopy(quats,  0, quats_all, quats0.length, quats.length);
+				}
+			}
+		}		
+		return quats_all;
+	}
+	
+	
+	
+	
 	public void printPimu(String out_pimu_path) {
 		out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
 		Path op_pimu = Paths.get(out_pimu_path);

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

@@ -751,7 +751,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 	}
 	
 	
-	public void listDidIns1(String path, boolean keep) throws IOException {
+	public void listDidIns1(String path, boolean keep, double [] vert_xyz) throws IOException {
 		double max_offset = 1000; // m
 		open();
 		setInsQuats();
@@ -772,7 +772,11 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 
 		        "u\tv\tw\t"+
 		        "lat\tlong\talt\t"+
-		        "ned:N\tned:E\tned:D\n"; 
+		        "ned:N\tned:E\tned:D"; 
+		if (vert_xyz != null) {
+			header +="\tvert:X\tvert:Y\tvert:Z";
+		}
+		header +="\n";
 		if (writer != null) {
 			writer.write(header);
 		} else {
@@ -813,7 +817,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 				String line =String.format(
 						"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+
 						"%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
-						"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f\n",
+						"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f",
 						nrec,
 						getMasterTS(nrec),
 						getLocalTS(nrec),
@@ -826,6 +830,18 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 						did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2],       //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
 						did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2]        //  North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude
 						);
+				if (vert_xyz != null) {
+					double [] quat = {ins_quats[nrec][0],ins_quats[nrec][1],ins_quats[nrec][2],ins_quats[nrec][3]};
+					double [][] R = QuatVertLMA.quaternionToRotationMatrix(quat);
+					double [] ims_down = new double[3];
+					for (int i = 0; i < 3; i++) {
+						for (int k = 0; k < 3; k++) {
+							ims_down[i] += R[i][k]*vert_xyz[k];
+						}
+					}
+					line +=String.format("\t%13.10f\t%13.10f\t%13.10f",ims_down[0],ims_down[1],ims_down[2]);
+				}
+				line += "\n";
 				if (writer != null) {
 					writer.write(line);
 				} else {
@@ -911,6 +927,129 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		}
 	}
 	
+	
+	public double [][] getQuats(
+			double ts_start, // or NaN
+			double ts_end, // or NaN
+			boolean keep) throws IOException {
+		class Double4 {
+			double [] quat = new double[4];
+			Double4 (float [] fdata){
+				double [] data = new double[fdata.length];
+				for (int i = 0; i < data.length; i++) {
+					data[i] = fdata[i];
+				}
+			}
+		}
+		ArrayList<Double4> quat_list = new ArrayList<Double4>();
+		open();
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("listDidIns2(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
+				if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
+					continue;
+				}
+				double ts = getMasterTS(nrec);
+				if (!(ts < ts_start) && !(ts > ts_end)) {
+					quat_list.add(new Double4(did_ins_2.qn2b));
+				}
+			}
+		}
+		if (!keep) {
+			close();
+		}
+		double [][] quats = new double[quat_list.size()][];
+		for (int i = 0; i < quats.length; i++) {
+			quats[i] = quat_list.get(i).quat;
+		}
+		return quats;
+	}
+	
+	
+	
+	public double [][] getQuatsIns1(
+			double ts_start, // or NaN
+			double ts_end, // or NaN
+			boolean keep) throws IOException {
+		class Double4 {
+			double [] quat;
+			Double4 (float [] fdata){
+				quat = new double[fdata.length];
+				for (int i = 0; i < quat.length; i++) {
+					quat[i] = fdata[i];
+				}
+				return;
+			}
+		}
+		ArrayList<Double4> quat_list = new ArrayList<Double4>();
+		double max_offset = 1000; // m
+		open();
+		setInsQuats();
+		float [] start_ned = null;
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("getQuatsIns1(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
+				if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
+					continue;
+				}
+				if ((start_ned != null) &&
+						(       (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) ||
+								(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) ||
+								(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
+								)){
+					start_ned = null;
+				}
+				if (start_ned == null) {
+					start_ned = did_ins_1.ned.clone();
+				}
+				start_ned = new float[] {0,0,0};
+				
+				double ts = getMasterTS(nrec);
+				if (!(ts < ts_start) && !(ts > ts_end)) {
+					quat_list.add(new Double4(ins_quats[nrec]));
+				}
+			}
+		}
+		if (!keep) {
+			close();
+		}
+		double [][] quats = new double[quat_list.size()][];
+		for (int i = 0; i < quats.length; i++) {
+			quats[i] = quat_list.get(i).quat;
+		}
+		return quats;
+	}
+	
 	public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
 		// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
 //		double max_offset = 1000; // m

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

@@ -7947,6 +7947,21 @@ public class Interscene {
     }
     
     // relative to the GPS/compass
+    /**
+     * Finding correspondence between scene XYZ (ers_reference.getSceneXYZ(ts)) and IMS axes (
+     *  Imx5.applyQuaternionTo(
+	 *				Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
+	 *						ims_mount_atr, // small misalignment, provided in clt_parameters  
+	 *						ims_ortho),    // fixed 90-degree rotation
+     * @param clt_parameters - configuration parameters
+     * @param quadCLTs
+     * @param ref_scene
+     * @param earliest_scene
+     * @param rms
+     * @param enu_corr
+     * @param debugLevel
+     * @return
+     */
     public static double [] getQuaternionCorrection(
 			CLTParameters  clt_parameters,
 			QuadCLT []     quadCLTs,

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

@@ -1583,7 +1583,7 @@ public class QuadCLTCPU {
     /**
      * Integrate position and orientation of the camera relative to the
      * (inertial) reference frame using IMU-derived local angular and 
-     * linera velocities
+     * linear velocities
      * @param clt_parameters configuration parameters
      * @param quadCLTs       scenes sequence
      * @param ref_index      reference scene index 
@@ -2802,6 +2802,11 @@ public class QuadCLTCPU {
     		}
     	}
     	double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
+		if (debug_level > -3) {
+			System.out.println("##### scene_abs_atr_enu = "+scene_abs_atr_enu[0]+", "+scene_abs_atr_enu[1]+", "+scene_abs_atr_enu[2]+" #####");
+		}
+    	
+    	
     	double [][] dls = getDLS();
     	if ((min_sfm_gain> 0) && (dls[3] != null)) {
     		int num_sfm = 0;

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

@@ -2203,32 +2203,6 @@ public class QuaternionLma {
 			};
 		}
 	}
-	public static double [][] composeQR_QdQ_(
-			double [] q,
-			double [] r) {
-		return new double [][] {
-			//  t=s*q' d/dQ0    
-			{-2*r[0]*q[0],
-			 -2*r[1]*q[0] - 2*r[2]*q[3] + 2*r[3]*q[2],
-			 -2*r[2]*q[0] - 2*r[3]*q[1] + 2*r[1]*q[3],
-			 -2*r[3]*q[0] - 2*r[1]*q[2] + 2*r[2]*q[1]},
-			// t=s*q' d/dQ1 
-			{-2*r[0]*q[1],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-			 -2*r[1]*q[2] - 2*r[3]*q[0] + 2*r[2]*q[1],
-			 +2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3]},
-			// t=s*q'd/dQ2
-			{-2*r[0]*q[2],
-			 -2*r[2]*q[1] + 2*r[3]*q[0] + 2*r[1]*q[2],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-			 -2*r[2]*q[3] - 2*r[1]*q[0] + 2*r[3]*q[2]},
-			// t=s*q'd/dQ3
-			{-2*r[0]*q[3],
-			 -2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
-			 -2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
-			 -2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
-		};
-	}
 
 	/**
 	 * Get derivatives of the composed quaternion (compose(q,r)) by the
@@ -2242,13 +2216,6 @@ public class QuaternionLma {
 	 */
 	public static double [][] composeDQ( // not used
 			double [] r) {
-		/*
-		return new double [][] {
-			{ r[0], -r[1], -r[2], -r[3]},
-			{ r[1],  r[0], -r[3],  r[2]},
-			{ r[2],  r[3],  r[0], -r[1]},
-			{ r[3],	-r[2],	r[1],  r[0]}};
-			*/
 		return new double [][] {
 			{ r[0], r[1], r[2], r[3]},            
 			{-r[1], r[0], r[3],-r[2]},
@@ -2276,13 +2243,6 @@ public class QuaternionLma {
 			{ q[1],  q[0],  q[3], -q[2]},
 			{ q[2], -q[3],  q[0],  q[1]},
 			{ q[3],  q[2], -q[1],  q[0]}};
-/*		
-		return new double [][] {                
-			{ q[0], q[1], q[2], q[3]}, 
-			{-q[1], q[0],-q[3], q[2]}, 
-			{-q[2], q[3], q[0],-q[1]},
-			{-q[3],-q[2], q[1], q[0]}};
-*/			
 	}
 
 	public static double [] addTo(
@@ -2313,7 +2273,7 @@ public class QuaternionLma {
 	}
 	
 	/**
-	 * Get derivatives of the rotated vector (see applyTo(q,xyz)) by the components of the quaterion q
+	 * Get derivatives of the rotated vector (see applyTo(q,xyz)) by the components of the quaternion q
 	 * @param q   4 components (scalar, vector) of the quaternion being applied
 	 *            to as vector.
 	 * @param  xyz 1-d array representing a 3D vector {X, Y, Z}