Added attitude correction from IMS to camera by images

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

@@ -175,7 +175,7 @@ public class Imx5 {
 				quat_enu_rot.getQ3()};
 	}
 	
-	public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
+	public static double [] applyQuaternionTo(double[]quat, double[] vector, boolean inverse) {
 		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
 		double [] rslt = new double[3];
 		if (inverse) {
@@ -187,6 +187,27 @@ public class Imx5 {
 		return rslt;
 	}
 
+	public static double [] applyQuaternionToQuaternion(
+			double[] quat,
+			double[] targ_quat,
+			boolean inverse) {
+		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
+		Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
+		Rotation rslt_rot;
+		if (inverse) {
+			rslt_rot=ims_rot.applyInverseTo(targ_rot);
+
+		} else {
+			rslt_rot=ims_rot.applyTo(targ_rot);
+		}
+		return new double[] {
+				rslt_rot.getQ0(),
+				rslt_rot.getQ1(),
+				rslt_rot.getQ2(),
+				rslt_rot.getQ3()};
+	}
+	
+	
 	public static double [] quaternionImsToCam(
 			double [] quat,      // ims_to_ned
 			double [] ims_atr, // -> mount_to_cam

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

@@ -430,7 +430,7 @@ public class Interscene {
 					d2.getQEnu(),
 					ims_mount_atr,
 					ims_ortho);
-			double [] cam_xyz_enu = Imx5.applyQuternionTo(
+			double [] cam_xyz_enu = Imx5.applyQuaternionTo(
 					// Offset in the reference scene frame, not in the current scene
 					cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
 					enu, // absolute offset (East, North, Up) in meters
@@ -3789,6 +3789,15 @@ public class Interscene {
 		int num_processed = 0;
 		double [][][] scenes_xyzatr =    new double [quadCLTs.length][][];
 		double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
+		// for testing QuaternionLma
+		double [] rms = new double [2];
+		double [] quatCorr = getQuaternionCorrection(
+				clt_parameters, // CLTParameters  clt_parameters,
+				quadCLTs,       // QuadCLT []     quadCLTs,
+				ref_index,      // int            ref_index,
+				earliest_scene, // int            earliest_scene,
+				rms);           // double []      rms // null or double[2];
+	    		
 		
 		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
 			QuadCLT scene = quadCLTs[nscene];
@@ -3817,6 +3826,7 @@ public class Interscene {
 		String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
 				"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
 				"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"+
+				"\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"+
 				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
@@ -3904,13 +3914,13 @@ public class Interscene {
 //imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
 			double [] double_qn2b = d2.getQn2b();
 			double [] double_uvw =  d2.getUvw();
-			double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
-			double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
+			double [] uvw_dir = Imx5.applyQuaternionTo(double_qn2b, double_uvw, false); // bad
+			double [] uvw_inv = Imx5.applyQuaternionTo(double_qn2b, double_uvw, true); // good
 //Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp] 			
 			
 			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
 			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
-			double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
+			double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
 //			String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
 //					"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
 			
@@ -3925,8 +3935,8 @@ public class Interscene {
 					ims_mount_atr, // new double[] {0, 0.13, 0},
 					ims_ortho);
 			
-			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
-			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
+			double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
+			double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
 			double [] cam_xyz_ned = test_xyz_ned(
 					Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
 					d2_ref.getQn2b(), // double[]  quat_ned,
@@ -3939,19 +3949,20 @@ public class Interscene {
 					ims_mount_atr, // double [] ims_mount_atr,
 					ims_ortho); //double [] ims_ortho)
 		    */
-			double [] cam_xyz_enu = Imx5.applyQuternionTo(
+			double [] cam_xyz_enu = Imx5.applyQuaternionTo(
 					Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
 							ims_mount_atr,
 							ims_ortho),
 					enu,
 					false);
-			
+			double [] quat_lma_enu_xyz = Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false);
 			
 			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
 			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]); //
+			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); //
 			
 			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
 			
@@ -3993,6 +4004,10 @@ public class Interscene {
 			// add lpf 
 			sb.append("\n");
 		}
+		// test QuaternionLMA here
+		
+		
+		
 		
 		if (path!=null) {
 			String footer=(comment != null) ? ("Comment: "+comment): "";
@@ -4004,6 +4019,84 @@ public class Interscene {
 		}
     }
  
+    public static double [] getQuaternionCorrection(
+			CLTParameters  clt_parameters,
+			QuadCLT []     quadCLTs,
+			int            ref_index,
+			int            earliest_scene,
+			double []      rms // null or double[2];
+    		) {
+		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][3];
+		double [][]   quat_lma_enu_xyz = new double [quadCLTs.length][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];
+			} 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 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;
+		boolean last_run =         false;
+		double      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();
+		quaternionLma.prepareLMA(
+				quat_lma_enu_xyz, // quat_lma_xyz,     // double [][] vect_x,
+				quat_lma_xyz,     // 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,
+				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;
+					}					
+				}
+			}
+			return quaternionLma.getQuaternion();
+		}
+    }
+    
     static double [] test_xyz_ned(
     		double [] ned,
 			double[]  quat_ned,
@@ -4013,7 +4106,7 @@ public class Interscene {
 		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_ned,
 				ims_mount_atr,
 				ims_ortho);
-		return Imx5.applyQuternionTo(cam_quat2, ned, false);
+		return Imx5.applyQuaternionTo(cam_quat2, ned, false);
     }
       
     static double [] test_xyz_enu(
@@ -4025,7 +4118,7 @@ public class Interscene {
 		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_enu,
 				ims_mount_atr,
 				ims_ortho);
-		return Imx5.applyQuternionTo(cam_quat2, enu, false);
+		return Imx5.applyQuaternionTo(cam_quat2, enu, false);
     }
    
     

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

