Tested other reference (FPN mitigation) with initial orientation

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

@@ -23,6 +23,7 @@
  */
 package com.elphel.imagej.tileprocessor;
 
+import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.concurrent.atomic.AtomicInteger;
 
@@ -427,7 +428,7 @@ public class Interscene {
 			}
 		}
 
-		boolean scale_ims_velocities = true;
+		boolean scale_ims_velocities = false; // true;
 		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
 				clt_parameters, // CLTParameters clt_parameters,
 				quadCLTs,       // QuadCLT[]     quadCLTs,
@@ -768,7 +769,7 @@ public class Interscene {
 		int tilesX =  quadCLTs[ref_index].getTileProcessor().getTilesX();
 		int tilesY =  quadCLTs[ref_index].getTileProcessor().getTilesY();
         int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
-		double min_offset =         0.0; // clt_parameters.imp.min_offset;
+		double min_offset =         clt_parameters.imp.min_offset; // 0.0; // 
 		double max_offset =         clt_parameters.imp.max_rel_offset * tilesX * tile_size;
 		double max_roll =           clt_parameters.imp.max_roll_deg*Math.PI/180.0;
 		double max_zoom_diff =      clt_parameters.imp.max_zoom_diff;
@@ -819,15 +820,15 @@ public class Interscene {
 		}
 		
 		double max_z_change = Double.NaN; // only applicable for drone images
-		if (max_zoom_diff > 0) { // ignore if set to
 		double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
+		if (max_zoom_diff > 0) { // ignore if set to
 			max_z_change = avg_z * max_zoom_diff;
 			if (debugLevel > -3) {
 				System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
 			}
 		}
 
-		boolean scale_ims_velocities = true;
+		boolean scale_ims_velocities = false; // true;
 		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
 				clt_parameters, // CLTParameters clt_parameters,
 				quadCLTs,       // QuadCLT[]     quadCLTs,
@@ -863,7 +864,13 @@ public class Interscene {
 		double  mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
 		TpTask[][]     tp_tasks_ref = new TpTask[2][];
 		double [][]    pXpYD_ref = new double [tilesX*tilesY][];
-		
+		boolean rectilinear = false;
+    	double  max_quad = 10; // use quad if center offset is less than this
+    	ArrayList<Integer> fpn_list = new ArrayList<Integer>();
+//		boolean [] need_fpn_mitigate = new boolean [quadCLTs.length];
+//		int        num_fpn_mitigate = 0;
+		double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
+		min_max[1] = max_offset;
 		for (int scene_index =  ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
 			if ((ref_index - scene_index) >= max_num_scenes){
 				earliest_scene = scene_index + 1;
@@ -892,8 +899,6 @@ public class Interscene {
 					initial_pose[1].clone()
 			};
 			// Refine with LMA
-			double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
-			min_max[1] = max_offset;
 			//boost_zoom_short
 			double max_z_change_scaled = max_z_change;
 			if ((ref_index - scene_index) < min_num_scenes) {
@@ -922,6 +927,31 @@ public class Interscene {
 							clt_parameters,
 							scenes_dxyzatr[scene_index])); // cam_dxyzatr));		
 // Trying new version with motion blur and single-setting of the reference frame
+    		double est_shift = quadCLTs[ref_index].estimateAverageShift(
+    				scenes_xyzatr[ref_index], // double [][] xyzatr0,
+    				scenes_xyzatr[scene_index],    // double [][] xyzatr1,
+    				avg_z,                    // double average_z,
+    				false,                    // boolean use_rot,
+    				rectilinear);                   // boolean rectilinear)
+    		if (est_shift < max_quad) {
+    			est_shift = quadCLTs[ref_index].estimateAverageShift(
+    					scenes_xyzatr[ref_index], // double [][] xyzatr0,
+    					scenes_xyzatr[scene_index],    // double [][] xyzatr1,
+    					avg_z,                    // double average_z,
+    					true,                    // boolean use_rot,
+    					rectilinear);                   // boolean rectilinear)
+    		}
+    		if (debugLevel > -4) {
+    			System.out.println("nscene="+scene_index+": est offset "+
+    					est_shift+"pix");
+    		}
+    		boolean adjust_OK = false;
+    		if (est_shift < min_max[0]) {
+    			fail_reason[0]=FAIL_REASON_MIN;
+    		} else {	
+    			/*
+fail_reason[0]==FAIL_REASON_MIN)
+    			 */
     			scenes_xyzatr[scene_index] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
     					clt_parameters,      // CLTParameters  clt_parameters,
     					use_lma_dsi,         // clt_parameters.imp.use_lma_dsi,
@@ -933,7 +963,7 @@ public class Interscene {
     					ref_index,           // int            ref_index,
     					tp_tasks_ref,        // TpTask[][]     tp_tasks_ref,  // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
     					// at set first scene to the GPU
-					pXpYD_ref,           // double [][]    pXpYD_ref,     // should be se or at least double [num_tiles][] if tp_tasks_ref[0] == null 			
+    					pXpYD_ref,           // double [][]    pXpYD_ref,     // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null 			
     					ref_index,           // int            nscene0,       // may be == ref_index
     					scene_index,         // int            nscene1,       // compares to nscene0
     					null,                // double []      ref_disparity, // null or alternative reference disparity
@@ -949,8 +979,8 @@ public class Interscene {
     					mb_tau,              // double         mb_tau,      // 0.008; // time constant, sec
     					mb_max_gain,         // double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
     					clt_parameters.imp.debug_level); // int            debugLevel);
-
-			boolean adjust_OK = scenes_xyzatr[scene_index] != null;
+    			adjust_OK = scenes_xyzatr[scene_index] != null;
+    		}
     		if (adjust_OK) { // check only for initial orientation, do not check on readjustments
 				if (Math.abs(scenes_xyzatr[scene_index][1][2]) > max_roll) {
 					fail_reason[0] = FAIL_REASON_ROLL;
@@ -975,8 +1005,9 @@ public class Interscene {
 					System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
 							" ("+getFailReason(fail_reason[0])+")");
 					if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
+						fpn_list.add(scene_index);
 						if (fpn_skip) {
-							System.out.println("fpn_skip is set, just using initial pose");
+							System.out.println("fpn_skip is set, just using initial pose, num_fpn_mitigate="+fpn_list.size());
 							scenes_xyzatr[scene_index] = initial_pose;
 							break handle_failure; // to next scene
 						} else {
@@ -1056,6 +1087,89 @@ public class Interscene {
 			}
 			return -1;
 		}
+//		fpn_list.add(ref_index); // just for testing
+		if (debugLevel > -4) {
+			System.out.println("num_fpn_mitigate= "+fpn_list.size());
+		}
+		boolean fpn_mitigate_en = true; // false; // true;
+		double fpn_mitigate_dist = 10.0; // 40.0; // 5.0 ; // try to find at least that far from the  the one needed for mitigation
+		if (fpn_mitigate_en && !fpn_list.isEmpty()) {
+			int [][] fpn_pairs = getFPNPairs(
+					fpn_list,          // ArrayList<Integer> fpn_list,
+					fpn_mitigate_dist, // double             fpn_mitigate_dist,
+					rectilinear,       // boolean            rectilinear,
+					quadCLTs,          // QuadCLT []         quadCLTs,
+					scenes_xyzatr,     // double [][][]      scenes_xyzatr,
+					avg_z,             // double             avg_z,
+					ref_index,         // int                ref_index, // >= earliest_scene 
+					earliest_scene);   // int                earliest_scene)
+			for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
+				if (debugLevel > -4) {
+					System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
+							" being too close to reference "+ref_index+
+							", using scene "+fpn_pairs[ipair][1]+" as a reference");
+				}
+				TpTask[][]     tp_tasks_rel_ref = new TpTask[2][];
+    			double [][] rel_xyzatr = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
+    					clt_parameters,      // CLTParameters  clt_parameters,
+    					use_lma_dsi,         // clt_parameters.imp.use_lma_dsi,
+    					false,               //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
+    					true,                // boolean        disable_ers,
+    					min_max,             // double []      min_max,       // null or pair of minimal and maximal offsets
+    					fail_reason,         // int []         fail_reason,   // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
+    					quadCLTs,            // QuadCLT []     quadCLTs,
+    					ref_index,           // int            ref_index,
+    					tp_tasks_rel_ref,    // TpTask[][]     tp_tasks_ref,  // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
+    					// at set first scene to the GPU
+    					pXpYD_ref,           // double [][]    pXpYD_ref,     // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null 			
+    					fpn_pairs[ipair][1], // ref_index,           // int            nscene0,       // may be == ref_index
+    					fpn_pairs[ipair][0],         // int            nscene1,       // compares to nscene0
+    					null,                // double []      ref_disparity, // null or alternative reference disparity
+    					reliable_ref,        //boolean []      reliable_ref, // null or bitmask of reliable reference tiles
+    					scenes_xyzatr[fpn_pairs[ipair][1]], // double [][]    scene0_xyzatr,,
+    					scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose,        // double [][]    scene1_xyzatr,
+    					scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose,        // double []      scene1_xyzatr_pull, // if both are not null, specify target values to pull to 
+    					clt_parameters.ilp.ilma_lma_select, // boolean[]      param_select,
+    					reg_weights,         // double []      param_regweights,
+    					lma_rms,             // double []      rms_out, // null or double [2]
+    					clt_parameters.imp.max_rms, // double         max_rms,
+    					mb_en,               // boolean        mb_en,
+    					mb_tau,              // double         mb_tau,      // 0.008; // time constant, sec
+    					mb_max_gain,         // double         mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
+    					clt_parameters.imp.debug_level); // int            debugLevel);
+    			boolean adjust_OK = rel_xyzatr != null;
+				if (!adjust_OK) {
+					System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][1]+
+							" referenced from scene "+fpn_pairs[ipair][0]+". Reason = "+fail_reason[0]+
+							" ("+getFailReason(fail_reason[0])+")");
+				} else {
+					scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr;
+///					scenes_xyzatr[fpn_pairs[ipair][0]] = ErsCorrection.combineXYZATR(
+///							rel_xyzatr,                          // new
+///							scenes_xyzatr[fpn_pairs[ipair][1]]); // reference for this
+					double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
+							clt_parameters,
+							quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().getErsXYZATR_dt(
+									));
+					ers_reference.addScene(quadCLTs[fpn_pairs[ipair][0]].getImageName(),
+							scenes_xyzatr[fpn_pairs[ipair][0]][0],
+							scenes_xyzatr[fpn_pairs[ipair][0]][1],
+							adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),		
+							adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()		
+							);
+				    if (lma_rms[0] > maximal_series_rms) {
+				        maximal_series_rms = lma_rms[0];
+				    }
+
+					if (debugLevel > -3) {
+						System.out.println("Pass multi scene "+fpn_pairs[ipair][0]+" (of "+ quadCLTs.length+") "+
+								quadCLTs[fpn_pairs[ipair][1]].getImageName() + "/" + quadCLTs[fpn_pairs[ipair][0]].getImageName()+
+								" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
+					}
+				}
+			}
+		}
+		
 		if (earliest_scene > 0) {
 			System.out.println("setInitialOrientationsIms(): Not all scenes matched, earliest useful scene = "+earliest_scene+
 					" (total number of scenes = "+(ref_index - earliest_scene + 1)+
@@ -1072,6 +1186,7 @@ public class Interscene {
         		ref_index,       // int            end1_scene,
         		scenes_xyzatr,   // double [][][]  scenes_xyzatr,
         		half_run_range); // double         half_run_range
+        /*
     	quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
     			dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double []    ers_xyz_dt,
     			dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
@@ -1083,7 +1198,7 @@ public class Interscene {
 					dxyzatr_dt[scene_index][1] // ers_scene.getErsATR_dt()		
 					);
 		}		
-		
+		*/
 		quadCLTs[ref_index].set_orient(1); // first orientation
 		quadCLTs[ref_index].set_accum(0);  // reset accumulations ("build_interscene") number
 		quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)  // null pointer
@@ -1093,6 +1208,58 @@ public class Interscene {
 	}
 
 
+	public static int [][] getFPNPairs(
+			ArrayList<Integer> fpn_list,
+			double             fpn_mitigate_dist,
+			boolean            rectilinear,
+			QuadCLT []         quadCLTs,
+			double [][][]      scenes_xyzatr,
+			double             avg_z,
+			int                ref_index, // >= earliest_scene 
+			int                earliest_scene){
+		int [][] fpn_pairs = new int [fpn_list.size()][2];
+//		int [] fpn_ref_scenes = new int [quadCLTs.length];
+		boolean [] need_fpn_mitigate = new boolean [quadCLTs.length];
+		for (int i:fpn_list) {
+			need_fpn_mitigate[i] = true;
+		}
+//		Arrays.fill(fpn_ref_scenes, -1);
+		int fpn_index = ref_index; //  - 1;
+		for (int ipair = 0; ipair < fpn_pairs.length; ipair++)	{ 
+			// find first needed
+			for (;(fpn_index >= 0) && !need_fpn_mitigate[fpn_index]; fpn_index--);
+			// find closest but farther than fpn_mitigate_dist;
+			fpn_pairs[ipair][0] = fpn_index; 
+			fpn_pairs[ipair][1] = -1; 
+			double best_dist = Double.NaN;
+			for (int i = ref_index; i >= earliest_scene; i--) {
+				double fpn_dist = quadCLTs[ref_index].estimateAverageShift(
+						scenes_xyzatr[i], // double [][] xyzatr0,
+						scenes_xyzatr[fpn_index],    // double [][] xyzatr1,
+						avg_z,                    // double average_z,
+						false,                    // boolean use_rot,
+						rectilinear);                   // boolean rectilinear)
+				if (fpn_dist < fpn_mitigate_dist) {
+					fpn_dist = quadCLTs[i].estimateAverageShift(
+							scenes_xyzatr[i], // double [][] xyzatr0,
+							scenes_xyzatr[fpn_index],    // double [][] xyzatr1,
+							avg_z,                    // double average_z,
+							true,                    // boolean use_rot,
+							rectilinear);                   // boolean rectilinear)
+				}
+				if (fpn_dist >= fpn_mitigate_dist) {
+					if (!(fpn_dist >= best_dist)) { // Double.NaN OK
+						best_dist = fpn_dist;
+						fpn_pairs[ipair][1] = i;
+						need_fpn_mitigate[fpn_pairs[ipair][0]] = false;
+					}
+				}
+			}
+		}
+		return fpn_pairs;
+	}
+	
+	
 	
 	public static int setInitialOrientations(
 			final CLTParameters          clt_parameters,

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

@@ -654,8 +654,8 @@ public class QuadCLTCPU {
     			{0.25*wh[0],0.75*wh[1]},
     			{0.75*wh[0],0.75*wh[1]}};
     	}
-    	double s2 = 0.0;
-    	for (double [] xy:xy_pairs) {
+    	double s2 = 0.0, s0=0.0;
+    	for (double [] xy:xy_pairs) if (xy != null){
     		double [] pXpYD = ers.getImageCoordinatesERS(
     				null, // QuadCLT cameraQuadCLT, // camera station that got image to be to be matched 
     				xy[0], // double px,                // pixel coordinate X in the reference view
@@ -668,11 +668,16 @@ public class QuadCLTCPU {
     				xyzatr1[0], // double [] camera_xyz,     // camera center in world coordinates
     				xyzatr1[1], // double [] camera_atr,     // camera orientation relative to world frame
     				OpticalFlow.LINE_ERR); // double    line_err);       // threshold error in scan lines (1.0)
-    		double dx = pXpYD[0]-xy[0];
+    		if (pXpYD != null) {
+    			double dx = pXpYD[0]-xy[0]; // null pointer
     			double dy = pXpYD[1]-xy[1];
     			s2 += dx*dx+dy*dy;
+    			s0+=1.0;
+    		} else {
+    			continue;
+    		}
     	}
-    	double offs_avg = Math.sqrt(s2/xy_pairs.length);
+    	double offs_avg = Math.sqrt(s2/s0);
     	return offs_avg;
     }