/**
 **
 ** StructureFromMotion - calculate depth from scene sequences
 **
 ** Copyright (C) 2023 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  StructureFromMotion.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */
package com.elphel.imagej.tileprocessor.sfm;

import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GPUTileProcessor;
//import com.elphel.imagej.cameras.ColorProcParameters;
//import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
//import com.elphel.imagej.ims.Did_ins_2;
//import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.tileprocessor.Correlation2d;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.Interscene;
import com.elphel.imagej.tileprocessor.IntersceneLma;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import com.elphel.imagej.tileprocessor.TDCorrTile;
import com.elphel.imagej.tileprocessor.TileNeibs;

import ij.ImagePlus;

public class StructureFromMotion {
	public static double [] ZERO3 = {0.0,0.0,0.0};
	public static int     THREADS_MAX =          100;  // maximal number of threads to launch
	public static final int MODE_STRONG=3;
	public static final int MODE_NEIB=  2;
	public static final int MODE_WEAK=  1;
	public static final int MODE_NONE=  0;
//	class SfmCorr{
//		double sfm_gain;
//		double [] corr_ind;  // {disparity, strength}
//		double [] corr_neib; // {disparity, strength}
//	}

	public static boolean sfmPair_ref_debug( // ref/scene
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT                scene,
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		double      range_disparity_offset = clt_parameters.imp.range_disparity_offset;
		boolean mb_en =       clt_parameters.imp.mb_en;
		double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		int     margin =      clt_parameters.imp.margin;
		int        sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
		boolean [] reliable_ref =  null;
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}
		

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky

        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		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 ?
        	/* use conditioned!
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        	*/
        }

        double [][] ref_pXpYD = null;
        double [] mb_ref_disparity =null;
        mb_ref_disparity = interscene_ref_disparity;
        if (mb_ref_disparity == null) {
        	mb_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
        ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        		mb_ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        		ref_scene,     // final QuadCLT     scene_QuadClt,
        		ref_scene);    // final QuadCLT     reference_QuadClt)

		// should have at least next or previous non-null
		double [][] mb_vectors_ref = null;
		TpTask[][] tp_tasks_ref = null;
		String ts = scene.getImageName();
		if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
			System.out.println("sfmPair(): no pose for timestamp "+ts);
			return false;
		}
		double [][]	scene_xyzatr = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
		double [][] scene_xyzatr_dt= {
				ers_reference.getSceneErsXYZ_dt(ts),
				ers_reference.getSceneErsATR_dt(ts)
		};
		for (int i = 0; i < scene_xyzatr_dt.length; i++) {
			if (scene_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
				scene_xyzatr_dt[i] = ZERO3.clone();
			}
		}
		double [][]        mb_vectors = null;
		
		if (mb_en) {
			// should get velocities from HashMap at reference scene from timestamp , not re-calculate.
			//				double [][] dxyzatr_dt_ref = ref_xyzatr_dt;
			mb_vectors_ref = OpticalFlow.getMotionBlur(
					ref_scene,             // QuadCLT        ref_scene,
					ref_scene,             // QuadCLT        scene,         // can be the same as ref_scene
					ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
					ZERO3,                 // double []      camera_xyz,
					ZERO3,                 // double []      camera_atr,
					ref_xyzatr_dt[0],      // double []      camera_xyz_dt,
					ref_xyzatr_dt[1],      // double []      camera_atr_dt,
					0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
					debugLevel);           // int            debug_level)
		}
		tp_tasks_ref = Interscene.setReferenceGPU (
				clt_parameters,            // CLTParameters      clt_parameters,			
				ref_scene,                 // QuadCLT            ref_scene,
				mb_ref_disparity,          // double []          ref_disparity, // null or alternative reference disparity
				ref_pXpYD,                 // double [][]        ref_pXpYD,
				null, 			    // final float [][]          fclt, 
				reliable_ref,              // final boolean []   selection, // may be null, if not null do not  process unselected tiles
				margin,                    // final int          margin,
				// motion blur compensation 
				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
				mb_vectors_ref,            // double [][]        mb_vectors,  // now [2][ntiles];
				debugLevel);               // int         debugLevel)
		if (mb_en) {
			mb_vectors = OpticalFlow.getMotionBlur(
					ref_scene,             // QuadCLT        ref_scene,
					scene,                 // QuadCLT        scene,         // can be the same as ref_scene
					ref_pXpYD,             // double [][]    ref_pXpYD,     // here it is scene, not reference!
					scene_xyzatr[0],       // double []      camera_xyz,
					scene_xyzatr[1],       // double []      camera_atr,
					scene_xyzatr_dt[0],    // double []      camera_xyz_dt,
					scene_xyzatr_dt[1],    // double []      camera_atr_dt,
					0,                     // int            shrink_gaps,  // will gaps, but not more that grow by this
					debugLevel);           // int            debug_level)
		}
		int [] fail_reason = new int[1];
		double [] ref_disparity = null;
		float [][][] facc_2d_img = new float [1][][]; // set it to null?
		// show debug image
    	@SuppressWarnings("unused")
		double [][] dpXYddisp = getSfmDpxDpyDdisp(
    			new double[][][] {new double[2][3], scene_xyzatr},// final double [][][] scenes_xyzatr,
    			ref_scene,              // scene_pairs[0][0], // final QuadCLT       scene0,
    			scene,                  // scene_pairs[0][1], // final QuadCLT       scene1,
    			ref_scene,              // final QuadCLT       ref_QuadClt,
    			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
    			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
    			batch_mode,             // final boolean       batch_mode,
    			debugLevel);            // final int           debug_level
		
		
		@SuppressWarnings("unused")
		double [][][] coord_motion = Interscene.interCorrPair( // new double [tilesY][tilesX][][];
				clt_parameters,      // CLTParameters  clt_parameters,
				false,               // use3D,               // boolean        use3D,         // generate disparity difference
				true,                // fpn_disable,         //	boolean        fpn_disable,   // disable fpn filter if images are known to be too close
				mb_max_gain,         // double         mb_max_gain,					
				null,                // 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
				ref_scene,           // QuadCLT        ref_scene,     // Scene for which DSI (ref_disparity) is calculated 
				ref_disparity,       // double []      ref_disparity, // null or alternative reference disparity
				ref_scene,           // QuadCLT        first_scene,
				ref_pXpYD,           // double [][]    pXpYD_ref,     // pXpYD for the reference scene			
				tp_tasks_ref[0],     // TpTask[]       tp_tasks_ref,  // only (main if MB correction) tasks for FPN correction
				scene,               // QuadCLT        scene_QuadCLT,
				scene_xyzatr[0],     //                xyz
				scene_xyzatr[1],     // pose[1],      // atr
				reliable_ref,        // ****null,     // final boolean [] selection, // may be null, if not null do not  process unselected tiles
				margin,              // final int      margin,
				sensor_mask_inter,   // final int      sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
				facc_2d_img,         // final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)final float [][][]   accum_2d_corr, // if [1][][] - return accumulated 2d correlations (all pairs)
				null,                //	final float [][] dbg_corr_fpn,
				true,                // near_important,      // boolean            near_important, // do not reduce weight of the near tiles
				false,               // boolean            all_fpn,        // do not lower thresholds for non-fpn (used during search)
				false,               // initial_adjust,      // boolean             initial_adjust,
				mb_vectors,          // double [][]        mb_vectors,  // now [2][ntiles];
				-1,                  // int                niter,
				false,               //	boolean            img_debug, // generate debug images
				clt_parameters.imp.debug_level, // int                imp_debug_level,
				debugLevel); // 1); // -1); // int debug_level);
		return true;
	}

	public static double [][] sfmPair_debug( // debug scene pairs
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT [][]           scene_pairs,
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		boolean        show_2d_correlations = false; // true;
		boolean        show_disp_corr =       false; //true;
		boolean        show_disp_seq =        true;
    	double         sfm_min_gain = 10.0;
    	double         sfm_min_frac = 0.5;
		
		int            num_readjust =         5;
		double min_strength =                 0.4;
		double      range_disparity_offset = clt_parameters.imp.range_disparity_offset;
		// 16 puts scale same as with older code
		double corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());	
		final int               corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
		double []  neib_weights_od = {0.7, 0.5};
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
//        int tile_size = ref_scene.getTileProcessor().getTileSize();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
//		boolean [] reliable_ref =  null;
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr =    new double [][] {ZERO3,ZERO3};
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky

        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		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 ?
        	/* use conditioned!
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        	*/
        }
        
        double []   ref_disparity = interscene_ref_disparity;
        if (ref_disparity == null) {
        	ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        double [][]disp_adj = new double [num_readjust+1][];
        disp_adj[0] = ref_disparity.clone();
    	final int num_pairs = scene_pairs.length;
        for (int ntry = 0; ntry < num_readjust; ntry++) {
        	// only for derivatives
        	double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        			null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        			ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        			ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        			ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        			ref_scene,     // final QuadCLT     scene_QuadClt,
        			ref_scene);    // final QuadCLT     reference_QuadClt)
        	TDCorrTile [][] pairs_TD_all = new TDCorrTile [2*(scene_pairs.length + 1)][];
        	final double[][][][] scenes_xyzatr = new double[num_pairs][2][][];   // 2 scenes
        	final double[][][][] scenes_xyzatr_dt  = new double[num_pairs][2][][];// 2 scenes
        	for (int npair = 0; npair < num_pairs; npair++) {
        		QuadCLT [] scenes = scene_pairs[npair];
        		// should have at least next or previous non-null
        		//        	final double[][][] scenes_xyzatr = new double[scenes.length][][];   // 2 scenes
        		//        	final double[][][] scenes_xyzatr_dt  = new double[scenes.length][][];// 2 scenes
        		for (int nscene = 0; nscene < scenes.length; nscene++) {
        			String ts = scenes[nscene].getImageName();
        			if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
        				System.out.println("sfmPair(): no pose for timestamp "+ts);
        				return null;
        			}
        			scenes_xyzatr[npair][nscene] = new double[][] {
        				ers_reference.getSceneXYZ(ts),
        				ers_reference.getSceneATR(ts)};
        				scenes_xyzatr_dt[npair][nscene]= new double[][] {
        					ers_reference.getSceneErsXYZ_dt(ts),
        					ers_reference.getSceneErsATR_dt(ts)};
        					for (int i = 0; i < scenes_xyzatr_dt[nscene].length; i++) {
        						if (scenes_xyzatr_dt[npair][nscene][i] == null) {
        							System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
        							scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
        						}
        					}
        		}
        	}        


        	double [][] dpXYddisp = getSfmDpxDpyDdisp(
        			scenes_xyzatr[0],          // final double [][][] scenes_xyzatr,
        			scene_pairs[0][0], // final QuadCLT       scene0,
        			scene_pairs[0][1], // final QuadCLT       scene1,
        			ref_scene,              // final QuadCLT       ref_QuadClt,
        			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
        			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
        			batch_mode,             // final boolean       batch_mode,
        			debugLevel);            // final int           debug_level

        	for (int npair = 0; npair < num_pairs; npair++) {
        		QuadCLT [] scenes = scene_pairs[npair];
        		pairs_TD_all[npair] = sfmPair_TD( // scene0/scene1
        				clt_parameters,  // final CLTParameters          clt_parameters,
        				ref_scene,       // final QuadCLT                ref_scene,
        				ref_disparity,   // final double []              ref_disparity, // here can not be null
        				ref_xyzatr,      // final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
        				ref_xyzatr_dt,   // final double [][]            ref_xyzatr_dt,
        				scenes,          // final QuadCLT []             scenes,          // 2 scenes
        				scenes_xyzatr[npair],   //final double[][][]	         scenes_xyzatr,   // 2 scenes
        				scenes_xyzatr_dt[npair],// final double[][][]	         scenes_xyzatr_dt,// 2 scenes
        				mb_max_gain,     // double                       mb_max_gain,
        				batch_mode,      // final boolean                batch_mode,
        				debugLevel);     // final int                    debugLevel);
        		// get neibs in TD
        	}
        	// accumulate all pairs
        	pairs_TD_all[num_pairs] = TDCorrTile.cloneTiles(pairs_TD_all[0]);
        	for (int npair = 1; npair < num_pairs; npair++) {
        		TDCorrTile.accumulate (
        				pairs_TD_all[num_pairs], // final TDCorrTile [] dst,
        				pairs_TD_all[npair],     // final TDCorrTile [] src,
        				1.0);                    // final double src_weight);
        	}
        	// create neighbors for individual and combo
        	for (int npair = 0; npair <= num_pairs; npair++) {
        		pairs_TD_all[num_pairs + 1 + npair] = TDCorrTile.calcNeibs(
        				pairs_TD_all[npair], // pairs_TD,        // TDCorrTile [] tiles,
        				tilesX,              // final int     tilesX,
        				neib_weights_od,     //double []     neib_weights_od, // {orhto, diag}
        				null,                // double [][]   corr_pd,
        				0.0,                 // double        neib_too_strong,
        				true);               // final boolean process_all	
        	}

        	double [][][] corr2d_PD = new double [pairs_TD_all.length][][];
        	for (int npair = 0; npair < pairs_TD_all.length; npair++) {
        		corr2d_PD[npair] = 	TDCorrTile.convertTDtoPD(
        				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
        				pairs_TD_all[npair],           // final TDCorrTile [] tiles,
        				0xFE,               // final int           corr_type, // 0xFE
        				corr_fz_inter,      // final double        gpu_fat_zero,
        				debugLevel);        // final int           debug_level

        	}
        	//		
        	if (show_2d_correlations) {
        		double [][] dbg_2d_corrs = ImageDtt.corr_partial_dbg( // not used in lwir
        				corr2d_PD,                           // final double [][][]     corr_data,       // [layer][tile][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
        				tilesX,                              // final int               tilesX,
        				corr_size,                           //final int               corr_size,       // 15
        				clt_parameters.corr_border_contrast, // final double            border_contrast,
        				debugLevel);                         // final int               globalDebugLevel)
        		String [] dbg_titles= new String[dbg_2d_corrs.length];
        		for (int npair = 0; npair < num_pairs; npair++) {
        			dbg_titles[npair] = "single-"+npair;
        			dbg_titles[npair+num_pairs+1] = "neibs-"+npair;
        		}
        		dbg_titles[num_pairs] = "single-avg"+num_pairs;
        		dbg_titles[2 * num_pairs + 1] = "neibs-avg"+num_pairs;

        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corrs,
        				tilesX * (corr_size + 1),
        				tilesY * (corr_size + 1),
        				true,
        				"corr2d-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs,
        						dbg_titles);
        	}
        	double [][] corr2d_PD_use = corr2d_PD[num_pairs];
        	final double centroid_radius = clt_parameters.imp.centroid_radius; // final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        	final int    n_recenter = clt_parameters.imp.n_recenter;           // when cosine window, re-center window this many times
        	double [][] disp_corr = getSfmDisparityCorrectionStrength(
        			clt_parameters,  // final CLTParameters  clt_parameters,
        			corr2d_PD_use,   // final double [][]    corr2d_PD,
        			dpXYddisp,       // final double [][]    dpXYddisp,
        			sfm_min_gain,    // double         sfm_min_gain,
        			sfm_min_frac,   //final double         sfm_min_frac,
        			centroid_radius, // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        			n_recenter,      // final int            n_recenter,      // when cosine window, re-center window this many times
        			corr_size,       //final int            corr_size, // 15
        			debugLevel);     // final int            debugLevel);
    		if (disp_corr == null) {
    			if (debugLevel > -3) {
    				System.out.println("sfmPair_debug(): not enough SfM tiles, bailing out");
    			}
    			return null;
    		}

        	for (int nTile = 0; nTile< disp_corr.length; nTile++) if (disp_corr[nTile] != null){
        		if (disp_corr[nTile][1] > min_strength) {
        			ref_disparity[nTile] += disp_corr[nTile][0];
        		}
        	}
        	// replace weak tiles with average?
        	disp_adj[ntry + 1] = ref_disparity.clone();
        	if (show_disp_corr) {
        		double [][] dbg_2d_corr = new double [3][disp_corr.length];
        		for (int i = 0; i < dbg_2d_corr.length; i++) {
        			Arrays.fill(dbg_2d_corr[i], Double.NaN);
        		}
        		for (int nTile = 0; nTile < disp_corr.length; nTile++)  if (disp_corr[nTile] != null) {
        			dbg_2d_corr[0][nTile] = disp_corr[nTile][0];
        			dbg_2d_corr[1][nTile] = disp_corr[nTile][1];
        		}
        		dbg_2d_corr[2] = ref_disparity.clone();
        		String [] dbg_titles = {"disp_corr","strength", "new_disp"};
        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corr,
        				tilesX,
        				tilesY,
        				true,
        				"disp_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs,
        						dbg_titles);
        	}
        	
        } // ntry
    	if (show_disp_seq) {
//    		String [] dbg_titles = new String[disp_adj.length]; // {"disp_corr","strength", "new_disp"};
    		ShowDoubleFloatArrays.showArrays(
    				disp_adj,
    				tilesX,
    				tilesY,
    				true,
    				"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
    						scene_pairs[0][0].getImageName()+"-"+num_pairs);
//    						dbg_titles);
    	}
        //combo_dsn_final
    	
    	// Re-calc BG?
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] =        ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] =         ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] =     ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		ref_scene.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,           // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn_final,       // dbg_data,         // double [][] data,
				tilesX,                // int         width,
				tilesY);               // int         height)
    	
        return combo_dsn_final;
	}

	@Deprecated
	public static double [][] sfmPair( // clean this up from extra data
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final QuadCLT [][]           scene_pairs,
			final double                 mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		boolean       show_disp_corr =       false; //true;
		boolean       show_disp_seq =        true;
    	final double  sfm_min_gain =         clt_parameters.imp.sfm_min_gain;
    	final double  sfm_min_frac =         clt_parameters.imp.sfm_min_frac;
		final int     num_readjust =         clt_parameters.imp.sfm_readjust;   // 5;
		final double  min_strength1 =        clt_parameters.imp.sfm_min_str1;   //  0.4; // update if correction strength exceeds
		final double  min_strength16=        clt_parameters.imp.sfm_min_str16;  //  0.4; // update if correction strength exceeds
		final double  neib_too_strong1 =     clt_parameters.imp.sfm_neib_too_str1;   //  0.4; // do not count neighbors stronger than that
		final double  neib_too_strong16 =    clt_parameters.imp.sfm_neib_too_str16;  //  0.4; // do not count neighbors stronger than that
		final int     sfm_shrink =           clt_parameters.imp.sfm_shrink;  // 5;
		final double  fade_sigma =           clt_parameters.imp.sfm_fade_sigma;   //   3.0;    // fade SfM gains at the edges 
		final boolean use_neibs =            clt_parameters.imp.sfm_use_neibs; // true;
		final double  min_neib_strength1=    clt_parameters.imp.sfm_neib_str1;  //  0.5; // update if no-individual and neibs correction strength exceeds 
		final double  min_neib_strength16=   clt_parameters.imp.sfm_neib_str16; //  0.5; // update if no-individual and neibs correction strength exceeds 
		final double  prev_sfm_frac =        clt_parameters.imp.sfm_prev_frac; //  0.6; // update if new sfm gain > this fraction of the old one
		final double  same_weight =          clt_parameters.imp.sfm_same_weight; // 0.8;
		final double  range_disparity_offset = clt_parameters.imp.range_disparity_offset;
    	final double  centroid_radius = clt_parameters.imp.centroid_radius; // final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    	final int     n_recenter = clt_parameters.imp.n_recenter;           // when cosine window, re-center window this many times
		// 16 puts scale same as with older code
		final double   corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());	