+/**
+ **
+ ** IntersceneLma - Adjust camera egomotion with LMA
+ **
+ ** Copyright (C) 2020-2023 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **
+ **  IntersceneLma.java is free software: you can redistribute it and/or modify
+ **  it under the terms of the GNU General Public License as published by
+ **  the Free Software Foundation, either version 3 of the License, or
+ **  (at your option) any later version.
+ **
+ **  This program is distributed in the hope that it will be useful,
+ **  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ **  GNU General Public License for more details.
+ **
+ **  You should have received a copy of the GNU General Public License
+ **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ ** -----------------------------------------------------------------------------**
+ **
+ */
+
 package com.elphel.imagej.tileprocessor;
 
 import java.io.ByteArrayOutputStream;

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

@@ -85,6 +85,9 @@ public class IntersceneMatchParameters {
 	public  int min_num_orient =         2; // make from parameters, should be >= 1
 	public  int min_num_interscene =     2; // make from parameters, should be >= 1
 
+	public  boolean calc_quat_corr =     true; // calculate camera orientation correction from predicted by IMS 
+    public  boolean apply_quat_corr =    true; // apply camera orientation correction from predicted by IMS
+	
 	public  boolean generate_egomotion = true; // generate egomotion table (image-based and ims)
 	public  boolean generate_mapped =    true; // generate any of the sequences - Tiff, video, mono or stereo
 	public  boolean reuse_video =        false; // dry-run video generation, just reassemble video fragments (active if !generate_mapped)
@@ -536,6 +539,10 @@ public class IntersceneMatchParameters {
 				"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
 		gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
 				"Minimal required number of re-calculations of the interscene-accumulated DSI.");
+		gd.addCheckbox ("Calculate IMS orientation correction",      this.calc_quat_corr,
+				"Calculate camera orientation correction from predicted by IMS .");
+		gd.addCheckbox ("Apply IMS orientation correction",          this.apply_quat_corr,
+				"Apply camera orientation correction from predicted by IMS.");
 		
 		gd.addMessage  ("Generate/show scene sequences");
 		gd.addCheckbox ("Generate egomotion table",                  this.generate_egomotion,
@@ -1227,6 +1234,8 @@ public class IntersceneMatchParameters {
 		this.run_ly =                         gd.getNextBoolean();
 		this.min_num_orient =           (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
 		this.min_num_interscene =       (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
+		this.calc_quat_corr =                 gd.getNextBoolean();
+		this.apply_quat_corr =                gd.getNextBoolean();
 		this.generate_egomotion =             gd.getNextBoolean();
 		this.generate_mapped =                gd.getNextBoolean();
 		this.reuse_video =                    gd.getNextBoolean();
@@ -1609,6 +1618,8 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"run_ly",                        this.run_ly + "");                      // boolean
 		properties.setProperty(prefix+"min_num_orient",                this.min_num_orient+"");                // int
 		properties.setProperty(prefix+"min_num_interscene",            this.min_num_interscene+"");            // int
+		properties.setProperty(prefix+"calc_quat_corr",                this.calc_quat_corr+"");                // boolean
+		properties.setProperty(prefix+"apply_quat_corr",               this.apply_quat_corr+"");               // boolean
 		properties.setProperty(prefix+"generate_egomotion",            this.generate_egomotion+"");            // boolean
 		properties.setProperty(prefix+"generate_mapped",               this.generate_mapped+"");               // boolean
 		properties.setProperty(prefix+"reuse_video",                   this.reuse_video+"");                   // boolean
@@ -1951,6 +1962,8 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"run_ly")!=null)                        this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));		
 		if (properties.getProperty(prefix+"min_num_orient")!=null)                this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
 		if (properties.getProperty(prefix+"min_num_interscene")!=null)            this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
