Bug fixing to make run for many scenes. Started LPF for the ground w/o

SfM

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 			System.out.println("=== IMS ===");
 			QUAD_CLT_AUX.showQuatCorr();
 			System.out.println("=== IMU ===");
-			QUAD_CLT_AUX.showPimuOffsets();
+			QUAD_CLT_AUX.showPimuOffsets(CLT_PARAMETERS); //
 			@SuppressWarnings("unused")
 			QuadCLT dbg_QUAD_CLT = QUAD_CLT;
 			@SuppressWarnings("unused")

src/main/java/com/elphel/imagej/readers/ElphelTiffWriter.java

@@ -295,7 +295,7 @@ public class ElphelTiffWriter {
 	private static IIOMetadataNode createTimeStamp(LocalDateTime dt, int digits_after) { // 3
 		int denom = 1;
 		for (int i = 0; i < digits_after; i++) denom *= 10;
-		int fsec = dt.getSecond()*denom+((int) Math.round(denom * dt.getNano()*1E-9));
+		int fsec = dt.getSecond()*denom+((int) Math.round(denom * (dt.getNano()*1E-9)));
 		
 	    IIOMetadataNode node_rationals = new IIOMetadataNode(TIFF_RATIONALS_TAG);
 	    IIOMetadataNode node_hrs =       new IIOMetadataNode(TIFF_RATIONAL_TAG);

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

@@ -118,6 +118,7 @@ public class IntersceneMatchParameters {
 	public  boolean apply_imu_orient =   true;  // apply  IMU misalignment to the camera if adjusted
 	public  boolean orient_by_move =     false; // use translation data to adjust IMU orientation  
 	public  boolean orient_by_rot =      true;  // use rotation data to adjust IMU orientation
+	public  boolean orient_combo =       true;  // use combined rotation+orientation for IMU/camera matching
 	public  boolean adjust_gyro =        false; // adjust qyro omegas offsets
 	public  boolean apply_gyro =         true;  // apply adjusted qyro omegas offsets
 	public  boolean adjust_accl =        false; // adjust IMU velocities scales
@@ -386,7 +387,9 @@ public class IntersceneMatchParameters {
 	public  double  max_zoom_diff =                 0;     // for down-views when changing altitude (0 - ignore)
 	public  boolean fpn_skip =                      true;  // skip too close scenes (false - abort, previous behavior)
 	public  boolean fpn_rematch =                   true;  // match fpn-failed scenes to later scenes with larger difference
-	public  boolean refine_invert =                 false; // Refine with LMA while inverting relative poses from other reference
+	// still not clear why it sometimes fails without refine_invert (too large initial mismatch)
+	public  boolean refine_invert =                 true;  // Refine with LMA while inverting relative poses from other reference
+	public  boolean use_precomp =                   false; // try to predict initial error from previous scenes
 	
 	// Remove moving objects (goal is not to detect slightest movement, but to improve pose matching
 	public  boolean mov_en =                        true;  // enable detection/removal of the moving objects during pose matching
@@ -673,12 +676,14 @@ public class IntersceneMatchParameters {
 				"Minimal required number of re-calculations of the interscene-accumulated DSI.");
 		gd.addCheckbox ("Adjust IMU orientation",                    this.adjust_imu_orient,
 				"Adjust IMU misalignment to the camera.");
-		gd.addCheckbox ("Adjust IMU orientation",                    this.apply_imu_orient,
+		gd.addCheckbox ("Apply IMU orientation",                     this.apply_imu_orient,
 				"Apply  IMU misalignment to the camera if adjusted.");
 		gd.addCheckbox ("Use translation for IMU orientation",       this.orient_by_move,
 				"Use translation data to adjust IMU orientation .");
 		gd.addCheckbox ("Use rotation for IMU orientation",          this.orient_by_rot,
 				"Use rotation data to adjust IMU orientation.");
+		gd.addCheckbox ("Use combo mode IMU orientation",            this.orient_combo,
+				"Use combined Z/h, R, A-X/h, T+Y/h for IMU mount-to-camera orientation correction. False - use X,Y,Z,A,T,R");
 		gd.addCheckbox ("Adjust gyro offsets",                       this.adjust_gyro,
 				"Adjust qyro omegas offsets.");
 		gd.addCheckbox ("Apply gyro offsets",                        this.apply_gyro,
@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters {
 				"Match fpn-failed scenes to later scenes with larger difference.");
 		gd.addCheckbox ("Refine inversion",                          this.refine_invert,
 				"Refine with LMA while inverting relative poses from other reference.");
+		gd.addCheckbox ("Precompensate orientation readjustment",    this.use_precomp,
+				"Guess needed initial precompensation from the previously processed scenes.");
 		
 		gd.addMessage  ("Detect and remove moving objects from pose matching");
 		gd.addCheckbox ("Enable movement detection/elimination",     this.mov_en,
@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters {
 		this.apply_imu_orient =               gd.getNextBoolean();
 		this.orient_by_move =                 gd.getNextBoolean();
 		this.orient_by_rot =                  gd.getNextBoolean();
+		this.orient_combo =                  gd.getNextBoolean();
 		this.adjust_gyro =                    gd.getNextBoolean();
 		this.apply_gyro =                     gd.getNextBoolean();
 		this.adjust_accl =                    gd.getNextBoolean();
@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters {
 		this.fpn_skip =                 gd.getNextBoolean();
 		this.fpn_rematch =              gd.getNextBoolean();
 		this.refine_invert =            gd.getNextBoolean();
+		this.use_precomp =              gd.getNextBoolean();
 		
 		this.mov_en =                   gd.getNextBoolean();
 		this.mov_sigma =                gd.getNextNumber();
@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"apply_imu_orient",              this.apply_imu_orient+"");              // boolean
 		properties.setProperty(prefix+"orient_by_move",                this.orient_by_move+"");                // boolean
 		properties.setProperty(prefix+"orient_by_rot",                 this.orient_by_rot+"");                 // boolean
+		properties.setProperty(prefix+"orient_combo",                  this.orient_combo+"");                  // boolean
 		properties.setProperty(prefix+"adjust_gyro",                   this.adjust_gyro+"");                   // boolean
 		properties.setProperty(prefix+"apply_gyro",                    this.apply_gyro+"");                    // boolean
 		properties.setProperty(prefix+"adjust_accl",                   this.adjust_accl+"");                   // boolean
@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"fpn_skip",             this.fpn_skip+"");            // boolean
 		properties.setProperty(prefix+"fpn_rematch",          this.fpn_rematch+"");         // boolean
 		properties.setProperty(prefix+"refine_invert",        this.refine_invert+"");       // boolean
+		properties.setProperty(prefix+"use_precomp",          this.use_precomp+"");         // boolean
 
 		properties.setProperty(prefix+"mov_en",               this.mov_en+"");              // boolean
 		properties.setProperty(prefix+"mov_sigma",            this.mov_sigma+"");           // double
@@ -2345,6 +2356,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"apply_imu_orient")!=null)              this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
 		if (properties.getProperty(prefix+"orient_by_move")!=null)                this.orient_by_move=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_move"));
 		if (properties.getProperty(prefix+"orient_by_rot")!=null)                 this.orient_by_rot=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_rot"));
+		if (properties.getProperty(prefix+"orient_combo")!=null)                  this.orient_combo=Boolean.parseBoolean(properties.getProperty(prefix+"orient_combo"));
 		if (properties.getProperty(prefix+"adjust_gyro")!=null)                   this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
 		if (properties.getProperty(prefix+"apply_gyro")!=null)                    this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
 		if (properties.getProperty(prefix+"adjust_accl")!=null)                   this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
@@ -2588,6 +2600,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"fpn_skip")!=null)             this.fpn_skip=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_skip"));
 		if (properties.getProperty(prefix+"fpn_rematch")!=null)          this.fpn_rematch=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_rematch"));
 		if (properties.getProperty(prefix+"refine_invert")!=null)        this.refine_invert=Boolean.parseBoolean(properties.getProperty(prefix+"refine_invert"));
+		if (properties.getProperty(prefix+"use_precomp")!=null)          this.use_precomp=Boolean.parseBoolean(properties.getProperty(prefix+"use_precomp"));
 		
 		if (properties.getProperty(prefix+"mov_en")!=null)               this.mov_en=Boolean.parseBoolean(properties.getProperty(prefix+"mov_en"));
 		if (properties.getProperty(prefix+"mov_sigma")!=null)            this.mov_sigma=Double.parseDouble(properties.getProperty(prefix+"mov_sigma"));
@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters {
 		imp.apply_imu_orient              = this.apply_imu_orient;
 		imp.orient_by_move                = this.orient_by_move;
 		imp.orient_by_rot                 = this.orient_by_rot;
+		imp.orient_combo                  = this.orient_combo;
 		imp.adjust_gyro                   = this.adjust_gyro;
 		imp.apply_gyro                    = this.apply_gyro;
 		imp.adjust_accl                   = this.adjust_accl;
@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters {
 		imp.fpn_skip              = this.fpn_skip;
 		imp.fpn_rematch           = this.fpn_rematch;
 		imp.refine_invert         = this.refine_invert;
+		imp.use_precomp           = this.use_precomp;
 		
 		imp.mov_en                = this.mov_en;
 		imp.mov_sigma             = this.mov_sigma;

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

@@ -29,8 +29,10 @@ import java.awt.Rectangle;
 import java.io.File;
 import java.io.IOException;
 import java.security.NoSuchAlgorithmException;
+import java.text.SimpleDateFormat;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Calendar;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.List;
@@ -5426,15 +5428,30 @@ public class OpticalFlow {
 				}				
 			}
 		}
+		
+//		quadCLTs[ref_index].getSmoothGround(clt_parameters);
+		
 		// later move to the right place
 		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
+			boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
+			QuadCLT.adjustImuOrient(
+					clt_parameters,     //CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+					orient_combo, // boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
+					quadCLTs, // QuadCLT[]     quadCLTs,
+					ref_index, // int           ref_index,
+					earliest_scene, // int           earliest_scene,
+					last_index, // int           last_index,
+					debugLevel); // 			    		int debugLevel
+			// Try both orient_combo/!orient_combo for the log!
 			QuadCLT.adjustImuOrient(
 					clt_parameters,     //CLTParameters clt_parameters,     // CLTParameters clt_parameters,
+					!orient_combo, // boolean orient_combo =     clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
 					quadCLTs, // QuadCLT[]     quadCLTs,
 					ref_index, // int           ref_index,
 					earliest_scene, // int           earliest_scene,
 					last_index, // int           last_index,
 					debugLevel); // 			    		int debugLevel
+			
 		}
 		if (run_ly) {
 			if (debugLevel > -3) {
@@ -5548,15 +5565,20 @@ public class OpticalFlow {
 				quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
 						null, // String path,             // full name with extension or w/o path to use x3d directory
 						debugLevel+1);
-				if (debugLevel > -3) {
+	        	StringBuffer sb = new StringBuffer();
+	        	sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
 	        	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 to the IMS to world orientation (rotating around IMS vertical):");
+				sb.append("Applying correction to the IMS to world orientation (rotating around IMS vertical):\n");
 				double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
 				double []  corr_degrees = new double[3];
 				for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
-					System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
-					System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
-					System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
+				sb.append("compass: quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]\n");
+				sb.append("compass: ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]\n");
+				sb.append("compass: ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]\n");
+	        	sb.append("------------------------\n\n");
+	    		quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String  suffix)
+				if (debugLevel > -3) {
+					System.out.print(sb.toString());
 				}
 			} else {
 				if (debugLevel> -3) {

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

@@ -35,10 +35,10 @@ import Jama.Matrix;
 public class QuaternionLma {
     private final static int  REGLEN =            1; // number of extra (regularization) samples
     public static final int MODE_XYZ =           0;
-    public static final int MODE_XYZQ =          1;
+    public static final int MODE_XYZQ =          1; // OK with [3]
     public static final int MODE_COMBO =         2;
     public static final int MODE_XYZQ_LOCAL =    3;
-    public static final int MODE_COMBO_LOCAL =   4;
+    public static final int MODE_COMBO_LOCAL =   4; // OK with [3]
     public static final int MODE_COMPASS =       5;
     public static final int MODE_XYZ4Q3 =        6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
     
@@ -67,6 +67,7 @@ public class QuaternionLma {
 	private double []         last_ymfx =       null;
 	private double [][]       last_jt =         null;
 	private double []         axis = null;
+	private double[] dbg_data;
 	
 	public double [] getQuaternion() {
 		if (parameters_vector.length == 3) {
@@ -748,7 +749,7 @@ public class QuaternionLma {
 		double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2);
 		//axis
 		double [] fx = new double [weights.length];
-		final double [] q = new double [] { c/2, s*axis[0]/2, s*axis[1]/2, s*axis[2]/2};
+		final double [] q = new double [] { c, s*axis[0], s*axis[1], s*axis[2]};
 		double [] dq_dv  =  new double [] {-s/2, c*axis[0]/2, c*axis[1]/2, c*axis[2]/2};
 		if (jt != null) {
 			for (int i = 0; i < vector.length; i++) {
@@ -759,6 +760,14 @@ public class QuaternionLma {
 		double [][] xyz_dq;
 		for (int i = 0; i < N; i++) {
 			int i3 = 3 * i;
+			has_data:{
+				for (int j = 0; j < samples; j++) {
+					if (weights[i3+j] > 0) {
+						break has_data;
+					}
+				}
+				continue; // nothing to process for this scene 
+			}
 			final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
 			xyz_rot = applyTo(q, xyz);
 			System.arraycopy(xyz_rot, 0, fx, i3, 3);
@@ -922,7 +931,7 @@ public class QuaternionLma {
 		}
 		return fx;
 	}
-	private double [] getFxDerivs6DofMode33(
+	private double [] getFxDerivs6DofMode33( // MODE_XYZQ_LOCAL =   3; // OK with [3]
 			double []         vector, //
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
@@ -933,6 +942,7 @@ public class QuaternionLma {
 		final double q0 = getQ0(vector); 
 //		final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
 		final double [] vector_r = new double[] { q0,q1,q2,q3};
+		 
 		if (jt != null) {
 			for (int i = 0; i < vector.length; i++) {
 				jt[i] = new double [weights.length];
@@ -1084,7 +1094,7 @@ public class QuaternionLma {
 		return fx;
 	}
 	
-	private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations
+	private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations MODE_XYZQ tested
 			double []         vector3, //
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
@@ -1428,10 +1438,14 @@ public class QuaternionLma {
 		return fx;
 	}
 	
-	private double [] getFxDerivsVisualMode43(
+	private double [] getFxDerivsVisualMode43( // tested MODE_COMBO_LOCAL =   4; // OK with [3]
 			double []         vector3,
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level) {
+		boolean dbg_out = debug_level>2;
+		if (dbg_out) {
+			dbg_data = new double[N * samples_x];
+		}
 		double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
 		double [] fx = new double [weights.length];
 		double [] qn = new double[4];
@@ -1476,12 +1490,15 @@ public class QuaternionLma {
 			fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3];                       // 2 * Q3
 			fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
 			fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
-		    /*
-			fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height;                   // Z
-			fx[i4 + 1] = -2 * comb_y[1][3]; // quat_rot[3];                       // 2 * Q3
-			fx[i4 + 2] = -2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
-			fx[i4 + 3] = -2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
-			*/
+			if (dbg_out) {
+				dbg_data[i7 + 0] = comb_y[0][0]/ height;
+				dbg_data[i7 + 1] = comb_y[0][1]/ height;
+				dbg_data[i7 + 2] = comb_y[0][2]/ height;
+				dbg_data[i7 + 3] = comb_y[1][0];
+				dbg_data[i7 + 4] = comb_y[1][1];
+				dbg_data[i7 + 5] = comb_y[1][2];
+				dbg_data[i7 + 6] = comb_y[1][3];
+			}
 			if (jt != null) {
 				xyz_dq =  applyToDQ(vector, xyz);
 				double [][] xyz_dq_local = new double [xyz_dq.length][];
@@ -1721,6 +1738,10 @@ public class QuaternionLma {
 					debug_level);      // final int         debug_level)
 			debugYfX ( "fx-",   // String pfx,
 					fx); // double [] data)
+			if (debug_level > 2) {
+				debugYfX ( "ffx-",   // String pfx,
+						dbg_data); // double [] data)
+			}
 			if (debug_level > 1) {
 				double    delta = 1E-5;
 				System.out.println("\n\n");
@@ -1773,6 +1794,11 @@ public class QuaternionLma {
 				 debugYfX ( "fx0-",   // String pfx,
 						 fx); // double [] data)
 			}
+			if (debug_level > 2) {
+				debugYfX ( "ffx0-",   // String pfx,
+						dbg_data); // double [] data)
+			}
+			
 			if (debug_level > 1) {
 				double    delta = 1E-5;
 				System.out.println("\n\n");
@@ -1919,6 +1945,9 @@ public class QuaternionLma {
 	public void debugYfX (
 			String pfx,
 			double [] data) {
+		if (data == null) {
+			return;
+		}
 //		if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
 		if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) {
 			System.out.println(String.format("%3s"+

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

@@ -2576,6 +2576,7 @@ public class TexturedModel {
 		double [] sfm_gain = (min_sfm_gain > 0.0) ? combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] : null;
 		// currently conditionInitialDS() zeroes disparity for blue_sky. TODO: allow some FG over blue_sky?
 		// gets Blue Sky from scene.dsi , not from the file!
+		
 		double [][] ds_fg = OpticalFlow.conditionInitialDS(
 				true,                // boolean        use_conf,       // use configuration parameters, false - use following  
 				clt_parameters,      // CLTParameters  clt_parameters,
@@ -3165,7 +3166,7 @@ public class TexturedModel {
 									hdr_whs[0],    // int            width, // int tilesX,
 									hdr_whs[1]);   // int            height, // int tilesY,
 							*/
-							scenes[ref_index].writeLwirGeoTiff32(
+							scenes[ref_index].writeLwirGeoTiff32( // Negative value supplied for TIFF_RATIONAL
 									clt_parameters,                     // final CLTParameters  clt_parameters,
 									cropped_z[0],                       // double []            data,
 									top_left_lla,                       // double []            lla,  // latitude, longitude, altitude (or null)