unfinished mod

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

@@ -27,8 +27,6 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.concurrent.atomic.AtomicInteger;
 
-import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
-
 import com.elphel.imagej.calibration.CalibrationFileManagement;
 import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.cameras.ColorProcParameters;
@@ -5365,14 +5363,14 @@ public class Interscene {
 					enu,
 					false);
 		}
-		double lambda =            0.1;
-		double lambda_scale_good = 0.5;
-		double lambda_scale_bad =  8.0;
-		double lambda_max =      100;
-		double rms_diff =          0.001;
-		int    num_iter =         20;
+		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;
-		double      reg_w = 0.25;
+		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;
 		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
 		int         debug_level = 1;
 		QuaternionLma quaternionLma = new QuaternionLma();

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

@@ -435,6 +435,15 @@ public class IntersceneMatchParameters {
     public  boolean readjust_xy_ims =        true;  // false;
     public  double  reg_weight_xy =          0;     // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
     
+    // Quaternion LMA parameters
+    public  double  quat_lambda =            0.1;
+    public  double  quat_lambda_scale_good = 0.5;
+    public  double  quat_lambda_scale_bad =  8.0;
+    public  double  quat_lambda_max =      100;
+    public  double  quat_rms_diff =          0.001;
+    public  int     quat_num_iter =         20;
+    public  double  quat_reg_w =             0.25;
+	
 	public  boolean stereo_merge =       true;
 	public  int     stereo_gap =         32;    // pixels between right and left frames
 	public  double  stereo_intereye =    63.5;  // mm 
@@ -1241,6 +1250,22 @@ public class IntersceneMatchParameters {
 		gd.addNumericField("LMA regularization weight",                this.reg_weight_xy, 5,7,"",
 				"Regularization weight for LMA");
 		
+		gd.addTab("Quaternion LMA","Parameters fitting rotation to two sets of vectors");
+		gd.addNumericField("LMA lambda",                                  this.quat_lambda, 6,8,"",
+				"Initial value of the LMA lambda");
+		gd.addNumericField("Scale lambda after successful LMA iteration", this.quat_lambda_scale_good, 3,5,"",
+				"Scale lambda (reduce) if the new RMSE is lower than the previous one.");
+		gd.addNumericField("Scale lambda after failed LMA iteration",     this.quat_lambda_scale_bad, 3,5,"",
+				"Scale lambda (increase) if the new RMSE is higher than the previous one.");
+		gd.addNumericField("Maximal value of lambda to try",              this.quat_lambda_max, 2,7,"",
+				"Fail LMA if the result is still worse than before parameters were updates.");
+		gd.addNumericField("Minimal relative RMSE improvement",           this.quat_rms_diff, 5,7,"",
+				"Exit LMA iterations if relative RMSE improvement drops below this value.");
+		gd.addNumericField("Maximal number of LMA iterations",            this.quat_num_iter, 0,3,"",
+				"A hard limit on LMA iterations.");
+		gd.addNumericField("Regularization weight",                       this.quat_reg_w, 3,5,"",
+				"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");
+		
 		
 		gd.addTab("Stereo/Video","Parameters for stereo video generation");
 		gd.addMessage  ("Stereo");
@@ -1673,6 +1698,14 @@ public class IntersceneMatchParameters {
 		this.readjust_xy_ims =          gd.getNextBoolean();
 		this.reg_weight_xy =            gd.getNextNumber();
 		
+		this.quat_lambda =              gd.getNextNumber();
+		this.quat_lambda_scale_good =   gd.getNextNumber();
+		this.quat_lambda_scale_bad =    gd.getNextNumber();
+		this.quat_lambda_max =          gd.getNextNumber();
+		this.quat_rms_diff =            gd.getNextNumber();
+		this.quat_num_iter =      (int) gd.getNextNumber();
+		this.quat_reg_w =               gd.getNextNumber();
+		
 		if (stereo_views.length > 0) {
 			int i =                     gd.getNextChoiceIndex();
 			if (i > 0) {
@@ -2105,6 +2138,14 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"readjust_xy_ims",      this.readjust_xy_ims+"");     // boolean
 		properties.setProperty(prefix+"reg_weight_xy",        this.reg_weight_xy+"");       // double
 		
+		properties.setProperty(prefix+"quat_lambda",          this.quat_lambda+"");         // double
+		properties.setProperty(prefix+"quat_lambda_scale_good",this.quat_lambda_scale_good+"");// double
+		properties.setProperty(prefix+"quat_lambda_scale_bad",this.quat_lambda_scale_bad+"");// double
+		properties.setProperty(prefix+"quat_lambda_max",      this.quat_lambda_max+"");     // double
+		properties.setProperty(prefix+"quat_rms_diff",        this.quat_rms_diff+"");       // double
+		properties.setProperty(prefix+"quat_num_iter",        this.quat_num_iter+"");       // int
+		properties.setProperty(prefix+"quat_reg_w",           this.quat_reg_w+"");          // double
+		
 		properties.setProperty(prefix+"stereo_merge",         this.stereo_merge+"");        // boolean
 		properties.setProperty(prefix+"stereo_gap",           this.stereo_gap+"");          // int
 		properties.setProperty(prefix+"stereo_intereye",      this.stereo_intereye+"");     // double
@@ -2498,6 +2539,15 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"readjust_xy_ims")!=null)      this.readjust_xy_ims=Boolean.parseBoolean(properties.getProperty(prefix+"readjust_xy_ims"));
 		if (properties.getProperty(prefix+"reg_weight_xy")!=null)        this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
 		
+		if (properties.getProperty(prefix+"quat_lambda")!=null)          this.quat_lambda=Double.parseDouble(properties.getProperty(prefix+"quat_lambda"));
+		if (properties.getProperty(prefix+"quat_lambda_scale_good")!=null)this.quat_lambda_scale_good=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_good"));
+		if (properties.getProperty(prefix+"quat_lambda_scale_bad")!=null)this.quat_lambda_scale_bad=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_bad"));
+		if (properties.getProperty(prefix+"quat_lambda_max")!=null)      this.quat_lambda_max=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_max"));
+		if (properties.getProperty(prefix+"quat_rms_diff")!=null)        this.quat_rms_diff=Double.parseDouble(properties.getProperty(prefix+"quat_rms_diff"));
+		if (properties.getProperty(prefix+"reg_weight_xy")!=null)        this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
+		if (properties.getProperty(prefix+"quat_num_iter")!=null)        this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
+		if (properties.getProperty(prefix+"quat_reg_w")!=null)           this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));
+		
 		if (properties.getProperty(prefix+"stereo_merge")!=null)         this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
 		if (properties.getProperty(prefix+"stereo_gap")!=null)           this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
 		if (properties.getProperty(prefix+"stereo_intereye")!=null)      this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye"));