//		final int      corr_size = 2 * GPUTileProcessor.DTT_SIZE -1;
//		final double [] neib_weights_od = {0.7, 0.5};
		int tilesX =    ref_scene.getTileProcessor().getTilesX();
		int tilesY =    ref_scene.getTileProcessor().getTilesY();
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		
		boolean use_combo_dsi =    true;
		boolean use_lma_dsi =      clt_parameters.imp.use_lma_dsi;
		double [][] ref_xyzatr =    new double [][] {ZERO3,ZERO3};
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}

        double [] disparity_raw = new double [tilesX * tilesY];
        Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
        if (combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] == null) {
        	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =
        			new double [combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP].length];
        	if (debugLevel > -3) {
        		System.out.println("sfmPair(): Initializing missing COMBO_DSN_INDX_SFM_GAIN data.");
        	}
        }
        final double [] ref_sfm_gain = combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN].clone();
        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		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 ?
        	/* use conditioned!
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        	*/
        }
        
        if (interscene_ref_disparity == null) {
        	interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        final double []   ref_disparity = interscene_ref_disparity;
        double [][] disp_adj = show_disp_seq? (new double [num_readjust+1][]):null;
        if (disp_adj != null) {
        	disp_adj[0] = ref_disparity.clone();
        }
        final int max_pairs_scale = 16;
    	final int num_pairs = scene_pairs.length;
    	final double strength_frac16 = (num_pairs < max_pairs_scale)? (Math.sqrt(num_pairs)-1)/(Math.sqrt(max_pairs_scale)-1): 1.0;
    	final double  min_strength = min_strength1 * (1.0-strength_frac16) + min_strength16* strength_frac16; 
    	final double  neib_too_strong = neib_too_strong1 * (1.0-strength_frac16) + neib_too_strong16* strength_frac16; 
    	final double  min_neib_strength = min_neib_strength1 * (1.0-strength_frac16) + min_neib_strength16* strength_frac16; 
    	
    	final double[][][][] scenes_xyzatr = new double[num_pairs][2][][];   // 2 scenes
    	final double[][][][] scenes_xyzatr_dt  = new double[num_pairs][2][][];// 2 scenes
    	for (int npair = 0; npair < num_pairs; npair++) {
    		QuadCLT [] scenes = scene_pairs[npair];
    		for (int nscene = 0; nscene < scenes.length; nscene++) {
    			String ts = scenes[nscene].getImageName();
    			if (ts.equals(ref_scene.getImageName())) {
    				scenes_xyzatr[npair][nscene] = ref_xyzatr;
    				scenes_xyzatr_dt[npair][nscene]= ref_xyzatr_dt;
    			} else {
    				if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
    					System.out.println("sfmPair(): no pose for timestamp "+ts);
    					return null;
    				}
    				scenes_xyzatr[npair][nscene] = new double[][] {
    					ers_reference.getSceneXYZ(ts),
    					ers_reference.getSceneATR(ts)};
    					scenes_xyzatr_dt[npair][nscene]= new double[][] {
    						ers_reference.getSceneErsXYZ_dt(ts),
    						ers_reference.getSceneErsATR_dt(ts)};
    						for (int i = 0; i < scenes_xyzatr_dt[npair][nscene].length; i++) {
    							if (scenes_xyzatr_dt[npair][nscene][i] == null) {
    								System.out.println("sfmPair(): scene_xyzatr_dt["+i+"] == null");
    								scenes_xyzatr_dt[npair][nscene][i] = ZERO3.clone();
    							}
    						}
    			}
    		}
    	}        
    	
        for (int ntry = 0; ntry < num_readjust; ntry++) {
        	SfmCorr [] sfmCorr = getSfmCorr(
        			clt_parameters,         // final CLTParameters  clt_parameters,
        			ref_scene,              // final QuadCLT        ref_scene,
        			ref_disparity,          // final double []      ref_disparity,
        			ref_xyzatr,             // final double[][]     ref_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        			ref_xyzatr_dt,          // final double[][]     ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        			scene_pairs,            // final QuadCLT[][]    scene_pairs,
        			scenes_xyzatr,          // final double[][][][] scenes_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        			scenes_xyzatr_dt,       // final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        			sfm_min_gain,           // final double         sfm_min_gain,
        			sfm_min_frac,           // final double         sfm_min_frac,

        			mb_max_gain,            // final double         mb_max_gain,
        			range_disparity_offset, // final double         range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;			final double         mb_max_gain,
        			corr_fz_inter,          // final double         corr_fz_inter,
        			use_neibs,              // final boolean        use_neibs,
        			neib_too_strong,        // final double         neib_too_strong,
        			centroid_radius,        // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
        			n_recenter,             // final int            n_recenter,      // when cosine window, re-center window this many times			 
        			batch_mode,             // final boolean        batch_mode,
        			debugLevel);            // final int            debugLevel)
    		if (sfmCorr == null) {
    			if (debugLevel > -3) {
    				System.out.println("sfmPair(): not enough SfM tiles, bailing out");
    			}
    			return null;
    		}
        	
        	if ((sfm_shrink > 0) || (fade_sigma > 0.0)) {
        		shrinkFadeSfmGain(
        				sfm_shrink, // final int        sfm_shrink,
        				fade_sigma, // final double     fade_sigma, 
        				sfmCorr,    // final SfmCorr [] sfmCorr,
        				tilesX);    // final int        tilesX)
        	}
        	
        	if (show_disp_corr) {
        		String [] dbg_titles = {"corr", "corr_str","neib_corr", "neib_str","old_sfm","new_sfm"};
        		
        		double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorr.length];
        		for (int i = 0; i < dbg_2d_corr.length; i++) {
        			Arrays.fill(dbg_2d_corr[i], Double.NaN);
        		}
        		dbg_2d_corr[4] = ref_sfm_gain.clone();
        		for (int nTile = 0; nTile < sfmCorr.length; nTile++) if (sfmCorr[nTile] != null) {
        			dbg_2d_corr[5][nTile] = sfmCorr[nTile].sfm_gain;
        			if (sfmCorr[nTile].corr_ind != null) {
        				dbg_2d_corr[0][nTile] = sfmCorr[nTile].corr_ind[0];
        				dbg_2d_corr[1][nTile] = sfmCorr[nTile].corr_ind[1];
        			}
        			if (sfmCorr[nTile].corr_neib != null) {
        				dbg_2d_corr[2][nTile] = sfmCorr[nTile].corr_neib[0];
        				dbg_2d_corr[3][nTile] = sfmCorr[nTile].corr_neib[1];
        			}
        		}
        		ShowDoubleFloatArrays.showArrays(
        				dbg_2d_corr,
        				tilesX,
        				tilesY,
        				true,
        				"sfm_corr-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
        						scene_pairs[0][0].getImageName()+"-"+num_pairs+"-"+String.format("%02d", ntry),
        						dbg_titles);
        	}
        	
