added East-North-Up that better matches camera orientation. Tested (d693f718) · Commits · Elphel / imagej-elphel

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

+10 −0

Original line number	Original line	Diff line number	Diff line
	@@ -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 */		/** 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 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) {		public Did_ins_1 (ByteBuffer bb) {
	bb.order(ByteOrder.LITTLE_ENDIAN);		bb.order(ByteOrder.LITTLE_ENDIAN);
	week= bb.getInt();		week= bb.getInt();

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

+20 −0

Original line number	Original line	Diff line number	Diff line
	@@ -5,6 +5,8 @@ import java.nio.ByteBuffer;
	import java.nio.ByteOrder;		import java.nio.ByteOrder;
	import java.util.Properties;		import java.util.Properties;

			import org.apache.commons.math3.geometry.euclidean.threed.Rotation;

	import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;		import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;

	//public class Did_ins_1 implements Serializable {		//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) {		public Did_ins_2(String prefix, Properties properties) {
	getProperties(prefix, 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) {		public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
	Did_ins_2 new_did = new Did_ins_2();		Did_ins_2 new_did = new Did_ins_2();

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

+38 −0

Original line number	Original line	Diff line number	Diff line
	@@ -163,6 +163,18 @@ public class Imx5 {
	// static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;		// static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
	// RotationConvention.VECTOR_OPERATOR		// RotationConvention.VECTOR_OPERATOR
	//RotationOrder.YXZ, ROT_CONV		//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) {		public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
	Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
	double [] rslt = new double[3];		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()};		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 ) {		public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
	RotationConvention rc = RotationConvention.FRAME_TRANSFORM;		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
	@@ -264,4 +292,14 @@ public class Imx5 {
	return ned;		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;
			}


	}		}

Admin message