working snapshot (working on IMU-to-camera rotation calibration)

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

@@ -249,7 +249,7 @@ public class Imx5 {
 	 * Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
 	 * @param ims_atr original ATR of the IMS mount
 	 * @param corr_q correction quaternion
-	 * @return corrected ATR of teh IMU mount
+	 * @return corrected ATR of the IMU mount
 	 */
 	public static double [] adjustMountAtrByQuat(
 			double [] ims_atr,

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

@@ -5181,7 +5181,7 @@ public class Interscene {
 				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):");
+			System.out.println("Applying correction to 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;
@@ -5476,20 +5476,20 @@ public class Interscene {
 		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][3];
-		double [][]   quat_lma_enu_xyz = new double [quadCLTs.length][3];
+		double [][][]   quat_lma_xyzatr =     new double [quadCLTs.length][2][3];
+		double [][][]   quat_lma_enu_xyzatr = new double [quadCLTs.length][2][3];
 		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];
+				quat_lma_xyzatr[nscene][0] =    new double[3];
 			} else {
 				String ts = scene.getImageName();
-				quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
+				quat_lma_xyzatr[nscene][0] = 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(
+			quat_lma_enu_xyzatr[nscene][0] = Imx5.applyQuaternionTo(
 					Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
 							ims_mount_atr,
 							ims_ortho),
@@ -5508,8 +5508,8 @@ public class Interscene {
 		int         debug_level = 1;
 		QuaternionLma quaternionLma = new QuaternionLma();
 		quaternionLma.prepareLMA(
-				quat_lma_enu_xyz, // quat_lma_xyz,     // double [][] vect_x,
-				quat_lma_xyz,     // double [][] vect_y,
+				quat_lma_enu_xyzatr, // quat_lma_xyz,     // double [][] vect_x,
+				quat_lma_xyzatr,     // double [][] vect_y,
 				null,             // double [][] vect_w, all same weight
 				reg_w,            // double      reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 				quat0,            // double []   quat0,

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

@@ -5425,68 +5425,13 @@ public class OpticalFlow {
 		}
 		// later move to the right place
 		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
-			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
-					clt_parameters, // final CLTParameters clt_parameters,
-					quadCLTs,       // final QuadCLT[]     quadCLTs,
-					ref_index,      // final int           ref_index,
-					null,           // double [][][]       dxyzatr,
-					earliest_scene, // final int           early_index,
-					last_index      //(quadCLTs.length -1) // int           last_index,
-		    		);
-			double [][][] xyzatr = new double [quadCLTs.length][][];
-			ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
-	    	for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
-	        	String ts = quadCLTs[nscene].getImageName();
-	        	xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
-	    	}
-	    	double [] rms = new double[5];
-	    	double [] quat = new double[4];
-	    	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);
-	    	
-	    	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
-	    			clt_parameters,     // CLTParameters clt_parameters,
-	    			quat_lma_mode,      // int quat_lma_mode,
-	    			avg_z,              // double        avg_height,
-	    			translation_weight, // double        translation_weight,
+			   QuadCLT.adjustImuOrient(
+					   clt_parameters,     //CLTParameters clt_parameters,     // CLTParameters clt_parameters,
 					   quadCLTs, // QuadCLT[]     quadCLTs,
-	    			xyzatr,             // double [][][] xyzatr,
-	    			pimu_xyzatr,        // double [][][] ims_xyzatr,
 					   ref_index, // int           ref_index,
-	    			earliest_scene,     // int           early_index,
+					   earliest_scene, // int           earliest_scene,
 					   last_index, // int           last_index,
-	    			rms,                //double []     rms, // null or double[5];
-	    			quat,               // double []     quaternion, // null or double[4]       
-	    			debug_lev); // int           debugLevel
-	    	if (rotated_xyzatr != null) {
-	        	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
-	        	if (debugLevel > -3) {
-	        		System.out.println("Applying correction ot the IMS mount orientation:");
-					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("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[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]+"]");
-	        	}
-	        	double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
-	        	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
-	        			ims_mount_atr, // double [] ims_atr,
-	        			quat); // double [] corr_q)
-	        	clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
-	        	if (debugLevel > -3) {
-	        		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;
-		    		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
-		    		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
-		    		System.out.println ("*** Need to save the main configuration file ***");
-	        	}
-	    	} else {
-	    		System.out.println ("*** Failed to calculate IMS mount correction! ***");
-	    	}
+					   debugLevel); // 			    		int debugLevel
 		}
 		if (run_ly) {
 			if (debugLevel > -3) {
@@ -5602,7 +5547,7 @@ public class OpticalFlow {
 						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):");
+					System.out.println("Applying correction to 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;

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

@@ -245,6 +245,7 @@ public class QuadCLTCPU {
     public static double [] getRotationFromXYZATRCameraIms(
     		CLTParameters clt_parameters,
     		int           quat_lma_mode,
+    		boolean       use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
     		double        avg_height,
     		double        translation_weight,
     		QuadCLT[]     quadCLTs,
@@ -256,7 +257,7 @@ public class QuadCLTCPU {
 			double []     rms, // null or double[5];
 			int           debugLevel
     		) {
-    	final boolean use3 = (quat_lma_mode == 3); // true;
+//    	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;
@@ -267,49 +268,50 @@ public class QuadCLTCPU {
 		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;
 		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
 		double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
 		for (int nscene = early_index; nscene <= last_index; nscene++) {
 			vect_x[nscene] = ims_xyzatr[nscene];
 			vect_y[nscene] = xyzatr[nscene];
 		}
+		double [][][] vect_y_scaled = QuaternionLma.scaleXYZ(
+				vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
+				vect_y, // double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
+				new int [] {early_index, last_index}); // int []    first_last){
+		if ((quat_lma_mode == quaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
 			quaternionLma.prepareLMA(
 					quat_lma_mode,      // int           mode,
 					avg_height,         // double        avg_height,
 					vect_x,             // double [][][] vect_x,
-					vect_y,             // double [][][] vect_y,
+					vect_y_scaled, // vect_y,             // double [][][] vect_y,
 					null,               // double []     vect_w, all same weight
 					translation_weight, // double        translation_weight, // 0.0 ... 1.0;
 					quat0,              // double []     quat0,
 					debugLevel);        // int           debug_level)
-		} else if ((quat_lma_mode == 1) || (quat_lma_mode == 3)) {
-			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
-			double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
-			for (int nscene = early_index; nscene <= last_index; nscene++) {
-				vect_x[nscene] = ims_xyzatr[nscene];
-				vect_y[nscene] = xyzatr[nscene];
-			}
+		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
+				(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
+				(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
 			quaternionLma.prepareLMA(
 					quat_lma_mode,      // int           mode,
 					vect_x,             // double [][][] vect_x,
-					vect_y,             // double [][][] vect_y,
+					vect_y_scaled, // vect_y,             // double [][][] vect_y,
 					null,               // double []     vect_w, all same weight
 					translation_weight, // double        translation_weight, // 0.0 ... 1.0;
 					reg_w,              // double        reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 					quat0,              // double []     quat0,
 					debugLevel);        // int           debug_level)
 		} else {
+			/*
 			double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
 			double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
 			for (int nscene = early_index; nscene <= last_index; nscene++) {
 				vect_x[nscene] = ims_xyzatr[nscene][0];
 				vect_y[nscene] = xyzatr[nscene][0];
 			}
-
+*/
 			quaternionLma.prepareLMA(
 					vect_x,        // quat_lma_xyz,     // double [][] vect_x,
-					vect_y,     // double [][] vect_y,
+					vect_y_scaled, // vect_y,             // double [][][] vect_y,
 					null,          // double [][] vect_w, all same weight
 					reg_w,         // double      reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 					quat0,         // double []   quat0,
@@ -360,9 +362,12 @@ public class QuadCLTCPU {
 			double []     quaternion, // null or double[4]       
 			int           debugLevel
     		) {
+    	final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
+    	boolean use_inv = false; //
     	double [] quat = getRotationFromXYZATRCameraIms(
     			clt_parameters,     // CLTParameters clt_parameters,
     			quat_lma_mode,      // int quat_lma_mode,
+    			use3,               // boolean       use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
     			avg_height,         // double        avg_height,    			
     			translation_weight, // double        translation_weight,
     			quadCLTs,           // QuadCLT[]     quadCLTs,
@@ -390,10 +395,19 @@ public class QuadCLTCPU {
     		Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
     				ims_xyzatr[nscene][1][0],    ims_xyzatr[nscene][1][1],    ims_xyzatr[nscene][1][2]);
 //    		Rotation rotation_atr = rot.applyTo(scene_atr);
-    		Rotation rotation_atr = rot.applyTo(ims_atr);
-    		Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
+//    		Rotation rotation_atr = rot.applyTo(ims_atr);
+//    		Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
+    		Rotation rotation_atr,rotation_atr2;
+    		if (use_inv) {
+        		rotation_atr = rot_invert.applyTo(ims_atr);
+        		rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
+    		} else {
+        		rotation_atr = rot.applyTo(ims_atr);
+        		rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
+    		}
     		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
     			rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
+///    			rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
     	}
         if (debugLevel > -1) {
 			double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
@@ -427,6 +441,78 @@ public class QuadCLTCPU {
     	return rotated_xyzatr;
     }
     
+    public static void adjustImuOrient(
+    		CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+    		QuadCLT[]     quadCLTs,
+    		int           ref_index,
+    		int           earliest_scene,
+    		int           last_index,
+    		int debugLevel
+    		) {
+		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
+				clt_parameters, // final CLTParameters clt_parameters,
+				quadCLTs,       // final QuadCLT[]     quadCLTs,
+				ref_index,      // final int           ref_index,
+				null,           // double [][][]       dxyzatr,
+				earliest_scene, // final int           early_index,
+				last_index      //(quadCLTs.length -1) // int           last_index,
+	    		);
+		double [][][] xyzatr = new double [quadCLTs.length][][];
+		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
+    	for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
+        	String ts = quadCLTs[nscene].getImageName();
+        	xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
+    	}
+    	double [] rms = new double[5];
+    	double [] quat = new double[4];
+    	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 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);
+    	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
+    			clt_parameters,     // CLTParameters clt_parameters,
+    			quat_lma_mode,      // int quat_lma_mode,
+    			avg_z,              // double        avg_height,
+    			translation_weight, // double        translation_weight,
+    			quadCLTs,           // QuadCLT[]     quadCLTs,
+    			xyzatr,             // double [][][] xyzatr,
+    			pimu_xyzatr,        // double [][][] ims_xyzatr,
+    			ref_index,          // int           ref_index,
+    			earliest_scene,     // int           early_index,
+    			last_index,         // int           last_index,
+    			rms,                //double []     rms, // null or double[5];
+    			quat,               // double []     quaternion, // null or double[4]       
+    			debug_lev); // int           debugLevel
+    	if (rotated_xyzatr != null) {
+        	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
+        	if (debugLevel > -3) {
+        		System.out.println("Applying correction to the IMS mount orientation:");
+				double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+				double []  corr_degrees = new double[3];
+				double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
+				for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
+	    		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
+	    		System.out.println("scale="+quat_scale);
+	    		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]+"]");
+        	}
+        	double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+        	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
+        			ims_mount_atr, // double [] ims_atr,
+        			quat); // double [] corr_q)
+        	clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
+        	if (debugLevel > -3) {
+        		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;
+	    		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
+	    		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
+	    		System.out.println ("*** Need to save the main configuration file ***");
+        	}
+    	} else {
+    		System.out.println ("*** Failed to calculate IMS mount correction! ***");
+    	}
+    }
     
     /**
      * Refine scene poses (now only linear) from currently adjusted poses

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

@@ -40,6 +40,8 @@ public class QuaternionLma {
     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_XYZ4Q3 =        6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
+    
 //    public static final int MODE_XYZ3 =          6;
 //    public static final int MODE_XYZQ3 =         7;
 //    public static final int MODE_COMBO3 =        8;
@@ -144,8 +146,8 @@ public class QuaternionLma {
 	}
 	
 	public void prepareLMA(
-			double [][] vect_x,
-			double [][] vect_y,
+			double [][][] vect_x,
+			double [][][] vect_y,
 			double [][]   vect_w,
 			double        reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
 			double []     quat0,
@@ -169,8 +171,8 @@ public class QuaternionLma {
 				}				
 			} 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];
+					x_vector[samples * i + j] = vect_x[i][0][j];
+					y_vector[samples * i + j] = vect_y[i][0][j];
 					double w = (vect_w != null)? vect_w[i][j] : 1.0;
 					weights[samples*i + j] = w;
 					sw += w;
@@ -195,6 +197,18 @@ 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 == MODE_XYZ4Q3) {
+			prepareLMA43(
+					mode, // int           mode,
+					vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
+					vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
+					vect_w, // double []     vect_w, // one per scene
+					translation_weight, // double        translation_weight, // 0.0 ... 1.0;
+					reg_w,              // double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
+					quat0,              // double []     quat0,
+					debug_level);       // final int     debug_level);
+			return;
+		}
 		if (mode != MODE_XYZQ) {
 			prepareLMAMode3(
 					mode, // int           mode,
@@ -209,18 +223,23 @@ public class QuaternionLma {
 		}
 		N = vect_x.length;
 		this.mode = mode;
-		samples = 7;
+		samples = 3+ quat0.length;
 		samples_x = 7;
 		pure_weight = 1.0 - reg_w;
-		x_vector = new double [samples * N];
-		y_vector = new double [samples * N + REGLEN];
-		weights =  new double [samples * N + REGLEN];
+		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
+		x_vector = new double [samples_x * N];
+		y_vector = new double [samples *   N + extra_samples];
+		weights =  new double [samples *   N + extra_samples];
 		parameters_vector = quat0.clone();
 		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;
+				}				
+				for (int j = 0; j < samples_x; j++) {
+					x_vector[samples_x * i + j] = 0.0;
 					y_vector[samples *   i + j] = 0.0;
 					weights [samples *   i + j] = 0.0;
 				}				
@@ -233,31 +252,43 @@ public class QuaternionLma {
 				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
 						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);
 				for (int j = 0; j < 3; j++) {
-					x_vector[samples * i + j] = vect_x[i][0][j];
-					
+					x_vector[samples_x * i + j] = vect_x[i][0][j];
 					y_vector[samples * i +   j] = vect_y[i][0][j];
 					weights[samples * i +    j] = w;
 					sw += w;
 				}
-				x_vector[samples * i + 3] = rot_x.getQ0();
-				x_vector[samples * i + 4] = rot_x.getQ1();
-				x_vector[samples * i + 5] = rot_x.getQ2();
-				x_vector[samples * i + 6] = rot_x.getQ3();
+				x_vector[samples_x * i + 3] = rot_x.getQ0();
+				x_vector[samples_x * i + 4] = rot_x.getQ1();
+				x_vector[samples_x * i + 5] = rot_x.getQ2();
+				x_vector[samples_x * i + 6] = rot_x.getQ3();
+				
+				w = (1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);
+				if (samples < samples_x) {
+					y_vector[samples * i + 3] = rot_y.getQ1();
+					y_vector[samples * i + 4] = rot_y.getQ2();
+					y_vector[samples * i + 5] = rot_y.getQ3();
+					for (int j = 0; j < 3; j++) {
+						weights[samples * i + 3 + j] = w;
+						sw += w;
+					}
+				} else {
 					y_vector[samples * i + 3] = rot_y.getQ0();
 					y_vector[samples * i + 4] = rot_y.getQ1();
 					y_vector[samples * i + 5] = rot_y.getQ2();
 					y_vector[samples * i + 6] = rot_y.getQ3();
-				w = 0.75*(1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);
-				for (int j = 0; j < 4; j++) {
+					for (int j = 1; j < 4; j++) {
 						weights[samples * i + 3 + j] = w;
 						sw += w;
 					}
 				}
 			}
+		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
+		if (extra_samples>0) {
 			weights [samples * N] = 1.0 - pure_weight;
 			y_vector[samples * N] = 1.0;
+		}
 		last_jt = new double [parameters_vector.length][];		
 		if (debug_level > 0) {
 			 debugYfX ( "",   // String pfx,
@@ -267,6 +298,79 @@ public class QuaternionLma {
 		}
 	}
 
+	public void prepareLMA43(
+			int           mode,
+			double [][][] vect_x, // []{{x,y,z},{a,t,r}}
+			double [][][] vect_y, // []{{x,y,z},{a,t,r}}
+			double []     vect_w, // one per scene
+			double        translation_weight, // 0.0 ... 1.0;
+			double        reg_w,      // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  
+			double []     quat0,
+			final int     debug_level) {
+		N = vect_x.length;
+		this.mode = mode;
+		samples = 6;
+		samples_x = 7;
+		pure_weight = 1.0 - reg_w;
+		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
+		x_vector = new double [samples_x * N];
+		y_vector = new double [samples *   N + extra_samples];
+		weights =  new double [samples *   N + extra_samples];
+		parameters_vector = quat0.clone();
+		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++) {
+					y_vector[samples * i + j] = 0.0;
+					weights [samples * i + j] = 0.0;
+				}				
+				for (int j = 0; j < samples_x; j++) {
+					x_vector[samples_x * i + j] = 0.0;
+					y_vector[samples *   i + j] = 0.0;
+					weights [samples *   i + j] = 0.0;
+				}				
+			} else {
+				// xyz
+				double w = translation_weight*((vect_w != null)?  vect_w[i] : 1.0);
+				// quaternions (both normalized)
+				Rotation   rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
+						vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
+				Rotation   rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
+						vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);
+				for (int j = 0; j < 3; j++) {
+					x_vector[samples_x * i + j] = vect_x[i][0][j];
+					y_vector[samples * i +   j] = vect_y[i][0][j];
+					weights[samples * i +    j] = w;
+					sw += w;
+				}
+				x_vector[samples_x * i + 3] = rot_x.getQ0();
+				x_vector[samples_x * i + 4] = rot_x.getQ1();
+				x_vector[samples_x * i + 5] = rot_x.getQ2();
+				x_vector[samples_x * i + 6] = rot_x.getQ3();
+				y_vector[samples * i + 3] = rot_y.getQ1();
+				y_vector[samples * i + 4] = rot_y.getQ2();
+				y_vector[samples * i + 5] = rot_y.getQ3();
+				w = (1.0 - translation_weight)*((vect_w != null)?  vect_w[i] : 1.0);
+				for (int j = 0; j < 3; j++) {
+					weights[samples * i + 3 + j] = w;
+					sw += w;
+				}
+			}
+		}
+		double k = (pure_weight)/sw;
+		for (int i = 0; i < weights.length; i++) weights[i] *= k;
+		if (extra_samples>0) {
+			weights [samples * N] = 1.0 - pure_weight;
+			y_vector[samples * N] = 1.0;
+		}
+		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 prepareLMAMode3(
@@ -283,7 +387,7 @@ public class QuaternionLma {
 		samples = 7;
 		samples_x = 7;
 		pure_weight = 1.0 - reg_w;
-		int extra_samples = (quat0.length < 4)? 0:REGLEN;
+		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
 		x_vector =     new double [samples *   N];
 		y_vector =     new double [samples *   N + extra_samples];
 		y_inv_vector = new double [samples_x * N + extra_samples]; 		
@@ -334,14 +438,16 @@ public class QuaternionLma {
 						quat_y);  // double [] quat_target  // to which is applied (was scene_atr)		
 				System.arraycopy(inv_y[0], 0, y_inv_vector, samples_x * i,      3);
 				System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3,  4);
+				y_vector[samples_x * i + 3] = 1.0; // no rotation
 				
 				double wt = translation_weight*((vect_w != null)?  vect_w[i] : 1.0);
-				double wr = (1.0 - 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);
 				for (int j = 0; j < 3; j++) {
 					weights[samples * i +  j] = wt;
 					sw += wt;
 				}
-				for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
+//				for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
+				for (int j = 0; j < 4; j++) {// 0 - q0, near 1.0
 					weights[samples * i + 3 + j] = wr;
 					sw += wr;
 				}
@@ -349,7 +455,7 @@ public class QuaternionLma {
 		}
 		double k = (pure_weight)/sw;
 		for (int i = 0; i < weights.length; i++) weights[i] *= k;
-		if (parameters_vector.length >=4) {
+		if (extra_samples > 0) {
 			weights [samples * N] = 1.0 - pure_weight;
 			y_inv_vector[samples * N] = 1.0;
 		}
@@ -535,10 +641,22 @@ public class QuaternionLma {
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
 		switch (mode) {
-		case MODE_XYZQ:return  getFxDerivs6Dof(
+		case MODE_XYZ4Q3:return  getFxDerivs6Dof43(
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
 				 debug_level); // final int         debug_level)
+		case MODE_XYZQ:
+			if (vector.length < 4) {
+				return  getFxDerivs6Dof33(
+						 vector,       // double []         vector,
+						 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+						 debug_level); // final int         debug_level)
+			} else {
+			return  getFxDerivs6Dof(
+				 vector,       // double []         vector,
+				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+				 debug_level); // final int         debug_level)
+			}
 		case MODE_COMBO:return  getFxDerivsVisual( // fill change
 				 vector,       // double []         vector,
 				 jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
@@ -634,18 +752,21 @@ public class QuaternionLma {
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
 		double [] fx = new double [weights.length];
+		// TODO: Implement use3 (no Q0)
 		final double q0 = vector[0]; 
 		final double q1 = vector[1]; 
 		final double q2 = vector[2]; 
 		final double q3 = vector[3];
-		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
+//		final double [] vector_r = normSign(new double[] {  q0,q1,q2,q3}); // was
+		final double [] vector_r = normSign(new double[] { -q0,q1,q2,q3});
+//		final double [] vector_r = normSign(new double[] { -q0,q2,q1,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];
+///				jt[i][samples * N] = 2 * vector[i];
 			}
 		}
-		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
+///		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
 		double [] xyz_rot;
 		double [] quat_rot;
 		double [][] xyz_dq;
@@ -676,7 +797,8 @@ public class QuaternionLma {
 			quat_rot = composeQR_Q(vector_r, quat_r);
 			System.arraycopy(quat_rot, 0, fx, i7+3, 4);
 			if (jt != null) {
-				quat_dq = composeQR_QdQ(vector_r,quat_r);
+				
+				quat_dq = composeQR_QdQ(vector_r,quat_r, true);
 				for (int j = 0; j < 4; j++) {
 					System.arraycopy(quat_dq[j], 0, jt[j], i7+3, 4);
 				}
@@ -698,10 +820,11 @@ public class QuaternionLma {
 		if (jt != null) {
 			for (int i = 0; i < vector.length; i++) {
 				jt[i] = new double [weights.length];
-				jt[i][samples * N] = 2 * vector[i];
+///				jt[i][samples * N] = 2 * vector[i];
 			}
 		}
-		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
+		///		fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
+		
 		double [] xyz_rot;
 		double [] quat_rot;
 		double [][] xyz_dq;
@@ -749,7 +872,7 @@ public class QuaternionLma {
 				for (int j = 0; j < 4; j++) {
 					System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);
 				}
-				quat_dq = composeQR_QdQ(vector_r,quat_r);
+				quat_dq = composeQR_QdQ(vector_r,quat_r,true);
 				// 2 alternative ways with the same result
 				if (debug_level < 1000) {
 					double [][] invy_mat = qMat(inv_y[1]);
@@ -840,7 +963,7 @@ public class QuaternionLma {
 				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);
+				quat_dq = composeQR_QdQ(vector_r,quat_r,true);
 				// 2 alternative ways with the same result
 				if (debug_level < 1000) {
 					double [][] invy_mat = qMat(inv_y[1]);
@@ -873,6 +996,144 @@ public class QuaternionLma {
 		return fx;
 	}
 	
+	private double [] getFxDerivs6Dof43( // vector[4], but only 3 for rotations
+			double []         vector, //
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		boolean use_inv = true;
+		double [] fx = new double [weights.length];
+		double [] qn = new double[4];
+		boolean [] invert = { false, use_inv, use_inv, use_inv};
+		final double [] vector_r = normSign(vector).clone(); // should be already q0>0 
+		invertDeriv(vector_r, invert);
+//		final double [] vector_r = normSign(new double[] { use_inv? -vector[0]:vector[0],vector[1],vector[2],vector[3]});
+		final double l = qNorm (vector_r, qn); // calculates qn // normalized
+		final double [][] dQn_dQ = dQndQ(vector_r);
+		double [] xyz_rot;
+		double [] quat_rot;
+		double [][] xyz_dq;
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+		}
+		
+		for (int i = 0; i < N; i++) {
+			int i6 = samples * i;
+			int i7 = samples_x * i;
+			has_data:{
+				for (int j = 0; j < samples; j++) {
+					if (weights[i6+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]};
+			xyz_rot = applyTo(vector, xyz); // rotate + scale
+			System.arraycopy(xyz_rot, 0, fx, i6, 3);
+			if (jt != null) {
+				xyz_dq =  applyToDQ(vector, xyz);
+				for (int j = 0; j < 4; j++) {
+					System.arraycopy(xyz_dq[j], 0, jt[j], i6, 3);
+				}
+			}
+			// rotations
+			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
+			quat_rot = composeQR_Q(qn, quat_r); // qn - normalized vector_r
+			System.arraycopy(quat_rot, 1, fx, i6+3, 3);
+			if (jt != null) {
+				double[][] quat_dqn = composeQR_QdQ(qn,quat_r,false); // use_inv); //
+				double[][] quat_dq = mulMat(dQn_dQ, quat_dqn);
+				invertDeriv(quat_dq, invert);
+				for (int j = 0; j < 4; j++) {
+					System.arraycopy(quat_dq[j], 1, jt[j], i6+3, 3);
+				}
+			}			
+		}
+		return fx;
+	}
+	
+	private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations
+			double []         vector3, //
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level) {
+		boolean use_inv = false; // true;
+		double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
+		double [] fx = new double [weights.length];
+		double [] qn = new double[4];
+		boolean [] invert = { false, use_inv, use_inv, use_inv};
+		final double [] vector_r = normSign(vector).clone(); // should be already q0>0 
+		invertDeriv(vector_r, invert);
+		qNorm (vector_r, qn); // calculates qn // normalized
+//		final double [][] dQn_dQ = dQndQ(vector_r);
+		final double [][] dQn_dQ123 = dQ_dQ123(vector3);
+		
+		
+		double [] xyz_rot;
+		double [] quat_rot;
+		double [][] xyz_dq;
+		if (jt != null) {
+			for (int i = 0; i < jt.length; i++) {
+				jt[i] = new double [weights.length];
+			}
+		}
+		
+		for (int i = 0; i < N; i++) {
+			int i6 = samples * i;
+			int i7 = samples_x * i;
+			has_data:{
+				for (int j = 0; j < samples; j++) {
+					if (weights[i6+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]};
+			xyz_rot = applyTo(vector, xyz); // rotate + scale
+			System.arraycopy(xyz_rot, 0, fx, i6, 3);
+			if (jt != null) {
+				double [][] xyz_dqn =  applyToDQ(vector, xyz);
+				/*
+				xyz_dq = mulMat(dQn_dQ, xyz_dqn);
+				for (int j = 1; j < 4; j++) {
+					System.arraycopy(xyz_dq[j], 0, jt[j-1], i6, 3);
+				}
+				*/
+				xyz_dq = mulMat(dQn_dQ123, xyz_dqn);
+				for (int j = 0; j < xyz_dq.length; j++) { // 3
+					System.arraycopy(xyz_dq[j], 0, jt[j], i6, 3);
+				}
+			}
+			// rotations
+			final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
+			quat_rot = composeQR_Q(qn, quat_r); // qn - normalized vector_r
+			System.arraycopy(quat_rot, 1, fx, i6+3, 3);
+			if (jt != null) {
+				double[][] quat_dqn = composeQR_QdQ(qn,quat_r,false); // use_inv); //
+				/*
+				double[][] quat_dq = mulMat(dQn_dQ, quat_dqn);
+				invertDeriv(quat_dq, invert);
+				for (int j = 1; j < 4; j++) {
+					System.arraycopy(quat_dq[j], 1, jt[j-1], i6+3, 3);
+				}
+				*/
+				double[][] quat_dq = mulMat(dQn_dQ123, quat_dqn); // [3][4]
+				invertDeriv(quat_dq, invert);
+				for (int j = 0; j < quat_dq.length; j++) { // 3
+					System.arraycopy(quat_dq[j], 1, jt[j], i6+3, 3);
+				}
+			}			
+		}
+		return fx;
+	}
+	
+	
 	private double compareJT(
 			double [] vector,
 			double    delta) {
@@ -1008,7 +1269,7 @@ public class QuaternionLma {
 			if (jt != null) {
 				xyz_dq =  applyToDQ(vector, xyz);
 //				quat_dq = composeQR_QdQ(vector,quat_r);
-				quat_dq = composeQR_QdQ(vector_r,quat_r);
+				quat_dq = composeQR_QdQ(vector_r,quat_r,true);
 				// Z
 				jt[0][i4 + 0] = xyz_dq[0][2] / height;
 				jt[1][i4 + 0] = xyz_dq[1][2] / height;
@@ -1095,7 +1356,7 @@ public class QuaternionLma {
 				            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)		
 				}
-				quat_dq = composeQR_QdQ(vector_r,quat_r);
+				quat_dq = composeQR_QdQ(vector_r,quat_r,true);
 				double [][] invy_mat = qMat(inv_y[1]);
 				double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
 				// Z
@@ -1479,17 +1740,18 @@ public class QuaternionLma {
 						data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
 			}
 			System.out.println();
-		} else  if ((mode == MODE_XYZQ) || (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+...]
+		} else  if (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",
 					"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
 					pfx+"A",pfx+"T",pfx+"R"));
 			for (int nscene = 0; nscene < N; nscene++) {
-				if (   (data[7*nscene + 3]*data[7*nscene + 3] +
-						data[7*nscene + 4]*data[7*nscene + 4] +
-						data[7*nscene + 5]*data[7*nscene + 5]+
-						data[7*nscene + 6]*data[7*nscene + 6]) < 0.001) {
+				if (   (data[samples_x*nscene + 3]*data[samples_x*nscene + 3] +
+						data[samples_x*nscene + 4]*data[samples_x*nscene + 4] +
+						data[samples_x*nscene + 5]*data[samples_x*nscene + 5]+
+						data[samples_x*nscene + 6]*data[samples_x*nscene + 6]) < 0.001) {
 					System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
 					continue;
 				}
@@ -1504,6 +1766,35 @@ public class QuaternionLma {
 						angles[0],angles[1],angles[2]));
 			}
 			System.out.println();
+		} else  if ((mode == MODE_XYZ4Q3) || (mode == MODE_XYZQ)) { // not when 7-long, it should be catched before
+			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",
+					"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
+					pfx+"A",pfx+"T",pfx+"R"));
+			for (int nscene = 0; nscene < N; nscene++) {
+				if (   (x_vector[samples_x*nscene + 3]*x_vector[samples_x*nscene + 3] +
+						x_vector[samples_x*nscene + 4]*x_vector[samples_x*nscene + 4] +
+						x_vector[samples_x*nscene + 5]*x_vector[samples_x*nscene + 5]+
+						x_vector[samples_x*nscene + 6]*x_vector[samples_x*nscene + 6]) < 0.001) {
+					System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
+					continue;
+				}
+				double q1 = data[samples*nscene + 3];
+				double q2 = data[samples*nscene + 4];
+				double q3 = data[samples*nscene + 5];
+				double q0 = Math.sqrt(1.0-q1*q1 - q2*q2 - q3*q3); // should be normalized!
+				Rotation rot = new Rotation(q0,q1,q2,q3, false);
+				double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+				System.out.println(String.format("%3d"+
+						"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
+						"\t%9.5f\t%9.5f\t%9.5f",
+						nscene,
+						data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2],
+						q0,q1,q2,q3,
+						angles[0],angles[1],angles[2]));
+			}
+			System.out.println();
 		} else { // if (mode == 2) {
 			System.out.println(String.format("%3s"+
 					"\t%9s\t%9s\t%9s\t%9s", // Z, 2*Q3, 2*Q2-X, 2*Q1+Y
@@ -1610,11 +1901,13 @@ public class QuaternionLma {
 	 */
 	public static double [][] composeQR_QdQ(
 			double [] q,
-			double [] r) {
+			double [] r,
+			boolean use_inv) {
+//		boolean use_inv = true;
+		if (use_inv) {
 			return new double [][] {
 				// for inverted {-q0,q1,q2,q3}
 				//  t=s*q' d/dQ0
-			/*
 				{ 2*r[0]*q[0],
 					2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
 					2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
@@ -1634,7 +1927,9 @@ public class QuaternionLma {
 								-2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
 								-2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
 								-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
-			*/ 
+			};
+		} else {
+			return new double [][] {
 				// for non-inverted {q0,q1,q2,q3}
 				//  t=s*q' d/dQ0    
 				{ 2*r[0]*q[0],
@@ -1658,6 +1953,7 @@ public class QuaternionLma {
 								2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3]}
 			};
 		}
+	}
 	public static double [][] composeQR_QdQ_(
 			double [] q,
 			double [] r) {
@@ -1856,7 +2152,7 @@ public class QuaternionLma {
 		return Math.sqrt(1.0-(q123[0]*q123[0]+q123[1]*q123[1]+q123[2]*q123[2]));
 	}
 	
-	public static double [] dQuat123(
+	public static double [] dQuat123( // not used
 			double [] q123,
 			double [] dq0123, // [4]
 			double q0) { // null or double[3]
@@ -1880,4 +2176,123 @@ public class QuaternionLma {
 		return dq123;
 	}
 	
+	public static double [][] dQ_dQ123(
+			double [] q123){
+		double d = 1.0;
+		for (int i = 0; i < q123.length; i++) {
+			d-= q123[i] * q123[i];
+		}
+		double q0 = Math.sqrt(d); 
+		double [][] mat = new double [q123.length][q123.length+1];
+		for (int i = 0; i < mat.length; i++) {
+			mat[i][0] =   -q123[i]/q0;
+			mat[i][i+1] = 1;
+		}
+		return mat;
+	}
+	
+	public static double qNorm(
+			final double [] q,
+			final double [] qn) {
+		double l = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+		if (qn != null) {
+			for (int i = 0; i < qn.length; i++) {
+				qn[i] = q[i]/l; 
+			}
+		}
+		return l;
+	}
+	
+	public static double [][] dQndQ(
+			final double [] q){
+		final double [][] dq = new double [q.length][q.length];
+		final double l = qNorm(q, null);
+		final double l3 = l*l*l;
+		for (int i = 0; i < q.length; i++) {
+			for (int j = 0; j < q.length; j++) {
+				dq[j][i] = -q[i]*q[j]/l3; // dq[i][j] = d; 
+			}
+			dq[i][i] += 1/l;
+		}
+		return dq;
+	}
+	
+	public static void invertDeriv(
+			double [][] deriv,
+			boolean [] invert){
+		for (int i = 0; i < deriv.length; i++) {
+			if (invert[i]) {
+				for (int j = 0; j < deriv[i].length; j++) {
+					deriv[i][j] *= -1;
+				}
+			}
+		}
+	}
+	public static void invertDeriv(
+			double [] deriv,
+			boolean [] invert){
+		for (int i = 0; i < deriv.length; i++) {
+			if (invert[i]) {
+				deriv[i] *= -1;
+			}
+		}
+	}
+	
+	public static double [][][] scaleXYZ(
+			double [][][] vect_x, // []{{x,y,z},{a,t,r}}
+			double [][][] vect_y, //  []{{x,y,z},{a,t,r}}
+			int []        first_last){
+		if (first_last == null) {
+			first_last = new int [] {0,vect_x.length-1}; 
+		}
+		double dia_x = getDiameter(vect_x,first_last);
+		double dia_y = getDiameter(vect_y,first_last);
+		if (!(dia_x > 0) || !(dia_y > 0)) return null;
+		double s = dia_x/dia_y;
+		double [][][] scaled_xyz = new double [vect_y.length][][];
+		for (int i = first_last[0]; i <= first_last[1]; i++) if (vect_y[i] != null){
+			scaled_xyz[i] = new double [][] {
+				{s* vect_y[i][0][0],s* vect_y[i][0][1],s* vect_y[i][0][2]},
+				vect_y[i][1]};
+		}
+		return scaled_xyz;
+	}
+	
+	public static double getDiameter(
+			double [][][] xyzatr,
+			int [] first_last) {
+		if (first_last == null) {
+			first_last = new int [] {0,xyzatr.length-1}; 
+		}
+		int i0= first_last[0];
+		for (;i0 <= first_last[1]; i0++) if (xyzatr[i0] != null) break;
+		if (i0 > first_last[1]) return Double.NaN; // empty sequence
+		double l2 = 0;
+		int i1 = i0;
+		for (int i = i0; i <= first_last[1]; i++) if (xyzatr[i] != null){
+			double d = 0;
+			for (int j = 0; j < 3; j++) {
+				double dd = xyzatr[i][0][j]-xyzatr[i0][0][j]; 
+				d+= dd*dd;
+			}
+			if (d > l2) {
+				l2 = d;
+				i1 = i;
+			}
+		}
+		for (int i = i1; i <= first_last[1]; i++) if (xyzatr[i] != null){
+			double d = 0;
+			for (int j = 0; j < 3; j++) {
+				double dd = xyzatr[i][0][j]-xyzatr[i1][0][j]; 
+				d+= dd*dd;
+			}
+			if (d > l2) {
+				l2 = d;
+				i0 = i;
+			}
+		}		
+		return Math.sqrt(l2);
+	}
+	
+	
 }