// combine corrections
    		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
    		final AtomicInteger ai = new AtomicInteger(0);
    		final double wscale = -0.5/Math.log(prev_sfm_frac);
    		for (int ithread = 0; ithread < threads.length; ithread++) {
    			threads[ithread] = new Thread() {
    				public void run() {
    					for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement())
    						if (sfmCorr[nTile] != null){
    							if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
									double w = 1.0;
									if (ref_sfm_gain[nTile] > 0) {
										w = same_weight + wscale*Math.log(sfmCorr[nTile].sfm_gain/ref_sfm_gain[nTile]);
									}
									if      (w < 0.0) w = 0;
									else if (w > 1.0) w = 1.0;
									// Update some strength too?
									if ((sfmCorr[nTile].corr_ind != null) && (sfmCorr[nTile].corr_ind[1] > min_strength)) {
										ref_disparity[nTile] +=  w *sfmCorr[nTile].corr_ind[0];
										// TODO: add same (as above) for strengths?
										ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorr[nTile].sfm_gain; 
									}  else if ((sfmCorr[nTile].corr_neib != null) && (sfmCorr[nTile].corr_neib[1] > min_neib_strength)) {
										ref_disparity[nTile] +=  w *sfmCorr[nTile].corr_neib[0];
										// TODO: add same (as above) for strengths?
										ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorr[nTile].sfm_gain; 
									}   								
    							}
    							
    							/*
    							if (sfmCorr[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
    								if (prev_sfm_frac*sfmCorr[nTile].sfm_gain >= ref_sfm_gain[nTile]){ // copy 100%
    									// Update some strength too?
    									if ((sfmCorr[nTile].corr_ind != null) && (sfmCorr[nTile].corr_ind[1] > min_strength)) {
    										ref_disparity[nTile] += sfmCorr[nTile].corr_ind[0];
    										ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain; 
    									} else if ((sfmCorr[nTile].corr_neib != null) && (sfmCorr[nTile].corr_neib[1] > min_neib_strength)) {
    										ref_disparity[nTile] += sfmCorr[nTile].corr_neib[0];
    										ref_sfm_gain[nTile] = sfmCorr[nTile].sfm_gain; 
    									}

    								}
    							}
    							*/
    						}
    				}
    			};
    		}		      
    		ImageDtt.startAndJoin(threads);
        	// replace weak tiles with average?
    		if (disp_adj != null) {
    			disp_adj[ntry + 1] = ref_disparity.clone();
    		}
        	
        } // ntry
    	if (disp_adj != null) {
    		ShowDoubleFloatArrays.showArrays(
    				disp_adj,
    				tilesX,
    				tilesY,
    				true,
    				"SfM-disparity-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
    						scene_pairs[0][0].getImageName()+"-"+num_pairs);
    	}
    	// Re-calc BG?
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] =        ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] =         ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] =     ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =    ref_sfm_gain.clone(); 
    	
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		ref_scene.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,           // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn_final,       // dbg_data,         // double [][] data,
				tilesX,                // int         width,
				tilesY);               // int         height)
        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,
			final QuadCLT                ref_scene,
			final QuadCLT [][][]         scene_pairs_sets,
			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;
		boolean       show_disp_corr =       clt_parameters.imp.sfm_show_corr &= !batch_mode;    //false; //true;
		
		final int dbg_tile = -( 52+58*80);
		final double  sfm_min_base =         clt_parameters.imp.sfm_min_base; 
    	final double  sfm_min_gain =         clt_parameters.imp.sfm_min_gain;
    	final double  sfm_min_frac =         clt_parameters.imp.sfm_min_frac;
		final int     num_readjust =         clt_parameters.imp.sfm_readjust;  // 5;
		final double  min_strength1 =        clt_parameters.imp.sfm_min_str1;   //  0.4; // update if correction strength exceeds
		final double  min_strength16=        clt_parameters.imp.sfm_min_str16;  //  0.4; // update if correction strength exceeds
		final double  neib_too_strong1 =     clt_parameters.imp.sfm_neib_too_str1;   //  0.4; // do not count neighbors stronger than that
		final double  neib_too_strong16 =    clt_parameters.imp.sfm_neib_too_str16;  //  0.4; // do not count neighbors stronger than that
		final int     sfm_shrink =           clt_parameters.imp.sfm_shrink;  // 5;
		final double  fade_sigma =           clt_parameters.imp.sfm_fade_sigma;   //   3.0;    // fade SfM gains at the edges 
		final boolean use_neibs =            clt_parameters.imp.sfm_use_neibs; // true;
		final double  min_neib_strength1=    clt_parameters.imp.sfm_neib_str1;  //  0.5; // update if no-individual and neibs correction strength exceeds 
		final double  min_neib_strength16=   clt_parameters.imp.sfm_neib_str16; //  0.5; // update if no-individual and neibs correction strength exceeds 
		
		final double  prev_sfm_frac =        clt_parameters.imp.sfm_prev_frac; //  0.6; // update if new sfm gain > this fraction of the old one
		final double  same_weight =          clt_parameters.imp.sfm_same_weight; // 0.8;
		final double  range_disparity_offset = clt_parameters.imp.range_disparity_offset;
    	final double  centroid_radius =      clt_parameters.imp.centroid_radius; // final double              centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    	final int     n_recenter =           clt_parameters.imp.n_recenter;           // when cosine window, re-center window this many times
		boolean       use_lma_dsi =          clt_parameters.imp.use_lma_dsi;
		// 16 puts scale same as with older code
		final double   corr_fz_inter = 16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome());
		
    	// TODO: assign !
//    	final double  inf_disp =             clt_parameters.imp.disparity_corr;     // disparity at infinity
    	final double  inf_disp =             clt_parameters.imp.disparity_corr + ref_scene.getDispInfinityRef() ; // 12/11/2025 - added ref_scene.getDispInfinityRef()
		final int     extrapolate_steps =    clt_parameters.imp.sfm_extrap_steps;
		final int     extrapolate_radius =   clt_parameters.imp.sfm_extrap_radius;
		
		final boolean average_neibs =        clt_parameters.imp.sfm_average_neibs; //false;
		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();
		
		ErsCorrection ers_reference = ref_scene.getErsCorrection();		

    	final double     zdisp = ers_reference.getDisparityFromZ(1.0);       // product of z and disparity    	
		boolean use_combo_dsi =    true;
		
		
		
		double [][] ref_xyzatr =    new double [][] {ZERO3,ZERO3};
		double [][] ref_xyzatr_dt= {
				ers_reference.getErsXYZ_dt(),
				ers_reference.getErsATR_dt()
		};
		for (int i = 0; i < ref_xyzatr_dt.length; i++) {
			if (ref_xyzatr_dt[i] == null) {
				System.out.println("sfmPair(): ref_xyzatr_dt["+i+"] == null");
				ref_xyzatr_dt[i] = ZERO3.clone();
			}
		}

