preparing for adding pairwise matching after single-connected are

subject to LMA

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

@@ -850,6 +850,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 			addButton("Warp pair",  panelLWIRWorld, color_process);
 			addButton("Read Tiff",  panelLWIRWorld, color_process);
 			addButton("Test video",  panelLWIRWorld, color_process);
+			addButton("Ortho",         panelLWIRWorld, color_process);
 			addButton("Ortho Pairs",  panelLWIRWorld, color_process);
 			addButton("Manual pair",  panelLWIRWorld, color_process);
 			addButton("Extract Objects",  panelLWIRWorld, color_process);
@@ -5718,7 +5719,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 				e.printStackTrace();
 			}
 //			ComboMatch.testReadTiff();
-		} else if (label.equals("Ortho Pairs") || label.equals("Extract Objects") || label.equals("Manual pair")) {
+		} else if (label.equals("Ortho") || label.equals("Ortho Pairs") || label.equals("Extract Objects") || label.equals("Manual pair")) {
 			DEBUG_LEVEL = MASTER_DEBUG_LEVEL;
 			EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
 			if (GPU_TILE_PROCESSOR == null) {
@@ -5739,6 +5740,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 			ComboMatch.openTestPairGps(
 					CLT_PARAMETERS,     // CLTParameters    clt_parameters,
 					GPU_TILE_PROCESSOR,
+					label.equals("Ortho"),// 			 boolean          dflt_options,
 					label.equals("Extract Objects"), // boolean    extract_mines,
 					label.equals("Manual pair"), // boolean          manual_pair,
 					DEBUG_LEVEL);

src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java

@@ -236,7 +236,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		if (undefined_only) {
 			String [] matches = pairwise_matches.keySet().toArray(new String[0]);
 			for (String match:matches) {
-				if (!getMatch(match).isDefined()) {
+				PairwiseOrthoMatch pm = getMatch(match); 
+				if ((pm == null) || !pm.isDefined()) {
 					unsetMatch(match);
 				}
 			}
@@ -483,7 +484,14 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
+		try {
 			lla = ElphelTiffReader.getLLA(imp_prop);
+		} catch (NullPointerException e) {
+			System.out.println("No GPS data in "+path);
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+
 		dt = ElphelTiffReader.getLocalDateTime(imp_prop);
 		vert_meters =      ElphelTiffReader.getXYOffsetMeters(imp_prop);
 		orig_pix_meters = ElphelTiffReader.getPixelSize(imp_prop)[0];

src/main/java/com/elphel/imagej/orthomosaic/PairwiseOrthoMatch.java

@@ -16,6 +16,7 @@ public class PairwiseOrthoMatch implements Serializable {
 	public transient int []      nxy = null; // not saved, just to communicate for logging
 	public transient double      overlap = 0.0;
 	public transient double []   equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]
+	public transient boolean     ok = false;
 
 //	public PairwiseOrthoMatch() {}
 	public double getOverlap() {
@@ -75,6 +76,7 @@ public class PairwiseOrthoMatch implements Serializable {
 			pom.nxy = nxy.clone();
 		}
 		pom.equalize1to0 = this.equalize1to0.clone();
+		pom.ok = this.ok;
 		return pom;
 	}
 	/**

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

@@ -4631,6 +4631,8 @@ public class OpticalFlow {
     	int min_num_interscene =             clt_parameters.imp.min_num_interscene; //  2; // make from parameters, should be >= 1
     	int min_num_orient =                 clt_parameters.imp.min_num_orient;    //  2; // make from parameters, should be >= 1
     	
+    	boolean sfm_only =                   clt_parameters.imp.sfm_only; // process only data with SfM
+    	
     	boolean export_images =              clt_parameters.imp.export_images;
     	boolean export_dsi_image =           clt_parameters.imp.export_ranges;
     	boolean debug_ranges =               clt_parameters.imp.debug_ranges && !batch_mode;
@@ -5171,10 +5173,17 @@ public class OpticalFlow {
 				System.out.println("BUG: saved reference scene "+quadCLTs[ref_index].getImageName()+
 						" is not really a reference");
 				System.out.println();
+				start_ref_pointers[0] = 0;
+				start_ref_pointers[1] = 0;
+				return quadCLTs[ref_index].getX3dTopDirectory(); //bailing out
 			} else 	if ((first_last_index [0] < 0) || (first_last_index [1] < 0)) {
 				System.out.println("BUG: saved reference scene "+quadCLTs[ref_index].getImageName()+
 						" does not have both first/last scenes defined: ["+first_last_index [0]+", "+first_last_index [1]+"]");
 				System.out.println();
+				start_ref_pointers[0] = 0;
+				start_ref_pointers[1] = 0;
+				return quadCLTs[ref_index].getX3dTopDirectory(); //bailing out
+				
 			} else {
 				earliest_scene = first_last_index[0];
 				last_index =  first_last_index[1];
@@ -5185,7 +5194,8 @@ public class OpticalFlow {
 		//		earliest_scene = getEarliestScene(quadCLTs);
 		// below ref_index is not necessary the last (fix all where it is supposed to be the last
 		ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection(); // only used in ml_export
-		while (!reuse_video && ((quadCLTs[ref_index].getNumOrient() < min_num_orient) || (quadCLTs[ref_index].getNumAccum() < min_num_interscene))) {
+		if (!reuse_video) {
+			while ((quadCLTs[ref_index].getNumOrient() < min_num_orient) || (quadCLTs[ref_index].getNumAccum() < min_num_interscene)) {
 				if (debugLevel > -3) {
 					System.out.println("quadCLTs["+ref_index+"].getNumOrient()="+quadCLTs[ref_index].getNumOrient()+
 							" of "+min_num_orient+", quadCLTs["+ref_index+"].getNumAccum()="+quadCLTs[ref_index].getNumAccum()+
@@ -5210,9 +5220,9 @@ public class OpticalFlow {
 										" is more than half-range. Reducing to "+num_avg_pairs);
 							}
 						}
-//					if (num_avg_pairs > (last_index - ref_index)) {
-//						num_avg_pairs = last_index - ref_index;
-//					}
+						//					if (num_avg_pairs > (last_index - ref_index)) {
+						//						num_avg_pairs = last_index - ref_index;
+						//					}
 						QuadCLT[][][] scenes_seq_pairs = new QuadCLT[3][num_avg_pairs][2];
 						int ref_index_mod1 = Math.max(ref_index, earliest_scene + num_avg_pairs - 1);
 						int ref_index_mod2 = Math.min(ref_index, last_index-num_avg_pairs + 1);
@@ -5228,6 +5238,7 @@ public class OpticalFlow {
 							scenes_seq_pairs[2][i][0] = quadCLTs[ref_index_mod2 + num_avg_pairs - 1 - i];
 							scenes_seq_pairs[2][i][1] = quadCLTs[earliest_scene + num_avg_pairs - 1 - i];
 						}
+
 						double [][] sfm_dsn = StructureFromMotion.sfmPairsSet(
 								clt_parameters,           // final CLTParameters          clt_parameters,
 								quadCLTs[ref_index],      // final QuadCLT                ref_scene,
@@ -5239,6 +5250,14 @@ public class OpticalFlow {
 							combo_dsn_final = sfm_dsn;
 							done_sfm = true;
 						}
+						if (!done_sfm && sfm_only) {
+							System.out.println("\nsfm_only is set and no SfM is available, skipping farther processing of this scene series.");
+							System.out.println("quadCLTs["+ref_index+"].getNumOrient()="+quadCLTs[ref_index].getNumOrient()+
+									" of "+min_num_orient+", quadCLTs["+ref_index+"].getNumAccum()="+quadCLTs[ref_index].getNumAccum()+
+									" of "+min_num_interscene+"\n");
+							break;
+						}
+						
 					} 
 					if (!done_sfm) { // first pass or sfm failed
 						boolean              compensate_dsi = true;
@@ -5400,46 +5419,10 @@ public class OpticalFlow {
 						if (debugLevel> -3) {
 							System.out.println("Egomotion table saved to "+ego_path);
 						}
-					/*
-					// reference to earliest
-					String  ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
-							quadCLTs[ref_index].getImageName()+
-							"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv"; 
-					Interscene.invertInitialOrientation(
-							clt_parameters,   // final CLTParameters          clt_parameters,
-							batch_mode,       // final boolean                batch_mode,
-							false,            // final boolean                readjust,
-							true,             // final int set_self,   // set record for the new reference to new reference
-							quadCLTs,         // final QuadCLT[]              quadCLTs, //
-							quadCLTs.length-1,// final int                    last_index,  // last to process (normally old reference)
-							earliest_scene,   // final int                    ref_index,  // new reference index (center)
-							ref_index,        // final int                    ref_old,    // original ref_index (normally == last_index)				
-							earliest_scene,   // final int                    earliest_index// first to process
-							debugLevel);      // final int                    debugLevel
-				    Interscene.generateEgomotionTable(
-				    		clt_parameters, // CLTParameters  clt_parameters,
-							quadCLTs, // QuadCLT []     quadCLTs,
-							earliest_scene,//            ref_indx,
-							earliest_scene, // int            earliest_scene,
-							ego_path_early, // String         path,
-							ego_comment); // String         comment);
-					if (debugLevel> -3) {
-						System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
-					}
-					*/
 					}				
 				}
 			}
-		/*
-		boolean calc_smooth_ground = clt_parameters.gsmth_enable;
-		if (calc_smooth_ground) {
-			double [] smooth_ground = quadCLTs[ref_index].getSmoothGround(clt_parameters, debugLevel);
-			quadCLTs[ref_index].setSmoothGround(
-					clt_parameters, // CLTParameters clt_parameters,
-					smooth_ground,  // double []     smooth_ground,
-					debugLevel);    // 			    		int debugLevel)
-		}
-		*/
+		} // if (!reuse_video) {
 		// 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

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

@@ -223,6 +223,9 @@ public class QuadCLTCPU {
     	enu_corr_metric = corr;
     }
     
+    public String toString() {
+    	return this.image_name;
+    }
      
     // find best rotation between IMU XYZ and camera XYZ
     /**

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

@@ -8636,7 +8636,7 @@ if (debugLevel > -100) return true; // temporarily !
 						updateStatus, // final boolean    updateStatus,
 						debugLevel+2); // final int        debugLevel)
 				if (model_directory == null) {
-					// TODO: Save continueation after failure not to retry each run
+					// TODO: Save continuation after failure not to retry each run
 					System.out.println("Failed to build sequence for series "+ref_index);
 					if (start_ref_pointers[0] < (min_num_scenes-1)) {
 						break;
@@ -8645,6 +8645,12 @@ if (debugLevel > -100) return true; // temporarily !
 					System.out.println("Continue from index "+ref_index);
 					continue; // and go to the to next scene sequence from the list
 				}
+				if ((start_ref_pointers[0]==0) && (start_ref_pointers[1]==0)) { // bail out for debug
+					System.out.println("\n ----- PROCESSING SCENE SEQUENCE "+nseq+" (last is "+(num_seq-1)+") ABORTED in "+
+							IJ.d2s(0.000000001*(System.nanoTime()-start_time_seq),3)+" sec ("+
+							IJ.d2s(0.000000001*(System.nanoTime()-start_time_all),3)+" sec from the overall start\n");
+					break;
+				}
 				first_in_series = false; // at least once success in this series
 				System.out.println("adding to video_sets_list start="+start_ref_pointers[0]+", end="+ref_index); // start_ref_pointers[1]);
 				for (int i= 0; i < video_list[0].length; i++) {

src/main/java/com/elphel/imagej/tileprocessor/sfm/StructureFromMotion.java

@@ -790,6 +790,76 @@ public class StructureFromMotion {
         return combo_dsn_final;
 	}
 
+	/**
+	 * Filter reference disparity from NaN (average immediate neighbors), dips and bumps
+	 * @param disp_in   
+	 * @param max_dip  maximal dip relative to lowest neighbor (0 - any)
+	 * @param max_bump maximal bump relative to highest neighbor (0 - any)
+	 * @param filter_nan replace NaN by neighbors average
+	 * @param tilesX
+	 * @return filtered disparity
+	 */
+	public static double [] filterRefDisparity(
+			final double [] disp_in,
+			final double    max_dip,
+			final double    max_bump,
+			final boolean   filter_nan,
+			final int       tilesX ) {
+		final double [] disp_out = disp_in.clone();
+		final int tilesY = disp_in.length / tilesX;
+		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					TileNeibs tn = new TileNeibs(tilesX,tilesY);
+					for (int nTile = ai.getAndIncrement(); nTile < disp_in.length; nTile = ai.getAndIncrement()) {
+						int navg = 0;
+						double s = 0;
+						if (Double.isNaN(disp_in[nTile])) {
+							for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
+								int tile1 = tn.getNeibIndex(nTile, dir);
+								if ((tile1 >=0) && !Double.isNaN(disp_in[tile1])){
+									s += disp_in[tile1];
+									navg++;
+								}
+							}
+							if (navg > 0) {
+								disp_out[nTile] = s/navg;
+							}
+						} else {
+							double mn = Double.NaN, mx = Double.NaN;
+							for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
+								int tile1 = tn.getNeibIndex(nTile, dir);
+								if (tile1 >=0){
+									double d =  disp_in[tile1];
+									if (!Double.isNaN(d)) {
+										if (Double.isNaN(mn)) {
+											mn = d;
+											mx = d;
+										} else {
+											mn = Math.min(mn, d);
+											mx = Math.max(mn, d);
+										}
+										s += d;
+										navg++;
+									}
+								}
+							}
+							if (navg > 0) {
+								if (    ((max_dip > 0) && (disp_in[nTile] < (mn - max_dip))) ||
+										((max_bump > 0) && (disp_in[nTile] > (mx + max_bump)))) {
+									disp_out[nTile] = s/navg;
+								}
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return disp_out;
+	}
 	
 	public static double [][] sfmPairsSet(
 			final CLTParameters          clt_parameters,
@@ -798,6 +868,7 @@ public class StructureFromMotion {
 			final double                 mb_max_gain,
 			final boolean                batch_mode,
 			final int                    debugLevel) {
+		
 		boolean       save_disp_seq =        clt_parameters.imp.sfm_save_seq; //true;
 		boolean       show_disp_seq =        clt_parameters.imp.sfm_show_seq &= !batch_mode; //true;
 		boolean       show_disp_corr_ind =   clt_parameters.imp.sfm_show_corr_ind &= !batch_mode; //false; //true;
@@ -836,6 +907,10 @@ public class StructureFromMotion {
 		final boolean fill_weak =            clt_parameters.imp.sfm_fill_weak; // false;
 		final boolean extrapolate =          clt_parameters.imp.sfm_extrapolate; // false;
 		
+		final double    max_dip =            clt_parameters.imp.sfm_max_dip; // 0.1;
+		final double    max_bump =           clt_parameters.imp.sfm_max_bump; // 0.5;
+		final boolean   filter_nan =         clt_parameters.imp.sfm_filter_nan; // true;
+		
 		final int tilesX =    ref_scene.getTileProcessor().getTilesX();
 		final int tilesY =    ref_scene.getTileProcessor().getTilesY();
 		
@@ -882,7 +957,7 @@ public class StructureFromMotion {
         		debugLevel);			
         double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
         if (use_combo_dsi) {
-        	interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
+        	interscene_ref_disparity = ds[0]; // .clone(); // use_lma_dsi ?
         	/* use conditioned!
         	if (use_lma_dsi) {
         		for (int i = 0; i < interscene_ref_disparity.length; i++) {
@@ -897,7 +972,15 @@ public class StructureFromMotion {
         if (interscene_ref_disparity == null) {
         	interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
         }
-        final double []   ref_disparity = interscene_ref_disparity;
+        boolean filter_ref = filter_nan || (max_dip > 0) || (max_bump > 0);
+        final double []   ref_disparity  = filter_ref ? 
+        		filterRefDisparity(
+        				interscene_ref_disparity, // final double [] disp_in,
+        				max_dip, // final double    max_dip,
+        				max_bump, // final double    max_bump,
+        				filter_nan,               // final boolean   filter_nan,        				 
+        				tilesX ): // final int       tilesX)
+        					interscene_ref_disparity.clone();
         double [][] disp_adj = (show_disp_seq || save_disp_seq)? (new double [num_readjust+2][]):null;
         if (disp_adj != null) {
         	disp_adj[0] = ref_disparity.clone();
@@ -1076,6 +1159,7 @@ public class StructureFromMotion {
 										if (sfmCorrCombo[nTile].corr_ind == null) { 
 											sfmCorrCombo[nTile].corr_ind= new double[2];
 										}
+										if (!Double.isNaN(sfmCorr[nTile].corr_ind[0]) && (sfmCorr[nTile].corr_ind[1] > 0)) {
 											sfmCorrCombo[nTile].corr_ind[0]+= sfmCorr[nTile].corr_ind[0] * sfmCorr[nTile].sfm_gain; 
 											sfmCorrCombo[nTile].corr_ind[1]+= sfmCorr[nTile].corr_ind[1] * sfmCorr[nTile].sfm_gain; 
 											sum_weights_ind[nTile]+=sfmCorr[nTile].sfm_gain;
@@ -1083,10 +1167,12 @@ public class StructureFromMotion {
 												sfmCorrCombo[nTile].sfm_gain = sfmCorr[nTile].sfm_gain; 
 											}
 										}
+									}
 									if (sfmCorr[nTile].corr_neib != null) {
 										if (sfmCorrCombo[nTile].corr_neib == null) { 
 											sfmCorrCombo[nTile].corr_neib= new double[2];
 										}
+										if (!Double.isNaN(sfmCorr[nTile].corr_neib[0]) && (sfmCorr[nTile].corr_neib[1] > 0)) {
 											sfmCorrCombo[nTile].corr_neib[0]+= sfmCorr[nTile].corr_neib[0] * sfmCorr[nTile].sfm_gain; 
 											sfmCorrCombo[nTile].corr_neib[1]+= sfmCorr[nTile].corr_neib[1] * sfmCorr[nTile].sfm_gain; 
 											sum_weights_neib[nTile]+=sfmCorr[nTile].sfm_gain;
@@ -1097,6 +1183,7 @@ public class StructureFromMotion {
 									}
         						}
         					}
+        				}
         			};
         		}		      
         		ImageDtt.startAndJoin(threads);