added East-North-Up that better matches camera orientation. Tested

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

@@ -27,6 +27,16 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
 	/** North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude */
 	public float []		ned = new float [3];
 
+	public double [] getTheta() {
+		return new double [] {theta[0], theta[1], theta[2]};
+	}
+	public double [] getUvw() {
+		return new double [] {uvw[0], uvw[1], uvw[2]};
+	}
+	public double [] getNed() {
+		return new double [] {ned[0], ned[1], ned[2]};
+	}
+	
 	public Did_ins_1 (ByteBuffer bb) {
 		bb.order(ByteOrder.LITTLE_ENDIAN);		
 		week=      bb.getInt();

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

@@ -5,6 +5,8 @@ import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.util.Properties;
 
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+
 import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
 
 //public class Did_ins_1 implements Serializable {
@@ -39,7 +41,25 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
 	public Did_ins_2(String prefix, Properties properties) {
 		getProperties(prefix, properties);
 	}
+	public double [] getQn2b() {
+		return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
+	}
+	
+	public double [] getQEnu () {
+		Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5),	Math.sqrt(0.5), 0, true);
+		Rotation quat_rot = new Rotation(qn2b[0],qn2b[1],qn2b[2],qn2b[3],true);
+		Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
+		return new double[] {
+				quat_enu_rot.getQ0(),
+				quat_enu_rot.getQ1(),
+				quat_enu_rot.getQ2(),
+				quat_enu_rot.getQ3()};
+	}
+
 	
+	public double [] getUvw() {
+		return new double [] {uvw[0], uvw[1], uvw[2]};
+	}
 
 	public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
 		Did_ins_2 new_did = new Did_ins_2();

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

@@ -163,6 +163,18 @@ public class Imx5 {
 //	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
 //	RotationConvention.VECTOR_OPERATOR
 //RotationOrder.YXZ, ROT_CONV
+	// move to did?
+	public static double [] quatEnu (double [] quat_ned) {
+		Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5),	Math.sqrt(0.5), 0, true);
+		Rotation quat_rot = new Rotation(quat_ned[0],quat_ned[1],quat_ned[2],quat_ned[3],true);
+		Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
+		return new double[] {
+				quat_enu_rot.getQ0(),
+				quat_enu_rot.getQ1(),
+				quat_enu_rot.getQ2(),
+				quat_enu_rot.getQ3()};
+	}
+	
 	public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
 		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		double [] rslt = new double[3];
@@ -202,7 +214,23 @@ public class Imx5 {
 		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
 	}
 	
+	public static double [] quaternionImsToCam(
+			double[]quat,
+			double [] ims_atr,
+			double [] quat_ort) {
+		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);
+		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;
@@ -264,4 +292,14 @@ public class Imx5 {
 		return ned;
 	}
 	
+	public static double [] enuFromLla (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,
+				(lla[2] - lla_ref[2])
+		};
+		return ned;
+	}
+	
+	
 }

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