//        double [] disparity_raw = new double [tilesX * tilesY];
//       Arrays.fill(disparity_raw,clt_parameters.disparity);
        double [][] combo_dsn_final = ref_scene.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
        if (combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] == null) {
        	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =
        			new double [combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP].length];
        	if (debugLevel > -3) {
        		System.out.println("sfmPair(): Initializing missing COMBO_DSN_INDX_SFM_GAIN data.");
        	}
        }
        final double [] ref_sfm_gain = combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN].clone();
        double [][] dls = { // Update to use FG? Or FG/no BG?
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
        		combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
        };
        double [][] ds = OpticalFlow.conditionInitialDS(
        		true,                // boolean        use_conf,       // use configuration parameters, false - use following  
        		clt_parameters,      // CLTParameters  clt_parameters,
        		dls,                 // double [][]    dls
        		ref_scene,           // QuadCLT        scene,
        		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 ?
        	/* use conditioned!
        	if (use_lma_dsi) {
        		for (int i = 0; i < interscene_ref_disparity.length; i++) {
        			if (Double.isNaN(dls[1][i])) {
        				interscene_ref_disparity[i] = Double.NaN;
        			}
        		}
        	}
        	*/
        }
        
        if (interscene_ref_disparity == null) {
        	interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
        }
        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();
        }
    	final int num_sets = scene_pairs_sets.length;
    	final int [] num_set_pairs = new int [num_sets];
    	final double[][][][][] scenes_xyzatr_sets = new double[num_sets][][][][];
    	final double[][][][][] scenes_xyzatr_dt_sets  = new double[num_sets][][][][];
    	final int num_pairs0 = scene_pairs_sets[0].length; // normally each set length is the same. Used for scaling min strengths.
        final int max_pairs_scale = 16;
    	final double strength_frac16 = (num_pairs0 > 1) ?( 
    			(num_pairs0 < max_pairs_scale)? (Math.sqrt(num_pairs0)-1)/(Math.sqrt(max_pairs_scale)-1): 1.0) : 0;
    	final double  min_strength = min_strength1 * (1.0-strength_frac16) + min_strength16* strength_frac16; 
    	final double  neib_too_strong = neib_too_strong1 * (1.0-strength_frac16) + neib_too_strong16* strength_frac16; 
    	final double  min_neib_strength = min_neib_strength1 * (1.0-strength_frac16) + min_neib_strength16* strength_frac16; 
    	final int min_neibs_fill = 4;
    	
    	
    	for (int nset=0; nset < num_sets; nset++) {
    		int num_pairs = scene_pairs_sets[nset].length;
    		num_set_pairs[nset] = num_pairs;
        	scenes_xyzatr_sets[nset]=   new double[num_pairs][2][][];   // 2 scenes
        	scenes_xyzatr_dt_sets[nset]=new double[num_pairs][2][][];// 2 scenes;
    		for (int npair = 0; npair < num_pairs; npair++) {
    			QuadCLT [] scenes = scene_pairs_sets[nset][npair];
    			for (int nscene = 0; nscene < scenes.length; nscene++) {
    				String ts = scenes[nscene].getImageName();
    				if (ts.equals(ref_scene.getImageName())) {
    					scenes_xyzatr_sets[nset][npair][nscene] = ref_xyzatr;
    					scenes_xyzatr_dt_sets[nset][npair][nscene]= ref_xyzatr_dt;
    				} else {
    					if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
    						System.out.println("sfmPairsSet(): no pose for timestamp "+ts);
    						return null;
    					}
    					scenes_xyzatr_sets[nset][npair][nscene] = new double[][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
    					scenes_xyzatr_dt_sets[nset][npair][nscene]=	new double[][] {ers_reference.getSceneErsXYZ_dt(ts),ers_reference.getSceneErsATR_dt(ts)};
    					for (int i = 0; i < scenes_xyzatr_dt_sets[nset][npair][nscene].length; i++) {
    						if (scenes_xyzatr_dt_sets[nset][npair][nscene][i] == null) {
    							System.out.println("sfmPairsSet(): scenes_xyzatr_dt_sets["+nset+"]["+i+"] == null");
    							scenes_xyzatr_dt_sets[nset][npair][nscene][i] = ZERO3.clone();
    						}
    					}
    				}
    			}
    		}        
    	}
    	// check each pair has sufficient baseline and contains exactly 2 scenes
    	int num_good_sets=0;
    	boolean [] good_set = new boolean [num_sets];
    	for (int nset=0; nset < num_sets; nset++) {
    		int num_pairs = scene_pairs_sets[nset].length;
    		check_set: {
    			for (int npair = 0; npair < num_pairs; npair++) {
    				double [][][] scenes_xyzatr= scenes_xyzatr_sets[nset][npair];
    				if (scenes_xyzatr.length != 2) {
    					System.out.println("sfmPairsSet(): BUG: Not a pair of scenes: scenes_xyzatr_sets["+nset+"]["+npair+
    							"].length = "+(scenes_xyzatr.length)+" != 2");
    					return null;
    				}
    				double dx = scenes_xyzatr[1][0][0] - scenes_xyzatr[0][0][0];
    				double dy = scenes_xyzatr[1][0][1] - scenes_xyzatr[0][0][1];
    				double base = Math.sqrt(dx*dx + dy*dy);
    				if (base < sfm_min_base) {
    					if (debugLevel > -3) {
    						System.out.println("sfmPairsSet(): stereo base for nset="+nset+", npair="+npair+
    								" = "+base+" < "+sfm_min_base+".");
    					}
    					break check_set; // return null;
    				}
    			}
    			good_set[nset] = true;
    			num_good_sets++;
    		}

    	}
    	
    	if (num_good_sets == 0) {
			if (debugLevel > -3) {
				System.out.println("sfmPairsSet(): No good sets with sufficient base found, bailing out from SfM ranging.");
			}
			return null;
    	}
    	for (int ntry = 0; ntry < num_readjust; ntry++) {
    		final SfmCorr [] sfmCorrCombo = new SfmCorr [tilesX*tilesY];
    		double [] sum_weights_ind= new double [tilesX*tilesY];
    		double [] sum_weights_neib=new double [tilesX*tilesY];
    		/*
    		 * 0 - not to be modified as SfM gain is lower than threshold
    		 * 1 - weak tile, never got individual or neighbor correction
    		 * 2 - corrected by a neighbor
    		 * 3 - strong tile, corrected individually
    		 */
    		final int [] corr_mode = new int [tilesX*tilesY]; 
    		
    		for (int nset=0; nset < num_sets; nset++) if (good_set[nset]){
    			SfmCorr [] sfmCorr = getSfmCorr(
    					clt_parameters,             // final CLTParameters  clt_parameters,
    					ref_scene,                  // final QuadCLT        ref_scene,
    					ref_disparity,              // final double []      ref_disparity,
    					ref_xyzatr,                 // final double[][]     ref_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
    					ref_xyzatr_dt,              // final double[][]     ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
    					scene_pairs_sets[nset],     // final QuadCLT[][]    scene_pairs,
    					scenes_xyzatr_sets[nset],   // final double[][][][] scenes_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
    					scenes_xyzatr_dt_sets[nset],// final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
    		   			sfm_min_gain,               // final double         sfm_min_gain,
    		   			sfm_min_frac,               // final double         sfm_min_frac,
    					mb_max_gain,                // final double         mb_max_gain,
    					range_disparity_offset,     // final double         range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;			final double         mb_max_gain,
    					corr_fz_inter,              // final double         corr_fz_inter,
    					use_neibs,                  // final boolean        use_neibs,
    					neib_too_strong,            // final double         neib_too_strong,
    					centroid_radius,            // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    					n_recenter,                 // final int            n_recenter,      // when cosine window, re-center window this many times			 
    					batch_mode,                 // final boolean        batch_mode,
    					debugLevel);                // final int            debugLevel)
        		if (sfmCorr == null) {
        			if (debugLevel > -3) {
        				System.out.println("sfmPairsSet(): not enough SfM tiles, bailing out");
        			}
        			continue; //return  null;
        		}
    			
    			if ((sfm_shrink > 0) || (fade_sigma > 0.0)) {
    				shrinkFadeSfmGain(
    						sfm_shrink, // final int        sfm_shrink,
    						fade_sigma, // final double     fade_sigma, 
    						sfmCorr,    // final SfmCorr [] sfmCorr,
    						tilesX);    // final int        tilesX)
    			}

    			if (show_disp_corr_ind) {
    				String [] dbg_titles = {"corr", "corr_str","neib_corr", "neib_str","old_sfm","new_sfm"};

    				double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorr.length];
    				for (int i = 0; i < dbg_2d_corr.length; i++) {
    					Arrays.fill(dbg_2d_corr[i], Double.NaN);
    				}
    				dbg_2d_corr[4] = ref_sfm_gain.clone();
    				for (int nTile = 0; nTile < sfmCorr.length; nTile++) if (sfmCorr[nTile] != null) {
    					dbg_2d_corr[5][nTile] = sfmCorr[nTile].sfm_gain;
    					if (sfmCorr[nTile].corr_ind != null) {
    						dbg_2d_corr[0][nTile] = sfmCorr[nTile].corr_ind[0];
    						dbg_2d_corr[1][nTile] = sfmCorr[nTile].corr_ind[1];
    					}
    					if (sfmCorr[nTile].corr_neib != null) {
    						dbg_2d_corr[2][nTile] = sfmCorr[nTile].corr_neib[0];
    						dbg_2d_corr[3][nTile] = sfmCorr[nTile].corr_neib[1];
    					}
    				}
    				ShowDoubleFloatArrays.showArrays(
    						dbg_2d_corr,
    						tilesX,
    						tilesY,
    						true,
    						"sfm_corr-"+nset+"-"+scene_pairs_sets[nset][num_set_pairs[nset]-1][1].getImageName()+"-"+
    								scene_pairs_sets[nset][0][0].getImageName()+"-"+num_set_pairs[nset]+"-"+String.format("%02d", ntry),
    								dbg_titles);
    			}
    			
    	   		// combine corrections
        		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() {
        					for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) {
        						if (nTile == dbg_tile) {
        							System.out.println("sfmPairsSet().1: dbg_tile="+dbg_tile);
        						}
        						if (sfmCorr[nTile] != null){
        							if (sfmCorrCombo[nTile]==null) {
        								sfmCorrCombo[nTile] = new SfmCorr();
        							}
									if (sfmCorr[nTile].corr_ind != null) {
										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;
											if (sfmCorr[nTile].sfm_gain > sfmCorrCombo[nTile].sfm_gain) {
												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;
											if (sfmCorr[nTile].sfm_gain > sfmCorrCombo[nTile].sfm_gain) {
												sfmCorrCombo[nTile].sfm_gain = sfmCorr[nTile].sfm_gain; 
											}
										}
									}
        						}
        					}
        				}
        			};
        		}		      
        		ImageDtt.startAndJoin(threads);
    		} // for (int nset=0; nset < num_sets; nset++) 
    		double [][] dbg_extra = show_disp_corr? new double [6][]: null;
    		if (dbg_extra !=null) {
    			dbg_extra[0] = ref_disparity.clone();
    		}
    		// combine corrections
    		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
    		final AtomicInteger ai = new AtomicInteger(0);
    		final double wscale = -0.5/Math.log(prev_sfm_frac);
    		for (int ithread = 0; ithread < threads.length; ithread++) {
    			threads[ithread] = new Thread() {
    				public void run() {
    					for (int nTile = ai.getAndIncrement(); nTile < sfmCorrCombo.length; nTile = ai.getAndIncrement()) {
    						if (nTile == dbg_tile) {
    							System.out.println("sfmPairsSet().2: dbg_tile="+dbg_tile);
    						}
    						if (sfmCorrCombo[nTile] != null){
    							if (sum_weights_ind[nTile] > 0) { // do it now just for debug images
    								sfmCorrCombo[nTile].corr_ind[0] /= sum_weights_ind[nTile];
    								sfmCorrCombo[nTile].corr_ind[1] /= sum_weights_ind[nTile];
    							}
    							if (sum_weights_neib[nTile] > 0) {
    								sfmCorrCombo[nTile].corr_neib[0] /= sum_weights_neib[nTile];
    								sfmCorrCombo[nTile].corr_neib[1] /= sum_weights_neib[nTile];
    							}
    							if (sfmCorrCombo[nTile].sfm_gain >= prev_sfm_frac*ref_sfm_gain[nTile]){
									double w = 1.0;
									if (ref_sfm_gain[nTile] > 0) {
										w = same_weight + wscale*Math.log(sfmCorrCombo[nTile].sfm_gain/ref_sfm_gain[nTile]);
									}
									if      (w < 0.0) w = 0;
									else if (w > 1.0) w = 1.0;
									// Update some strength too?
									if ((sfmCorrCombo[nTile].corr_ind != null) && (sfmCorrCombo[nTile].corr_ind[1] > min_strength)) {
										ref_disparity[nTile] +=  w *sfmCorrCombo[nTile].corr_ind[0];
										// TODO: add same (as above) for strengths?
										ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorrCombo[nTile].sfm_gain; 
										corr_mode[nTile] = MODE_STRONG;
									}  else if ((sfmCorrCombo[nTile].corr_neib != null) && (sfmCorrCombo[nTile].corr_neib[1] > min_neib_strength)) {
										ref_disparity[nTile] +=  w *sfmCorrCombo[nTile].corr_neib[0];
										// TODO: add same (as above) for strengths?
										ref_sfm_gain[nTile] = (1-w) *ref_sfm_gain[nTile] + w * sfmCorrCombo[nTile].sfm_gain;
										corr_mode[nTile] = MODE_NEIB;
									} else {
										corr_mode[nTile] = MODE_WEAK;
									}
    							}
    						}
    					}
    				}
    			};
    		}		      
    		ImageDtt.startAndJoin(threads);
    		if (dbg_extra !=null) {
    			dbg_extra[1] = ref_disparity.clone();
    		}
    		if (average_neibs) {
    			averageNeighborsDisparity(
    					ref_disparity, // final double [] ref_disparity,
    					corr_mode,     // final int []    corr_mode,
    					tilesX,        // final int       tilesX,
    					tilesY);       // final int       tilesY
    		}
    		// now fill weak tiles (MODE_WEAK) from MODE_NEIB neighbors
    		// repeat until any new tiles
    		// then - try from strong tiles - repeat until new
    		// ************************************
    		if (dbg_extra !=null) {
    			dbg_extra[2] = ref_disparity.clone();
    			dbg_extra[3] = new double [corr_mode.length];
    			for (int i = 0; i < corr_mode.length; i++) {
    				dbg_extra[3][i] = corr_mode[i];
    			}
    		}
    		boolean [] defined = null;
    		if (fill_weak) {
    			defined = 	fillWeakTiles(
    					ref_disparity,  // final double [] ref_disparity,
    					corr_mode,      // final int []    corr_mode,
    					min_neibs_fill, // final int       min_neibs_fill,
    					tilesX,         // final int       tilesX,
    					tilesY);        // final int       tilesY
    			if (dbg_extra !=null) {
    				dbg_extra[4] = ref_disparity.clone();
    				dbg_extra[5] = new double [corr_mode.length];
    				for (int i = 0; i < defined.length; i++) {
    					dbg_extra[5][i] = defined[i]? 1.0:0.0;
    				}
    			}
    		}
    		
    		// Can be done once after all iteration, but one row can help neighbors
    		// may make max_steps= 1 for all but the last run
    		if (extrapolate) {
    			if (defined == null) {
    				defined = new boolean[corr_mode.length];
    				for (int i = 0; i < corr_mode.length; i++) {
    					defined[i] = (corr_mode[i] > 0);
    				}
    			}
    			int total_filled = extrapolateEdges(
    					ref_disparity, // final double []  ref_disparity,
    					defined,       // final boolean [] defined,
    					extrapolate_radius,        // final int        radius,
    					extrapolate_steps,     // final int        max_steps,
    					tilesX,        // final int        tilesX,
    					tilesY,        // final int        tilesY,
    					zdisp,         //final double     zdisp,       // product of z and disparity
    					inf_disp);     // final double     inf_disp     // disparity at infinity
    			if (debugLevel > -3) {
    				System.out.println("extrapolateEdges() -> "+total_filled+" new tiles");
    			}
    		}
    		
			if (show_disp_corr) {
				String [] dbg_titles = {
						"corr", "corr_str","neib_corr", "neib_str", // 0-3
						"disp_before","disp_init","disp_avg","disp_weak","disp_extrap", // 4-8
						"old_sfm","new_sfm", "corr_mode","defined"}; //9-12
				double [][] dbg_2d_corr = new double [dbg_titles.length][sfmCorrCombo.length];
				for (int i = 0; i < dbg_2d_corr.length; i++) {
					Arrays.fill(dbg_2d_corr[i], Double.NaN);
				}
				dbg_2d_corr[9] = ref_sfm_gain.clone();
				for (int nTile = 0; nTile < sfmCorrCombo.length; nTile++) if (sfmCorrCombo[nTile] != null) {
					dbg_2d_corr[10][nTile] = sfmCorrCombo[nTile].sfm_gain;
					if (sfmCorrCombo[nTile].corr_ind != null) {
						dbg_2d_corr[0][nTile] = sfmCorrCombo[nTile].corr_ind[0];
						dbg_2d_corr[1][nTile] = sfmCorrCombo[nTile].corr_ind[1];
					}
					if (sfmCorrCombo[nTile].corr_neib != null) {
						dbg_2d_corr[2][nTile] = sfmCorrCombo[nTile].corr_neib[0];
						dbg_2d_corr[3][nTile] = sfmCorrCombo[nTile].corr_neib[1];
					}
					dbg_2d_corr[11][nTile] = corr_mode[nTile];
				}
				dbg_2d_corr[ 4] = dbg_extra[0];
				dbg_2d_corr[ 5] = dbg_extra[1];
				dbg_2d_corr[ 6] = dbg_extra[2];
				dbg_2d_corr[ 7] = dbg_extra[4];
				dbg_2d_corr[ 8] = ref_disparity.clone();
				dbg_2d_corr[12] = dbg_extra[5];
				ShowDoubleFloatArrays.showArrays(
						dbg_2d_corr,
						tilesX,
						tilesY,
						true,
						"sfm_corr-combo-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
								scene_pairs_sets[0][0][0].getImageName()+"-"+String.format("%02d", ntry),
								dbg_titles);
			}
			
    		// replace weak tiles with average?
    		if (disp_adj != null) {
    			disp_adj[ntry + 1] = ref_disparity.clone();
    		}
			if (debugLevel > -3) {
				System.out.println("sfmPairsSet(): Finished pass "+(ntry + 1)+" (of "+ num_readjust+")");
			}
    	} // ntry
    	if (disp_adj != null) {
    		disp_adj[disp_adj.length-1] = ref_sfm_gain;
    		String [] tiltes = new String[disp_adj.length];
    		tiltes[0] = "orig";
    		tiltes[disp_adj.length-1] = "gain";
        	for (int ntry = 0; ntry < num_readjust; ntry++) {
        		tiltes[ntry + 1]="pass-"+ntry;
        	}
//        	String dbg_title = "SfM-disparity-multi-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
//					scene_pairs_sets[0][num_set_pairs[0]-1][0].getImageName()+"-"+num_set_pairs.length;
        	//ref_scene.
        	String suffix = String.format("-SfM%02d-%02d", num_set_pairs.length, ref_scene.getNumAccum());
        	String title = ref_scene.getImageName()+suffix;
        	
        	if (show_disp_seq) {
        		ShowDoubleFloatArrays.showArrays(
        				disp_adj,
        				tilesX,
        				tilesY,
        				true,
        				title,
        				tiltes);
        	}
        	if (save_disp_seq) {
        		ref_scene.saveDoubleArrayInModelDirectory( // error
        				suffix,           // String      suffix,
        				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
        				combo_dsn_final,       // dbg_data,         // double [][] data,
        				tilesX,                // int         width,
        				tilesY);               // int         height)
        	}
    	}
    	// Re-calc BG?
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] =        ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA] =         ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_FG] =     ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP_BG_ALL] = ref_disparity.clone();
    	combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] =    ref_sfm_gain.clone(); 
    	
		String rslt_suffix = "-INTER-INTRA";
		rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
		ref_scene.saveDoubleArrayInModelDirectory( // error
				rslt_suffix,           // String      suffix,
				OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null,          // String []   labels, // or null
				combo_dsn_final,       // dbg_data,         // double [][] data,
				tilesX,                // int         width,
				tilesY);               // int         height)
        return combo_dsn_final;
	}
	
	public static int extrapolateEdges(
			final double []  ref_disparity,
			final boolean [] defined,
			final int        radius,
			final int        max_steps,
			final int        tilesX,
			final int        tilesY,
			final double     zdisp,       // product of z and disparity
			final double     inf_disp     // disparity at infinity
			) {
		final double [][] weights = new double [radius+1][radius+1];
		for (int i =0; i <= radius; i++) {
			double y = (1.0 * i) / (radius+1);   
			for (int j =0; j <= radius; j++) {
				double x = (1.0 * j) / (radius+1);  
				double r = Math.sqrt(x*x + y*y);
				if (r < 1.0) {
					weights[i][j] = Math.cos(r* Math.PI);
				}
			}
		}
		final TileNeibs tn = new TileNeibs(tilesX,tilesY);
    	final double normal_damping = 0.001; // pull to horizontal if not enough data 
    	final double [] damping = new double [] {normal_damping, normal_damping};
		final boolean [] new_defined = defined.clone();
      	final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger acounter = new AtomicInteger(0);
		final AtomicInteger afilled = new AtomicInteger(0);
		final AtomicInteger ai = new AtomicInteger(0);
		int total_filled = 0;
		for (int nstep=0; nstep < max_steps; nstep++) {
			acounter.set(0); // remains after last step;
			afilled.set(0);
			ai.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						double [][][] mdata = new double [(2 * radius + 1) * (2 * radius + 1)][][];
						PolynomialApproximation pa = new PolynomialApproximation();
						for (int nTile = ai.getAndIncrement(); nTile < ref_disparity.length; nTile = ai.getAndIncrement())
							if (!defined[nTile]) { // only add new that were not defined before
								// find if there is any defined neighbor
								boolean got_it = false;
								for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
									int tile1 = tn.getNeibIndex(nTile, dir);
									if ((tile1 >=0) && defined[tile1]){
										got_it=true;
										break;
									}
								}
								if (got_it) {
									Arrays.fill(mdata,  null);
									int mindx = 0;
									for (int idy = -radius; idy <= radius; idy++) {
										for (int idx = -radius; idx <= radius; idx++) {
											int tile1 = tn.getNeibIndex(nTile, idx, idy);
											if ((tile1 >= 0) && defined[tile1]) {
												double z = zdisp / (ref_disparity[nTile] + inf_disp);
												double w = weights[Math.abs(idy)][Math.abs(idx)];
												mdata[mindx] = new double[3][];
												mdata[mindx][0] = new double [2];
												mdata[mindx][0][0] =  idx;
												mdata[mindx][0][1] =  idy;
												mdata[mindx][1] = new double [1];
												mdata[mindx][1][0] =  z; 
												mdata[mindx][2] = new double [1];
												mdata[mindx][2][0] =w;
												mindx++;
											}
										}
									}
									double[][] approx2d = pa.quadraticApproximation(
											mdata,
											true,       // boolean forceLinear,  // use linear approximation
											damping,    // double [] damping, null OK
											-1);        // debug level
									new_defined[nTile] = true;
									// 	z_tilts= new double [] { approx2d[0][2], approx2d[0][0], approx2d[0][1]};
									//	double z = zdisp / (ref_disparity[nTile] + inf_disp);

									double z0 = approx2d[0][2];
									ref_disparity[nTile] = zdisp / z0 - inf_disp;
									afilled.getAndIncrement();
								} else {
									acounter.getAndIncrement();
								}

							}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			if (afilled.get() > 0) {
				total_filled+= afilled.get();
				System.arraycopy(new_defined,       0, defined,      0, ref_disparity.length);
			} else {
				break;
			}
		}
		return total_filled; // acounter.get() - remains not filled
	}
	
	public static boolean [] fillWeakTiles(
			final double [] ref_disparity,
			final int []    corr_mode,
	    	final int       min_neibs_fill, //  = 4;
			final int       tilesX,
			final int       tilesY
			){
		double []  nwo = {0.7, 0.5};
		final double []  dir_weights = new double[] {nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1]};
		final TileNeibs tn = new TileNeibs(tilesX,tilesY);
		final double [] ref_disparity_in = ref_disparity.clone();
  		final boolean [] defined = new boolean [tilesX*tilesY];
		final boolean [] new_defined = new boolean [tilesX*tilesY];
		final int max_try_fill = tilesX+tilesY;
      	final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger acounter = new AtomicInteger(0);
		final AtomicInteger afilled = new AtomicInteger(0);
		final AtomicInteger ai = new AtomicInteger(0);
		for (boolean weak_only: new boolean[] {true, false}) {
			// set initial defined, count number of undefined to be filled, update when switching to all from neibs only
//			Arrays.fill(defined, false);
			ai.set(0);
			acounter.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement())
							if (!defined[nTile]) { // only add new that were not defined before
								switch (corr_mode[nTile] ) {
								case MODE_STRONG:
									if (weak_only) break;
								case MODE_NEIB:
									defined[nTile] = true;
									break;
								case MODE_WEAK:
									acounter.getAndIncrement();
								}
							}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
    		if (acounter.get() == 0) {
    			continue; // no points to adjust
    		}
