Another snapshot

src/main/java/com/elphel/imagej/gpu/TpTask.java

 package com.elphel.imagej.gpu;
 
+import java.util.Arrays;
+
+import com.elphel.imagej.common.ShowDoubleFloatArrays;
+
 public class TpTask {
 	public int        task;
 	// task bits 0..7 - texture neighbors (0 - N, 1 - NE, ..., 7 - NW)
@@ -232,4 +236,47 @@ public class TpTask {
 		}
 		return flt;
 	}
+	
+	public static void showTpTask(
+			TpTask[] tp_tasks,
+			int      tilesX,
+			int      tilesY,
+			String   title) {
+		int numSensors = tp_tasks[0].num_sensors;
+		String [] titles0 = {"X-cent", "Y-cent", "Disp","Scale"};
+		String [] titles = new String[titles0.length+2*numSensors];
+		for (int i = 0; i < titles0.length; i++) {
+			titles[i] = titles0[i];
+		}
+		for (int i = 0; i < numSensors; i++) {
+			titles[titles0.length + 2*i + 0] = "X-"+i;
+			titles[titles0.length + 2*i + 1] = "Y-"+i;
+		}
+		double [][] data = new double [titles.length][tilesX*tilesY];
+		for (int i = 0; i < data.length; i++) {
+			Arrays.fill(data[i], Double.NaN);
+		}
+		for (int nTask = 0; nTask < tp_tasks.length; nTask++) {
+			TpTask task = tp_tasks[nTask];
+			int nTile = task.ty*tilesX+task.tx;
+			data [0][nTile] = task.centerXY[0];
+			data [1][nTile] = task.centerXY[1];
+			data [2][nTile] = task.target_disparity;
+			data [3][nTile] = task.scale;
+			for (int i = 0; i < numSensors; i++) {
+				data [titles0.length + 2*i + 0][nTile] = task.xy[i][0];
+				data [titles0.length + 2*i + 1][nTile] = task.xy[i][1];
+			}
+		}
+		ShowDoubleFloatArrays.showArrays(
+				data,
+				tilesX,
+				tilesY,
+				true,
+				title,
+				titles);
+	}
+	
+	
+	
 }
\ No newline at end of file

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

@@ -98,6 +98,13 @@ public class IntersceneLma {
 		return new double[] {
 				full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]};
 	}