@@ -2901,6 +2951,14 @@ public class IntersceneMatchParameters {
 		imp.readjust_xy_ims       = this.readjust_xy_ims;
 		imp.reg_weight_xy         = this.reg_weight_xy;
 		
+		imp.quat_lambda           = this.quat_lambda;
+		imp.quat_lambda_scale_good= this.quat_lambda_scale_good;
+		imp.quat_lambda_scale_bad = this.quat_lambda_scale_bad;
+		imp.quat_lambda_max       = this.quat_lambda_max;
+		imp.quat_rms_diff         = this.quat_rms_diff;
+		imp.quat_num_iter         = this.quat_num_iter;
+		imp.quat_reg_w            = this.quat_reg_w;
+		
 		imp.stereo_merge          = this.stereo_merge;
 		imp.stereo_gap            = this.stereo_gap;
 		imp.stereo_intereye       = this.stereo_intereye;

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

@@ -5416,6 +5416,38 @@ public class OpticalFlow {
 				}				
 			}
 		}
+		// later move to the right place
+		if ((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];
+	    	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
+	    			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,
+	    			last_index,     // int           last_index,
+	    			rms,            //double []     rms, // null or double[5];
+	    			quat,           // double []     quaternion, // null or double[4]       
+	    			0); // int           debugLevel
+		}
+		
+		
 		if (run_ly) {
 			if (debugLevel > -3) {
 				System.out.println("**** Running LY adjustments *****");

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

@@ -221,6 +221,157 @@ public class QuadCLTCPU {
     	pimu_offsets = offsets;
     }
     
+    // find best rotation between IMU XYZ and camera XYZ
+    /**
+     * Refine scene poses (now only linear) from currently adjusted poses
+     * (adjusted using IMS orientations) and IMS positions (currently
+     * only linear are used). Refined scene poses may be used as pull targets
+     * with free orientations (and angular velocities for ERS).
+     * Result poses are {0,0,0} for the reference frame.
+     * 
+     * Assuming correct IMU angular velocities and offset linear ones.   
+     * 
+     * @param clt_parameters configuration parameters
+     * @param quadCLTs       scenes sequence (for timestamps)
+     * @param xyzatr         current scenes [[x,y,z],[a,t,r]] in reference scene frame
+     * @param ims_xyzatr     IMU-derived (integrated) scene position and orientation in reference
+     *                       scene frame (only position is currently used)
+     * @param ref_index      reference scene index 
+     * @param early_index    earliest (lowest) scene index to use
+     * @param last_index     last (highest) scene index to use
+     * @return quaternion    componets 
+     */
+    
+    public static double [] getRotationFromXYZCameraIms(
+    		CLTParameters clt_parameters,
+    		QuadCLT[]     quadCLTs,
+    		double [][][] xyzatr,
+    		double [][][] ims_xyzatr,
+    		int           ref_index,
+    		int           early_index,
+    		int           last_index,
+			double []     rms, // null or double[5];
+			int           debugLevel
+    		) {
+    	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];
+    	}
+		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;
+		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;
+		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
+		QuaternionLma quaternionLma = new QuaternionLma();
+		quaternionLma.prepareLMA(
+				vect_x, // quat_lma_xyz,     // double [][] vect_x,
+				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,
+				debugLevel);     // 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,
+				debugLevel);     // 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;
+					}					
+				}
+			}
+			return quaternionLma.getQuaternion();
+		}
+    }
+    
+    public static double [][][] rotateImsToCameraXYZ(
+    		CLTParameters clt_parameters,
+    		QuadCLT[]     quadCLTs,
+    		double [][][] xyzatr,
+    		double [][][] ims_xyzatr,
+    		int           ref_index,
+    		int           early_index,
+    		int           last_index,
+			double []     rms, // null or double[5];
+			double []     quaternion, // null or double[4]       
+			int           debugLevel
+    		) {
+    	double [] quat = getRotationFromXYZCameraIms(
+    			clt_parameters, // CLTParameters clt_parameters,
+    			quadCLTs,       // QuadCLT[]     quadCLTs,
+    			xyzatr,         // double [][][] xyzatr,
+    			ims_xyzatr,     // double [][][] ims_xyzatr,
+    			ref_index,      // int           ref_index,
+    			early_index,    // int           early_index,
+    			last_index,     // int           last_index,
+    			rms,            // double []     rms, // null or double[5];
+    			debugLevel);    // int           debugLevel
+    	if (quat == null) {
+    		return null;
+    	}
+    	if (quaternion != null) {
+    		System.arraycopy(quat, 0, quaternion, 0, 4);
+    	}
+    	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
+    	double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS
+    	double [] rotated_xyz = new double[3];
+//    	double [] rotated_atr = new double[3];
+    	for (int nscene = early_index; nscene <= last_index; nscene++) {
+    		rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);
+    		Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
+    				xyzatr[nscene][1][0],    xyzatr[nscene][1][1],    xyzatr[nscene][1][2]);
+    		Rotation rotation_atr = rot.applyTo(scene_atr);
+    		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
+    			rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
+    	}
+        if (debugLevel > -1) {
+    		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
+    		System.out.println(String.format("%3s"+
+    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
+    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
+    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
+    				"N","X","Y","Z","A","T","R",
+    				"PIMU-X","PIMU-Y","PIMU-Z","PIMU-A","PIMU-T","PIMU-R",
+    				"ROT-X","ROT-Y","ROT-Z","ROT-A","ROT-T","ROT-R"));
+        	for (int nscene = early_index; nscene <= last_index; nscene++) {
+        		System.out.println(String.format("%3s"+
+        				"\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\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,
+        				xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
+        				xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
+        				ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],
+        				ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],
+        				rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],
+        				rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));
+        	}    	
+        }
+    	return rotated_xyzatr;
+    }
+    
+    
     /**
      * Refine scene poses (now only linear) from currently adjusted poses
      * (adjusted using IMS orientations) and IMS velocities (currently