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;
 
@@ -206,6 +207,18 @@ public class Imx5 {
 				rslt_rot.getQ2(),
 				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(

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

@@ -263,7 +263,9 @@ public class Interscene {
 					ref_index,//            ref_indx,
 					cent_index, // 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);
 			}
@@ -335,7 +337,8 @@ public class Interscene {
 					cent_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);
 			}
@@ -370,7 +373,8 @@ public class Interscene {
 					cent_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);
 			}
@@ -2043,7 +2047,8 @@ public class Interscene {
         			earliest_scene,  // int            start_scene,
         			last_scene,       // int            end1_scene,
         			scenes_xyzatr,   // double [][][]  scenes_xyzatr,
-        			half_run_range); // double         half_run_range
+        			half_run_range, // double         half_run_range
+					debugLevel); // int            debugLevel);
         	break;
         default: // do nothing - already read
         }
@@ -2084,7 +2089,8 @@ public class Interscene {
             		ref_index,      // int           ref_index,
             		earliest_scene, // int           early_index,
             		last_scene);    // int           last_index)
-        } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
+//        } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
+        } else if (readjust_xy_ims) {
 			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
 					clt_parameters, // final CLTParameters clt_parameters,
 					quadCLTs,       // final QuadCLT[]     quadCLTs,
@@ -2095,7 +2101,7 @@ public class Interscene {
 		    		);
 	    	double [] rms = new double[5];
 	    	double [] quat = new double[4];
-	    	int quat_lma_mode = 2; // 1; // 2;
+	    	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
 	    	int debug_lev = debugLevel; // 3;
 //	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
 	    	double  translation_weight =   1.0 / (avg_z + 1.0);
@@ -5155,7 +5161,8 @@ public class Interscene {
 			int            ref_index,
 			int            earliest_scene,
 			String         path,
-			String         comment) {
+			String         comment,
+			int            debugLevel) {
 		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
 		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
 		QuadCLT ref_scene = quadCLTs[ref_index];
@@ -5170,8 +5177,18 @@ public class Interscene {
 				quadCLTs,       // QuadCLT []     quadCLTs,
 				ref_index,      // int            ref_index,
 				earliest_scene, // int            earliest_scene,
-				rms);           // double []      rms // null or double[2];
-	    		
+				rms,           // double []      rms // null or double[2];
+				debugLevel); // int            debugLevel
+		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]+"]");
+		}
 		
 		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
 			QuadCLT scene = quadCLTs[nscene];
@@ -5196,6 +5213,7 @@ public class Interscene {
 				"\tINS->X\tINS->Y\tINS->Z"+
 				"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
 				"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
+				"\traw_A_enu\traw_T_enu\traw_R_enu"+
 				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
 		
 		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
@@ -5216,7 +5234,8 @@ public class Interscene {
 					earliest_scene,    // int            start_scene,
 					quadCLTs.length-1, // int            end1_scene,
 					scenes_xyzatr,     // double [][][]  scenes_xyzatr, // 5.0
-					clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
+					clt_parameters.ofp.lpf_series, // half_run_range); // double         half_run_range
+					debugLevel); // int            debugLevel);
 		}
 		
 				
