Cleaning up SfM

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

@@ -2506,6 +2506,20 @@ public class Correlation2d {
 	}
 
 	
+	/**
+	 * Find maximum of the 2d array projected on a specified vector using centroid.
+	 * On the first stage integer maximum is found,  then several refining operations
+	 * multiply vicinity of the 2d max by a window function and locate the center
+	 * of mass. In parallel (can be optimized) the center of mass is calculated for the
+	 * dot-product of the vector and 2d array coordinates  
+	 * @param data square data array (normally result of the 2d phase correlation) in linescan order
+	 * @param data_width size of the square (2 * transform_size - 1 for correlation)
+	 * @param radius window: 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
+	 * @param refine number of refines
+	 * @param direction_XY [x,y] components of the direction to project to
+	 * @param debug debug output if true
+	 * @return a pair of {projection_max, strength}
+	 */
 	public static double [] getMaxProjCm(
 			double []  data,
 			int        data_width,   //  = 2 * transform_size - 1;

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

@@ -153,36 +153,36 @@ public class ErsCorrection extends GeometryCorrection {
 	
 	// returned arrays have the zero element with coordinates, not derivatives
 	// Reference parameters
-	static final int DP_DPX =   0; // dw_dpX,  (pix)
-	static final int DP_DPY =   1; // dw_dpY   (pix)
-	static final int DP_DD =    2; // dw_dd,   (pix)
-	static final int DP_DVAZ =  3; // dw_dvaz, (rad/sec)
-	static final int DP_DVTL =  4; // dw_dvtl, (rad/sec)
-	static final int DP_DVRL =  5; // dw_dvrl, (rad/sec)
-	static final int DP_DVX =   6; // dw_dvx,  (m/s)
-	static final int DP_DVY =   7; // dw_dvy,  (m/s)
-	static final int DP_DVZ =   8; // dw_dvz,  (m/s)
-	static final int DP_DAZ =   9; // dw_daz,  (rad)
-	static final int DP_DTL =  10; // dw_dtl,  (rad)
-	static final int DP_DRL =  11; // dw_drl,  (rad)
-	static final int DP_DX =   12; // dw_dx,   (m)
-	static final int DP_DY =   13; // dw_dy,   (m)
-	static final int DP_DZ =   14; // dw_dz};  (m)
+	public static final int DP_DPX =   0; // dw_dpX,  (pix)
+	public static final int DP_DPY =   1; // dw_dpY   (pix)
+	public static final int DP_DD =    2; // dw_dd,   (pix)
+	public static final int DP_DVAZ =  3; // dw_dvaz, (rad/sec)
+	public static final int DP_DVTL =  4; // dw_dvtl, (rad/sec)
+	public static final int DP_DVRL =  5; // dw_dvrl, (rad/sec)
+	public static final int DP_DVX =   6; // dw_dvx,  (m/s)
+	public static final int DP_DVY =   7; // dw_dvy,  (m/s)
+	public static final int DP_DVZ =   8; // dw_dvz,  (m/s)
+	public static final int DP_DAZ =   9; // dw_daz,  (rad)
+	public static final int DP_DTL =  10; // dw_dtl,  (rad)
+	public static final int DP_DRL =  11; // dw_drl,  (rad)
+	public static final int DP_DX =   12; // dw_dx,   (m)
+	public static final int DP_DY =   13; // dw_dy,   (m)
+	public static final int DP_DZ =   14; // dw_dz};  (m)
 	// Scene parameters
-	static final int DP_DSVAZ =15; // dw_dvaz, (rad/sec)
-	static final int DP_DSVTL =16; // dw_dvtl, (rad/sec)
-	static final int DP_DSVRL =17; // dw_dvrl, (rad/sec)
-	static final int DP_DSVX = 18; // dw_dvx,  (m/s)
-	static final int DP_DSVY = 19; // dw_dvy,  (m/s)
-	static final int DP_DSVZ = 20; // dw_dvz,  (m/s)
-	static final int DP_DSAZ = 21; // dw_daz,  (rad)
-	static final int DP_DSTL = 22; // dw_dtl,  (rad)
-	static final int DP_DSRL = 23; // dw_drl,  (rad)
-	static final int DP_DSX =  24; // dw_dx,   (m)
-	static final int DP_DSY =  25; // dw_dy,   (m)
-	static final int DP_DSZ =  26; // dw_dz};  (m)
-	static final int DP_NUM_PARS = DP_DSZ+1;
-	static final int [] DP_ERS_INDICES=
+	public static final int DP_DSVAZ =15; // dw_dvaz, (rad/sec)
+	public static final int DP_DSVTL =16; // dw_dvtl, (rad/sec)
+	public static final int DP_DSVRL =17; // dw_dvrl, (rad/sec)
+	public static final int DP_DSVX = 18; // dw_dvx,  (m/s)
+	public static final int DP_DSVY = 19; // dw_dvy,  (m/s)
+	public static final int DP_DSVZ = 20; // dw_dvz,  (m/s)
+	public static final int DP_DSAZ = 21; // dw_daz,  (rad)
+	public static final int DP_DSTL = 22; // dw_dtl,  (rad)
+	public static final int DP_DSRL = 23; // dw_drl,  (rad)
+	public static final int DP_DSX =  24; // dw_dx,   (m)
+	public static final int DP_DSY =  25; // dw_dy,   (m)
+	public static final int DP_DSZ =  26; // dw_dz};  (m)
+	public static final int DP_NUM_PARS = DP_DSZ+1;
+	public static final int [] DP_ERS_INDICES=
 		{       DP_DVAZ,  DP_DVTL,  DP_DVRL,
 				DP_DVX,   DP_DVY,   DP_DVZ,
 				DP_DSVAZ, DP_DSVTL, DP_DSVRL,

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

@@ -55,6 +55,7 @@ import com.elphel.imagej.ims.Did_ins_2;
 import com.elphel.imagej.ims.Did_pimu;
 import com.elphel.imagej.ims.Imx5;
 import com.elphel.imagej.jp4.JP46_Reader_camera;
+import com.elphel.imagej.tileprocessor.sfm.StructureFromMotion;
 
 import Jama.Matrix;
 import ij.ImagePlus;
@@ -70,7 +71,7 @@ import ij.plugin.filter.GaussianBlur;
 
 public class OpticalFlow {
 	public static String [] COMBO_DSN_TITLES = {"disp", "strength","disp_lma","num_valid","change",
-			"disp_bg", "strength_bg","disp_lma_bg","change_bg","disp_fg","disp_bg_all","blue_sky"};
+			"disp_bg", "strength_bg","disp_lma_bg","change_bg","disp_fg","disp_bg_all","blue_sky","sfm_gain"};
 	public static int COMBO_DSN_INDX_DISP =        0; // cumulative disparity (from CM or POLY), FG
 	public static int COMBO_DSN_INDX_STRENGTH =    1; // strength, FG
 	public static int COMBO_DSN_INDX_LMA =         2; // masked copy from 0 - cumulative disparity
@@ -83,6 +84,7 @@ public class OpticalFlow {
 	public static int COMBO_DSN_INDX_DISP_FG =     9; // cumulative disparity (from CM or POLY), FG
 	public static int COMBO_DSN_INDX_DISP_BG_ALL =10; // cumulative BG disparity (Use FG where no BG is available)
 	public static int COMBO_DSN_INDX_BLUE_SKY =   11; // Detected featureless infinity (sky)
+	public static int COMBO_DSN_INDX_SFM_GAIN =   12;  // SfM disparity gain pixel/pixel
 	
 // move to Interscene class?	
 	// interscene adjustments failure reasons.
@@ -4884,32 +4886,32 @@ public class OpticalFlow {
 					if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
 						mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
 					}
-					
-					done_sfm = StructureFromMotion.sfmPair(
+					/*
+					done_sfm = StructureFromMotion.sfmPair_ref_debug(
 							clt_parameters,           // final CLTParameters          clt_parameters,
 							quadCLTs[ref_index],      // final QuadCLT                ref_scene,
 							quadCLTs[earliest_scene], // final QuadCLT                scene,
 							mb_max_gain,              // double                       mb_max_gain,
 							batch_mode,               // final boolean                batch_mode,
 							debugLevel);              // final int                    debugLevel)
+					*/
 					int      num_avg_pairs = 16; // number of scene pairs to average
 					QuadCLT[][] scenes_pairs = new QuadCLT[num_avg_pairs][2];
 					for (int i = 0; i < num_avg_pairs; i++) {
 						scenes_pairs[i][0] = quadCLTs[ref_index - 1 - i];
 						scenes_pairs[i][1] = quadCLTs[earliest_scene + num_avg_pairs - 1 - i];
 					}
-//					QuadCLT[]  scenes_pair = new QuadCLT[]{
-//							quadCLTs[ref_index - 1],
-//							quadCLTs[earliest_scene]};
-					combo_dsn_final = StructureFromMotion.sfmPair(
+					double [][] sfm_dsn = StructureFromMotion.sfmPair(
 							clt_parameters,           // final CLTParameters          clt_parameters,
 							quadCLTs[ref_index],      // final QuadCLT                ref_scene,
 							scenes_pairs,             // final QuadCLT [][]           scenes_pairs,
-//							scenes_pair,              // final QuadCLT []             scenes,
-//							num_avg_pairs,            // final int                    num_avg_pairs, // number of scene pairs to average
 							mb_max_gain,              // double                       mb_max_gain,
 							batch_mode,               // final boolean                batch_mode,
 							debugLevel);              // final int                    debugLevel)
+					if (sfm_dsn != null) {
+						combo_dsn_final = sfm_dsn;
+						done_sfm = true;
+					}
 				} 
 				if (!done_sfm) { // first pass or sfm failed
 					// should skip scenes w/o orientation 06/29/2022
@@ -11543,7 +11545,7 @@ public class OpticalFlow {
 	}
 	
 	
-	static double [][] conditionInitialDS(
+	public static double [][] conditionInitialDS(
 			boolean        use_conf,       // use configuration parameters, false - use following  
 			CLTParameters  clt_parameters,
 			double [][]    dls,

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

@@ -1521,6 +1521,10 @@ public class QuadCLTCPU {
 			this.dsi[is_aux?TwoQuadCLT.DSI_BLUE_SKY_AUX:TwoQuadCLT.DSI_BLUE_SKY_MAIN] =
 					combo_dsi[OpticalFlow.COMBO_DSN_INDX_BLUE_SKY];
 		}
+		if ((combo_dsi.length > OpticalFlow.COMBO_DSN_INDX_SFM_GAIN) && (combo_dsi[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] != null)) {
+			this.dsi[is_aux?TwoQuadCLT.DSI_SFM_GAIN_AUX:TwoQuadCLT.DSI_SFM_GAIN_MAIN] =
+					combo_dsi[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN];
+		}
 	}
 	
 	
@@ -1568,7 +1572,7 @@ public class QuadCLTCPU {
 	/**
 	 * Tries to read combo DSI, if successful - sets this.dsi and blue sky
 	 * @param silent
-	 * @return combo DSI if read, null if failed to read. Result has full lenghth
+	 * @return combo DSI if read, null if failed to read. Result has full length
 	 *               (OpticalFlow.COMBO_DSN_TITLES.length), missing slices are null
 	 */
 	public double [][] restoreComboDSI (boolean silent) {
@@ -1627,18 +1631,26 @@ public class QuadCLTCPU {
 			double [] reduced_strength // if not null will return >0 if had to reduce strength (no change if did not reduce)
 			) {
 		int NUM_BINS = 1024;
+// 10.15.2023 - was error here,	readComboDSI (silent) returns combo_dsi, not converted to this.dsi format;	
+//		double [][] main_dsi = use_combo? readComboDSI (silent): readDsiMain();
+		double [][] main_dsi = null;
 		boolean silent = false;
-		double [][] main_dsi = use_combo? readComboDSI (silent): readDsiMain();
+		if (use_combo) {
+			readComboDSI (silent);
+			main_dsi = this.dsi;
+		} else {
+			main_dsi =  readDsiMain();
+		}
 		if (main_dsi == null) {
 			return null;
 		}
 		double [] disparity_lma = main_dsi[isAux()?TwoQuadCLT.DSI_DISPARITY_AUX_LMA:TwoQuadCLT.DSI_DISPARITY_MAIN_LMA];
 		double [] strength = main_dsi[isAux()?TwoQuadCLT.DSI_STRENGTH_AUX:TwoQuadCLT.DSI_STRENGTH_MAIN];
+		double [] sfm_gain = main_dsi[isAux()?TwoQuadCLT.DSI_SFM_GAIN_AUX:TwoQuadCLT.DSI_SFM_GAIN_MAIN];
 		if ((strength == null) || (needs_lma && (disparity_lma == null) )) {
 			return null;
 		}
 		int	min_reliable = (int) Math.round (strength.length * min_ref_frac);
-		
 		strength = strength.clone();
 		boolean [] reliable = new boolean [strength.length];
 		for (int i = 0; i < reliable.length; i++) {
@@ -1654,6 +1666,20 @@ public class QuadCLTCPU {
 				}
 			}
 		}
+		int num_sfm_gain = 0;
+		for (int i = 0; i < reliable.length; i++) {
+			if (sfm_gain[i] >0) {
+				num_sfm_gain++;
+			}
+		}
+		if (num_sfm_gain > min_reliable) {
+			for (int i = 0; i < reliable.length; i++) {
+				if (sfm_gain[i] <= 0){
+					reliable[i] = false;
+					strength[i] = 0.0;
+				}
+			}
+		}
 		
 		int num_reliable = 0;
 		for (boolean b: reliable) if (b) num_reliable++;

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

@@ -66,17 +66,6 @@ public class TDCorrTile {
 		}
 	}
 	
-	
-	/*
-		if (fcorr_combo_td != null) {
-			gpuQuad.getCorrTilesTdInterCombo(
-					fcorr_combo_td); //    final float [][] corr_tiles_combo)
-		}
-		return;
-
-	 */
-	
-	
 	/**
 	 * Get Transform-domain 2D correlation tile weight needed for fat zero in phase
 	 * correlation and transform to the pixel domain.
@@ -334,7 +323,6 @@ public class TDCorrTile {
 			final double        gpu_fat_zero,
 			final int           debug_level
 			){
-//		final double gpu_fat_zero = clt_parameters.getGpuFatZeroInter(monochrome);
 		final int corr_size_td =    4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE;
 		final int gpu_corr_rad =    GPUTileProcessor.DTT_SIZE -1;
 		final int [] indices = new int [tiles.length];
@@ -397,6 +385,13 @@ public class TDCorrTile {
 		return mapped_corrs;
 	}
 	
+	/**
+	 * Get GPU TD data after interscene correlation of 2 scenes (only use
+	 * combo of all channels)  
+	 * @param gpuQuad GPU quad instance
+	 * @return TDCorrTile [] array, with weight equal to number of channels
+	 *         combined (normally 16)
+	 */
 	public static TDCorrTile [] getFromGpu(
 			GpuQuad gpuQuad) {
 		int tilesX =     gpuQuad.getImageWidth()  / GpuQuad.getDttSize();
@@ -429,12 +424,4 @@ public class TDCorrTile {
 		ImageDtt.startAndJoin(threads);
 		return tiles;
 	}
-/*
-		if (fcorr_combo_td != null) {
-			gpuQuad.getCorrTilesTdInterCombo(
-					fcorr_combo_td); //    final float [][] corr_tiles_combo)
-		}
-		return;
-
- */
 }

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

@@ -80,7 +80,9 @@ public class TwoQuadCLT {
 	public static  int DSI_AVGVAL_AUX =        12;
 	public static  int DSI_BLUE_SKY_MAIN =     13;
 	public static  int DSI_BLUE_SKY_AUX =      14;
-	public static  int DSI_LENGTH =            DSI_BLUE_SKY_AUX+1;
+	public static  int DSI_SFM_GAIN_MAIN =     15; // SfM disparity gain pixel/pixel, RGB
+	public static  int DSI_SFM_GAIN_AUX =      16; // SfM disparity gain pixel/pixel, LWIR
+	public static  int DSI_LENGTH =            DSI_SFM_GAIN_AUX+1;
 
 	public static String DSI_COMBO_SUFFIX = "-DSI_COMBO";
 	public static String DSI_MAIN_SUFFIX =  "-DSI_MAIN";
@@ -100,7 +102,9 @@ public class TwoQuadCLT {
 			    "avgval_main",
 			    "avgval_aux",
 			    "blue_sky_main",
-			    "blue_sky_aux"};
+			    "blue_sky_aux",
+			    "sfm_gain_main",
+			    "sfm_gain_aux"};
 
 	public long                                            startTime;     // start of batch processing
 	public long                                            startSetTime;  // start of set processing

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

+package com.elphel.imagej.tileprocessor.sfm;
+
+class SfmCorr{
+	double sfm_gain;
+	double [] corr_ind = null;  // {disparity, strength}
+	double [] corr_neib = null; // {disparity, strength}
+}