+	public double [][] getSceneXYZATR(boolean initial) {
+		double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
+		return new double[][] {
+				{full_vector[ErsCorrection.DP_DSX],full_vector[ErsCorrection.DP_DSY],full_vector[ErsCorrection.DP_DSZ]},
+				{full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]}};
+	}
+	
 	public double [] getReferenceXYZ(boolean initial) {
 		double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
 		return new double[] {
@@ -108,6 +115,12 @@ public class IntersceneLma {
 		return new double[] {
 				full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]};
 	}
+	public double [][] getReferenceXYZATR(boolean initial) {
+		double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
+		return new double[][] {
+				{full_vector[ErsCorrection.DP_DX],full_vector[ErsCorrection.DP_DY],full_vector[ErsCorrection.DP_DZ]},
+				{full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]}};
+	}
 
 	public double [] getSceneERSXYZ(boolean initial) { // never used
 		double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
@@ -228,6 +241,144 @@ public class IntersceneLma {
 		return String.format(fmt, vector[0], vector[1], vector[2]);
 	}
 
+	public void prepareLMA(
+			final double [][] scene_xyzatr0,     // camera center in world coordinates (or null to use instance)
+			final double [][] scene_xyzatr_pull, // if both are not null, specify target values to pull to 
+			final double [][] ref_xyzatr,        // 
+			// reference atr, xyz are considered 0.0 - not anymore?
+			final QuadCLT     scene_QuadClt,
+			final QuadCLT     reference_QuadClt,
+			final boolean[]   param_select,
+			final double []   param_regweights,
+			// now includes optional Disparity as the last element (for num_components==3) 
+			final double [][] vector_XYSDS,// optical flow X,Y, confidence obtained from the correlate2DIterate()
+			final double [][] centers,     // macrotile centers (in pixels and average disparities
+			
+			boolean           first_run,
+			final int         debug_level) {
+
+		scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
+		par_mask = param_select;
+		macrotile_centers = centers;
+		num_samples = num_components * centers.length;
+		ErsCorrection ers_ref =   reference_QuadClt.getErsCorrection();
+		ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
+		final double []   scene_xyz = (scene_xyzatr0 != null) ? scene_xyzatr0[0] : ers_scene.camera_xyz;
+		final double []   scene_atr = (scene_xyzatr0 != null) ? scene_xyzatr0[1] : ers_scene.camera_atr;
+		final double []   reference_xyz = (ref_xyzatr != null)? ref_xyzatr[0]:     ers_ref.camera_xyz; // new double[3];
+		final double []   reference_atr = (ref_xyzatr != null)? ref_xyzatr[1]:     ers_ref.camera_xyz; // new double[3];
+		double [] full_parameters_vector = new double [] {
+				0.0,                             0.0,                             0.0,
+				ers_ref.ers_watr_center_dt[0],   ers_ref.ers_watr_center_dt[1],   ers_ref.ers_watr_center_dt[2],
+				ers_ref.ers_wxyz_center_dt[0],   ers_ref.ers_wxyz_center_dt[1],   ers_ref.ers_wxyz_center_dt[2],
+				reference_atr[0],                reference_atr[1],                reference_atr[2],
+				reference_xyz[0],                reference_xyz[1],                reference_xyz[2],
+				ers_scene.ers_watr_center_dt[0], ers_scene.ers_watr_center_dt[1], ers_scene.ers_watr_center_dt[2],
+				ers_scene.ers_wxyz_center_dt[0], ers_scene.ers_wxyz_center_dt[1], ers_scene.ers_wxyz_center_dt[2],
+				scene_atr[0],                    scene_atr[1],                    scene_atr[2],
+				scene_xyz[0],                    scene_xyz[1],                    scene_xyz[2]};
+		parameters_full = full_parameters_vector.clone();
+		if ((vector_XYSDS != null) && (first_run || (backup_parameters_full == null))) {
+			backup_parameters_full = full_parameters_vector.clone();
+		}
+		int num_pars = 0;
+		for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) num_pars++;
+		par_indices = new int [num_pars];
+		num_pars = 0;
+		for (int i = 0; i < par_mask.length; i++) {
+			if (par_mask[i]) par_indices[num_pars++] = i;
+		}
+		parameters_vector = new double [par_indices.length];
+		for (int i = 0; i < par_indices.length; i++) {
+			parameters_vector[i] = full_parameters_vector[par_indices[i]];
+		}
+
+		if ((scene_xyzatr_pull != null) && (scene_xyzatr_pull[0] != null) && (scene_xyzatr_pull[1] != null)) {
+			double [] parameters_pull_full =  parameters_full.clone();
+			for (int i = 0; i < 3; i++)	{
+				parameters_pull_full[ErsCorrection.DP_DSX +  i] =  scene_xyzatr_pull[0][i]; 
+				parameters_pull_full[ErsCorrection.DP_DSAZ +  i] = scene_xyzatr_pull[1][i]; 
+				// 			parameters_pull = 
+			}
+			parameters_pull = new double [par_indices.length];
+			for (int i = 0; i < par_indices.length; i++) {
+				parameters_pull[i] = parameters_pull_full[par_indices[i]];
+			}
+		}
+		
+		if (vector_XYSDS != null) {// skip when used for the motion blur vectors, not LMA
+			setSamplesWeights(vector_XYSDS);  // not regularized yet ! // 3d updated
+		} else {
+			weights = null; // new double[2 * centers.length];
+		}
+
+		last_jt = new double [parameters_vector.length][];
+		if (debug_level > 1) {
+			System.out.println("prepareLMA() 1");
+		}
+		double [] fx = getFxDerivs(
+				parameters_vector, // double []         vector,
+				last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+				scenesCLT[1],      // final QuadCLT     scene_QuadClt,
+				scenesCLT[0],      // final QuadCLT     reference_QuadClt,
+				debug_level);      // final int         debug_level)
+		if (vector_XYSDS == null) {
+			return; // for MB vectors (noLMA)
+		}
+		
+		double [][] wjtj = getWJtJlambda( // USED in lwir all NAN
+				0.0,               // final double      lambda,
+				last_jt);               // final double [][] jt) all 0???
+		for (int i = 0; i < parameters_vector.length; i++) {
+			int indx = num_samples + i;
+			if (wjtj[i][i] == 0) {  // why is it zero (leading to NaN)
+				weights[indx] = 0;
+			} else {
+				weights[indx] = param_regweights[par_indices[i]]/Math.sqrt(wjtj[i][i]);
+			}
+		}
+		normalizeWeights(); // make full weight == 1.0; pure_weight <= 1.0;
+		// remeasure fx - now with regularization terms.
+
+		if (debug_level > 1) {
+			System.out.println("prepareLMA() 2");
+		}
+		fx = getFxDerivs(
+				parameters_vector, // double []         vector,
+				last_jt,                // final double [][] jt, // should be null or initialized with [vector.length][]
+				scene_QuadClt,     // final QuadCLT     scene_QuadClt,
+				reference_QuadClt, // final QuadCLT     reference_QuadClt,
+				debug_level);      // final int         debug_level)
+		// Why y_vector starts with initial value of fx??? 
+		y_vector = fx.clone();
+		for (int i = 0; i < vector_XYSDS.length; i++) {
+			if (vector_XYSDS[i] != null){
+				y_vector[num_components * i + 0] += vector_XYSDS[i][0];
+				y_vector[num_components * i + 1] += vector_XYSDS[i][1];
+				if (num_components > 2) {
+					y_vector[num_components * i + 2] += vector_XYSDS[i][2];
+				}
+				
+			}
+		}
+		if (parameters_pull != null){
+			for (int i = 0; i < par_indices.length; i++) {
+//				y_vector [i + num_components * macrotile_centers.length] += parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
+				y_vector [i + num_components * macrotile_centers.length] = parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
+			}			
+		}
+		
+		last_rms = new double [2];
+		last_ymfx = getYminusFxWeighted(
+				fx, // final double []   fx,
+				last_rms); // final double []   rms_fp // null or [2]
+		initial_rms = last_rms.clone();
+		good_or_bad_rms = this.last_rms.clone();
+			
+	}
+	
+	
+	
 	public void prepareLMA(
 			final double []   scene_xyz0,     // camera center in world coordinates (or null to use instance)
 			final double []   scene_atr0,     // camera orientation relative to world frame (or null to use instance)

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

@@ -2830,7 +2830,7 @@ public class OpticalFlow {
 		boolean debug_ers = false; // true; // false; // true; // true; //11.01.2022
 		boolean ignore_ers = false; // false;
 		TileProcessor tp = scene_QuadClt.getTileProcessor();
-		final int tilesX = (full_woi_in==null) ? tp.getTilesX() : full_woi_in.width; // full width,includeing extra
+		final int tilesX = (full_woi_in==null) ? tp.getTilesX() : full_woi_in.width; // full width,including extra
 		final int tilesY = (full_woi_in==null) ? tp.getTilesY() : full_woi_in.height;
 		final int offsetX_ref = (full_woi_in==null) ? 0 : full_woi_in.x;
 		final int offsetY_ref = (full_woi_in==null) ? 0 : full_woi_in.y;