+		if (properties.getProperty(prefix+"calc_quat_corr")!=null)                this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
+		if (properties.getProperty(prefix+"apply_quat_corr")!=null)               this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
 		if (properties.getProperty(prefix+"generate_egomotion")!=null)            this.generate_egomotion=Boolean.parseBoolean(properties.getProperty(prefix+"generate_egomotion"));
 		if (properties.getProperty(prefix+"generate_mapped")!=null)               this.generate_mapped=Boolean.parseBoolean(properties.getProperty(prefix+"generate_mapped"));
 		if (properties.getProperty(prefix+"reuse_video")!=null)                   this.reuse_video=Boolean.parseBoolean(properties.getProperty(prefix+"reuse_video"));
@@ -2320,6 +2333,8 @@ public class IntersceneMatchParameters {
 		imp.run_ly                        = this.run_ly;
 		imp.min_num_orient                = this.min_num_orient;
 		imp.min_num_interscene            = this.min_num_interscene;
+		imp.calc_quat_corr                = this.calc_quat_corr;
+		imp.apply_quat_corr               = this.apply_quat_corr;
 		imp.generate_egomotion            = this.generate_egomotion;
 		imp.generate_mapped               = this.generate_mapped;
 		imp.reuse_video                   = this.reuse_video;

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

@@ -4655,8 +4655,9 @@ public class OpticalFlow {
 				save_mapped_mono_color[1]  || gen_avi_mono_color[1] || show_mono_color[1]} ;
 		// skip completely if no color or mono, tiff or video
 		
+		boolean calc_quat_corr =             clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS 
+		boolean apply_quat_corr =            clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
 		boolean generate_egomotion =         clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
-
 		boolean generate_mapped =            clt_parameters.imp.generate_mapped &&
 				(gen_seq_mono_color[0] || gen_seq_mono_color[1]);   // generate sequences - Tiff and/or video
 		boolean export3d =           clt_parameters.imp.export3d; //  true;
@@ -5329,6 +5330,30 @@ public class OpticalFlow {
 			}
 		}
 		
