Rendered with the ground planes

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

@@ -762,11 +762,18 @@ public class ErsCorrection extends GeometryCorrection {
 			double [] xyz,
 			double [] atr)
 	{
-		if (xyz != null) 	printAngle("       XYZ",xyz);
-		if (atr != null) 	printAngle("       ATR",atr);
+		printVectors(new double [][] {xyz, atr});
+//		if (xyz != null) 	printAngle("       XYZ",xyz);
+//		if (atr != null) 	printAngle("       ATR",atr);
 		printAngle(                    "ERS XYZ_dt",ers_wxyz_center_dt);
 		printAngle(                    "ERS ATR_dt",ers_watr_center_dt);
 	}
+	public static void printVectors(double[][] xyzatr) {
+		if (xyzatr != null) {
+			if (xyzatr[0] != null) printAngle("       XYZ",xyzatr[0]);
+			if (xyzatr[1] != null) printAngle("       ATR",xyzatr[1]);
+		}
+	}
 	
 	public void setupERS()
 	{

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

@@ -283,35 +283,38 @@ public class Interscene {
 			double [] quat_rms = new double [5];
 			double  scale_quat = clt_parameters.imp.imsq_scale_quat;
 			double  reg_weight = clt_parameters.imp.imsq_reg_weight;
-			double [] quatCorr = Interscene.getQuaternionCorrection(
-					clt_parameters,       // CLTParameters  clt_parameters,
-					scale_quat,           // double         scale_quat,
-					reg_weight,           // double         reg_weight,
-					quadCLTs,             // QuadCLT []     quadCLTs,
-					cent_index,           // 				ref_index,      // int            ref_index,
-					quadCLTs[cent_index], // QuadCLT        ref_scene, // may be one of quadCLTs or center_CLT
-					earliest_scene,       // int            earliest_scene,
-					latest_scene2,        // int            latest_scene,				
-					quat_rms,             // double []      rms // null or double[2];
-					null, // enu_corr,    //double []      enu_corr,
-					debugLevel);          // int            debugLevel
-			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]+")");
-					QuadCLTCPU.showQuatCorr(quatCorr,null); // enu_corr);
+			
+			boolean calc_quat_corr = clt_parameters.imp.segm_quat_corr; // calculate camera orientation correction from predicted by IMS
+			if (calc_quat_corr) {
+				double [] quatCorr = Interscene.getQuaternionCorrection(
+						clt_parameters,       // CLTParameters  clt_parameters,
+						scale_quat,           // double         scale_quat,
+						reg_weight,           // double         reg_weight,
+						quadCLTs,             // QuadCLT []     quadCLTs,
+						cent_index,           // 				ref_index,      // int            ref_index,
+						quadCLTs[cent_index], // QuadCLT        ref_scene, // may be one of quadCLTs or center_CLT
+						earliest_scene,       // int            earliest_scene,
+						latest_scene2,        // int            latest_scene,				
+						quat_rms,             // double []      rms // null or double[2];
+						null, // enu_corr,    //double []      enu_corr,
+						debugLevel);          // int            debugLevel
+				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]+")");
+						QuadCLTCPU.showQuatCorr(quatCorr,null); // enu_corr);
 
+					}
+					quadCLTs[cent_index].setQuatCorr(quatCorr);
 				}
-				quadCLTs[cent_index].setQuatCorr(quatCorr);
 			}
-
 			
 			
 			
 			
 			
-			// set pointers
+			// set pointers Check here - why reference is not set?
 			String cent_ts = quadCLTs[cent_index].getImageName();
 			quadCLTs[latest_scene2].setRefPointer(cent_ts); // write pointer to center scene to reference scene
 			quadCLTs[latest_scene2].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)  // null pointer

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