//			System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
			System.arraycopy(defined,       0, new_defined,      0, ref_disparity.length);
    		for (int nfill = 0; nfill < max_try_fill; nfill++) {
//    			System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
//    			System.arraycopy(defined,       0, new_defined,      0, ref_disparity.length);
    			ai.set(0);
    			acounter.set(0);
    			afilled.set(0);
        		for (int ithread = 0; ithread < threads.length; ithread++) {
        			threads[ithread] = new Thread() {
        				public void run() {
        					for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement()) 
        					if ((corr_mode[nTile] == MODE_WEAK ) && !defined[nTile]){
//        						acounter.getAndIncrement(); // left to fill
        						// see if any defined neighbor
        						int num_neibs = 0;
        						double sw=0, swd=0;
    							for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
    								int tile1 = tn.getNeibIndex(nTile, dir);
    								if ((tile1 >=0) && defined[tile1]){
    									double w = dir_weights[dir]; // use strength?
    									sw += w;
    									swd += w * ref_disparity_in[tile1];
    									num_neibs++;
    								}
    							}
    							if (num_neibs >=  min_neibs_fill) {
    								ref_disparity[nTile] = swd/sw;
    								new_defined[nTile] = true;
    								afilled.getAndIncrement();
    							} else {
    								acounter.getAndIncrement(); // left unfilled
    							}
        					}
        				}
        			};
        		}		      
        		ImageDtt.startAndJoin(threads);
        		if (afilled.get() > 0) {
        			System.arraycopy(ref_disparity, 0, ref_disparity_in, 0, ref_disparity.length);
        			System.arraycopy(new_defined,      0, defined,       0, ref_disparity.length);
        			if (acounter.get() == 0) {
        				break; // this run filled all remaining
        			}
        		} else {
        			break; // for (int nfill = 0; nfill < max_try_fill; nfill++) 
        		}
    		}
		}
