Unfinished mods

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

@@ -9,6 +9,7 @@ import java.nio.ByteOrder;
 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 
 import com.elphel.imagej.tileprocessor.ErsCorrection;
 
@@ -207,6 +208,18 @@ public class Imx5 {
 				rslt_rot.getQ3()};
 	}
 
+	/**
+	 * Get up (for IMS) vector relative to the camera reference frame
+	 * @param ims_atr mount to camera correction
+	 * @return {x,y,z} of the IMS-up relative to the camera frame (reference scene)
+	 */
+	public static double [] getUpAxis(
+			double [] ims_atr ) {// -> mount_to_cam
+		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
+				ims_atr[0],    ims_atr[1],    ims_atr[2]);
+		return mount_to_cam.applyInverseTo(new Vector3D(0, 0, 1)).toArray();
+	}
+	
 	
 	public static double [] quaternionImsToCam(
 			double [] quat,      // ims_to_ned

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

@@ -5032,7 +5032,9 @@ public class OpticalFlow {
 							ref_index,//            ref_indx,
 							earliest_scene, // int            earliest_scene,
 							ego_path, // String         path,
-							ego_comment); // String         comment);
+							ego_comment, // String         comment);
+							debugLevel); // int            debugLevel);
+							
 					if (debugLevel> -3) {
 						System.out.println("Egomotion table saved to "+ego_path);
 					}
@@ -5386,7 +5388,8 @@ public class OpticalFlow {
 							ref_index,//            ref_indx,
 							earliest_scene, // int            earliest_scene,
 							ego_path, // String         path,
-							ego_comment); // String         comment);
+							ego_comment, // String         comment);
+							debugLevel); // int debugLevel)
 					if (debugLevel> -3) {
 						System.out.println("Egomotion table saved to "+ego_path);
 					}
@@ -5438,7 +5441,7 @@ public class OpticalFlow {
 	    	}
 	    	double [] rms = new double[5];
 	    	double [] quat = new double[4];
-	    	int quat_lma_mode = 4; // 3; //  2; // 1;
+	    	int quat_lma_mode = 03; // 4; // 3; //  2; // 1;
 	    	int debug_lev = debugLevel; // 3;
 	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
 	    	double  translation_weight =   1.0 / (avg_z + 1.0);
@@ -5583,7 +5586,8 @@ public class OpticalFlow {
 					quadCLTs,       // QuadCLT []     quadCLTs,
 					ref_index,      // int            ref_index,
 					earliest_scene, // int            earliest_scene,
-					quat_rms);           // double []      rms // null or double[2];
+					quat_rms,           // double []      rms // null or double[2];
+					debugLevel); // int            debugLevel
 			if (quatCorr != null) {
 				int num_iter = (int) quat_rms[4];
 				if (debugLevel> -3) {
@@ -5596,6 +5600,16 @@ public class OpticalFlow {
 				quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
 						null, // String path,             // full name with extension or w/o path to use x3d directory
 						debugLevel+1);
+				if (debugLevel > -3) {
+		        	Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
+					System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
+					double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+					double []  corr_degrees = new double[3];
+					for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
+					System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
+					System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
+					System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
+				}
 			} else {
 				if (debugLevel> -3) {
 					System.out.println("Failed to perform attitude correction with QuaternionLma.");
@@ -5616,7 +5630,9 @@ public class OpticalFlow {
 					ref_index,//            ref_indx,
 					earliest_scene, // int            earliest_scene,
 					ego_path, // String         path,
-					ego_comment); // String         comment);
+					ego_comment, // String         comment);
+					debugLevel); // int debugLevel)
+
 			if (debugLevel> -3) {
 				System.out.println("Egomotion table saved to "+ego_path);
 			}
@@ -5628,7 +5644,8 @@ public class OpticalFlow {
 		    			ref_index,//            ref_indx,
 						earliest_scene, // int            earliest_scene,
 		    			ego_path, // String         path,
-		    			ego_comment); // String         comment);
+		    			ego_comment, // String         comment);
+						debugLevel); // int debugLevel)
 		    }
 		}
 		boolean test_ground = false; // true;
@@ -8256,7 +8273,8 @@ public class OpticalFlow {
 			int            start_scene,
 			int            end_scene,
 			double [][][]  scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available
-			double         half_run_range
+			double         half_run_range,
+			int            debugLevel
 			){
 		double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][];
 		if (half_run_range <=0 ) {
@@ -8314,9 +8332,13 @@ public class OpticalFlow {
 						for (int m = 0; m < sy.length; m++) {
 							for (int d = 0; d < sy[m].length; d++) {
 								if (scenes_xyzatr[ns]== null ) {
+									if (debugLevel > -1) {
 										System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
+									}
 								}else if (scenes_xyzatr[ns][m]== null ) {
+									if (debugLevel > -1) {
 										System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
+									}
 								} else {
 									double y = scenes_xyzatr[ns][m][d];
 									sy [m][d] += w *  y;

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

@@ -256,6 +256,7 @@ public class QuadCLTCPU {
 			double []     rms, // null or double[5];
 			int           debugLevel
     		) {
+    	final boolean use3 = (quat_lma_mode == 3); // true;
 		double lambda =            clt_parameters.imp.quat_lambda; // 0.1;
 		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
 		double lambda_scale_bad =  clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
@@ -264,7 +265,7 @@ public class QuadCLTCPU {
 		int    num_iter =          clt_parameters.imp.quat_num_iter; // 20;
 		boolean last_run =         false;
 		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;
-		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
+		double []   quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
 		QuaternionLma quaternionLma = new QuaternionLma();
 		if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {
 			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR