Initial ERS from IMS

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

@@ -186,26 +186,12 @@ 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]);
@@ -213,76 +199,25 @@ public class Imx5 {
 		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()};
 	}
-	
+
+	// 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

@@ -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/IntersceneMatchParameters.java

@@ -41,7 +41,10 @@ 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
 	public  double  sfm_min_gain =       5.0;    // Minimal SfM gain to apply SfM to the depth map
@@ -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

@@ -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,

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

@@ -196,6 +196,57 @@ public class QuadCLTCPU {
     	return timestamp_reference;
     }
     
+	public double [][] getDxyzatrIms(
+			CLTParameters clt_parameters,
+			boolean       scale) {
+		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
+		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+		double [] quat_ims_cam = Imx5.quaternionImsToCam(
+				ims_mount_atr, // new double[] {0, 0.13, 0},
+				ims_ortho);
+		double [] double_uvw =  did_ins_2.getUvw();
+		double [] omegas=new double[] {did_pimu.theta[0]/did_pimu.dt,
+				did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt};
+		double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
+		double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
+		// a (around Y),t (around X), r (around Z)
+		double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
+		if (scale) {
+			for (int i = 0; i < 3; i++) {
+				dxyzatyr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
+				dxyzatyr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
+			}
+		}
+		return dxyzatyr;
+	}
+
+	public static double [][] scaleDtToErs(
+			CLTParameters clt_parameters,
+			double [][] cam_dxyzatr){
+		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
+		double [][] scaled_dxyzatr = new double[2][3];
+		for (int i = 0; i < scaled_dxyzatr.length; i++) {
+			for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
+				scaled_dxyzatr[i][j] = cam_dxyzatr[i][j]* dbg_scale_dt[i][j];
+			}
+		}
+		return scaled_dxyzatr;
+	}
+
+	public static double [][] scaleDtFromErs(
+			CLTParameters clt_parameters,
+			double [][] scaled_dxyzatr){
+		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
+		double [][] cam_dxyzatr = new double[2][3];
+		for (int i = 0; i < scaled_dxyzatr.length; i++) {
+			for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
+				cam_dxyzatr[i][j] = scaled_dxyzatr[i][j] / dbg_scale_dt[i][j];
+			}
+		}
+		return cam_dxyzatr;
+	}
+	
+	
     /**
      * If this scene has a reference to one of the scenes array, return its index in the array
      * otherwise return -1 if there is no pointer to reference scene and -2 if pointer is not