//		return acounter.get();
		return defined;
	}
	
	public static void averageNeighborsDisparity(
			final double [] ref_disparity,
			final int []    corr_mode,
			final int       tilesX,
			final int       tilesY
			) {
		double []  nwo = {0.7, 0.5};
		final double [] dir_weights = new double[] {nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1],nwo[0],nwo[1]};
		final TileNeibs tn = new TileNeibs(tilesX,tilesY);
		final double [] ref_disparity_in = ref_disparity.clone();
      	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() {
					for (int nTile = ai.getAndIncrement(); nTile < corr_mode.length; nTile = ai.getAndIncrement())
						if (corr_mode[nTile] == MODE_NEIB){ // only neighbors
							double sw = 1, swd = ref_disparity_in[nTile];
							for (int dir = 0; dir < TileNeibs.DIRS; dir++) {
								int tile1 = tn.getNeibIndex(nTile, dir);
								if ((tile1 >=0) && (corr_mode[tile1] == MODE_NEIB)){
									double w = dir_weights[dir]; // use strength?
									sw += w;
									swd += w * ref_disparity_in[tile1];
								}
							}
							ref_disparity[nTile] = swd/sw;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
	}
	
	
	
	/**
	 * Calculate SfM disparity correction from a set of scene pairs with approximately
	 * the same position offset in each pair by consolidating 2D correlation data in
	 * Transform Domain before converting to the Pixel Domain. Optionally provide such
	 * correction for averaged with 8 immediate neighbors to deal with very low contrast
	 * areas. Additionally provide SfM gain (saved with the disparity data) to indicate
	 * estimated accuracy of disparity data.
	 * Reference scene disparity and scene poses are provided explicitly to allow tuning.
	 *   
	 * @param clt_parameters         processing parameters  
	 * @param ref_scene              reference scene instance
	 * @param ref_disparity          reference disparity array (NaN for undefined)
	 * @param ref_xyzatr             reference scene pose {{x,y,z},{a,t,r}}
	 * @param ref_xyzatr_dt          reference scene pose derivatives (for ERS)
	 * @param scene_pairs            pairs of scene instances to correlate ([1] to [0])
	 * @param scenes_xyzatr          pairs of scene poses relative to the reference one
	 * @param scenes_xyzatr_dt       pairs of scene poses derivatives (for ERS)
	 * @param sfm_min_gain           minimal SfM gain to apply SfM to the depth map
	 * @param sfm_min_frac           minimal fraction of defined tiles to have SfM correction
	 * @param mb_max_gain            motion blur correction maximal gain
	 * @param range_disparity_offset disparity at actual infinity. Here can be just 0.
	 * @param corr_fz_inter          Fat zero correction. Recommended value:
	 *                               (16* clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome())
	 * @param use_neibs              if true, will generate data for averaging between neighbors
	 * @param neib_too_strong        do not accumulate neighbor if it is too strong
	 * @param centroid_radius
	 * @param n_recenter
	 * @param batch_mode             batch mode (suppress all debug images) 
	 * @param debugLevel             debug level
	 * @return per tile array (sparse) of SfmCorr instances, containing tile's SfM gain,
	 *  individual disparity correction (to add to the current disparity)/strength and
	 *  optionally averaged between neighbors correction/strength. Returns null if there
	 *  insufficient tiles with high enough disparity gain. 
	 */
	public static SfmCorr [] getSfmCorr(
			final CLTParameters  clt_parameters,
			final QuadCLT        ref_scene,
			final double []      ref_disparity,
        	final double[][]     ref_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        	final double[][]     ref_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
			final QuadCLT[][]    scene_pairs,
        	final double[][][][] scenes_xyzatr,   // new double[num_pairs][2][][];   // 2 scenes
        	final double[][][][] scenes_xyzatr_dt, // new double[num_pairs][2][][];// 2 scenes
        	final double         sfm_min_gain,
        	final double         sfm_min_frac,
        	final double         mb_max_gain,
        	final double         range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;			final double         mb_max_gain,
			final double         corr_fz_inter,
			final boolean        use_neibs,
			final double         neib_too_strong,
			final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
			final int            n_recenter,      // when cosine window, re-center window this many times			 
			final boolean        batch_mode,
			final int            debugLevel) {
		
		final int  num_pairs = scene_pairs.length;
		double []  neib_weights_od = {0.7, 0.5};
		final int  corr_size = 2 * ref_scene.tp.getTileSize() -1;
		int tilesX = ref_scene.tp.getTilesX();
    	// only for derivatives
    	double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
    			null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
    			ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
    			ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
    			ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
    			ref_scene,     // final QuadCLT     scene_QuadClt,
    			ref_scene);    // final QuadCLT     reference_QuadClt)
    	double [][][] dpXYddisp = new double [num_pairs][][]; // will be averaged between all pairs
    	TDCorrTile [] accum_TD = null;
    	for (int npair = 0; npair < num_pairs; npair++) {
    		dpXYddisp[npair] = getSfmDpxDpyDdisp( // will be averaged
        			scenes_xyzatr[0],          // final double [][][] scenes_xyzatr,
        			scene_pairs[0][0], // final QuadCLT       scene0,
        			scene_pairs[0][1], // final QuadCLT       scene1,
        			ref_scene,              // final QuadCLT       ref_QuadClt,
        			ref_pXpYD,              // final double [][]   ref_pXpYD, // centers
        			range_disparity_offset, // final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
        			batch_mode,             // final boolean       batch_mode,
        			debugLevel);            // final int           debug_level
    		QuadCLT [] scenes = scene_pairs[npair];
    		TDCorrTile [] pair_TD =sfmPair_TD( // scene0/scene1
    				clt_parameters,  // final CLTParameters          clt_parameters,
    				ref_scene,       // final QuadCLT                ref_scene,
    				ref_disparity,   // final double []              ref_disparity, // here can not be null
    				ref_xyzatr,      // final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
    				ref_xyzatr_dt,   // final double [][]            ref_xyzatr_dt,
    				scenes,          // final QuadCLT []             scenes,          // 2 scenes
    				scenes_xyzatr[npair],   //final double[][][]	         scenes_xyzatr,   // 2 scenes
    				scenes_xyzatr_dt[npair],// final double[][][]	         scenes_xyzatr_dt,// 2 scenes
    				mb_max_gain,     // double                       mb_max_gain,
    				batch_mode,      // final boolean                batch_mode,
    				debugLevel);     // final int                    debugLevel);
    		if (npair == 0 ) {
    			accum_TD = pair_TD;
    		} else {
    			TDCorrTile.accumulate (accum_TD, pair_TD);
    		}
    	}
    	double [][] accum_PD = TDCorrTile.convertTDtoPD(
				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
				accum_TD,           // final TDCorrTile [] tiles,
				0xFE,               // final int           corr_type, // 0xFE
				corr_fz_inter,      // final double        gpu_fat_zero,
				debugLevel);        // final int           debug_level
    	double [][] neibs_PD = null;
    	if (use_neibs) {
    		TDCorrTile [] neibs_TD = TDCorrTile.calcNeibs(
    				accum_TD,            // pairs_TD,        // TDCorrTile [] tiles,
    				tilesX,              // final int     tilesX,
    				neib_weights_od,     // double []     neib_weights_od, // {orhto, diag}
    				accum_PD,            // double [][]   corr_pd,
    				neib_too_strong,     // double        neib_too_strong,
    				true);               // final boolean process_all
    		neibs_PD = TDCorrTile.convertTDtoPD(
    				ref_scene.getGPU(), // final GpuQuad       gpuQuad,
    				neibs_TD,           // final TDCorrTile [] tiles,
    				0xFE,               // final int           corr_type, // 0xFE
    				corr_fz_inter,      // final double        gpu_fat_zero,
    				debugLevel);        // final int           debug_level
    	}
    	final double [][] dpXYddisp_avg = new double [accum_PD.length][];
		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() {
					for (int nTile = ai.getAndIncrement(); nTile < accum_PD.length; nTile = ai.getAndIncrement()) {
						double [] sxy = new double[2];
						int na = 0;
						for (int npair = 0; npair < num_pairs; npair++) {
							if (dpXYddisp[npair][nTile] != null) {
								for (int i = 0; i < 2; i++) {
									sxy[i]+=dpXYddisp[npair][nTile][i];
								}
								na++;
							}
						}
						if (na > 0) {
							for (int i = 0; i < 2; i++) {
								sxy[i] /= na;
							}
							dpXYddisp_avg[nTile] = sxy;
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		
    	final SfmCorr [] sfmCorr = new SfmCorr [accum_PD.length];
    	final double [][][] data_PD = (neibs_PD == null) ? (new double[][][] {accum_PD}):(new double[][][] {accum_PD,neibs_PD});
    	boolean show_2d_correlations = debugLevel>10;
       	if (show_2d_correlations) {
       		String [] dbg_titles = (neibs_PD == null) ? (new String[] {"accum"}):(new String[] {"accum","neibs"});
    		double [][] dbg_2d_corrs = ImageDtt.corr_partial_dbg( // not used in lwir
    				data_PD,                           // final double [][][]     corr_data,       // [layer][tile][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
    				tilesX,                              // final int               tilesX,
    				corr_size,                           //final int               corr_size,       // 15
    				clt_parameters.corr_border_contrast, // final double            border_contrast,
    				debugLevel);                         // final int               globalDebugLevel)
    		int tilesY = data_PD[0].length/tilesX;
    		ShowDoubleFloatArrays.showArrays(
    				dbg_2d_corrs,
    				tilesX * (corr_size + 1),
    				tilesY * (corr_size + 1),
    				true,
    				"sfm_corr2d-"+scene_pairs[num_pairs-1][1].getImageName()+"-"+
    						scene_pairs[0][0].getImageName()+"-"+num_pairs+"_fz"+((int) corr_fz_inter),
    						dbg_titles);
    	}
   	
    	
    	
    	final double [][][] disp_corr = new double [data_PD.length][][];
    	for (int i = 0; i < data_PD.length; i++) {
    		disp_corr[i] = getSfmDisparityCorrectionStrength(
    				clt_parameters,  // final CLTParameters  clt_parameters,
    				data_PD[i],      // final double [][]    corr2d_PD,
    				dpXYddisp_avg,   // final double [][]    dpXYddisp,
    	   			sfm_min_gain,           // final double         sfm_min_gain,
    	   			sfm_min_frac,           // final double         sfm_min_frac,
    				centroid_radius, // final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
    				n_recenter,      // final int            n_recenter,      // when cosine window, re-center window this many times
    				corr_size,       //final int            corr_size, // 15
    				debugLevel);     // final int            debugLevel);
    		if (disp_corr[i] == null) {
    			if (debugLevel > -3) {
    				System.out.println("getSfmCorr(): not enough SfM tiles, bailing out");
    			}
    			return null;
    		}
    	}
    	
    	ai.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < accum_PD.length; nTile = ai.getAndIncrement()) {
//						if (nTile==4600) {
//							System.out.println("getSfmCorr(): nTile="+nTile);
//						}
						if ((disp_corr[0][nTile] != null) || (use_neibs && (disp_corr[1][nTile] != null) )) {
							sfmCorr[nTile] = new SfmCorr();
							double [] sxy = dpXYddisp_avg[nTile];
							sfmCorr[nTile].sfm_gain = Math.sqrt(sxy[0]*sxy[0] + sxy[1]*sxy[1]);
							sfmCorr[nTile].corr_ind = disp_corr[0][nTile];
							if (use_neibs) {
								sfmCorr[nTile].corr_neib = disp_corr[1][nTile];
							}
						}
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
    	return sfmCorr;
	}
	
	public static void shrinkFadeSfmGain(
			final int        sfm_shrink,
			final double     fade_sigma, 
			final SfmCorr [] sfmCorr,
			final int        tilesX) {
		final int extra = sfm_shrink + (int)Math.ceil(fade_sigma);
		final int        tilesY = sfmCorr.length / tilesX;
		
		final int        tilesX_extra = tilesX+2*extra;
		final int        tilesY_extra = tilesY+2*extra;
		final boolean [] sfm_defined_extra = new boolean [tilesX_extra*tilesY_extra];
		final TileNeibs tn = new TileNeibs(tilesX_extra,tilesY_extra);
		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() {
					for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) if (sfmCorr[nTile] != null){
						int tileX = nTile % tilesX + extra;
						int tileY = nTile / tilesX + extra;
						int nTile_extra=tileX + tileY * tilesX_extra;
						sfm_defined_extra[nTile_extra] = sfmCorr[nTile].sfm_gain > 0;
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		if (sfm_shrink > 0) {
			tn.shrinkSelection(
					sfm_shrink,        // final int        shrink,           // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
					sfm_defined_extra, // final boolean [] tiles,  
					null);           // final boolean [] prohibit)
		}
		if (fade_sigma > 0) {
			final double [] gain_scale = new double [sfm_defined_extra.length];
			ai.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < sfm_defined_extra.length; nTile = ai.getAndIncrement()) if (sfm_defined_extra[nTile]){
							gain_scale[nTile] = 2.0;
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
			DoubleGaussianBlur gb = new DoubleGaussianBlur(); 
			gb.blurDouble(gain_scale, tilesX_extra, tilesY_extra, fade_sigma, fade_sigma, 0.01);
			ai.set(0);
			for (int ithread = 0; ithread < threads.length; ithread++) {
				threads[ithread] = new Thread() {
					public void run() {
						for (int nTile = ai.getAndIncrement(); nTile < sfmCorr.length; nTile = ai.getAndIncrement()) if (sfmCorr[nTile] != null){
							int tileX = nTile % tilesX + extra;
							int tileY = nTile / tilesX + extra;
							int nTile_extra=tileX + tileY * tilesX_extra;
							if (sfm_defined_extra[nTile_extra]) {
								double g = gain_scale[nTile_extra]-1;
								sfmCorr[nTile].sfm_gain *= g * g;
							} else {
								sfmCorr[nTile].sfm_gain = 0.0; // null; // or use sfmCorr[nTile] = null?
							}
						}
					}
				};
			}		      
			ImageDtt.startAndJoin(threads);
		}
	}
	
	
	
	
	/**
	 * Calculate disparity correction from the pixel-domain 2D correlation data
	 * and the SfM gain vector array (nominal disparity pixels per SfM correlation
	 * pixels in X and Y directions.
	 * @param clt_parameters   processing parameters 
	 * @param corr2d_PD        per-tile sparse array of the 2D correlation results 
	 * @param dpXYddisp        per-tile sparse array of the X,Y components of the SfM gain
	 * @param sfm_min_gain     minimal SfM gain to apply SfM to the depth map
	 * @param sfm_min_frac     minimal fraction of defined tiles to have SfM correction
	 * @param centroid_radius  windowing for centroid argmax: 0 - use all tile, >0 - cosine
	 *                         window around local max 
	 * @param n_recenter       number of refining of the argmax
	 * @param corr_size        2d correlation tile size (side of a square, normally 15) 
	 * @param debugLevel       debug level
	 * @return   per tile sparse array of {disparity_correction, strength} pairs. Will return
	 *           null if number of sfm tiles is less than sfm_min_frac of defined tiles             
	 */
	public static double [][] getSfmDisparityCorrectionStrength(
			final CLTParameters  clt_parameters,
			final double [][]    corr2d_PD,
			final double [][]    dpXYddisp,
        	final double         sfm_min_gain,
        	final double         sfm_min_frac,
			final double         centroid_radius, // 0 - use all tile, >0 - cosine window around local max
			final int            n_recenter,      // when cosine window, re-center window this many times
			final int            corr_size, // 15
			final int            debugLevel) {
		final double [][] disp_corr = new double [corr2d_PD.length][];
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger anum_defined = new AtomicInteger(0);
		final AtomicInteger anum_sfm =     new AtomicInteger(0);
		final double sfm_min_gain2 = sfm_min_gain * sfm_min_gain;
		
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int nTile = ai.getAndIncrement(); nTile < corr2d_PD.length; nTile = ai.getAndIncrement())
						if ((corr2d_PD[nTile] != null) && (dpXYddisp[nTile] != null)){
							double [] v =     dpXYddisp[nTile];
							double l2 =       v[0]*v[0]+ v[1]*v[1];
							double [] inv_v = {v[0]/l2, v[1]/l2}; 
							double [] corr_cm = Correlation2d.getMaxProjCm(
									corr2d_PD[nTile], // double []  data,
									corr_size,        // int        data_width,   //  = 2 * transform_size - 1;
									centroid_radius,  // double     radius,       //  0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
									n_recenter,       // int        refine,       //  re-center window around new maximum. 0 -no refines (single-pass)
									inv_v,            // double []  direction_XY,
									debugLevel > 2);  // boolean    debug)
							if (corr_cm != null){
								anum_defined.getAndIncrement();
								if (l2 >= sfm_min_gain2) {
									anum_sfm.getAndIncrement();
									disp_corr[nTile] = corr_cm; // may be null
								}
							}
						}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		if ((anum_sfm.get() == 0) || (anum_sfm.get() < anum_defined.get() * sfm_min_frac)) {
			return null;
		}
		return disp_corr;
	}
	
	
	/**
	 * Calculate a transform-domain cross-correlation of 2 scenes, keep only all-channels combined
	 * as a TDCorrTile [] sparse array. Each non-null element contains weight (normally 16 as number of
	 * channels combined) and 256 elements of the TD representation of the correlation data.
	 * Reference scene disparity and scene poses are provided explicitly to allow tuning.
	 * Both scenes are separate from the reference scene (TODO: verify when 1 is the reference) 
	 * 
	 * @param clt_parameters   processing parameters  
	 * @param ref_scene        reference scene instance
	 * @param ref_disparity    reference disparity array (NaN for undefined)
	 * @param ref_xyzatr       reference scene [2][3] pose {{x,y,z},{a,t,r}}
	 * @param ref_xyzatr_dt    reference scene pose derivatives (for ERS)
	 * @param scenes           pair of scene instances to correlate ([1] to [0])
	 * @param scenes_xyzatr    pair of scene poses relative to the reference one
	 * @param scenes_xyzatr_dt pair of scene poses derivatives (for ERS)
	 * @param mb_max_gain      motion blur correction maximal gain
	 * @param batch_mode       batch mode (suppress all debug images) 
	 * @param debugLevel       debug level
	 * @return TDCorrTile [] sparse array representing a transform-domain interscene correlation
	 *         result.
	 */
	public static TDCorrTile [] sfmPair_TD( // scene0/scene1
			final CLTParameters          clt_parameters,
			final QuadCLT                ref_scene,
			final double []              ref_disparity, // here can not be null
			final double [][]            ref_xyzatr, // {ZERO3,ZERO3};
			final double [][]            ref_xyzatr_dt,
			final QuadCLT []             scenes,          // 2 scenes
			final double[][][]	         scenes_xyzatr,   // 2 scenes
			final double[][][]	         scenes_xyzatr_dt,// 2 scenes
			double                       mb_max_gain,
			final boolean                batch_mode,
			final int                    debugLevel) {
		int                imp_debug_level = 0; // MANUALLY change to 2 for debug!
		boolean show_render_ref =      !batch_mode && clt_parameters.imp.renderRef(imp_debug_level); // false; //true;
		boolean show_render_scene =    !batch_mode && clt_parameters.imp.renderScene(imp_debug_level); // false; // true;
		boolean toRGB = true;		
		boolean mb_en =       clt_parameters.imp.mb_en;
		double  mb_tau =      clt_parameters.imp.mb_tau;      // 0.008; // time constant, sec
		int     margin =      clt_parameters.imp.margin;
		int        sensor_mask_inter = clt_parameters.imp.sensor_mask_inter ; //-1;
        double [][][] scenes_pXpYD = new double [scenes.length][][]; 
        // ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
		double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
        		null, // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
        		ref_disparity, // dls[0],  // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
        		ZERO3,         // final double []   scene_xyz, // camera center in world coordinates
        		ZERO3,         // final double []   scene_atr, // camera orientation relative to world frame
        		ref_scene,     // final QuadCLT     scene_QuadClt,
        		ref_scene);    // final QuadCLT     reference_QuadClt)
        
		@SuppressWarnings("unused")
		TpTask[][] tp_tasks_ref; // only were used for the FPN removal
		double [][][]  mb_vectors_scenes = new double [scenes.length][][];
		
		for (int nscene = 0; nscene < scenes.length; nscene++) {
			scenes_pXpYD[nscene] = OpticalFlow.transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
					null,                     // final Rectangle [] extra_woi,    // show larger than sensor WOI (or null)
					ref_disparity,            // dls[0], // final double []   disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
					scenes_xyzatr[nscene][0], // final double []   scene_xyz, // camera center in world coordinates
					scenes_xyzatr[nscene][1], // final double []   scene_atr, // camera orientation relative to world frame
					scenes[nscene],           // final QuadCLT     scene_QuadClt,
					ref_scene);               // final QuadCLT     reference_QuadClt)
					
			if (mb_en) {
				mb_vectors_scenes[nscene] = OpticalFlow.getMotionBlur(
						ref_scene,                  // QuadCLT        ref_scene,
						scenes[nscene],             // QuadCLT        scene,         // can be the same as ref_scene
						ref_pXpYD,                  // double [][]    ref_pXpYD,     // here it is scene, not reference!
						scenes_xyzatr[nscene][0],   // double []      camera_xyz,
						scenes_xyzatr[nscene][1],   // double []      camera_atr,
						scenes_xyzatr_dt[nscene][0],// double []      camera_xyz_dt,
						scenes_xyzatr_dt[nscene][1],// double []      camera_atr_dt,
						0,                          // int            shrink_gaps,  // will gaps, but not more that grow by this
						debugLevel);                // int            debug_level)
			}
		}
		// remove tiles from both that are null for any
		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() {
					for	(int nTile = ai.getAndIncrement(); nTile < ref_pXpYD.length; nTile = ai.getAndIncrement())
						if (ref_pXpYD[nTile] != null){
							boolean undefined = false;
							for (int nscene = 0; nscene < scenes.length; nscene++) {
								if (scenes_pXpYD[nscene][nTile] == null) {
									undefined=true;
									break;
								}
							}
							if (undefined) {
								for (int nscene = 0; nscene < scenes.length; nscene++) {
									scenes_pXpYD[nscene][nTile] = null;
								}								
							}
						
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);

		
		tp_tasks_ref = // were used only for the FPN removal
				Interscene.setReferenceGPU (
				clt_parameters,      // CLTParameters      clt_parameters,			
				scenes[0],           // ref_scene,                 // QuadCLT            ref_scene,
				null,                // ref_disparity,             // double []          ref_disparity, // null or alternative reference disparity
				scenes_pXpYD[0],     // ref_pXpYD,                 // double [][]        ref_pXpYD,
				null, 			    // final float [][]          fclt, 
				null,                // reliable_ref,              // final boolean []   selection, // may be null, if not null do not  process unselected tiles
				margin,              // final int          margin,
				// motion blur compensation 
				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
				mb_vectors_scenes[0],// mb_vectors_ref,            // double [][]        mb_vectors,  // now [2][ntiles];
				debugLevel);         // int         debugLevel)
		// is it needed here?
		scenes[0].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry

		ImageDtt image_dtt;
		image_dtt = new ImageDtt(
				ref_scene.getNumSensors(), // ,
				clt_parameters.transform_size,
				clt_parameters.img_dtt,
				ref_scene.isAux(),
				ref_scene.isMonochrome(),
				ref_scene.isLwir(),
				clt_parameters.getScaleStrength(ref_scene.isAux()),
				ref_scene.getGPU());
		image_dtt.getCorrelation2d(); // initiate image_dtt.correlation2d, needed if disparity_map != null
		//ref_scene.getGPU() can not be null
		ref_scene.getGPU().setGpu_debug_level(debugLevel - 4); // monitor GPU ops >=-1
//		final double disparity_corr = clt_parameters.imp.disparity_corr; // 04/07/2023 // 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
		final double disparity_corr = clt_parameters.imp.disparity_corr + ref_scene.getDispInfinityRef() ; // 12/11/2025 - added ref_scene.getDispInfinityRef()
		final double gpu_sigma_corr =     clt_parameters.getGpuCorrSigma(ref_scene.isMonochrome());
		final double gpu_sigma_rb_corr =  ref_scene.isMonochrome()? 1.0 : clt_parameters.gpu_sigma_rb_corr;
		final double gpu_sigma_log_corr = clt_parameters.getGpuCorrLoGSigma(ref_scene.isMonochrome());
		
		TpTask[][] tp_tasks;
		if (mb_en && (mb_vectors_scenes[1]!=null)) {
			tp_tasks  =  GpuQuad.setInterTasksMotionBlur(
					scenes[1].getNumSensors(),
					scenes[1].getErsCorrection().getSensorWH()[0],
					!scenes[1].hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scenes_pXpYD[1],              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					null,                         // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					// motion blur compensation 
					mb_tau,                       // final double              mb_tau,      // 0.008; // time constant, sec
					mb_max_gain,                  // final double              mb_max_gain, // 5.0;   // motion blur maximal gain (if more - move second point more than a pixel
					mb_vectors_scenes[1],         // final double [][]         mb_vectors,  //
					scenes[1].getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					THREADS_MAX);                 // final int                 threadsMax)  // maximal number of threads to launch
		} else {
			tp_tasks = new TpTask[1][];
			tp_tasks[0]  =  GpuQuad.setInterTasks(
					scenes[1].getNumSensors(),
					scenes[1].getErsCorrection().getSensorWH()[0],
					!scenes[1].hasGPU(),          // final boolean             calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
					scenes_pXpYD[1],              // final double [][]         pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
					null, // selection,           // final boolean []          selection, // may be null, if not null do not  process unselected tiles
					scenes[1].getErsCorrection(), // final GeometryCorrection  geometryCorrection,
					disparity_corr,               // final double              disparity_corr,
					margin,                       // final int                 margin,      // do not use tiles if their centers are closer to the edges
					null,                         // final boolean []          valid_tiles,            
					THREADS_MAX);                 // final int                 threadsMax)  // maximal number of threads to launch
		}
		scenes[1].saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
		// Generate 2D phase correlations from the CLT representation
		// generates sum of the per-channel correlations as the last slot.
		// updates gpuQuad.gpu_corr_indices, gpuQuad.gpu_corrs_td and some other
		if (mb_en && (mb_vectors_scenes[1]!=null)) {
			image_dtt.interCorrTDMotionBlur(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks,                   // final TpTask[][]            tp_tasks,
					null,                       // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                // final int                 threadsMax,       // maximal number of threads to launch
					debugLevel);                // final int                 globalDebugLevel);
		} else {
			image_dtt.interCorrTD(
					clt_parameters.img_dtt,     // final ImageDttParameters  imgdtt_params,    // Now just extra correlation parameters, later will include, most others
					tp_tasks[0],                // final TpTask[]            tp_tasks,
					null,                       // final float  [][][][]     fcorr_td,        // [tilesY][tilesX][pair][4*64] transform domain representation of 6 corr pairs
					clt_parameters.gpu_sigma_r, // final double              gpu_sigma_r,     // 0.9, 1.1
					clt_parameters.gpu_sigma_b, // final double              gpu_sigma_b,     // 0.9, 1.1
					clt_parameters.gpu_sigma_g, // final double              gpu_sigma_g,     // 0.6, 0.7
					clt_parameters.gpu_sigma_m, // final double              gpu_sigma_m,     //  =       0.4; // 0.7;
					gpu_sigma_rb_corr,          // final double              gpu_sigma_rb_corr,    //  = 0.5; // apply LPF after accumulating R and B correlation before G, monochrome ? 1.0 :
					gpu_sigma_corr,             // final double              gpu_sigma_corr,       //  =    0.9;gpu_sigma_corr_m
					gpu_sigma_log_corr,         // final double              gpu_sigma_log_corr,   // hpf to reduce dynamic range for correlations
					clt_parameters.corr_red,    // final double              corr_red, // +used
					clt_parameters.corr_blue,   // final double              corr_blue,// +used
					sensor_mask_inter,          // final int                 sensor_mask_inter, // The bitmask - which sensors to correlate, -1 - all.
					THREADS_MAX,                // final int                 threadsMax,       // maximal number of threads to launch
					debugLevel);                // final int                 globalDebugLevel);
		}
		// get TD interscene correlation of 2 scenes, use only combo (all channels) data
		TDCorrTile [] tiles = TDCorrTile.getFromGpu(
				ref_scene.getGPU());

		if (show_render_ref) {
			ImagePlus imp_render_ref = scenes[0].renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					true, //boolean use_reference
					"GPU-SHIFTED-REF"); // String  suffix)
			imp_render_ref.show();
		}
		if (show_render_scene) { //set toRGB=false
			ImagePlus imp_render_scene = scenes[1].renderFromTD (
					-1,                  // final int         sensor_mask,
					false,               // boolean             merge_channels,
					clt_parameters,                                 // CLTParameters clt_parameters,
					clt_parameters.getColorProcParameters(ref_scene.isAux()), //ColorProcParameters colorProcParameters,
					clt_parameters.getRGBParameters(),              //EyesisCorrectionParameters.RGBParameters rgbParameters,
					null, // int []  wh,
					toRGB, // boolean toRGB,
					false, //boolean use_reference
					"GPU-SHIFTED-SCENE"); // String  suffix)
			imp_render_scene.show();
		}
		return tiles;
	}
	/**
	 * 	Get pX and pY derivatives with respect to disparity
	 * @param scenes_xyzatr           {scene0_xyzatr,scene1_xyzatr}
	 * @param scene0 scene0          instance of QuadCLT (used if scenes_xyzatr[0]== null. Needed even if scenes_xyzatr[i] != null
	 * @param scene1 scene0          instance of QuadCLT (used if scenes_xyzatr[0]== null. Needed even if scenes_xyzatr[i] != null
	 * @param ref_QuadClt            reference scene instance
	 * @param ref_pXpYD              sparce reference array of {pX,pY,Disparity}
	 * @param range_disparity_offset disparity at infinity
	 * @param batch_mode             batch mode (reduce images)
	 * @param debug_level            debug level
	 * @return sparse per-tile array of dx, dy pairs or nulls
	 */
	public static double [][]   getSfmDpxDpyDdisp(
			final double [][][] scenes_xyzatr, // cameras center in world coordinates (or null to use instance)
			final QuadCLT       scene0,
			final QuadCLT       scene1,
			final QuadCLT       ref_QuadClt,
			final double [][]   ref_pXpYD, // centers
			final double        range_disparity_offset, // disparity at actual infinity // clt_parameters.imp.range_disparity_offset ;
			final boolean       batch_mode,
			final int           debug_level
			){
		boolean debug_img = !batch_mode && (debug_level > 0);
		double [][] dbg_img = debug_img? (new double[6] [ref_pXpYD.length]):null;
		if (dbg_img != null) {
			for (int i = 0; i <dbg_img.length;i++) {
				Arrays.fill(dbg_img[i],Double.NaN);
			}
		}
		final QuadCLT[]     scenes = {scene0, scene1};
		boolean[] param_select = new boolean[ErsCorrection.DP_NUM_PARS];
		final int [] par_indices = new int[] {
				ErsCorrection.DP_DD};
		for (int i: par_indices) {
			param_select[i]=true;
		}
		final double [][][] last_jts = new double [scenes.length][][];
		IntersceneLma intersceneLma= new IntersceneLma(
				false,            // clt_parameters.ilp.ilma_thread_invariant);
				0.0);             // always no disparity
		for (int nscene = 0; nscene < scenes.length; nscene++) {
			// try if one is reference?
			if (scenes[nscene].getImageName().equals(ref_QuadClt.getImageName())) {
				last_jts[nscene] = new double[1][2 * ref_pXpYD.length];
			} else {
				intersceneLma.prepareLMA(
						scenes_xyzatr[nscene][0], // final double []   scene_xyz0,     // camera center in world coordinates (or null to use instance)
						scenes_xyzatr[nscene][1], // final double []   scene_atr0,     // camera orientation relative to world frame (or null to use instance)
						null,             // final double []   scene_xyz_pull, // if both are not null, specify target values to pull to 
						null,             // final double []   scene_atr_pull, // 
						scenes[nscene],   // final QuadCLT     scene_QuadClt,
						ref_QuadClt,      // final QuadCLT     reference_QuadClt,
						param_select,     // final boolean[]   param_select,
						null,             // final double []   param_regweights,
						null,             // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
						0 , // final double      min_confidence,// minimal tile confidence to use. Use 0 to disable
						ref_pXpYD,        // final double [][] centers,    // macrotile centers (in pixels and average disparities
						false,            // final boolean     same_weights,
						false,            // boolean           first_run,
						debug_level);     // final int         debug_level)
				last_jts[nscene] = intersceneLma. getLastJT(); // alternating x,y for each selected parameters
			}
		}
		
		int [] sensor_wh =    ref_QuadClt.getGeometryCorrection().getSensorWH();
		final double width =  sensor_wh[0];
		final double height = sensor_wh[1];
		final double min_disparity = .05; // -0.5;
		final double max_disparity = 100.0;
		final double [][] dpXYddisp = new double [ref_pXpYD.length][];
		
		final int debug_tile = -1;
		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() {
					for (int nTile = ai.getAndIncrement(); nTile < ref_pXpYD.length; nTile = ai.getAndIncrement()) if (ref_pXpYD[nTile] != null){
						if (nTile == debug_tile) {
							System.out.println("getSfmDpxDpyDdisp(): nTile="+nTile);
						}
						double disp_eff = ref_pXpYD[nTile][2] - range_disparity_offset;
						if (    (ref_pXpYD[nTile][0] < 0) || (ref_pXpYD[nTile][0] >= width) ||
								(ref_pXpYD[nTile][1] < 0) || (ref_pXpYD[nTile][1] >= height) ||
								(disp_eff < min_disparity) || (disp_eff >= max_disparity)) {
							continue;
						}
						dpXYddisp[nTile] = new double [2];
						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];
				            dbg_img[2][nTile] = last_jts[0][0][2*nTile + 0]; 
				            dbg_img[3][nTile] = last_jts[0][0][2*nTile + 1]; 
				            dbg_img[4][nTile] = last_jts[1][0][2*nTile + 0]; 
				            dbg_img[5][nTile] = last_jts[1][0][2*nTile + 1]; 
				        }
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		if (dbg_img != null) {
			int tilesX = ref_QuadClt.tp.getTilesX();
			int tilesY = ref_QuadClt.tp.getTilesY();
			String [] dbg_titles = {"dPx_dd","dPy_dd","dPx0_dd","dPy0_dd","dPx1_dd","dPy1_dd"}; // ,"dPx_dZ0","dPy_dZ0","dPx_dZ","dPy_dZ"};
        	ShowDoubleFloatArrays.showArrays(
        			dbg_img,
        			tilesX,
        			tilesY,
        			true,
        			"getSfmDpxDpyDdisp-"+ref_QuadClt.getImageName(),
        			dbg_titles);
		}		
		return dpXYddisp;
	}
}
