Before interscene refactoring

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

@@ -10,6 +10,8 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
 
+import com.elphel.imagej.tileprocessor.ErsCorrection;
+
 
 public class Imx5 {
 	
@@ -173,6 +175,35 @@ 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);
+		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);
+		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
+	}
+	
+	
+	
 	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;
@@ -226,8 +257,8 @@ public class Imx5 {
 
 	public static double [] nedFromLla (double [] lla, double [] lla_ref) {
 		double [] ned = new double[] { 
-				EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
 				EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
+				EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
 				-(lla[2] - lla_ref[2])
 		};
 		return ned;

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

@@ -187,7 +187,7 @@ public class ErsCorrection extends GeometryCorrection {
 				DP_DVX,   DP_DVY,   DP_DVZ,
 				DP_DSVAZ, DP_DSVTL, DP_DSVRL,
 				DP_DSVX,  DP_DSVY,  DP_DSVZ};
-	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
+	public static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
 	static final double THRESHOLD = 1E-10;
 	static final double LINE_ERR = 0.001; // line accuracy for ERS when converting from world to pixels.
 	// parameters for the ERS distortion calculation

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

@@ -26,12 +26,8 @@ package com.elphel.imagej.tileprocessor;
 import java.awt.Color;
 import java.awt.Font;
 import java.awt.Rectangle;
-import java.io.ByteArrayOutputStream;
 import java.io.File;
 import java.io.IOException;
-import java.io.ObjectOutputStream;
-import java.io.Serializable;
-import java.security.MessageDigest;
 import java.security.NoSuchAlgorithmException;
 import java.util.ArrayList;
 import java.util.Arrays;
@@ -42,9 +38,6 @@ import java.util.Properties;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.atomic.DoubleAccumulator;
-import javax.xml.bind.DatatypeConverter;
-
-import org.apache.commons.math3.complex.Quaternion;
 
 import java.util.concurrent.ThreadLocalRandom;
 
@@ -55,14 +48,11 @@ import com.elphel.imagej.cameras.EyesisCorrectionParameters;
 import com.elphel.imagej.common.DoubleGaussianBlur;
 import com.elphel.imagej.common.ShowDoubleFloatArrays;
 import com.elphel.imagej.correction.CorrectionColorProc;
-import com.elphel.imagej.correction.Eyesis_Correction;
 import com.elphel.imagej.gpu.GpuQuad;
 import com.elphel.imagej.gpu.TpTask;
-import com.elphel.imagej.ims.Did_gps_pos;
 import com.elphel.imagej.ims.Did_ins_1;
 import com.elphel.imagej.ims.Did_ins_2;
 import com.elphel.imagej.ims.Did_pimu;
-import com.elphel.imagej.ims.EventLogger;
 import com.elphel.imagej.ims.Imx5;
 import com.elphel.imagej.jp4.JP46_Reader_camera;
 
@@ -76,7 +66,6 @@ import ij.process.FloatProcessor;
 import ij.process.ImageConverter;
 import ij.process.ImageProcessor;
 import ij.text.TextWindow;
-import ij.plugin.filter.AVI_Writer;
 import ij.plugin.filter.GaussianBlur;
 
 public class OpticalFlow {
@@ -134,7 +123,7 @@ public class OpticalFlow {
 			{W,W,1,W,W,W,1,W},
 			{W,W,W,1,W,W,W,1},
 			{W,1,W,W,W,1,W,W}};
-	// Same for a larged kernel (5x5)
+	// Same for a larger kernel (5x5)
 	public static int [][] KERN_FG2 = {
 		//   0 1 2 3 4 5 6 7 8 91011121314151617181920212223
 			{1,1,3,2,2,2,3,1,1,1,0,0,3,0,0,2,2,2,0,0,3,0,0,1},	
@@ -6251,7 +6240,9 @@ public class OpticalFlow {
 			"\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"+
 				"\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";
@@ -6377,7 +6368,7 @@ public class OpticalFlow {
 			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)\tlong\tlat\talt";
+//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]);
@@ -6392,14 +6383,40 @@ public class OpticalFlow {
 			
 			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 [] quaternionImsToCam(double[]quat, boolean rvrs)
+			double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b);
+			double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
+//					new double[] {Math.PI/4,0,0});
+					new double[] {0, 0.13, 0});
+			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
+			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
+			
+/*
+	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);
+		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);
+		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
+	}
+
+ */
+			
+			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]); // 
+			
 			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));