Implementing global LMA adjustment of the scene segment

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

@@ -493,6 +493,7 @@ public class EstimateSceneRange {
 		}
 		// calculate only needed (copy quadCLTs with nulls)?
 		double [][][] ims_xyzatr;
+		// get IMS poses (now inertial_only== false) , use second branch 
 		if (inertial_only) {
 			ims_xyzatr = QuadCLT.integratePIMU( // does nor use quat_corr
 						clt_parameters, // final CLTParameters clt_parameters,
@@ -556,7 +557,7 @@ public class EstimateSceneRange {
 		final double [] accum_weights =  new double [tilesX*tilesY]; // sfm gain squared
 		int num_sfm = 0;  // do not use if no sfm was performed (for LY with too few scenes and high altitude)
 		
-		
+		//Iterate from the reference scene back in time, then forward in time
 		for (int scan_dir = 1; scan_dir >= -1; scan_dir -=2) {
 			double [][] last_corr_xyzatr = {Interscene.ZERO3,Interscene.ZERO3};
 			for (int scene_index =  cent_index + scan_dir;
@@ -585,6 +586,7 @@ public class EstimateSceneRange {
 								clt_parameters,
 								scenes_dxyzatr[scene_index])); // cam_dxyzatr));		
 	// Trying new version with motion blur and single-setting of the reference frame
+				// Make sure pixel shift between images is not too small otherwise FPN will dominate the useful features in correlation					
 	    		double est_shift = quadCLTs[cent_index].estimateAverageShift(
 	    				scenes_xyzatr[cent_index], // double [][] xyzatr0,
 	    				scenes_xyzatr[scene_index],    // double [][] xyzatr1,
@@ -609,6 +611,8 @@ public class EstimateSceneRange {
 	      			System.out.println("center_offset_xy== null");
 	      			
 	      		}
+	      		
+	      		// Add too near tiles to unreliable
 	    		boolean [] reliable_scene =  quadCLTs[cent_index].maskByOverlap(
 	    				reliable_ref, // boolean [] reliable_ref_tiles,
 	    				center_offset_xy); // double []  offset)
@@ -619,8 +623,10 @@ public class EstimateSceneRange {
 	    		if (reliables != null) {
 	    			reliables[scene_index]=reliable_scene;
 	    		}
+	    		// calculate linear horizontal shift between camera poses to see if SfM makes sense
 				double hor_move = Math.sqrt(initial_pose[0][0]*initial_pose[0][0]+initial_pose[0][1]*initial_pose[0][1]);
 				
+				// Disable fitting of zoom and roll if overlap is too small (both require significant overlap between the images
 	    		boolean no_zr = est_shift > lma_ovlp_ZR * (tilesX * tile_size);
 	    		boolean [] param_select_use = no_zr ? param_select_nozr: param_select;
 	    		
@@ -635,6 +641,8 @@ public class EstimateSceneRange {
 	    		boolean adjust_OK = false;
 	    		// TODO: add min_offset_initial to other adjustments, make scale configurable
 	    		boolean prefiltered = false;
+	    		
+	    		// If pixel shift between image pairs is too small, skip it to mitigate late, when others scenes are done	
 	       		if (est_shift < min_offset) { // min_max[0]) {
 	    			fail_reason[0]=Interscene.FAIL_REASON_MIN;
 	    			prefiltered = true;
@@ -715,6 +723,8 @@ public class EstimateSceneRange {
 						clt_parameters,
 						quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
 								));
+				
+				// Add adjustment results to the list that later will be saved to the .corr-xml file
 				ers_reference.addScene(scene_QuadClt.getImageName(),
 						scenes_xyzatr[scene_index][0],
 						scenes_xyzatr[scene_index][1],
@@ -746,8 +756,6 @@ public class EstimateSceneRange {
 					 * Rehabilitate all strong enough?
 					 *  
 					 */
-					
-					
 					boolean sfm_OK = adjustDisparitySFM( // modifies one slice of the quadCLTs[cent_index].dsi
 							clt_parameters, // final CLTParameters           clt_parameters,
 							quadCLTs,       // final QuadCLT[]           	 quadCLTs,
@@ -792,6 +800,8 @@ public class EstimateSceneRange {
 		if (debugLevel > -4) {
 			System.out.println("num_fpn_mitigate= "+fpn_list.size());
 		}
+		// Mitigate FPN - orient the scenes that were too close to the reference by matching them to other, not-so-close scenes
+		// with already known poses
 		if (fmg_initial_en && !fpn_list.isEmpty()) {
 			// here max_offset is not critical, min_offset can be 0 too
 //			double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
@@ -914,7 +924,7 @@ public class EstimateSceneRange {
 					reliables, // boolean  [][] reliables);
 					"-prescan-initial"); // String suffix)
 		}
-		// perform final filtering
+		// perform final filtering of the SFM-found disparities
 		if (debugLevel > -3) {
 			System.out.println("Number of scenes with SfM =  "+num_sfm);
 		}

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

@@ -4641,6 +4641,77 @@ public class Interscene {
 		return earliest_scene;
 	}
 
+	/**
+	 * Run global sparse pose refinement with a fixed reference scene.
+	 * Keeps legacy pairwise readjustment flow untouched and provides a separate entry point.
+	 *
+	 * @param clt_parameters processing parameters
+	 * @param quadCLTs scenes
+	 * @param center_CLT fixed reference scene
+	 * @param center_index index of center scene
+	 * @param range scene index range {earliest,last}
+	 * @param scenes_xyzatr scene poses [scene][{xyz,atr}][3], updated in place
+	 * @param param_select selected LMA parameters (full DP_NUM_PARS size)
+	 * @param param_regweights local regularization weights (full DP_NUM_PARS size)
+	 * @param param_lpf inter-scene curvature pull weights (full DP_NUM_PARS size)
+	 * @param reliable_ref optional reliability mask for the reference disparity tiles
+	 * @param center_disparity optional reference disparity (null to use center_CLT current)
+	 * @param disable_ers disable ERS fit terms
+	 * @param mb_max_gain maximal motion blur gain
+	 * @param debugLevel debug level
+	 * @return global refinement summary
+	 */
+	public static IntersceneGlobalRefine.Result reAdjustPairsLMAIntersceneGlobalReference(
+			final CLTParameters clt_parameters,
+			final QuadCLT[] quadCLTs,
+			final QuadCLT center_CLT,
+			final int center_index,
+			final int[] range,
+			final double[][][] scenes_xyzatr,
+			final boolean[] param_select,
+			final double[] param_regweights,
+			final double[] param_lpf,
+			final boolean[] reliable_ref,
+			final double[] center_disparity,
+			final boolean disable_ers,
+			final double mb_max_gain,
+			final int debugLevel) {
+		final int earliest_scene = Math.max(0, range[0]);
+		final int last_scene = Math.min(quadCLTs.length - 1, range[1]);
+		final IntersceneGlobalRefine.Options opts = new IntersceneGlobalRefine.Options();
+		opts.outerIterations = Math.max(1, clt_parameters.imp.max_cycles);
+		opts.pcgIterations = Math.max(40, clt_parameters.ilp.ilma_num_iter * 8);
+		opts.lambdaDiag = clt_parameters.ilp.ilma_lambda;
+		opts.minOffset = clt_parameters.imp.min_offset;
+		opts.maxOffset = 256.0;
+		opts.pcgTolerance = 1.0e-7;
+		opts.deltaStop = 1.0e-5;
+		opts.smoothWeightAtr = 1.0;
+		opts.smoothWeightXyz = 0.15;
+		if (debugLevel > -4) {
+			System.out.println("reAdjustPairsLMAIntersceneGlobalReference(): center=" + center_index +
+					", range=[" + earliest_scene + "," + last_scene + "], outer=" + opts.outerIterations +
+					", pcgIter=" + opts.pcgIterations + ", minOffset=" + opts.minOffset);
+		}
+		return IntersceneGlobalRefine.refineAllToReference(
+				clt_parameters,
+				quadCLTs,
+				center_CLT,
+				center_index,
+				earliest_scene,
+				last_scene,
+				scenes_xyzatr,
+				param_select,
+				param_regweights,
+				param_lpf,
+				center_disparity,
+				reliable_ref,
+				disable_ers,
+				mb_max_gain,
+				opts,
+				debugLevel);
+	}
+
 	
 	
 	/**

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

@@ -95,6 +95,67 @@ public class IntersceneLma {
 		return last_jt;
 	}
 
+	public double [] getLastYminusFxWeightedCopy() {
+		return (last_ymfx == null) ? null : last_ymfx.clone();
+	}
+
+	public int [] getParameterIndicesCopy() {
+		return (par_indices == null) ? null : par_indices.clone();
+	}
+
+	public double [] getParametersVectorCopy() {
+		return (parameters_vector == null) ? null : parameters_vector.clone();
+	}
+
+	public static class NormalEquationData {
+		public final double [][] h;
+		public final double [] b;
+		public final int [] parameterIndices;
+		public final double [] parameterVector;
+		public final double [] rms;
+
+		public NormalEquationData(
+				final double [][] h,
+				final double [] b,
+				final int [] parameterIndices,
+				final double [] parameterVector,
+				final double [] rms) {
+			this.h = h;
+			this.b = b;
+			this.parameterIndices = parameterIndices;
+			this.parameterVector = parameterVector;
+			this.rms = rms;
+		}
+	}
+
+	/**
+	 * Return weighted normal-equation components for the current linearization:
+	 * <pre>
+	 * H = J^T W J
+	 * b = J^T W (y - f(x))
+	 * </pre>
+	 * Use this for global sparse coupling of multiple local problems.
+	 *
+	 * @param lambda add lambda*diag(H) when > 0 (same convention as local LMA)
+	 * @return normal-equation data or {@code null} when the current linearization is not initialized
+	 */
+	public NormalEquationData getNormalEquationData(
+			final double lambda) {
+		if ((last_jt == null) || (last_ymfx == null) || (parameters_vector == null) || (par_indices == null)) {
+			return null;
+		}
+		final double [][] h = getWJtJlambda(
+				lambda,
+				last_jt);
+		final Matrix bMatrix = (new Matrix(last_jt)).times(new Matrix(last_ymfx, last_ymfx.length));
+		return new NormalEquationData(
+				h,
+				bMatrix.getColumnPackedCopy(),
+				par_indices.clone(),
+				parameters_vector.clone(),
+				(last_rms == null) ? null : last_rms.clone());
+	}
+	
 	public double getSumWeights() {
 		return sum_weights;
 	}

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

@@ -1107,6 +1107,7 @@ min_str_neib_fpn	0.35
 
     public boolean  wser_orient_en =        true;  // per-series orientation dcamera-to-imu adjust enable
     public double   wser_orient_transl =    0.5;   // weight of translations for orientation, AGL-corrected.
+    public double   wser_orient_compass =   0.05;  // weight of IMS compass angle relative to others (1.0 - all equal)
     public boolean  wser_orient_save =      true;  // Save per-series infinity adjustments data as CSV file
     public boolean  wser_orient_series =    true;  // Save per-series orientations and summary
     public boolean  wser_orient_apply =     false; // Apply per-series infinity correction
@@ -3261,6 +3262,8 @@ min_str_neib_fpn	0.35
 				"Per-series orientation dcamera-to-imu adjust enable.");
 		gd.addNumericField("Translations weight",                     this.wser_orient_transl, 5,7,"",
 				"Weight of translations for orientation (remaining - orintation).");
+		gd.addNumericField("Compass weight",                          this.wser_orient_compass, 5,7,"",
+				"Reduce weight of the IMS compass angle compare to the two others, as IMS compass is less accurate. Use translation instead.");
 		gd.addCheckbox ("Save scene sequence orientation correction",     this.wser_orient_save,
 				"Save scene sequence orintation adjustments data as CSV file.");
 		gd.addCheckbox ("Save per-series orientation correction",     this.wser_orient_series,
@@ -4626,6 +4629,7 @@ min_str_neib_fpn	0.35
 		this.wser_inf_thresh =          gd.getNextNumber();
 		this.wser_orient_en =           gd.getNextBoolean();
 		this.wser_orient_transl =       gd.getNextNumber();
+		this.wser_orient_compass =      gd.getNextNumber();
 		this.wser_orient_save =         gd.getNextBoolean();
 		this.wser_orient_series =       gd.getNextBoolean();
 		this.wser_orient_apply =        gd.getNextBoolean();
@@ -5872,6 +5876,7 @@ min_str_neib_fpn	0.35
 		properties.setProperty(prefix+"wser_inf_thresh",      this.wser_inf_thresh+"");     // double
 		properties.setProperty(prefix+"wser_orient_en",       this.wser_orient_en+"");      // boolean
 		properties.setProperty(prefix+"wser_orient_transl",   this.wser_orient_transl+"");  // double
+		properties.setProperty(prefix+"wser_orient_compass",  this.wser_orient_compass+""); // double
 		properties.setProperty(prefix+"wser_orient_save",     this.wser_orient_save+"");    // boolean
 		properties.setProperty(prefix+"wser_orient_series",   this.wser_orient_series+"");  // boolean
 		properties.setProperty(prefix+"wser_orient_apply",    this.wser_orient_apply+"");   // boolean
@@ -7102,6 +7107,7 @@ min_str_neib_fpn	0.35
 		if (properties.getProperty(prefix+"wser_inf_thresh")!=null)      this.wser_inf_thresh=Double.parseDouble  (properties.getProperty(prefix+"wser_inf_thresh"));
 		if (properties.getProperty(prefix+"wser_orient_en")!=null)       this.wser_orient_en=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_en"));
 		if (properties.getProperty(prefix+"wser_orient_transl")!=null)   this.wser_orient_transl=Double.parseDouble  (properties.getProperty(prefix+"wser_orient_transl"));
+		if (properties.getProperty(prefix+"wser_orient_compass")!=null)  this.wser_orient_compass=Double.parseDouble  (properties.getProperty(prefix+"wser_orient_compass"));
 		if (properties.getProperty(prefix+"wser_orient_save")!=null)     this.wser_orient_save=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_save"));
 		if (properties.getProperty(prefix+"wser_orient_series")!=null)   this.wser_orient_series=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_series"));
 		if (properties.getProperty(prefix+"wser_orient_apply")!=null)    this.wser_orient_apply=Boolean.parseBoolean(properties.getProperty(prefix+"wser_orient_apply"));
@@ -8311,6 +8317,7 @@ min_str_neib_fpn	0.35
 		imp.wser_inf_thresh =       this.wser_inf_thresh;
 		imp.wser_orient_en =        this.wser_orient_en;
 		imp.wser_orient_transl =    this.wser_orient_transl;
+		imp.wser_orient_compass =   this.wser_orient_compass;
 		imp.wser_orient_save =      this.wser_orient_save;
 		imp.wser_orient_series =    this.wser_orient_series;
 		imp.wser_orient_apply =     this.wser_orient_apply;