+		if (calc_quat_corr) {
+			double [] quat_rms = new double [5];
+			double [] quatCorr = Interscene.getQuaternionCorrection(
+					clt_parameters, // CLTParameters  clt_parameters,
+					quadCLTs,       // QuadCLT []     quadCLTs,
+					ref_index,      // int            ref_index,
+					earliest_scene, // int            earliest_scene,
+					quat_rms);           // double []      rms // null or double[2];
+			if (quatCorr != null) {
+				int num_iter = (int) quat_rms[4];
+				if (debugLevel> -3) {
+					System.out.println("LMA done on iteration "+num_iter+
+							" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
+					System.out.println("Correction quaternion = ["+quatCorr[0]+", "+
+							quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
+				}
+				quadCLTs[ref_index].setQuatCorr(quatCorr);
+				quadCLT_main.setQuatCorr(quatCorr);
+				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 (generate_egomotion) {
 			boolean ego_show = !clt_parameters.batch_run; //true; 
 			String  ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+

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

@@ -185,6 +185,7 @@ public class QuadCLTCPU {
     public Did_gps_pos did_gps2_pos =     null;
     public Did_gps_pos did_gps1_ubx_pos = null;
     public String      ims_last_path =    null;
+    public double []   quat_corr =        null; // correction for IMS camera frame to actual camera frame (for reference frames)
     
     // latest (or any? first also?) scene in a sequence has a timestamp of the reference scene  
     public String      timestamp_reference = null;
@@ -196,6 +197,12 @@ public class QuadCLTCPU {
     	return timestamp_reference;
     }
 
+    public double [] getQuatCorr() {
+    	return quat_corr;
+    }
+    public void setQuatCorr(double[] quat) {
+    	quat_corr = quat;
+    }
 	public double [][] getDxyzatrIms(
 			CLTParameters clt_parameters,
 			boolean       scale) {
@@ -207,8 +214,8 @@ public class QuadCLTCPU {
 		double [] double_uvw =  did_ins_2.getUvw();
 		double [] omegas=new double[] {did_pimu.theta[0]/did_pimu.dt,
 				did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt};
-		double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
-		double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
+		double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false);
+		double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
 		// a (around Y),t (around X), r (around Z)
 		double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
 		if (scale) {
@@ -692,14 +699,26 @@ public class QuadCLTCPU {
 		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
 		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
 		double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz;    // not yet used
-//		double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
-//		double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
+		boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
 		
 		Did_ins_2 d2 = did_ins_2;
 		double [] cam_quat_enu =Imx5.quaternionImsToCam(
 				d2.getQEnu(),
 				ims_mount_atr,
 				ims_ortho);
+//
+		double [] quat_corr = getQuatCorr();
+		if (apply_quat_corr && (quat_corr != null)) {
+			cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
+	    	if (debug_level > -3) {
+	    		System.out.println("Applying attitude correction from quaternion ["+
+	    				quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"] to the ground plane");
+	    	}
+		} else {
+	    	if (debug_level > -3) {
+	    		System.out.println("No attitude correction is applied to the ground plane");
+	    	}
+		}
 		double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
     	double [][] dls = getDLS();
     	if (dls==null) {
@@ -3737,6 +3756,12 @@ public class QuadCLTCPU {
 		if (this.timestamp_last != null) {
 			properties.setProperty(prefix+"timestamp_last", this.timestamp_last);
 		}
+		
+		if (this.quat_corr != null) {
+			properties.setProperty(prefix+"quat_corr",  IntersceneMatchParameters.doublesToString(this.quat_corr));		
+
+		}
+		
 		return properties;
 	}
 
@@ -3894,12 +3919,17 @@ public class QuadCLTCPU {
 		if (properties.getProperty(prefix+"timestamp_reference")!=null) {
 			this.timestamp_reference= (String) properties.getProperty(prefix+"timestamp_reference");
 		}
+		
 		if (properties.getProperty(prefix+"timestamp_first")!=null) {
 			this.timestamp_first= (String) properties.getProperty(prefix+"timestamp_first");
 		}
 		if (properties.getProperty(prefix+"timestamp_last")!=null) {
 			this.timestamp_last= (String) properties.getProperty(prefix+"timestamp_last");
 		}
+		if (properties.getProperty(prefix+"quat_corr")!=null) {
+	        this.quat_corr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"quat_corr"),4);
+		}
+
 		
 	}
 

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

+/**
+ **
+ ** QuaternionLma - Find quaternion to best transform a set of input 3D vectors
+ **                into a set of output 3D vectors
+ **
+ ** Copyright (C) 2023 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **
+ **  QuaternionLma.java is free software: you can redistribute it and/or modify
+ **  it under the terms of the GNU General Public License as published by
+ **  the Free Software Foundation, either version 3 of the License, or
+ **  (at your option) any later version.
+ **
+ **  This program is distributed in the hope that it will be useful,
+ **  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ **  GNU General Public License for more details.
+ **
+ **  You should have received a copy of the GNU General Public License
+ **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ ** -----------------------------------------------------------------------------**
+ **
+ */
+
+package com.elphel.imagej.tileprocessor;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import Jama.Matrix;
+
+public class QuaternionLma {
+    private final static int  REGLEN =            1; // number of extra (regularization) samples  
+	private int               N =               0; 
+	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
+	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
+	private double []         initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
+	private double []         parameters_vector = null;
+	private double []         x_vector =        null;
+	private double []         y_vector =        null;
+	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization
+	private double            pure_weight; // weight of samples only	
+	private double []         last_ymfx =       null;
+	private double [][]       last_jt =         null;
+	
+	public double [] getQuaternion() {
+		return parameters_vector;
+	}
+	
+	public double [] getLastRms() {
+		return last_rms;
+	}
+	public double [] getInitialRms() {
+		return initial_rms;
+	}
+	public void prepareLMA(
+			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,
+			final int   debug_level) {
+		N = vect_x.length;
+		pure_weight = 1.0 - reg_w;
+		x_vector = new double [3* N];
+		y_vector = new double [3* N + REGLEN];
+		weights =  new double [3* N + REGLEN];
+		parameters_vector = quat0.clone();
+		double sw = 0;
+		for (int i = 0; i < N; i++) {
+			for (int j = 0; j < 3; j++) {
+				x_vector[3 * i + j] = vect_x[i][j];
+				y_vector[3 * i + j] = vect_y[i][j];
+				double w = (vect_w != null)? vect_w[i][j] : 1.0;
+				weights[3*i + j] = w;
+				sw += w;
+			}
+		}
+		double k = (pure_weight)/sw;
+		for (int i = 0; i < weights.length; i++) weights[i] *= k;
+		weights[3 * N] = 1.0 - pure_weight;
+		y_vector[3 * N] = 1.0;
+		last_jt = new double [parameters_vector.length][];		
+	}
+	
+	// TODO: Consider adding differences between x and y for regularization (or it won't work)
+	// goal - to minimize "unneded" rotation along the commonn axis
+	private double [] getFxDerivs(
+			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 q0 = vector[0]; 
+		final double q1 = vector[1]; 
+		final double q2 = vector[2]; 
+		final double q3 = vector[3];
+		if (jt != null) {
+			for (int i = 0; i < vector.length; i++) {
+				jt[i] = new double [weights.length];
+				jt[i][3 * N] = 2 * vector[i];
+			}
+		}
+		fx[3 * N] = q0*q0 + q1*q1 + q2 * q2 + q3*q3;
+		for (int i = 0; i < N; i++) {
+			int i3 = 3 * i;
+			final double x = x_vector[i3 + 0];
+			final double y = x_vector[i3 + 1];
+			final double z = x_vector[i3 + 2];
+			final double s = q1 * x + q2 * y + q3 * z;
+			fx[i3 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
+			fx[i3 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
+			fx[i3 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
+			if (jt != null) {
+				jt[0][i3 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;
+				jt[1][i3 + 0] = 2*s +    2*q1*x;
+				jt[2][i3 + 0] = 2*z*q0 + 2*q1*y;
+				jt[3][i3 + 0] = 2*y*q0 + 2*q1*z;
+
+				jt[0][i3 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1;
+				jt[1][i3 + 1] = 2*z*q0 + 2*x*q2; 
+				jt[2][i3 + 1] = 2*s +    2*y*q2;
+				jt[3][i3 + 1] =-2*x*q0+  2*z*q2;
+
+				jt[0][i3 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;
+				jt[1][i3 + 2] =-2*y*q0 + 2*x*q3;
+				jt[2][i3 + 2] = 2*x*q0 + 2*y*q3;
+				jt[3][i3 + 2] = 2*s    + 2*z*q3;
+			}
+		}
+		return fx;
+	}
+	
+	private double [] getYminusFxWeighted(
+			final double []   fx,
+			final double []   rms_fp // null or [2]
+			) {
+		final double []     wymfw =       new double [fx.length];
+		double s_rms=0; 
+		double rms_pure=0; 
+		for (int i = 0; i < fx.length; i++) {
+			double d = y_vector[i] - fx[i];
+			double wd = d * weights[i];
+			if (Double.isNaN(wd)) {
+				System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
+						", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
+				wd = 0.0;
+				d = 0.0;
+			}
+			if (i == (3 * N)) {
+				rms_pure = Math.sqrt(s_rms/pure_weight);;	
+			}
+			wymfw[i] = wd;
+			s_rms += d * wd;
+		}
+		double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
+
+		if (rms_fp != null) {
+			rms_fp[0] = rms;
+			rms_fp[1] = rms_pure;
+		}
+
+		return wymfw;
+	}
+	
+	// reusing multithreaded
+	private double [][] getWJtJlambda( // USED in lwir
+			final double      lambda,
+			final double [][] jt)
+	{
+		final int num_pars = jt.length;
+		final int num_pars2 = num_pars * num_pars;
+		final int nup_points = jt[0].length;
+		final double [][] wjtjl = new double [num_pars][num_pars];
+		final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
+						int i = indx / num_pars;
+						int j = indx % num_pars;
+						if (j >= i) {
+							double d = 0.0;
+							for (int k = 0; k < nup_points; k++) {
+								if (jt[i][k] != 0) {
+									d+=0;
+								}
+								d += weights[k]*jt[i][k]*jt[j][k];
+							}
+							wjtjl[i][j] = d;
+							if (i == j) {
+								wjtjl[i][j] += d * lambda;
+							} else {
+								wjtjl[j][i] = d;
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return wjtjl;
+	}
+	
+	
+	public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+			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
+			boolean last_run,
+			int    debug_level) {
+		boolean [] rslt = {false,false};
+		this.last_rms = null; // remove?
+		int iter = 0;
+		for (iter = 0; iter < num_iter; iter++) {
+			rslt =  lmaStep(
+					lambda,
+					rms_diff,
+					debug_level);
+			if (rslt == null) {
+				return -1; // false; // need to check
+			}
+			if (debug_level > 1) {
+				System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
+						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+			}
+			if (rslt[1]) {
+				break;
+			}
+			if (rslt[0]) { // good
+				lambda *= lambda_scale_good;
+			} else {
+				lambda *= lambda_scale_bad;
+				if (lambda > lambda_max) {
+					break; // not used in lwir
+				}
+			}
+		}
+		if (rslt[0]) { // better
+			if (iter >= num_iter) { // better, but num tries exceeded
+				if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
+			}
+
+		} else { // improved over initial ?
+			if (last_rms[0] < initial_rms[0]) { // NaN
+				rslt[0] = true;
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
+			}
+		}
+		boolean show_intermediate = true;
+		if (show_intermediate && (debug_level > 0)) {
+			System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+		}
+		if (debug_level > 2){ 
+			String [] lines1 = printOldNew(false); // boolean allvectors)
+			System.out.println("iteration="+iter);
+			for (String line : lines1) {
+				System.out.println(line);
+			}
+		}
+		if (debug_level > 0) {
+			if ((debug_level > 1) ||  last_run) { // (iter == 1) || last_run) {
+				if (!show_intermediate) {
+					System.out.println("LMA: iter="+iter+",   full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+				}
+				String [] lines = printOldNew(false); // boolean allvectors)
+				for (String line : lines) {
+					System.out.println(line);
+				}
+			}
+		}
+		if ((debug_level > -2) && !rslt[0]) { // failed
+			if ((debug_level > 1) || (iter == 1) || last_run) {
+				System.out.println("LMA failed on iteration = "+iter);
+				String [] lines = printOldNew(true); // boolean allvectors)
+				for (String line : lines) {
+					System.out.println(line);
+				}
+			}
+			System.out.println();
+		}
+
+		return rslt[0]? iter : -1;
+	}
+	
+	private boolean [] lmaStep(
+			double lambda,
+			double rms_diff,
+			int debug_level) {
+		boolean [] rslt = {false,false};
+		// maybe the following if() branch is not needed - already done in prepareLMA !
+		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
+			last_rms = new double[2];
+			if (debug_level > 1) {
+				System.out.println("lmaStep(): first step");
+			}
+			double [] fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					last_rms); // final double []   rms_fp // null or [2]
+			this.initial_rms = this.last_rms.clone();
+			this.good_or_bad_rms = this.last_rms.clone();
+
+			if (debug_level > -1) { // temporary
+				/*
+				dbgYminusFxWeight(
+						this.last_ymfx,
+						this.weights,
+						"Initial_y-fX_after_moving_objects");
+                */
+			}
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			// TODO: Restore/implement
+			if (debug_level > 3) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+				*/
+			}
+		}
+		Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
+
+		Matrix wjtjlambda = new Matrix(getWJtJlambda(
+				lambda, // *10, // temporary
+				this.last_jt)); // double [][] jt)
+		
+		if (debug_level>2) {
+			System.out.println("JtJ + lambda*diag(JtJ");
+			wjtjlambda.print(18, 6);
+		}
+		Matrix jtjl_inv = null;
+		try {
+			jtjl_inv = wjtjlambda.inverse(); // check for errors
+		} catch (RuntimeException e) {
+			rslt[1] = true;
+			if (debug_level > 0) {
+				System.out.println("Singular Matrix!");
+			}
+
+			return rslt;
+		}
+		if (debug_level>2) {
+			System.out.println("(JtJ + lambda*diag(JtJ).inv()");
+			jtjl_inv.print(18, 6);
+		}
+//last_jt has NaNs
+		Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
+		if (debug_level>2) {
+			System.out.println("Jt * (y-fx)");
+			jty.print(18, 6);
+		}
+		
+		
+		Matrix mdelta = jtjl_inv.times(jty);
+		if (debug_level>2) {
+			System.out.println("mdelta");
+			mdelta.print(18, 6);
+		}
+
+		double scale = 1.0;
+		double []  delta =      mdelta.getColumnPackedCopy();
+		double []  new_vector = parameters_vector.clone();
+		for (int i = 0; i < parameters_vector.length; i++) {
+			new_vector[i] += scale * delta[i];
+		}
+		
+		
+		double [] fx = getFxDerivs(
+				new_vector, // double []         vector,
+				last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+				debug_level);      // final int         debug_level)
+		double [] rms = new double[2];
+		last_ymfx = getYminusFxWeighted(
+				fx, // final double []   fx,
+				rms); // final double []   rms_fp // null or [2]
+		if (debug_level > 2) {
+			/*
+			dbgYminusFx(this.last_ymfx, "next y-fX");
+			dbgXY(new_vector, "XY-correction");
+			*/
+		}
+
+		if (last_ymfx == null) {
+			return null; // need to re-init/restart LMA
+		}
+
+		this.good_or_bad_rms = rms.clone();
+		if (rms[0] < this.last_rms[0]) { // improved
+			rslt[0] = true;
+			rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
+			this.last_rms = rms.clone();
+
+			this.parameters_vector = new_vector.clone();
+			if (debug_level > 2) {
+				// print vectors in some format
+				/*
+				System.out.print("delta: "+corr_delta.toString()+"\n");
+				System.out.print("New vector: "+new_vector.toString()+"\n");
+				System.out.println();
+				*/
+			}
+		} else { // worsened
+			rslt[0] = false;
+			rslt[1] = false; // do not know, caller will decide
+			// restore state
+			fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					this.last_rms); // final double []   rms_fp // null or [2]
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			if (debug_level > 2) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+							*/
+			}
+		}
+		return rslt;
+	}
+	
+	
+	
+	
+	//TODO: implement
+	public String [] printOldNew(boolean allvectors) {
+		return new String[] {};
+	}
+
+}