Initial ERS from IMS (e9aa9780) · Commits · Elphel / imagej-elphel

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

+12 −77

Original line number	Diff line number	Diff line
		@@ -187,25 +187,11 @@ 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);
		public static double [] quaternionImsToCam(
		double [] quat, // ims_to_ned
		double [] ims_atr, // -> mount_to_cam
		double [] quat_ort) { // -> ims_to_mount_ortho
		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]);
		@@ -214,75 +200,24 @@ public class Imx5 {
		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
		}

		// from ims frame to camera frame
		public static double [] quaternionImsToCam(
		double[]quat,
		double [] ims_atr,
		double [] quat_ort) {
		double [] ims_atr, // -> mount_to_cam
		double [] quat_ort) { // -> ims_to_mount_ortho
		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);
		Rotation cam_quat = mount_to_cam.applyTo(ims_to_mount_ortho);
		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;
		// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
		RotationOrder ro = RotationOrder.XYZ;
		switch (ord) {
		case 0: ro = RotationOrder.XYZ; break;
		case 1: ro = RotationOrder.XZY; break;
		case 2: ro = RotationOrder.YXZ; break;
		case 3: ro = RotationOrder.YZX; break;
		case 4: ro = RotationOrder.ZXY; break;
		case 5: ro = RotationOrder.ZYX; break;
		}
		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
		// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
		double [] angles1 = cam_to_ims.getAngles(ro1, rc);
		Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
		//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
		double [] angles = comb_rot.getAngles(ro1, rc);
		return angles;
		}

		public static double [] imsQToCamRotations(double [] ims_qn2b, int ord, boolean rev_order, boolean rev_matrix ) {
		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
		// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
		RotationOrder ro = RotationOrder.XYZ;
		switch (ord) {
		case 0: ro = RotationOrder.XYZ; break;
		case 1: ro = RotationOrder.XZY; break;
		case 2: ro = RotationOrder.YXZ; break;
		case 3: ro = RotationOrder.YZX; break;
		case 4: ro = RotationOrder.ZXY; break;
		case 5: ro = RotationOrder.ZYX; break;
		}
		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
		// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
		double [] angles1 = cam_to_ims.getAngles(ro1, rc);

		// Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
		// public Rotation(double q0, double q1, double q2, double q3, boolean needsNormalization) {
		Rotation ims_rot = new Rotation(ims_qn2b[0],ims_qn2b[1],ims_qn2b[2],ims_qn2b[3],true);
		//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
		double [] angles = comb_rot.getAngles(ro1, rc);
		return angles;
		}

		public static double [] nedFromLla (double [] lla, double [] lla_ref) {
		double [] ned = new double[] {
		EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,

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

+7 −0

Original line number	Diff line number	Diff line
		@@ -260,6 +260,13 @@ public class ErsCorrection extends GeometryCorrection {
		this.ers_wxyz_center_dt = ers_xyz_dt;
		this.ers_watr_center_dt = ers_atr_dt;
		}

		public void setErsDt(
		double [][] ers_xyzatr_dt) {
		this.ers_wxyz_center_dt = ers_xyzatr_dt[0];
		this.ers_watr_center_dt = ers_xyzatr_dt[1];
		}

		public void setErsDt_test(
		double [] ers_xyz_dt,
		double [] ers_atr_dt) {

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

+102 −427

File changed.

Preview size limit exceeded, changes collapsed.

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

+16 −0

Original line number	Diff line number	Diff line
		@@ -41,6 +41,9 @@ public class IntersceneMatchParameters {
		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 [] ims_scale_xyz = {1.1, 1.1, 1.1};
		public double [] ims_scale_atr = {1.1, 1.1, 1.1};


		public boolean sfm_use = true; // use SfM to improve depth map
		public double sfm_min_base = 2.0; // use SfM if baseline exceeds this
		@@ -462,6 +465,11 @@ public class IntersceneMatchParameters {
		gd.addStringField ("IMS mount XYZ correction (m)", IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
		"MS center (X,Y,Z m) in camera coordinates.");

		gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
		"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
		gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
		"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1'.");

		gd.addTab ("SfM", "Structure from Motion to improve depth map for the lateral views");
		gd.addCheckbox ("Use SfM", this.sfm_use,
		"Use SfM for the depth map enhancement for laterally moving camera.");
		@@ -1186,6 +1194,8 @@ public class IntersceneMatchParameters {
		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.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
		this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
		this.sfm_use = gd.getNextBoolean();
		this.sfm_min_base = gd.getNextNumber();
		this.sfm_min_gain = gd.getNextNumber();
		@@ -1566,6 +1576,8 @@ public class IntersceneMatchParameters {
		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+"ims_scale_xyz", IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
		properties.setProperty(prefix+"ims_scale_atr", IntersceneMatchParameters.doublesToString(this.ims_scale_atr));
		properties.setProperty(prefix+"sfm_use", this.sfm_use + ""); // boolean
		properties.setProperty(prefix+"sfm_min_base", this.sfm_min_base+""); // double
		properties.setProperty(prefix+"sfm_min_gain", this.sfm_min_gain+""); // double
		@@ -1906,6 +1918,8 @@ public class IntersceneMatchParameters {
		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+"ims_scale_xyz")!=null) this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
		if (properties.getProperty(prefix+"ims_scale_atr")!=null) this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
		if (properties.getProperty(prefix+"sfm_use")!=null) this.sfm_use=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_use"));
		if (properties.getProperty(prefix+"sfm_min_base")!=null) this.sfm_min_base=Double.parseDouble(properties.getProperty(prefix+"sfm_min_base"));
		if (properties.getProperty(prefix+"sfm_min_gain")!=null) this.sfm_min_gain=Double.parseDouble(properties.getProperty(prefix+"sfm_min_gain"));
		@@ -2273,6 +2287,8 @@ public class IntersceneMatchParameters {
		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.ims_scale_xyz = this.ims_scale_xyz.clone();
		imp.ims_scale_atr = this.ims_scale_atr.clone();
		imp.sfm_use = this.sfm_use;
		imp.sfm_min_base = this.sfm_min_base;
		imp.sfm_min_gain = this.sfm_min_gain;

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

+21 −3

Original line number	Diff line number	Diff line
		@@ -4773,6 +4773,20 @@ public class OpticalFlow {
		int center_index =quadCLTs[last_index].getReferenceIndex(null);
		if (center_index == -1) {
		build_ref_dsi = true;
		} else {
		QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnQuadCLT( // will conditionImageSet
		quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
		clt_parameters,
		colorProcParameters, //
		threadsMax,
		debugLevel-2);
		if ((try_ref_scene == null) \|\| (try_ref_scene.getDLS() == null)) {
		if (debugLevel >-2) {
		System.out.println("DSI data for scene "+quadCLTs[last_index].getReferenceTimestamp()+
		" does not exist, forcing initial orientation.");
		}
		force_initial_orientations = true;
		}
		}
		} else {
		if ((quadCLTs[last_index] == null) \|\| !quadCLTs[last_index].dsiExists()) {
		@@ -4995,7 +5009,7 @@ public class OpticalFlow {
		ref_index =quadCLTs[last_index].getReferenceIndex(quadCLTs);
		quadCLTs[ref_index].restoreInterProperties(null, false, debugLevel); //null
		int [] first_last_index = quadCLTs[ref_index].getFirstLastIndex(quadCLTs);
		earliest_scene = first_last_index[0];
		earliest_scene = first_last_index[0]; // null pointer if does not exist
		last_index = first_last_index[1];
		}
		// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
		@@ -5188,8 +5202,12 @@ public class OpticalFlow {
		}
		boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
		boolean ers_from_ims = false; // change later
		int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
		ers_mode = 1; // TODO: remove later!
		// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
		int ers_mode = 0; // keep
		// with IMS it is already set during initial orientation. In non-IMS mode
		if (!ers_from_ims && (quadCLTs[ref_index].getNumOrient() < 2)) {
		ers_mode = 1; // calculate velocity
		}
		earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
		clt_parameters, // CLTParameters clt_parameters,
		mb_max_gain, // double mb_max_gain,