@@ -63,7 +63,7 @@ public class IntersceneLma {
 	private double  []        parameters_pull = null; // for regularization - error is proportional to difference between
 	//                                                   current vector and parameters_pull
 	private double  [][]      macrotile_centers = null;  // (will be used to pull for regularization)
-	private double            infinity_disparity = 0.1;  // treat lower as infinity
+	private double            infinity_disparity = 0.01;  // treat lower as infinity
 	private int               num_samples = 0;
 	///////////////////////////////////////////////////////////
 	// thread_invariant is needed for LMA, otherwise even with the same parameter vector RMS may be slightly different
@@ -1426,6 +1426,9 @@ public class IntersceneLma {
 			threads[ithread] = new Thread() {
 				public void run() {
 					for (int iMTile = ai.getAndIncrement(); iMTile < macrotile_centers.length; iMTile = ai.getAndIncrement()) {
+//						if (iMTile == 1473) {
+//							System.out.println("getFxDerivs(): iMTile="+iMTile);
+//						}
 						if ((macrotile_centers[iMTile]!=null) && (mb_mode || (weights[2*iMTile] > 0.0))){  // was: weights[iMTile]?
 							//infinity_disparity
 							boolean is_infinity = macrotile_centers[iMTile][2] < infinity_disparity;

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

@@ -48,6 +48,7 @@ import java.util.Calendar;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.Enumeration;
+import java.util.HashSet;
 import java.util.Properties;
 import java.util.Random;
 import java.util.Set;
@@ -233,7 +234,8 @@ public class QuadCLTCPU {
     // only reference scene has a pair of first/last scene in a sequence
     public String      timestamp_first =     null;
     public String      timestamp_last =      null;
-    
+    public String      timestamp_index =     null; // timestamp of the scene with index of all reference scenes in this sequence
+    public HashSet<String> ref_scenes =      null; // Set of timestamps of the reference scenes in this sequence    
     // combined clt for center view, only used in cuas mode
     public float []    center_clt =           null; // clt data (single accumulated channel)
     
@@ -244,8 +246,34 @@ public class QuadCLTCPU {
     @Deprecated
     public boolean     center_is_parent =     false; // when true - use image_path that discards current version
     public String      center_parent_dir =    null;
-    public ImagePlus   imp_center_average =  null;
-    public CorrectionFPN correctionFPN = null;
+    public ImagePlus   imp_center_average =   null;
+    public CorrectionFPN correctionFPN =      null;
+    
+    public GroundPlane ground_plane =         null;
+    
+    public HashSet<String> getRefScenes(){
+    	return ref_scenes;
+    }
+    
+    public void resetRefScenes() {
+    	ref_scenes = null;
+    }
+    
+    public void addRefScene(String ts) {
+    	if (ref_scenes == null) {
+    		ref_scenes = new HashSet<String>();
+    	}
+    	ref_scenes.add(ts);
+    }
+    
+    public GroundPlane getGroundPlane() {
+    	return ground_plane;
+    }
+    
+    public void setGroundPlane(GroundPlane gp) {
+    	this.ground_plane = gp;
+    }
+    
     
     public int getWidth() {
     	return tp.getTilesX()*tp.getTileSize();
@@ -1780,6 +1808,11 @@ public class QuadCLTCPU {
     	return did_ins_2.lla;
     }
 
+    public double [] getQEnu() {
+    	return did_ins_2.getQEnu();
+    }
+    
+    
     public LocalDateTime getLocalDateTime() {
     	return did_ins_2.getLocalDateTime();
     }
@@ -1820,7 +1853,8 @@ public class QuadCLTCPU {
     public int getReferenceIndex(QuadCLTCPU [] scenes) {
     	// saving existing data by providing a self-reference (is it needed)
  //   	if ((timestamp_reference == null) && (timestamp_first != null) && (timestamp_last != null)) {
- //   		timestamp_reference = getImageName();
+ //   		timestamp_reference = getImageName(); // remove ? OF5126:	int center_index =quadCLTs[last_index].getReferenceIndex(new QuadCLT[] {quadCLTs[last_index]}); // null); self, may return 0
+
  //   	}
     	if (timestamp_reference != null) {
     		if (scenes != null) {
@@ -1905,7 +1939,23 @@ public class QuadCLTCPU {
     			scene0.getImageName(),
     			scene1.getImageName());
     }
-
+    /**
+     * Get timestamp of the scene with the set of all reference scenes in this sequence
+     * @return timestamp of the index scene
+     */
+    public String getTimestampIndex() {
+    	return timestamp_index;
+    }
+    /**
+     * Set timestamp of the scene with the set of all reference scenes in this series
+     * @param timestamp
+     */
+    public void setTimestampIndex(String timestamp) {
+    	timestamp_index = timestamp;
+    }
+    
+    
+    
     public void inc_orient()        {num_orient++;}
     public void inc_accum()         {num_accum++;}
     public void set_orient(int num) {num_orient = num;}
@@ -1957,7 +2007,8 @@ public class QuadCLTCPU {
     		return getAverageZ(false);
     	}
     	double disp_avg = swd/sw;
-    	double z_avg = getGeometryCorrection().getZFromDisparity(disp_avg);
+    	// Using getDispInfinityRef() - defined only for reference scenes
+    	double z_avg = getGeometryCorrection().getZFromDisparity(disp_avg - getDispInfinityRef());
     	return z_avg;
     }
 
@@ -3087,7 +3138,7 @@ 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
-    	boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply 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
 
     	Did_ins_2 d2 = did_ins_2;
     	double [] cam_quat_enu =Imx5.quaternionImsToCam(
@@ -5636,7 +5687,7 @@ public class QuadCLTCPU {
     				disparity,
     				strength,
     				//    			disparity_lma,
-    				((rbg.length>2)?rbg[2]:rbg[0])}; // [2] - for old compatibility, [0] - new (2021)
+    				((rbg.length>2)?rbg[2]:rbg[0])}; // [2] - for old compatibility, [0] - new (2021) java.lang.ArrayIndexOutOfBoundsException: Index 0 out of bounds for length 0
     		} else {
     			this.dsrbg = new double[][] {
     				disparity,
@@ -7283,6 +7334,9 @@ public class QuadCLTCPU {
 		if (this.timestamp_first != null) {
 			properties.setProperty(prefix+"timestamp_first", this.timestamp_first);
 		}
+		if (this.timestamp_index != null) {
+			properties.setProperty(prefix+"timestamp_index", this.timestamp_index);
+		}
 		if (this.timestamp_last != null) {
 			properties.setProperty(prefix+"timestamp_last", this.timestamp_last);
 		}
@@ -7294,9 +7348,40 @@ public class QuadCLTCPU {
 		if (this.enu_corr_metric != null) {
 			properties.setProperty(prefix+"enu_corr_metric",  IntersceneMatchParameters.doublesToString(this.enu_corr_metric));		
 		}
+		
+		setPropertiesReferenceScenes(prefix, properties, ref_scenes);
+		
+		if (ground_plane != null) ground_plane.setProperties(prefix, properties);
 		return properties;
 	}
 
+	public static HashSet<String> getPropertiesReferenceScenes(String parent_prefix,Properties properties) {
+		String prefix = parent_prefix+"refscenes_";
+		HashSet<String> timestamps = new HashSet<String>();
+		for (Enumeration<?> e = properties.propertyNames(); e.hasMoreElements();) {
+			String key = (String) e.nextElement();
+			if (key.startsWith(prefix)) { // 
+				timestamps.add(key.substring(prefix.length()));
+			}
+		}
+		if (timestamps.isEmpty()) {
+			return null;
+		}
+		return timestamps;
+	}
+
+	public static void setPropertiesReferenceScenes(String parent_prefix,Properties properties, HashSet<String> ref_timestamps) {
+		String prefix = parent_prefix+"refscenes_";
+		if (ref_timestamps != null) {
+			for (String ts : ref_timestamps) {
+				properties.setProperty(prefix+ts,  ""); // set empty string - nothing to put there		
+			}
+		}
+	
+	}
+	
+	
+	
 
 	public void copyPropertiesFrom(Properties other_properties, String other_prefix, String this_prefix){ // save // not used in lwir
 		for (int n = 0; n < fine_corr.length; n++){
@@ -7469,12 +7554,18 @@ public class QuadCLTCPU {
 		if (properties.getProperty(prefix+"timestamp_last")!=null) {
 			this.timestamp_last= (String) properties.getProperty(prefix+"timestamp_last");
 		}
+		if (properties.getProperty(prefix+"timestamp_index")!=null) {
+			this.timestamp_index= (String) properties.getProperty(prefix+"timestamp_index");
+		}
 		if (properties.getProperty(prefix+"quat_corr")!=null) {
 	        this.quat_corr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"quat_corr"),4);
 		}
 		if (properties.getProperty(prefix+"enu_corr_metric")!=null) {
 	        this.enu_corr_metric= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"enu_corr_metric"),3);
 		}
+		this.ref_scenes = getPropertiesReferenceScenes(prefix,properties);
+		
+		this.ground_plane = GroundPlane.getGroundPlaneProperties(prefix, properties);
 	
 	}
 

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

@@ -43,6 +43,7 @@ import java.util.Collections;
 import java.util.Comparator;
 import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 import java.util.Properties;
 import java.util.Random;
 import java.util.concurrent.atomic.AtomicInteger;
@@ -8527,6 +8528,7 @@ if (debugLevel > -100) return true; // temporarily !
 		switch (cuas_proc_mode) {
 		case 0 :
 			String uas_sky_mask_path = quadCLT_main.correctionsParameters.getUasSkyMask();
+			QuadCLT[] index_scenes = {null,null}; // [0] - first for this series, [1] - overall first
 			for (int nseq = 0; nseq < num_seq; nseq++) {
 				long start_time_seq = System.nanoTime();
 				System.out.println("\nSTARTED PROCESSING SCENE SEQUENCE "+nseq+" (last is "+(num_seq-1)+")\n");
@@ -8560,7 +8562,7 @@ if (debugLevel > -100) return true; // temporarily !
 				int [][] widths_list = new int [1][];
 				int ref_index = -1; // -1 - last
 				int [] start_ref_pointers = new int[2]; //{earlist, reference} - reference may be center
-				boolean     first_in_series = true;
+				index_scenes[0] = null; // boolean     first_in_series = true; 
 				/*
 				// Process UAS logs 
 				UasLogReader uasLogReader = null;
@@ -8575,11 +8577,11 @@ if (debugLevel > -100) return true; // temporarily !
 				if (debugLevel > -4)  {
 					System.out.println("cuas_centers="+((cuas_centers != null)?("[\""+cuas_centers[0]+"\",\""+cuas_centers[1]+"\"]"):"null"));
 				}
-
 				while ((ref_index < 0) || ((ref_index + 1) >= min_num_scenes)) {
 					String model_directory = opticalFlow.buildSeries(
-							(pathFirstLast != null),   //boolean                                              batch_mode,
-							first_in_series, // boolean                     first_in_series,
+							(pathFirstLast != null),   // boolean                                              batch_mode,
+							index_scenes,              // QuadCLT[]                                            index_scenes,
+//							first_in_series, // boolean                     first_in_series,
 							quadCLT_main,              // QuadCLT                                              quadCLT_main, // tiles should be set
 							ref_index,                 // int                                                  ref_index, // -1 - last
 							clt_parameters,            // CLTParameters             clt_parameters,
@@ -8618,7 +8620,7 @@ if (debugLevel > -100) return true; // temporarily !
 								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
+//					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]);
 					//				if ((video_list != null) && (video_list.length>0) && (video_list[0] == null)) {
 					//					video_list[0]= new String[0];
@@ -8701,6 +8703,42 @@ if (debugLevel > -100) return true; // temporarily !
 				if (debugLevel > -4){
 					System.out.println("5. cuas_centers="+((cuas_centers != null)?("[\""+cuas_centers[0]+"\",\""+cuas_centers[1]+"\"]"):"null"));
 				}
+				// List ground planes in this series:
+				Map<String, GroundPlane> map_gp = GroundPlane.getGroundPlanes(
+						clt_parameters,      // CLTParameters        clt_parameters,
+						colorProcParameters, // ColorProcParameters  colorProcParameters,
+						index_scenes[0],     // QuadCLT              index_scene,
+						null,                // QuadCLT []           quadCLTs, // may be null or contain nulls
+						false, // true,                // boolean              print_rslt, 
+						debugLevel);         // int                  debugLevel){
+				if ((map_gp != null) && !map_gp.isEmpty()) {
+					String  gp_path =index_scenes[0].getX3dDirectory()+Prefs.getFileSeparator()+
+							index_scenes[0].getImageName()+
+							"-ground_planes.csv"; 
+					GroundPlane.csvGroundPlanes(
+							clt_parameters,  // CLTParameters            clt_parameters,
+							map_gp,          // Map<String, GroundPlane> planes_map,
+							index_scenes[0], // QuadCLT                  ref_scene,
+							gp_path,         // String                   path,
+							null,            // String                   comment)
+							debugLevel);     // int                      debugLevel) {
+					// add link to the index models
+					String index_model_directory = index_scenes[0].getX3dTopDirectory();
+
+					String linkedModelsDirectory = quadCLT_main.correctionsParameters.selectLinkedModelsDirectory(true,true);
+					if ((linkedModelsDirectory != null) && (linkedModelsDirectory.length() > 0)) {
+						Path pathAbsolute = Paths.get(index_model_directory);
+						Path pathBase = Paths.get(linkedModelsDirectory);
+						Path pathRelative = pathBase.relativize(pathAbsolute);
+						File linkDir = new File(linkedModelsDirectory);
+						linkDir.mkdirs();
+						File link = new File(linkDir, pathAbsolute.getFileName().toString()+"-index");
+						if (link.exists()) {
+							link.delete();
+						}
+						Files.createSymbolicLink(link.toPath(), pathRelative);
+					}
+				}
 			} // for (int nseq = 0; nseq < num_seq; nseq++) {
 
 			// combine videos if generated

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

@@ -2326,8 +2326,16 @@ public class StructureFromMotion {
 							continue;
 						}
 						dpXYddisp[nTile] = new double [2];
-						dpXYddisp[nTile][0] = last_jts[1][0][2*nTile + 0]-last_jts[0][0][2*nTile + 0];
-						dpXYddisp[nTile][1] = last_jts[1][0][2*nTile + 1]-last_jts[0][0][2*nTile + 1];
+						double dx = last_jts[1][0][2*nTile + 0]-last_jts[0][0][2*nTile + 0];
+						double dy = last_jts[1][0][2*nTile + 1]-last_jts[0][0][2*nTile + 1];
+						if ((dx*dx + dy*dy) == 0) {
+							System.out.println("getSfmDpxDpyDdisp(): *** BUG*** dx*dx + dy*dy == 0 for nTile="+nTile);
+							// This happens for infinity (disparity < small constant).
+							// Here it is OK to put a large number, because derivatives length will be in a denominator
+							 dx = 1.0;
+						}
+						dpXYddisp[nTile][0] = dx;
+						dpXYddisp[nTile][1] = dy;
 				        if (dbg_img != null) {
 				            dbg_img[0][nTile] = dpXYddisp[nTile][0]; 
 				            dbg_img[1][nTile] = dpXYddisp[nTile][1];