@@ -37,8 +37,17 @@ public class IntersceneMatchParameters {
 	public  boolean ims_use =            true;   // use IMS data
 	public  boolean ims_rebuild =        false;  // recalculate INS data (after lag change)
 	public  double  ims_offset =         0.0;    // IMS data lead relative to images (seconds) 
-	public  double  gmt_plus =           3.0;    // (hrs) filename timestamp is this lower than in IMU log 
-		
+	public  double  gmt_plus =           3.0;    // (hrs) filename timestamp is this lower than in IMU log
+	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 [] getImsMountATR() {
+		return new double [] {
+				ims_mount_atr[0] * Math.PI/180,
+				ims_mount_atr[1] * Math.PI/180,
+				ims_mount_atr[2] * Math.PI/180};
+	}
 	public  boolean force_ref_dsi =      false; // true;
 	public  boolean force_orientations = false;
 	public  boolean run_ly =             false; // will return just after LY adjustments, skipping all output generation
@@ -217,6 +226,9 @@ public class IntersceneMatchParameters {
 	public  double  disp_boost_amount  =               2.0;
 	public  double  scale_combo_strength  =            0.4; // reduce strength when it comes from combo, not DSI-MAIN
 	public  boolean save_debug_images =               true;  // Save blue sky debug images in model directory
+	/// IMS intergration
+	
+	
 	
 	// Some "AGC" to adjust how much to discard
 	public  int     margin =                       1;      // do not use tiles if their centers are closer to the image edge
@@ -402,17 +414,26 @@ public class IntersceneMatchParameters {
 	public void dialogQuestions(GenericJTabbedDialog gd) {
 		//		gd.addMessage  ("Scene parameters selection");
 //		gd.addTab         ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
-		gd.addTab         ("Scene Series", "Processing series of scenes and multi-series sets");
-		gd.addMessage  ("Build series options");
-		
+		gd.addTab         ("IMS", "IMS Integration for interscene matching");
 		gd.addCheckbox ("Use IMS data",                              this.ims_use,
-				"Use INS data for egomotion.");
+				"Use IMS data for egomotion.");
 		gd.addCheckbox ("Recalculate IMS",                           this.ims_rebuild,
 				"Recalculate scene INS data.");
 		gd.addNumericField("IMS offset",                             this.ims_offset, 5,8,"s",
 				"Positive if the IMS data has lower lag than images.");
 		gd.addNumericField("GMT plus",                               this.gmt_plus, 5,8,"hrs",
 				"Timestamp filename is lower by this hours than in the IMU log.");
+		
+		gd.addStringField ("IMS ortho mount quaterion", IntersceneMatchParameters.doublesToString(ims_ortho), 80,
+				"Approximate (90-deg) IMS to camera {0.5, -0.5, 0.5, -0.5}.");
+		gd.addStringField ("IMS mount ATR correction (deg)",  IntersceneMatchParameters.doublesToString(ims_mount_atr), 80,
+				"IMS mount fine correction (A,T,R around camera axes).");
+		gd.addStringField ("IMS mount XYZ correction (m)",  IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
+				"MS center (X,Y,Z m) in camera coordinates.");
+		
+		gd.addTab         ("Scene Series", "Processing series of scenes and multi-series sets");
+		gd.addMessage  ("Build series options");
+		
 		gd.addCheckbox ("Force reference scene DSI calculation",     this.force_ref_dsi,
 				"Calculate reference scene DSI even if the file exists.");
 		gd.addCheckbox ("Force egomotion calculation",               this.force_orientations,
@@ -1072,6 +1093,9 @@ public class IntersceneMatchParameters {
 		this.ims_rebuild =                    gd.getNextBoolean();
 		this.ims_offset =                     gd.getNextNumber();
 		this.gmt_plus =                       gd.getNextNumber();
+		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.force_ref_dsi =                  gd.getNextBoolean();
 		this.force_orientations =             gd.getNextBoolean();
 		this.run_ly =                         gd.getNextBoolean();
@@ -1420,12 +1444,12 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"ims_rebuild",                   this.ims_rebuild + "");                 // boolean
 		properties.setProperty(prefix+"ims_offset",                    this.ims_offset+"");                    // double
 		properties.setProperty(prefix+"gmt_plus",                      this.gmt_plus+"");                      // double
-		
+		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+"force_ref_dsi",                 this.force_ref_dsi + "");               // boolean
 		properties.setProperty(prefix+"force_orientations",            this.force_orientations + "");          // boolean
 		properties.setProperty(prefix+"run_ly",                        this.run_ly + "");                      // boolean
-//		properties.setProperty(prefix+"readjust_orient",               this.readjust_orient + "");             // boolean
-//		properties.setProperty(prefix+"force_interscene",              this.force_interscene + "");            // boolean
 		properties.setProperty(prefix+"min_num_orient",                this.min_num_orient+"");                // int
 		properties.setProperty(prefix+"min_num_interscene",            this.min_num_interscene+"");            // int
 		properties.setProperty(prefix+"generate_egomotion",            this.generate_egomotion+"");               // boolean
@@ -1732,6 +1756,12 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"ims_offset")!=null)                    this.ims_offset=Double.parseDouble(properties.getProperty(prefix+"ims_offset"));
 		if (properties.getProperty(prefix+"gmt_plus")!=null)                      this.gmt_plus=Double.parseDouble(properties.getProperty(prefix+"gmt_plus"));
 				
+		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+"force_ref_dsi")!=null)                 this.force_ref_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"force_ref_dsi"));		
 		if (properties.getProperty(prefix+"force_orientations")!=null)            this.force_orientations=Boolean.parseBoolean(properties.getProperty(prefix+"force_orientations"));		
 		if (properties.getProperty(prefix+"run_ly")!=null)                        this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));		
@@ -2068,6 +2098,10 @@ public class IntersceneMatchParameters {
  		imp.ims_offset                    = this.ims_offset;
  		imp.gmt_plus                      = this.gmt_plus;
  		
+ 		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.force_ref_dsi                 = this.force_ref_dsi;
 		imp.force_orientations            = this.force_orientations;
 		imp.run_ly                        = this.run_ly;