Refactoring

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

@@ -23,7 +23,7 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
 //	public int				hdwStatus; // uint32_t
 	/** Euler angles: roll, pitch, yaw in radians with respect to NED */
 	/** Quaternion */
-	public float []		qn2b = new float[4];
+	public float []		qn2b = new float[4]; // orientation relative to NED
 	/** Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )". */
 //	public float []		uvw =   new float [3];
 	/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
@@ -75,10 +75,10 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
 		return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
 	}
 	
-	public double [] getQEnu () {
+	public double [] getQEnu () {// apply absolute orientation to ortho rotation
 		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);
+		Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned); // apply absolute orientation to ortho rotation
 		return new double[] {
 				quat_enu_rot.getQ0(),
 				quat_enu_rot.getQ1(),

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

@@ -189,8 +189,8 @@ public class Imx5 {
 	}
 
 	public static double [] applyQuaternionToQuaternion(
-			double[] quat,
-			double[] targ_quat,
+			double[] quat,      // additional correction
+			double[] targ_quat, // quaternion to be corrected
 			boolean inverse) {
 		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
@@ -222,9 +222,9 @@ public class Imx5 {
 	
 	
 	public static double [] quaternionImsToCam(
-			double [] quat,      // ims_to_ned
-			double [] ims_atr, // -> mount_to_cam
-			double [] quat_ort) { // -> ims_to_mount_ortho
+			double [] quat,       // ims_to_ned // normally d2_ref.getQEnu(), // current IMS absolute orientation relative to ENU world frame
+			double [] ims_atr,    // -> IMS_to_cam (angles) 
+			double [] quat_ort) { // -> ims_to_mount_ortho additional 90-degree rotaions
 		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,

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

@@ -340,7 +340,9 @@ public class IntersceneMatchParameters {
 	public  boolean apply_imu_orient =   true;  // apply  IMU misalignment to the camera if adjusted
 	public  boolean orient_by_move =     false; // use translation data to adjust IMU orientation  
 	public  boolean orient_by_rot =      true;  // use rotation data to adjust IMU orientation
-	public  boolean orient_combo =       true;  // use combined rotation+orientation for IMU/camera matching
+	public  boolean orient_combo =       true; 
+	public  boolean use3 =               true; // when comparing orientations for LMA, use only 3 vector components of the quaternion (false - all 4) 
+	// use combined rotation+orientation for IMU/camera matching
 	public  boolean adjust_gyro =        false; // adjust qyro omegas offsets
 	public  boolean apply_gyro =         true;  // apply adjusted qyro omegas offsets
 	public  boolean adjust_accl =        false; // adjust IMU velocities scales
@@ -1139,6 +1141,8 @@ min_str_neib_fpn	0.35
     public  int     quat_num_iter =         20;
     public  double  quat_reg_w =             0.25;
 
+    public  boolean quat_scale_y =           true; // Scale XYZ before orientation correction to match lengths  
+    
     public  double  quat_max_ratio =         3.0; // 5.0; // max derivatives ratio
     public  double  quat_max_damping =       0.5; // maximal regularization weight
     public  double  quat_max_change =        0.1; // radians
@@ -1866,6 +1870,8 @@ min_str_neib_fpn	0.35
 				"Use rotation data to adjust IMU orientation.");
 		gd.addCheckbox ("Use combo mode IMU orientation",            this.orient_combo,
 				"Use combined Z/h, R, A-X/h, T+Y/h for IMU mount-to-camera orientation correction. False - use X,Y,Z,A,T,R");
+		gd.addCheckbox ("Use only 3 quaternion components",            this.use3,
+				"When comparing orientations fro LMA, use only 3 vector components of the quaternions. False - all 4");
 		gd.addCheckbox ("Adjust gyro offsets",                       this.adjust_gyro,
 				"Adjust qyro omegas offsets.");
 		gd.addCheckbox ("Apply gyro offsets",                        this.apply_gyro,
@@ -3319,6 +3325,9 @@ min_str_neib_fpn	0.35
 				"A hard limit on LMA iterations.");
 		gd.addNumericField("Regularization weight",                       this.quat_reg_w, 3,5,"",
 				"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");
+		gd.addCheckbox ("Scale XYZ before irientation matching",          this.quat_scale_y,
+				"Scale XYZ before orientation correction to match lengths.");
+		
 		
 		gd.addNumericField("Maximal derivatives by axes ratio",           this.quat_max_ratio, 3,5,"x",
 				"If difference is larger, add regularization to reduce it.");
@@ -3903,6 +3912,7 @@ min_str_neib_fpn	0.35
 		this.orient_by_move =                 gd.getNextBoolean();
 		this.orient_by_rot =                  gd.getNextBoolean();
 		this.orient_combo =                   gd.getNextBoolean();
+		this.use3 =                           gd.getNextBoolean();
 		this.adjust_gyro =                    gd.getNextBoolean();
 		this.apply_gyro =                     gd.getNextBoolean();
 		this.adjust_accl =                    gd.getNextBoolean();
@@ -4648,6 +4658,7 @@ min_str_neib_fpn	0.35
 		this.quat_rms_diff =            gd.getNextNumber();
 		this.quat_num_iter =      (int) gd.getNextNumber();
 		this.quat_reg_w =               gd.getNextNumber();
+		this.quat_scale_y =             gd.getNextBoolean();
 		
 		this.quat_max_ratio =           gd.getNextNumber();
 		this.quat_max_damping =         gd.getNextNumber();
@@ -5174,6 +5185,7 @@ min_str_neib_fpn	0.35
 		properties.setProperty(prefix+"orient_by_move",                this.orient_by_move+"");                // boolean
 		properties.setProperty(prefix+"orient_by_rot",                 this.orient_by_rot+"");                 // boolean
 		properties.setProperty(prefix+"orient_combo",                  this.orient_combo+"");                  // boolean
+		properties.setProperty(prefix+"use3",                          this.use3+"");                          // boolean
 		properties.setProperty(prefix+"adjust_gyro",                   this.adjust_gyro+"");                   // boolean
 		properties.setProperty(prefix+"apply_gyro",                    this.apply_gyro+"");                    // boolean
 		properties.setProperty(prefix+"adjust_accl",                   this.adjust_accl+"");                   // boolean
@@ -5891,6 +5903,7 @@ min_str_neib_fpn	0.35
 		properties.setProperty(prefix+"quat_rms_diff",        this.quat_rms_diff+"");       // double
 		properties.setProperty(prefix+"quat_num_iter",        this.quat_num_iter+"");       // int
 		properties.setProperty(prefix+"quat_reg_w",           this.quat_reg_w+"");          // double
+		properties.setProperty(prefix+"quat_scale_y",         this.quat_scale_y+"");        // boolean
 		
 		properties.setProperty(prefix+"quat_max_ratio",       this.quat_max_ratio+"");      // double
 		properties.setProperty(prefix+"quat_max_damping",     this.quat_max_damping+"");    // double
@@ -6386,6 +6399,7 @@ min_str_neib_fpn	0.35
 		if (properties.getProperty(prefix+"orient_by_move")!=null)                this.orient_by_move=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_move"));
 		if (properties.getProperty(prefix+"orient_by_rot")!=null)                 this.orient_by_rot=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_rot"));
 		if (properties.getProperty(prefix+"orient_combo")!=null)                  this.orient_combo=Boolean.parseBoolean(properties.getProperty(prefix+"orient_combo"));
+		if (properties.getProperty(prefix+"use3")!=null)                          this.use3=Boolean.parseBoolean(properties.getProperty(prefix+"use3"));
 		if (properties.getProperty(prefix+"adjust_gyro")!=null)                   this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
 		if (properties.getProperty(prefix+"apply_gyro")!=null)                    this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
 		if (properties.getProperty(prefix+"adjust_accl")!=null)                   this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
@@ -7119,7 +7133,8 @@ min_str_neib_fpn	0.35
 		if (properties.getProperty(prefix+"reg_weight_xy")!=null)        this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
 		if (properties.getProperty(prefix+"quat_num_iter")!=null)        this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
 		if (properties.getProperty(prefix+"quat_reg_w")!=null)           this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));
-		
+		if (properties.getProperty(prefix+"quat_scale_y")!=null)               this.quat_scale_y=Boolean.parseBoolean(properties.getProperty(prefix+"quat_scale_y"));
+
 		if (properties.getProperty(prefix+"quat_max_ratio")!=null)       this.quat_max_ratio=Double.parseDouble(properties.getProperty(prefix+"quat_max_ratio"));
 		if (properties.getProperty(prefix+"quat_max_damping")!=null)     this.quat_max_damping=Double.parseDouble(properties.getProperty(prefix+"quat_max_damping"));
 		if (properties.getProperty(prefix+"quat_max_change")!=null)      this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
@@ -7630,6 +7645,7 @@ min_str_neib_fpn	0.35
 		imp.orient_by_move                = this.orient_by_move;
 		imp.orient_by_rot                 = this.orient_by_rot;
 		imp.orient_combo                  = this.orient_combo;
+		imp.use3                          = this.use3;                         
 		imp.adjust_gyro                   = this.adjust_gyro;
 		imp.apply_gyro                    = this.apply_gyro;
 		imp.adjust_accl                   = this.adjust_accl;
@@ -8327,6 +8343,7 @@ min_str_neib_fpn	0.35
 		imp.quat_rms_diff         = this.quat_rms_diff;
 		imp.quat_num_iter         = this.quat_num_iter;
 		imp.quat_reg_w            = this.quat_reg_w;
+		imp.quat_scale_y          = this.quat_scale_y;
 		
 		imp.quat_max_ratio        = this.quat_max_ratio;
 		imp.quat_max_damping      = this.quat_max_damping;