improving matching with MB, working version

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

@@ -24,6 +24,7 @@
 
 package com.elphel.imagej.tileprocessor;
 
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collections;
@@ -2147,8 +2148,15 @@ public class ErsCorrection extends GeometryCorrection {
 			xyz4[1] /= xyz4[2];
 			xyz4[2] =  1.0;
 		}
-		
-		Matrix dpscene_dxyz = dx_dpscene.inverse();
+		Matrix dpscene_dxyz = null;
+		try {
+			dpscene_dxyz = dx_dpscene.inverse();
+		} catch (Exception e) {
+			// TODO Auto-generated catch block
+			System.out.println("dx_dpscene is singular");
+			e.printStackTrace();
+			return null;
+		}
 		Matrix dpscene_dxyz_minus = dpscene_dxyz.times(-1.0); // negated to calculate /d{pX,pY,D) for the scene parameters
 		double[][] derivatives = new double[DP_NUM_PARS+1][]; // includes [0] - pXpYD vector
 		// scene pX, pY, Disparity

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

@@ -1331,7 +1331,7 @@ public class ImageDtt extends ImageDttCPU {
 			final double [][]         pXpYD,           // pXpYD for the reference scene 
 			final double [][]         fpn_offsets,     // null, or per-tile X,Y offset to be blanked
 			final double              fpn_radius,      // radius to be blanked around FPN offset center
-			final boolean             fpn_ignore_border, // only if fpn_mask != null - ignore tile if maximum touches fpn_mask			
+			final boolean             fpn_ignore_border, // NOT used! only if fpn_mask != null - ignore tile if maximum touches fpn_mask			
 			final double[][][]        motion_vectors,  // [tilesY*tilesX][][] -> [][num_sel_sensors+1 or 2][3]
 			final boolean             run_poly,        // polynomial max, if false - centroid
 			final boolean             use_partial,     // find motion vectors for individual pairs, false - for sum only

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

@@ -45,6 +45,13 @@ public class IntersceneLmaParameters {
 	public int        ilma_debug_level =       1;
 	public boolean    ilma_debug_adjust_series = false; // Debug images for series of pose and ers
 	public boolean    ilma_debug_invariant =     false; // Monitoring variations when restarting program (should be ilma_thread_invariant=true)
+	
+	public double     ilma_motion_filter =     3.5; // filter atr, xyz between consecutive scenes to get velocities
+	public double []  ilma_scale_xyz =         {1.0, 1.0, 1.0};
+	public double []  ilma_scale_atr =         {1.0, 1.0, 1.0}; // roll - minus?
+	public boolean    ilma_debug_ers =         false; // Debug ers-related operation
+	
+	
 
 	public IntersceneLmaParameters() {
 		ilma_lma_select[ErsCorrection.DP_DVAZ]=  true;
@@ -141,6 +148,18 @@ public class IntersceneLmaParameters {
 	    		"Generate debug images and text output to verify same results regardless of threads" );
 		gd.addNumericField("Debug level",                                 this.ilma_debug_level, 0,3,"",
 				"Debug level of interscene LMA operation.");
+		
+		gd.addMessage("Debug ERS-related operation");
+		gd.addNumericField("Motion filter (half number of scenes)",       this.ilma_motion_filter, 5,7,"",
+				"filter atr, xyz between consecutive scenes to get velocities.");
+		gd.addStringField ("XYZ scales", IntersceneMatchParameters.doublesToString(ilma_scale_xyz), 80,
+				"Scales for X, Y, and Z velocities, such as '1.0 1.0 1.0'.");
+		gd.addStringField ("ATR scales", IntersceneMatchParameters.doublesToString(ilma_scale_atr), 80,
+				"Scales for Azimuth, Tilt, and Roll velocities, such as '1.0 1.0 1.0'.");
+	    gd.addCheckbox    ("Debug ERS",                                  this.ilma_debug_ers,
+	    		"Debug ERS-related parameters during pose fitting." );
+		
+		
 	}
 	
 	public void dialogAnswers(GenericJTabbedDialog gd) {
@@ -164,6 +183,12 @@ public class IntersceneLmaParameters {
 		this.ilma_debug_adjust_series =           gd.getNextBoolean();
 		this.ilma_debug_invariant =               gd.getNextBoolean();
 		this.ilma_debug_level =             (int) gd.getNextNumber();
+		
+		this.ilma_motion_filter =                 gd.getNextNumber();
+		this.ilma_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
+		this.ilma_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
+		this.ilma_debug_ers =                    gd.getNextBoolean();
+		
 	}
 	public void setProperties(String prefix,Properties properties){
 		properties.setProperty(prefix+"ilma_thread_invariant",  this.ilma_thread_invariant+"");	
@@ -184,6 +209,11 @@ public class IntersceneLmaParameters {
 		properties.setProperty(prefix+"ilma_debug_adjust_series",this.ilma_debug_adjust_series+"");	
 		properties.setProperty(prefix+"ilma_debug_invariant",    this.ilma_debug_invariant+"");	
 		properties.setProperty(prefix+"ilma_debug_level",        this.ilma_debug_level+"");	
+		
+		properties.setProperty(prefix+"ilma_motion_filter",      this.ilma_motion_filter+"");
+		properties.setProperty(prefix+"ilma_scale_xyz", IntersceneMatchParameters.doublesToString(ilma_scale_xyz));
+		properties.setProperty(prefix+"ilma_scale_atr", IntersceneMatchParameters.doublesToString(ilma_scale_atr));
+		properties.setProperty(prefix+"ilma_debug_ers",          this.ilma_debug_ers+"");	
 	}
 	public void getProperties(String prefix,Properties properties){
 		if (properties.getProperty(prefix+"ilma_thread_invariant")!=null)  this.ilma_thread_invariant=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_thread_invariant"));
@@ -206,6 +236,20 @@ public class IntersceneLmaParameters {
 		if (properties.getProperty(prefix+"ilma_debug_adjust_series")!=null)this.ilma_debug_adjust_series=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_debug_adjust_series"));
 		if (properties.getProperty(prefix+"ilma_debug_invariant")!=null)    this.ilma_debug_invariant=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_debug_invariant"));
 		if (properties.getProperty(prefix+"ilma_debug_level")!=null)        this.ilma_debug_level=Integer.parseInt(properties.getProperty(prefix+"ilma_debug_level"));
+
+		if (properties.getProperty(prefix+"ilma_motion_filter")!=null)      this.ilma_motion_filter=Double.parseDouble(properties.getProperty(prefix+"ilma_motion_filter"));
+		if (properties.getProperty(prefix+"ilma_scale_xyz")!=null) {
+			this.ilma_scale_xyz= IntersceneMatchParameters.StringToDoubles(
+					properties.getProperty(prefix+"ilma_scale_xyz"),3);
+		}
+		if (properties.getProperty(prefix+"ilma_scale_atr")!=null) {
+			this.ilma_scale_atr= IntersceneMatchParameters.StringToDoubles(
+					properties.getProperty(prefix+"ilma_scale_atr"),3);
+		}
+	
+	
+		if (properties.getProperty(prefix+"ilma_debug_ers")!=null)          this.ilma_debug_ers=Boolean.parseBoolean(properties.getProperty(prefix+"ilma_debug_ers"));
+	
 	}
 	
 	@Override
@@ -227,6 +271,12 @@ public class IntersceneLmaParameters {
 		ilp.ilma_debug_adjust_series = this.ilma_debug_adjust_series;
 		ilp.ilma_debug_invariant =     this.ilma_debug_invariant;
 		ilp.ilma_debug_level =         this.ilma_debug_level;
+		
+		ilp.ilma_motion_filter =       this.ilma_motion_filter;
+		ilp.ilma_scale_xyz =           this.ilma_scale_xyz.clone();
+		ilp.ilma_scale_atr =           this.ilma_scale_atr.clone();
+		ilp.ilma_debug_ers =           this.ilma_debug_ers;
+		
 		return ilp;
 	}	
 }

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

@@ -248,6 +248,7 @@ public class IntersceneMatchParameters {
 	public  boolean mb_en =                 true;
 	public  double  mb_tau =                0.008; // time constant, sec
 	public  double  mb_max_gain =           5.0;   // motion blur maximal gain (if more - move second point more than a pixel
+	public  double  mb_max_gain_inter =     2.0;   // same for interscene correlation for pose adjustment
 	
 	
 	public  boolean stereo_merge =       true;
@@ -732,6 +733,8 @@ public class IntersceneMatchParameters {
 				"Sensor exponential decay in seconds.");
 		gd.addNumericField("Maximal gain",                           this.mb_max_gain, 5,7,"x",
 				"Maximal gain for motion blur correction (if needed more for 1 pixel, increase offset).");
+		gd.addNumericField("Maximal gain pose",                      this.mb_max_gain_inter, 5,7,"x",
+				"Maximal gain for motion blur correction during interscene correlation (if needed more for 1 pixel, increase offset).");
 		
 		gd.addTab("Stereo/Video","Parameters for stereo video generation");
 		gd.addMessage  ("Stereo");
@@ -1016,7 +1019,7 @@ public class IntersceneMatchParameters {
 		this.mb_en =                    gd.getNextBoolean();
 		this.mb_tau =                   gd.getNextNumber();    
 		this.mb_max_gain =              gd.getNextNumber();    
-		
+		this.mb_max_gain_inter =        gd.getNextNumber();    
 		if (stereo_views.length > 0) {
 			int i =                           gd.getNextChoiceIndex();
 			if (i > 0) {
@@ -1296,7 +1299,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"mb_en",                this.mb_en+"");               // boolean
 		properties.setProperty(prefix+"mb_tau",               this.mb_tau+"");              // double
 		properties.setProperty(prefix+"mb_max_gain",          this.mb_max_gain+"");         // double
-
+		properties.setProperty(prefix+"mb_max_gain_inter",    this.mb_max_gain_inter+"");   // double
 		properties.setProperty(prefix+"stereo_merge",         this.stereo_merge+"");        // boolean
 		properties.setProperty(prefix+"stereo_gap",           this.stereo_gap+"");          // int
 		properties.setProperty(prefix+"stereo_intereye",      this.stereo_intereye+"");     // double
@@ -1531,6 +1534,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"mb_en")!=null)                this.mb_en=Boolean.parseBoolean(properties.getProperty(prefix+"mb_en"));
 		if (properties.getProperty(prefix+"mb_tau")!=null)               this.mb_tau=Double.parseDouble(properties.getProperty(prefix+"mb_tau"));
 		if (properties.getProperty(prefix+"mb_max_gain")!=null)          this.mb_max_gain=Double.parseDouble(properties.getProperty(prefix+"mb_max_gain"));
+		if (properties.getProperty(prefix+"mb_max_gain_inter")!=null)    this.mb_max_gain_inter=Double.parseDouble(properties.getProperty(prefix+"mb_max_gain_inter"));
 		
 		if (properties.getProperty(prefix+"stereo_merge")!=null)         this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
 		if (properties.getProperty(prefix+"stereo_gap")!=null)           this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
@@ -1782,6 +1786,7 @@ public class IntersceneMatchParameters {
 		imp.mb_en =                 this.mb_en;
 		imp.mb_tau =                this.mb_tau;
 		imp.mb_max_gain =           this.mb_max_gain;
+		imp.mb_max_gain_inter =     this.mb_max_gain_inter;
 
 		imp.stereo_merge          = this.stereo_merge;
 		imp.stereo_gap            = this.stereo_gap;

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

@@ -1277,6 +1277,28 @@ public class QuadCLT extends QuadCLTCPU {
 				debugLevel);
 	}	
 	
+	/**
+	 * Making scene_xyz==null or scene_atr==null to use uniform grid (for rendering raw images)
+	 * @param sensor_mask
+	 * @param merge_channels
+	 * @param full_woi_in
+	 * @param clt_parameters
+	 * @param disparity_ref
+	 * @param mb_tau
+	 * @param mb_max_gain
+	 * @param mb_vectors
+	 * @param scene_xyz
+	 * @param scene_atr
+	 * @param scene
+	 * @param ref_scene
+	 * @param toRGB
+	 * @param show_nan
+	 * @param suffix
+	 * @param threadsMax
+	 * @param debugLevel
+	 * @return
+	 */
+	
 	public static ImagePlus renderGPUFromDSI(
 			final int         sensor_mask,
 			final boolean     merge_channels,
@@ -1288,8 +1310,8 @@ public class QuadCLT extends QuadCLTCPU {
 			double            mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
 			double [][]       mb_vectors,  // now [2][ntiles];
 			
-			final double []   scene_xyz, // camera center in world coordinates
-			final double []   scene_atr, // camera orientation relative to world frame
+			double []         scene_xyz, // camera center in world coordinates. If null - no shift, no ers
+			double []         scene_atr, // camera orientation relative to world frame
 			final QuadCLT     scene,
 			final QuadCLT     ref_scene, // now - may be null - for testing if scene is rotated ref
 			final boolean     toRGB,
@@ -1297,14 +1319,28 @@ public class QuadCLT extends QuadCLTCPU {
 			String            suffix,
 			int               threadsMax,
 			final int         debugLevel){
-		double [][] pXpYD =OpticalFlow.transformToScenePxPyD( // now should work with offset ref_scene
-				full_woi_in,   // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
-				disparity_ref, // final double []   disparity_ref, // invalid tiles - NaN in disparity
-				scene_xyz,     // final double []   scene_xyz, // camera center in world coordinates
-				scene_atr,     // final double []   scene_atr, // camera orientation relative to world frame
-				scene,         // final QuadCLT     scene_QuadClt,
-				ref_scene,     // final QuadCLT     reference_QuadClt, // now - may be null - for testing if scene is rotated ref
-				threadsMax);   // int               threadsMax)
+		double [][] pXpYD;
+		if ((scene_xyz == null) || (scene_atr == null)) {
+			scene_xyz = new double[3];
+			scene_atr = new double[3];
+			pXpYD=OpticalFlow.transformToScenePxPyD( // now should work with offset ref_scene
+					full_woi_in,   // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
+					disparity_ref, // final double []   disparity_ref, // invalid tiles - NaN in disparity
+					scene_xyz,     // final double []   scene_xyz, // camera center in world coordinates
+					scene_atr,     // final double []   scene_atr, // camera orientation relative to world frame
+					ref_scene,     // final QuadCLT     scene_QuadClt,
+					ref_scene,     // final QuadCLT     reference_QuadClt, // now - may be null - for testing if scene is rotated ref
+					threadsMax);   // int               threadsMax)
+		} else {
+			pXpYD=OpticalFlow.transformToScenePxPyD( // now should work with offset ref_scene
+					full_woi_in,   // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
+					disparity_ref, // final double []   disparity_ref, // invalid tiles - NaN in disparity
+					scene_xyz,     // final double []   scene_xyz, // camera center in world coordinates
+					scene_atr,     // final double []   scene_atr, // camera orientation relative to world frame
+					scene,         // final QuadCLT     scene_QuadClt,
+					ref_scene,     // final QuadCLT     reference_QuadClt, // now - may be null - for testing if scene is rotated ref
+					threadsMax);   // int               threadsMax)
+		}
 		int rendered_width = scene.getErsCorrection().getSensorWH()[0];
 		if (full_woi_in != null) {
 			rendered_width = full_woi_in.width * GPUTileProcessor.DTT_SIZE;
@@ -1429,7 +1465,7 @@ public class QuadCLT extends QuadCLTCPU {
 	
 	/**
 	 * Prepare 16x16 texture tiles using GPU from disparity reference. Includes motion blur correction
-	 * Does not use scene.saveQuadClt() to re-load new set of Bayer images to the GPU -- should be done by callaer
+	 * Does not use scene.saveQuadClt() to re-load new set of Bayer images to the GPU -- should be done by caller
 	 * @param clt_parameters processing parameters
 	 * @param disparity_ref disparity scene reference
 	 * @param mb_tau     	thermal constant

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

@@ -921,10 +921,14 @@ public class TexturedModel {
 						scene_ers_atr_dt); // double []    ers_atr_dt)(ers_scene_original_xyz_dt);
 			}
 			double [][] dxyzatr_dt = null;
+			// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
 			if (mb_en) { // all scenes have the same name/path
-				dxyzatr_dt = OpticalFlow.getVelocities( // looks at previous/next scene poses
-						scenes,   // QuadCLT []     quadCLTs,
-						nscene);  // int            nscene)
+//				dxyzatr_dt = OpticalFlow.getVelocities( // looks at previous/next scene poses
+//						scenes,   // QuadCLT []     quadCLTs,
+//						nscene);  // int            nscene)
+				dxyzatr_dt = new double[][] { // for all, including ref
+					scenes[nscene].getErsCorrection().getErsXYZ_dt(),
+					scenes[nscene].getErsCorrection().getErsATR_dt()};				
 			}
 			scenes[nscene].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU)
 			for (int nslice = 0; nslice < num_slices; nslice++) { // prepare and measure textures for each combo textures