@@ -5324,7 +5343,7 @@ public class Interscene {
 			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
 			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
 			double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
-			
+//"\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"			
 			sb.append("\t"+ned[0]+  "\t"+ned[1]+  "\t"+ned[2]);             // global axes
 			sb.append("\t"+ims_xyz[0]+  "\t"+ims_xyz[1]+  "\t"+ims_xyz[2]); // imu axes
 			
@@ -5335,6 +5354,9 @@ public class Interscene {
 			double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
 					ims_mount_atr, // new double[] {0, 0.13, 0},
 					ims_ortho);
+			double [] cam_quat_enu_raw =Imx5.quaternionImsToCam(d2.getQEnu(),
+					new double[3], // new double[] {0, 0.13, 0},
+					ims_ortho);
 			
 			double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
 			double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
@@ -5352,14 +5374,16 @@ public class Interscene {
 			double [] quat_lma_enu_xyz = (quatCorr != null) ?
 					Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
 						cam_xyz_enu;
-			// NED without ims_mount_atr correction
+//"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"			
+			// NED without ims_mount_atr correction \tcam_X1\tcam_Y1\tcam_Z1
 			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
-			// NED with ims_mount_atr correction
+			// NED with ims_mount_atr correction \tcam_X2\tcam_Y2\tcam_Z2
 			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //
-
-			sb.append("\t"+cam_xyz_ned[0]+      "\t"+cam_xyz_ned[1]+      "\t"+cam_xyz_ned[2]); // 
-			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr
-			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr
+//"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"
+			sb.append("\t"+cam_xyz_ned[0]+      "\t"+cam_xyz_ned[1]+      "\t"+cam_xyz_ned[2]); // \tned->X\tned->Y\tned->Z
+			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr \tenu->X\tenu->Y\tenu->Z
+//"\tINS->X\tINS->Y\tINS->Z"			
+			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr "\tINS->X\tINS->Y\tINS->Z"
 			
 			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
 			
@@ -5370,6 +5394,7 @@ public class Interscene {
 			
 			double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
 			double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
+			double [] scene_raw_atr_enu = Imx5.quatToCamAtr(cam_quat_enu_raw); // not corrected by ims_mount_atr
 
 			double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
 					ims_scene_xyzatr_enu,
@@ -5380,6 +5405,8 @@ public class Interscene {
 //          "\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"			
 			sb.append("\t"+scene_abs_atr_enu[0]+  "\t"+scene_abs_atr_enu[1]+  "\t"+scene_abs_atr_enu[2]); // 
 			sb.append("\t"+scene_rel_atr_enu[0]+  "\t"+scene_rel_atr_enu[1]+  "\t"+scene_rel_atr_enu[2]); //
+//			"\traw_A_enu\traw_T_enu\traw_R_enu"+
+			sb.append("\t"+scene_raw_atr_enu[0]+  "\t"+scene_raw_atr_enu[1]+  "\t"+scene_raw_atr_enu[2]); //
 //			"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
 			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
 			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
@@ -5412,9 +5439,20 @@ public class Interscene {
 			sb.append("\n");
 		}
 		// test QuaternionLMA here
-		
-		
-		
+		// Add another data
+		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+		double [] degrees = new double[3];
+		for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
+		sb.append("New ATR mount (rad):[\t"+new_atr[0]+"\t"+new_atr[1]+"\t"+new_atr[2]+"]\n");
+		sb.append("New ATR mount (deg):[\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"]\n");
+//		double [] quatCorr
+    	Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
+		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;
+		sb.append("quatCorr=[\t"+quatCorr[0]+"\t"+quatCorr[1]+"\t"+quatCorr[2]+"\t"+quatCorr[3]+"]\n");
+		sb.append("ATR(rad)=[\t"+corr_angles[0]+"\t"+corr_angles[1]+"\t"+corr_angles[2]+"]\n");
+		sb.append("ATR(deg)=[\t"+corr_degrees[0]+"\t"+corr_degrees[1]+"\t"+corr_degrees[2]+"]\n");
 		
 		if (path!=null) {
 			String footer=(comment != null) ? ("Comment: "+comment): "";
@@ -5425,8 +5463,9 @@ public class Interscene {
 			new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
 		}
     }
- 
-    public static double [] getQuaternionCorrection(
+    @Deprecated
+    // adjusts by all 3 axis rotation 
+    public static double [] getQuaternionCorrection_Old(
 			CLTParameters  clt_parameters,
 			QuadCLT []     quadCLTs,
 			int            ref_index,
@@ -5503,6 +5542,90 @@ public class Interscene {
 			return quaternionLma.getQuaternion();
 		}
     }
+
+    
+    public static double [] getQuaternionCorrection(
+			CLTParameters  clt_parameters,
+			QuadCLT []     quadCLTs,
+			int            ref_index,
+			int            earliest_scene,
+			double []      rms, // null or double[2];
+			int            debugLevel
+    		) {
+		double []     ims_ortho =     clt_parameters.imp.ims_ortho;
+		double []     ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+		QuadCLT       ref_scene = quadCLTs[ref_index];
+		ErsCorrection ers_reference = ref_scene.getErsCorrection();
+		double [][]   quat_lma_xyz =     new double [quadCLTs.length][];
+		double [][]   quat_lma_enu_xyz = new double [quadCLTs.length][];
+		Did_ins_2     d2_ref = quadCLTs[ref_index].did_ins_2;
+		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
+			QuadCLT scene = quadCLTs[nscene];
+			if (nscene == ref_index) {
+				quat_lma_xyz[nscene] =    new double[3];
+			} else {
+				String ts = scene.getImageName();
+				quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
+			}
+			Did_ins_2   d2 = scene.did_ins_2;
+			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
+			quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
+					Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
+							ims_mount_atr,
+							ims_ortho),
+					enu,
+					false);
+		}
+		double [] up_axis = Imx5.getUpAxis(
+				ims_mount_atr); // double [] ims_atr )
+		
+		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;
+		double  lambda_max =        clt_parameters.imp.quat_lambda_max; // 100;
+		double  rms_diff =          clt_parameters.imp.quat_rms_diff; // 0.001;
+		int     num_iter =          clt_parameters.imp.quat_num_iter; // 20;
+		boolean last_run =         false;
+		int     debug_level = debugLevel;
+		QuaternionLma quaternionLma = new QuaternionLma();
+		quaternionLma.prepareCompassLMA(
+				quat_lma_enu_xyz, // quat_lma_xyz,     // double [][] vect_x,
+				quat_lma_xyz,     // double [][] vect_y,
+				null,             // double [][] vect_w, all same weight
+				up_axis,            // double []   vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
+				debug_level);     // int   debug_level)
+		int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+				lambda,           // double lambda,           // 0.1
+				lambda_scale_good,// double lambda_scale_good,// 0.5
+				lambda_scale_bad, // double lambda_scale_bad, // 8.0
+				lambda_max,       // double lambda_max,       // 100
+				rms_diff,         // double rms_diff,         // 0.001
+				num_iter,         // int    num_iter,         // 20
+				last_run,         // boolean last_run,
+				debug_level);     // int    debug_level)
+		if (lma_result < 0) {
+			return null;
+		} else {
+			if (rms != null) { // null or double[2];
+				double [] last_rms = quaternionLma.getLastRms();
+				rms[0] = last_rms[0];
+				rms[1] = last_rms[1];
+				if (rms.length >= 4) {
+					double [] initial_rms = quaternionLma.getInitialRms();
+					rms[2] = initial_rms[0];
+					rms[3] = initial_rms[1];
+					if (rms.length >= 5) {
+						rms[4] = lma_result;
+					}					
+				}
+			}
+			if (debugLevel > -3) {
+				System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
+				System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
+			}
+			return quaternionLma.getAxisQuat();
+		}
+    }
     
     static double [] test_xyz_ned(
     		double [] 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 ) {
-									System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
+									if (debugLevel > -1) {
+										System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
+									}
 								}else if (scenes_xyzatr[ns][m]== null ) {
-									System.out.println("getVelocitiesFromScenes():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

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

@@ -33,7 +33,19 @@ import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
 import Jama.Matrix;
 
 public class QuaternionLma {
-    private final static int  REGLEN =            1; // number of extra (regularization) samples  
+    private final static int  REGLEN =            1; // number of extra (regularization) samples
+    public static final int MODE_XYZ =           0;
+    public static final int MODE_XYZQ =          1;
+    public static final int MODE_COMBO =         2;
+    public static final int MODE_XYZQ_LOCAL =    3;
+    public static final int MODE_COMBO_LOCAL =   4;
+    public static final int MODE_COMPASS =       5;
+//    public static final int MODE_XYZ3 =          6;
+//    public static final int MODE_XYZQ3 =         7;
+//    public static final int MODE_COMBO3 =        8;
+//    public static final int MODE_XYZQ_LOCAL3 =   9;
+//    public static final int MODE_COMBO_LOCAL3 = 10;
+    
 	private int               N =               0;
 	// Mode2 - compensating camera uncertainty:dpx/dx ~= -dpx/daz/height(m) dpy/dy ~= dpy/dtl/height(m)
 	private int               mode =            0; // 0 xyz, 1 - xyz,quat, 2: Z/x, 2Q3, 2Q2-X/h, 2Q1+Y/h
@@ -52,17 +64,85 @@ public class QuaternionLma {
 	private double            pure_weight; // weight of samples only	
 	private double []         last_ymfx =       null;
 	private double [][]       last_jt =         null;
+	private double []         axis = null;
 	
 	public double [] getQuaternion() {
+		if (parameters_vector.length == 3) {
+			return new double [] {
+					getQ0(parameters_vector),
+					parameters_vector[0],
+					parameters_vector[1],
+					parameters_vector[2]};
+		}
 		return parameters_vector;
 	}
 	
 	public double [] getLastRms() {
 		return last_rms;
 	}
+	
 	public double [] getInitialRms() {
 		return initial_rms;
 	}
+	
+	public double [] getAxis() {
+		return axis;
+	}
+	public double [] getAxisQuat() {
+		double c = Math.cos(parameters_vector[0]/2), s = Math.sin(parameters_vector[0]/2);
+		return new double [] { c, s*axis[0], s*axis[1], s*axis[2]};
+	}
+
+	public void prepareCompassLMA(
+		double [][] vect_x,  // GPS-derived X,Y,Z relative to the reference frame
+		double [][] vect_y,  // Camera X,Y,Z relative to the reference frame
+		double [][] vect_w,
+		double []   vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
+		final int   debug_level) {
+		double axis_l = Math.sqrt(
+				vector_up[0]*vector_up[0]+
+				vector_up[1]*vector_up[1]+
+				vector_up[2]*vector_up[2]);
+		axis = new double [] {vector_up[0]/axis_l, vector_up[1]/axis_l, vector_up[2]/axis_l};
+		N = vect_x.length;
+		pure_weight = 1.0;
+		mode = MODE_COMPASS;
+		samples = 3;
+		samples_x = 3;
+		x_vector = new double [3* N];		
+		y_vector = new double [3* N];
+		weights =  new double [3* N];
+		parameters_vector = new double[1];
+		double sw = 0;
+		for (int i = 0; i < N; i++) {
+			if ((vect_x[i]== null) || (vect_y[i]== null)) {
+				for (int j = 0; j < samples; j++) {
+					x_vector[samples * i + j] = 0.0;
+					y_vector[samples * i + j] = 0.0;
+					weights[samples*i + j] = 0.0;
+				}				
+			} else {
+				for (int j = 0; j < 3; j++) {
+					x_vector[samples * i + j] = vect_x[i][j];
+					y_vector[samples * i + j] = vect_y[i][j];
+					double w = (vect_w != null)? vect_w[i][j] : 1.0;
+					weights[samples*i + j] = w;
+					sw += w;
+				}
+			}
+		}
+		double k = (pure_weight)/sw;
+		for (int i = 0; i < weights.length; i++) weights[i] *= k;
+		last_jt = new double [parameters_vector.length][];
+		if (debug_level > 0) {
+			 debugYfX ( "",   // String pfx,
+						y_vector); // double [] data)
+			 debugYfX ( "PIMU-",   // String pfx,
+						x_vector); // double [] data)
+		}
+		
+	}
+	
 	public void prepareLMA(
 			double [][] vect_x,
 			double [][] vect_y,
@@ -72,7 +152,7 @@ public class QuaternionLma {
 			final int   debug_level) {
 		N = vect_x.length;
 		pure_weight = 1.0 - reg_w;
-		mode = 0;
+		mode = MODE_XYZ;
 		samples = 3;
 		samples_x = 3;
 		x_vector = new double [3* N];
@@ -115,7 +195,7 @@ public class QuaternionLma {
 			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 			double []     quat0,
 			final int     debug_level) {
-		if (mode != 1) {
+		if (mode != MODE_XYZQ) {
 			prepareLMAMode3(
 					mode, // int           mode,
 					vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
@@ -203,13 +283,14 @@ public class QuaternionLma {
 		samples = 7;
 		samples_x = 7;
 		pure_weight = 1.0 - reg_w;
+		int extra_samples = (quat0.length < 4)? 0:REGLEN;
 		x_vector =     new double [samples *   N];
-		y_vector =     new double [samples *   N + REGLEN];
-		y_inv_vector = new double [samples_x * N + REGLEN]; 		
-		weights =      new double [samples *   N + REGLEN];
+		y_vector =     new double [samples *   N + extra_samples];
+		y_inv_vector = new double [samples_x * N + extra_samples]; 		
+		weights =      new double [samples *   N + extra_samples];
 		parameters_vector = quat0.clone();
 		
-		double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
+//		double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
 		double sw = 0;
 		for (int i = 0; i < N; i++) {
 			if ((vect_x[i]== null) || (vect_y[i]== null)) {
@@ -255,12 +336,12 @@ public class QuaternionLma {
 				System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3,  4);
 				
 				double wt = translation_weight*((vect_w != null)?  vect_w[i] : 1.0);
-				double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);
+				double wr = (1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);
 				for (int j = 0; j < 3; j++) {
 					weights[samples * i +  j] = wt;
 					sw += wt;
 				}
-				for (int j = 0; j < 4; j++) {
+				for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
 					weights[samples * i + 3 + j] = wr;
 					sw += wr;
 				}
@@ -268,8 +349,10 @@ public class QuaternionLma {
 		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
-		weights [samples * N] = 1.0 - pure_weight;
-		y_inv_vector[samples * N] = 1.0;
+		if (parameters_vector.length >=4) {
+			weights [samples * N] = 1.0 - pure_weight;
+			y_inv_vector[samples * N] = 1.0;
+		}
 		last_jt = new double [parameters_vector.length][];		
 		if (debug_level > 0) {
 			 debugYfX ( "Y-INV-",   // String pfx,
@@ -289,7 +372,7 @@ public class QuaternionLma {
 			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 			double []     quat0,
 			final int     debug_level) {
-		if (mode != 2) {
+		if (mode != MODE_COMBO) {
 			prepareLMAMode4(
 					mode,       // int           mode,
 					avg_height, // double        avg_height,
@@ -452,24 +535,37 @@ public class QuaternionLma {
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
 		switch (mode) {
-		case 1:return  getFxDerivs6Dof(
+		case MODE_XYZQ:return  getFxDerivs6Dof(
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 				 debug_level); // final int         debug_level)
-		case 2:return  getFxDerivsVisual( // fill change
+		case MODE_COMBO:return  getFxDerivsVisual( // fill change
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 				 debug_level); // final int         debug_level)
-		case 3:return  getFxDerivs6DofMode3(
+		case MODE_XYZQ_LOCAL:
+			if (parameters_vector.length < 4) {
+				return  getFxDerivs6DofMode33(
+						vector,       // double []         vector,
+						jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+						debug_level); // final int         debug_level)
+			} else {
+				return  getFxDerivs6DofMode3(
+						vector,       // double []         vector,
+						jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+						debug_level); // final int         debug_level)
+			}
+		case MODE_COMBO_LOCAL:
+			     return  getFxDerivsVisualMode4( // fill change
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 				 debug_level); // final int         debug_level)
-		case 4:return  getFxDerivsVisualMode4( // fill change
+		case MODE_COMPASS:return  getFxDerivsCompass( // fill change
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 				 debug_level); // final int         debug_level)
 		}
-		// remains here for mode 0
+		// remains here for mode MODE_XYZ
 		double [] fx = new double [weights.length];
 		final double q0 = vector[0]; 
 		final double q1 = vector[1]; 
@@ -499,6 +595,40 @@ public class QuaternionLma {
 		return fx;
 	}
 	
+	private double [] getFxDerivsCompass(
+			double []         vector,
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2);
+		//axis
+		double [] fx = new double [weights.length];
+		final double [] q = new double [] { c/2, s*axis[0]/2, s*axis[1]/2, s*axis[2]/2};
+		double [] dq_dv  =  new double [] {-s/2, c*axis[0]/2, c*axis[1]/2, c*axis[2]/2};
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+		}
+		double [] xyz_rot;
+		double [][] xyz_dq;
+		for (int i = 0; i < N; i++) {
+			int i3 = 3 * i;
+			final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
+			xyz_rot = applyTo(q, xyz);
+			System.arraycopy(xyz_rot, 0, fx, i3, 3);
+			if (jt != null) {
+				xyz_dq =  applyToDQ(q, xyz);
+				for (int j = 0; j < 3; j++) {
+					jt[0][i3+j] = 0.0;
+					for (int k = 0; k < 4; k++) {
+						jt[0][i3+j] += dq_dv[k] * xyz_dq[k][j];
+					}
+				}
+			}
+		}
+		return fx;
+	}
+
 	private double [] getFxDerivs6Dof(
 			double []         vector,
 			final double [][] jt, // should be null or initialized with [vector.length][]
@@ -641,16 +771,121 @@ public class QuaternionLma {
 		}
 		return fx;
 	}
-	
-	
+	private double [] getFxDerivs6DofMode33(
+			double []         vector, //
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		double [] fx = new double [weights.length];
+		final double q1 = vector[0]; 
+		final double q2 = vector[1]; 
+		final double q3 = vector[2];
+		final double q0 = getQ0(vector); 
+//		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
+		final double [] vector_r = new double[] { q0,q1,q2,q3};
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+//				jt[i][samples * N] = 2 * vector[i];
+			}
+		}
+//		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
+		double [] xyz_rot;
+		double [] quat_rot;
+		double [][] xyz_dq;
+		double [][] quat_dq;
+		double [][] inv_y = new double [][] {new double[3],new double[4]}; 			
+		for (int i = 0; i < N; i++) {
+			int i7 = samples * i;
+			has_data:{
+				for (int j = 0; j < samples; j++) {
+					if (weights[i7+j] > 0) {
+						break has_data;
+					}
+				}
+				continue; // nothing to process for this scene 
+			}
+			
+			// translations
+			final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};
+			// rotations
+			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
+			xyz_rot = applyTo(vector_r, xyz);
+			quat_rot = composeQR_Q(vector_r, quat_r);
+			System.arraycopy(y_inv_vector, i7,   inv_y[0], 0, 3);
+			System.arraycopy(y_inv_vector, i7+3, inv_y[1], 0, 4);
+			
+		    double [][] comb_y = combineTransRot( // 
+		            inv_y[0],   // double [] xyz_src,     // transformation to apply to (was reference_xyz)
+		            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)
+		            xyz_rot,    // double [] xyz_target,  // to which is applied (was scene_xyz)
+		            quat_rot);  // double [] quat_target  // to which is applied (was scene_atr)		
+			System.arraycopy(comb_y[0], 0, fx, i7,   3);
+			System.arraycopy(comb_y[1], 0, fx, i7+3, 4);
+
+			
+			if (jt != null) {
+				xyz_dq =  applyToDQ(vector_r, xyz);
+				double [][] xyz_dq_local = new double [xyz_dq.length][];
+				for (int j = 0; j < xyz_dq.length; j++) {
+					xyz_dq_local[j] =combineTransRot(
+				            null,       // double [] xyz_src,     // transformation to apply to (was reference_xyz)
+				            inv_y[1],   // double [] quat_src,    // transformation to apply to (was reference_atr)
+				            xyz_dq[j],  // double [] xyz_target,  // to which is applied (was scene_xyz)
+				            null)[0];   // double [] quat_target  // to which is applied (was scene_atr)		
+				}
+				double [][] xyz_dq_local3 = dQuat123(
+						vector,        // double [] q123,
+						xyz_dq_local,  // double [][] dq0123, // [4][]
+						q0);           // double q0)
+				for (int j = 0; j < xyz_dq_local3.length; j++) { // xyz_dq_local3.length==3
+					System.arraycopy(xyz_dq_local3[j], 0, jt[j], i7, 3);
+				}
+				quat_dq = composeQR_QdQ(vector_r,quat_r);
+				// 2 alternative ways with the same result
+				if (debug_level < 1000) {
+					double [][] invy_mat = qMat(inv_y[1]);
+					double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
+					double [][] quat_dq_local3 =dQuat123(
+							vector,        // double [] q123,
+							quat_dq_local,  // double [][] dq0123, // [4][]
+							q0);           // double q0)
+ 
+					for (int j = 0; j < quat_dq_local3.length; j++) {
+						System.arraycopy(quat_dq_local3[j], 0, jt[j], i7+3, 4);
+					}
+				} else {
+					double [][] dcomp_dsecond = composeDR(inv_y[1]);
+					double [][] quat_dq_local1 = new double [4][];
+					for (int j = 0; j < quat_dq_local1.length; j++) {
+						quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
+					}
+					double [][] quat_dq_local13 =dQuat123(
+							vector,        // double [] q123,
+							quat_dq_local1,  // double [][] dq0123, // [4][]
+							q0);           // double q0)
+
+					for (int j = 0; j < quat_dq_local13.length; j++) {
+						System.arraycopy(quat_dq_local13[j], 0, jt[j], i7+3, 4);
+					}
+				}
+			}			
+		}
+		return fx;
+	}
 	
 	private double compareJT(
 			double [] vector,
 			double    delta) {
 		double []  errors=new double [vector.length];
 		double [][] jt =  new double [vector.length][];
-		System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
-		double [] fx_center =  getFxDerivs(
+//		System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
+		System.out.print("Parameters vector = [");
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(vector[i]);
+			if (i < (vector.length -1)) System.out.print(", ");
+		}
+		System.out.println("]");
+		getFxDerivs(
 				vector,
 				jt, // final double [][] jt, // should be null or initialized with [vector.length][]
 				1); // debug_level);
@@ -659,18 +894,39 @@ public class QuaternionLma {
 				delta, // final double      delta,
 				-1); // final int         debug_level)
 		for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {
+			System.out.print(String.format("%3d",n));
+			for (int i = 0; i < vector.length; i++) {
+				System.out.print(String.format("\t%12.9f",jt[i][n]));
+			}			
+			for (int i = 0; i < vector.length; i++) {
+				System.out.print(String.format("\t%12.9f",jt_delta[i][n]));
+			}			
+			for (int i = 0; i < vector.length; i++) {
+				System.out.print(String.format("\t%12.9f",jt[i][n]-jt_delta[i][n]));
+			}			
+			System.out.println();
+			/*
 			System.out.println(String.format(
 					"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
 					n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],
 					jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],
 					jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
+					*/
 			for (int i = 0; i < vector.length; i++) {
 				errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
 			}
 		}
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print("\t\t");
+		}			
+		for (int i = 0; i < vector.length; i++) {
+			System.out.print(String.format("\t%12.9f",errors[i]));
+		}			
+        /*
 		System.out.println(String.format(
 				"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
 				errors[0], errors[1], errors[2], errors[3]));
+				*/
 		double err=0;
 		for (int i = 0; i < vector.length; i++) {
 			err = Math.max(errors[i], err);
@@ -874,7 +1130,7 @@ public class QuaternionLma {
 			) {
 		final double []     wymfw =       new double [fx.length];
 		double s_rms=0; 
-		double rms_pure=0;
+		double rms_pure=Double.NaN;
 //		int num_comp = use_6dof? 7 : 3;
 		for (int i = 0; i < fx.length; i++) {
 			double d = y_vector[i] - fx[i];
@@ -892,7 +1148,9 @@ public class QuaternionLma {
 			s_rms += d * wd;
 		}
 		double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
-
+		if (Double.isNaN(rms_pure)) {
+			rms_pure=rms;
+		}
 		if (rms_fp != null) {
 			rms_fp[0] = rms;
 			rms_fp[1] = rms_pure;
@@ -1210,7 +1468,7 @@ public class QuaternionLma {
 			String pfx,
 			double [] data) {
 //		if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
-		if (mode == 0 ) {
+		if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) {
 			System.out.println(String.format("%3s"+
 					"\t%9s\t%9s\t%9s",
 					"N",pfx+"X",pfx+"Y",pfx+"Z"));
@@ -1221,7 +1479,7 @@ public class QuaternionLma {
 						data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
 			}
 			System.out.println();
-		} else  if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
+		} else  if ((mode == MODE_XYZQ) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
 			System.out.println(String.format("%3s"+
 					"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
 					"\t%9s\t%9s\t%9s",
@@ -1594,5 +1852,32 @@ public class QuaternionLma {
 		}
 		return tmat;
 	}
-
+	public static double getQ0(double [] q123) {
+		return Math.sqrt(1.0-(q123[0]*q123[0]+q123[1]*q123[1]+q123[2]*q123[2]));
+	}
+	
+	public static double [] dQuat123(
+			double [] q123,
+			double [] dq0123, // [4]
+			double q0) { // null or double[3]
+		double [] dq123 = new double[3];
+		for (int i = 0; i < dq123.length; i++) {
+			dq123[i]=dq0123[i+1]- dq0123[0] * q123[i]/q0;
+		}
+		return dq123;
+	}
+	
+	public static double [][] dQuat123(
+			double [] q123,
+			double [][] dq0123, // [4][]
+			double q0) { // null or double[3]
+		double [][] dq123 = new double[3][dq0123[0].length];
+		for (int n = 0; n < dq123[0].length; n++) {
+			for (int i = 0; i < dq123.length; i++) {
+				dq123[i][n]=dq0123[i+1][n]- dq0123[0][n]*q123[i]/q0;
+			}
+		}
+		return dq123;
+	}
+	
 }