working snapshot for VegetationLMA, some debug tools

src/main/java/com/elphel/imagej/vegetation/VegetationLMA.java

@@ -7,8 +7,12 @@ import java.util.concurrent.atomic.AtomicInteger;
 import com.elphel.imagej.common.ShowDoubleFloatArrays;
 import com.elphel.imagej.tileprocessor.ImageDtt;
 import com.elphel.imagej.tileprocessor.QuadCLT;
+import com.elphel.imagej.tileprocessor.TileNeibs;
 
 import Jama.Matrix;
+import ij.ImagePlus;
+import ij.ImageStack;
+import ij.io.FileSaver;
 
 public class VegetationLMA {
 	public static final int TVAO_TERRAIN =          0;
@@ -34,6 +38,7 @@ public class VegetationLMA {
 	public double [][]        tvao;            // [0][image_length] - terrain, [1][image_length] - vegetation,
 //[2][image_length] - alpha, [3][num_scenes] - per-scene offset (mostly for vignetting. MAYBE add per-scene pair of tilts
 	public int [][]           par_index;       // indices -  [0..3][full_pixel] - same as for tvao, value - parameter index
+	public int [][]           par_rindex;      // parameter -> pair of type (0..3) and full window pixel index
 //	private double            default_alpha;
 	private int               num_pars_terrain; 
 	private int               num_pars_vegetation; 
@@ -55,11 +60,17 @@ public class VegetationLMA {
 	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization
 	public double             alpha_loss;
 	public double             alpha_offset =    0; // if >0,  start losses above 0.0 and below 1.0;
+	public double             alpha_lpf =       0;
+	// data used to calculate lpf pull of the alpha pixel to average of four neighbors
+	public int [][]           alpha_neibs;   // corresponds to parameters for alpha (num_pars_vegetation_alpha), each has 4 ortho neibs, -1 - border, >= 0
+	// - parameter index, <=-2 -2-full_pixel_index
+	
 	public double             delta=            1e-5; // 7;
 	
 	public int                debug_index;
 	public double [][]        debug_image;
 	public static double []   debug_alpha_scales = {100,150};
+	public String             debug_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/debug/vegetation/lma/";
 	
 	public int []             indices;
 	public int [][]           cpairs =          null;
@@ -105,18 +116,23 @@ public class VegetationLMA {
 	}
 	
 	public int prepareLMA(
+			final boolean            keep_parameters,
 			final Rectangle          woi,
 			final int                min_scenes,     // minimal number of scenes (inside woi) vegetation pixel must influence
 			final double             default_alpha,
-			final double []          scene_weights, // null or per-scene weights (higher for larger offsets)?
 			final double             reg_weights,    // fraction of the total weight used for regularization
 			final double             alpha_loss,     // quadratic loss when alpha reaches -1.0 or 2.0 
 			final double             alpha_offset,   // quadratic loss when alpha reaches -1.0 or 2.0
+			final double             alpha_lpf,      // pull to average of 4 neighbors
+			final double             boost_parallax, // increase weight of scene with maximal parallax relative to the reference scene
+			final String             parameters_read_path, 
 			final int                debugLevel) {
 		this.woi =          woi;
 		this.alpha_loss =   alpha_loss;
 		this.alpha_offset = alpha_offset;
-//		this.default_alpha = default_alpha;
+		this.alpha_lpf =    alpha_lpf;
+		final double []     scene_weights = setupSceneWeights(
+				boost_parallax); // double boost_parallax)
 		int     min_scenes_uses = min_scenes;
 		int     min_scenes_used = min_scenes * 4;
 		boolean [][] valid_woi = filterValidWoi (
@@ -124,7 +140,19 @@ public class VegetationLMA {
 				min_scenes_used, // int     min_scenes_used,
 				debugLevel > 3); // boolean debug_img){ // 4x?
 		setupParametersIndices(valid_woi);
+		alpha_neibs = getAlphaNeighbors();
+		
+		
+		if (!keep_parameters) {
 			setupParametersVector (default_alpha); // areas where both terrain and vegetation are available
+		}
+		if (parameters_read_path != null) {
+			 readParametersFromImage(
+					 parameters_read_path, // String    path,
+						parameters_vector, // double [] vector,
+						1) ;// int       gap)
+		}
+		
 		setupDataSource(); // condensed
 		setupYVector();
 		setupWeights( // after setupParametersIndices
@@ -134,6 +162,45 @@ public class VegetationLMA {
 		return weights.length;
 	}
 	
+	private double [] setupSceneWeights(
+			double boost_parallax) {
+		// find max parallax
+		final double [] scene_weights = new double[vegetation_offsets.length];
+		if (boost_parallax == 0) {
+			for (int nscene = 0; nscene < vegetation_offsets.length; nscene++) {
+				scene_weights[nscene] = 1.0;
+			}
+		} else {
+			final double [] max_parallax2 = new double [vegetation_offsets.length];
+			double super_max_parallax2 = 0;
+			for (int nscene = 0; nscene < vegetation_offsets.length; nscene++) {
+				// only compare for selected window!
+				for (int drow = 0; drow < woi.height; drow++) {
+					int row = woi.y + drow;
+					for (int dcol = 0; dcol < woi.width; dcol++) {
+						int col = woi.x + dcol;	
+						int windx =dcol + drow * woi.width;
+						int indx = row*full.width + col;
+						double [] d =  vegetation_offsets[nscene][indx];
+						if (d != null) {
+							double d2 = d[0]*d[0] + d[1]*d[1];
+							/*
+						if (d2 > max_parallax2[nscene]) {
+							System.out.println("setupSceneWeights(): nscene="+nscene+", pix =  ("+(i%640)+","+(i/640)+") ="+i+" d2="+d2);
+						}
+							 */
+							max_parallax2[nscene] = Math.max(max_parallax2[nscene], d2);
+						}
+					}
+				}
+				super_max_parallax2 = Math.max(super_max_parallax2, max_parallax2[nscene]);
+			}
+			for (int nscene = 0; nscene < vegetation_offsets.length; nscene++) {
+				scene_weights[nscene] = 1.0+(boost_parallax - 1) * Math.sqrt(max_parallax2[nscene]/super_max_parallax2);
+			}
+		}
+		return scene_weights;
+	}
 	
 	private double [] getYminusFxWeighted(
 			final double []   fx,
@@ -226,7 +293,7 @@ public class VegetationLMA {
 			if (rslt == null) {
 				return -1; // false; // need to check
 			}
-			if (debug_level > 1) {
+			if (debug_level > -2) { // 1) {
 				System.out.println("LMA step"+String.format("%3d",iter)+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
 						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
 			}
@@ -420,28 +487,18 @@ public class VegetationLMA {
 			rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
 			this.last_rms = rms.clone();
 			this.parameters_vector = new_vector.clone();
-			if (debug_level > 2) {
-				String title = (debug_level > 3)? "parameters.vector":null;
-				int [] wh = new int [2];
-				double [] debug_img =  parametersImage (
-						parameters_vector, // double [] vector,
-						1,                // int       gap)
-						wh,
-						title);
-				if ((debug_image != null) && (debug_index < debug_image.length)) {
-					debug_image[debug_index++] = debug_img;
-				}
-				if (debug_level > 4) {
-					ShowDoubleFloatArrays.showArrays(
-							debug_image,
-							wh[0],
-							wh[1],
-							true,
-							"parameters_vector_live.tiff");
-					
-				}
-
-				// print vectors in some format
+			if (debug_level > -2) { //
+				String  save_dir =    debug_path;
+				String  debug_title = "parameters_vector-x"+woi.x+"-y"+woi.y+"-w"+woi.width+"-h"+woi.height;
+				boolean save_all =    (debug_image != null);
+				boolean show_this =   debug_level > 3;
+				boolean show_all =    debug_level > 4;
+				debugImageSaveShow(
+						save_dir,     // String  save_dir, // save if not null
+						debug_title, // String  title,    // not null
+						save_all,    // boolean save_all,
+						show_this,   // boolean show_this,
+						show_all);   // boolean show_all)
 				/*
 				System.out.print("delta: "+corr_delta.toString()+"\n");
 				System.out.print("New vector: "+new_vector.toString()+"\n");
@@ -474,6 +531,57 @@ public class VegetationLMA {
 		return rslt;
 	}
 	
+	public void debugImageSaveShow(
+			String  save_dir, // save if not null
+			String  title,    // not null
+			boolean save_all,
+			boolean show_this,
+			boolean show_all) {
+		int [] wh = new int [2];
+		double [] debug_img =  parametersImage (
+				parameters_vector, // double [] vector,
+				1,                // int       gap)
+				wh,
+				null); // title);
+		String title_all = title+"-live";
+		if ((debug_image != null) && (debug_index < debug_image.length)) {
+			title += "-"+debug_index;
+			debug_image[debug_index++] = debug_img;
+		}
+		
+		if (show_this) {
+			ShowDoubleFloatArrays.showArrays(
+					debug_img,
+					wh[0],
+					wh[1],
+					title);
+		}
+		if (save_all || show_all) {
+			double [][] dbg_img = new double [debug_index][];
+			String [] titles = new String[debug_index];
+			for (int i = 0; i < debug_index; i++) {
+				titles[i] = ""+i;
+				dbg_img[i] = debug_image[i];
+			}
+			if (show_all) {
+				ShowDoubleFloatArrays.showArrays(
+						debug_image,
+						wh[0],
+						wh[1],
+						true,
+						title_all,
+						titles);
+			}
+			if (save_all) {
+				String file_path = save_dir+title_all+".tiff";
+				ImageStack imageStack = ShowDoubleFloatArrays.makeStack(dbg_img, wh[0], wh[1], titles);
+				ImagePlus imp = new ImagePlus( title, imageStack);
+				FileSaver fs=new FileSaver(imp);
+				fs.saveAsTiff(file_path);
+				System.out.println("saveDoubleArrayInModelDirectory(): saved "+file_path);
+			}
+		}
+	}
 	
 	
 	private double [][] getWJtJlambda(
@@ -617,6 +725,35 @@ public class VegetationLMA {
 								jt[np][nx] = 2 * alpha_loss * d; // d/dalpha[i]		
 							}							
 						}
+						// add cost for difference between this alpha and average of 4 neighbors (when they exist
+						if (alpha_lpf > 0) {
+							double avg = 0;
+							int nn = 0;
+							for (int i = 0; i < alpha_neibs[n].length; i++) { // now 4, may be increased
+								int di =  alpha_neibs[n][i];
+								d=0;
+								if (di >= 0) {
+									d = vector[di];  // d - full parameter index
+									avg+=d;
+									nn++;
+								} else if (di < -1) {
+									d = tvao[TVAO_VEGETATION_ALPHA][-di - 2]; 
+									avg+=d;
+									nn++;
+								}
+							}
+							avg /= nn; // average
+							fX[nx] += alpha_lpf * (vector[np] - avg);
+							if (jt != null) {
+								jt[np][nx] += alpha_lpf;
+								for (int i = 0; i < alpha_neibs[n].length; i++) { // now 4, may be increased
+									int di =  alpha_neibs[n][i];
+									if (di > 0) {
+										jt[di][nx] -= alpha_lpf/nn;
+									}
+								}								
+							}							
+						}
 					}
 				}
 			};
@@ -681,6 +818,58 @@ public class VegetationLMA {
 		return indices;
 	}
 	
+	public void showYfX(
+			double [] vector,
+			String title) {
+		if (vector == null) {
+			vector = parameters_vector;
+		}
+		double [][][] img_data = new double [3][num_scenes][woi.width*woi.height];
+		for (int n = 0; n < img_data.length; n++) {
+			for (int nscene = 0; nscene < num_scenes; nscene++) {
+				Arrays.fill(img_data[n][nscene], Double.NaN);
+			}
+		}
+		double [] fX = getFxDerivs( // longer than y_vector
+				vector, // final double []   vector,
+				null,   // final double [][] jt, // should be null or initialized with [vector.length][]
+				-1);    // final int         debug_level)
+		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 ny = ai.getAndIncrement(); ny < data_source.length; ny = ai.getAndIncrement()) {
+						int nscene =   data_source[ny][0][0];
+						int indx =     data_source[ny][0][1];
+						int wx = (indx % full.width) - woi.x; 
+						int wy = (indx / full.width) - woi.y;
+						int windx = wx + wy * woi.width;
+						img_data[0][nscene][windx] = y_vector[ny];
+						img_data[1][nscene][windx] = fX[ny];
+						img_data[2][nscene][windx] = y_vector[ny]-fX[ny];
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		String [] top_titles = {"Y", "fX", "Y-fX"};
+		String [] scene_titles = new String[num_scenes];
+		for (int i = 0; i < num_scenes; i++) {
+			scene_titles[i] = ""+i;
+		}
+		ShowDoubleFloatArrays.showArraysHyperstack(
+				img_data,           // double[][][] pixels, 
+				woi.width,                // int          width, 
+				title, // "terrain_vegetation_render.tiff", // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
+				scene_titles,                     // String []    titles, // all slices*frames titles or just slice titles or null
+				top_titles,                     // String []    frame_titles, // frame titles or null
+				true);                            // boolean      show)
+
+		return;
+		
+	}
+	
 	public double [] parametersImage (
 			double [] vector,
 			int       gap,
@@ -696,7 +885,8 @@ public class VegetationLMA {
 				dbg_img[i]= vector[indices[i]];
 				// scale alpha for the same image
 				int w = i % woi_width3;
-				if (w > 2 *( woi.width+ gap)) {
+				int h = i / woi_width3;
+				if ((w > 2 *( woi.width+ gap)) && (h < woi.height))  {
 					dbg_img[i] = debug_alpha_scales[0] + (debug_alpha_scales[1] - debug_alpha_scales[0]) * dbg_img[i];  
 				}
 			}
@@ -715,6 +905,40 @@ public class VegetationLMA {
 		return dbg_img;
 	}
 
+	public void readParametersFromImage(
+			String    path,
+			double [] vector,
+			int       gap) {
+		int [] indices = getParamDebugIndices (gap);
+		ImagePlus imp_pars = new ImagePlus (path);
+		if (imp_pars.getWidth()==0) {
+			throw new IllegalArgumentException("Could not read "+path);
+		}
+		int woi_width3=woi.width*3 + 2 * gap;
+		int height = indices.length/woi_width3;
+		if ((woi_width3 != imp_pars.getWidth()) || (height != imp_pars.getHeight())) {
+			throw new IllegalArgumentException("Read image size does not match required: ("+imp_pars.getWidth()+"x"+imp_pars.getHeight()+
+					") != ("+woi_width3+"x"+height+")");
+		}
+		float [] pixels = (float[]) imp_pars.getProcessor().getPixels();
+		for (int i = 0; i < indices.length; i++) {
+			if (indices[i] >= 0) {
+				double d = pixels[i];
+				// scale alpha for the same image
+				int w = i % woi_width3;
+				int h = i / woi_width3;
+				if ((w > 2 *( woi.width+ gap)) && (h < woi.height))  {
+					d = (d- debug_alpha_scales[0])/(debug_alpha_scales[1] - debug_alpha_scales[0]);
+				}
+				vector[indices[i]] = d;
+			}
+		}
+		return;
+	}
+	
+	
+	
+	
 	
 	public double debugDerivs (
 			double [] vector,
@@ -810,6 +1034,37 @@ public class VegetationLMA {
 		ImageDtt.startAndJoin(threads);
 		return;
 	}
+	private int [][] getAlphaNeighbors(){
+		final int [][] alpha_neibs = new int[num_pars_vegetation_alpha][4];
+		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() {
+				TileNeibs tn = new TileNeibs(full.width, full.height);
+				public void run() {
+					for (int i = ai.getAndIncrement(); i < num_pars_vegetation_alpha; i = ai.getAndIncrement()) {
+						int pindx = ind_pars_vegetation_alpha + i; // full parameter index
+						int indx = par_rindex[pindx][1]; // full pixel index
+						for (int dir4 = 0; dir4 < 4; dir4++) { // ortho only
+							int dir = 2 * dir4;
+							int nindx = tn.getNeibIndex(indx, dir);
+							if (nindx < 0) {
+								alpha_neibs[i][dir4] = -1;
+							} else if (par_index[TVAO_VEGETATION_ALPHA][nindx] >= 0) {
+								alpha_neibs[i][dir4] = par_index[TVAO_VEGETATION_ALPHA][nindx];
+							} else { // pull to fixed alpha value
+								alpha_neibs[i][dir4] = -2 -indx; // OFFSET by 2 !
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		//ind_pars_vegetation_alpha
+		return alpha_neibs;
+	}
 	
 	private void setupDataSource() {
 		final int woi_length = woi.width*woi.height;
@@ -939,6 +1194,12 @@ public class VegetationLMA {
 		par_index[TVAO_VEGETATION] =       new int [image_length];
 		par_index[TVAO_VEGETATION_ALPHA] = new int [image_length];
 		par_index[TVAO_SCENE_OFFSET] =     new int [num_scenes];
+		int max_pars = 0;
+		for (int i = 0; i < par_index.length; i++) {
+			max_pars += par_index[i].length;
+		}
+		int [][] par_rindex = new int [max_pars][2];
+		
 		Arrays.fill(par_index[TVAO_TERRAIN],          -1);
 		Arrays.fill(par_index[TVAO_VEGETATION],       -1);
 		Arrays.fill(par_index[TVAO_VEGETATION_ALPHA], -1);
@@ -951,6 +1212,8 @@ public class VegetationLMA {
 				int windx =dcol + drow * woi.width;
 				if (valid_woi[0][windx]) {
 					int indx = row*full.width + col;
+					par_rindex[par_num][0] = 0;
+					par_rindex[par_num][1] = indx;
 					par_index[TVAO_TERRAIN][indx] = par_num++;
 				}
 			}
@@ -964,6 +1227,8 @@ public class VegetationLMA {
 				int windx =dcol + drow * woi.width;
 				if (valid_woi[1][windx]) {
 					int indx = row*full.width + col;
+					par_rindex[par_num][0] = 1;
+					par_rindex[par_num][1] = indx;
 					par_index[TVAO_VEGETATION][indx] = par_num++;
 				}
 			}
@@ -977,6 +1242,8 @@ public class VegetationLMA {
 				int windx =dcol + drow * woi.width;
 				if (valid_woi[1][windx]) {
 					int indx = row*full.width + col;
+					par_rindex[par_num][0] = 2;
+					par_rindex[par_num][1] = indx;
 					par_index[TVAO_VEGETATION_ALPHA][indx] = par_num++;
 				}
 			}
@@ -985,8 +1252,12 @@ public class VegetationLMA {
 //		int num_pars_pixel = par_num;
 		ind_pars_scenes =    par_num;
 		for (int i = 0; i < par_index[TVAO_SCENE_OFFSET].length; i++) {
+			par_rindex[par_num][0] = 1;
+			par_rindex[par_num][1] = i;
 			par_index[TVAO_SCENE_OFFSET][i] = par_num++;
 		}
+		this.par_rindex = new int [par_num][2];
+		System.arraycopy(par_rindex, 0, this.par_rindex, 0, par_num);
 		num_pars_scenes = par_num - ind_pars_scenes;
 		parameters_vector = new double [par_num];
 	}

src/main/java/com/elphel/imagej/vegetation/VegetationModel.java

@@ -697,27 +697,41 @@ public class VegetationModel {
 		Rectangle woi50 =       new Rectangle(143,317,35,35);
 		int       min_scenes =  10;
 		double    default_alpha = 0.8;
-		double [] scene_weights = null;
 		double    reg_weights =   0.25;        // fraction of the total weight used for regularization
-		double    alpha_loss =    1.0;         // quadratic loss when alpha reaches -1.0 or 2.0 
+		double    alpha_loss =    1000.0; // 100.; // 10.0;         // quadratic loss when alpha reaches -1.0 or 2.0 
 		double    alpha_offset =  0.0;         // if >0,  start losses above 0.0 and below 1.0;
-		
+		double    alpha_lpf =     2.0; // 1.5; // 5.0; // 0.5;         // pull to average of 4 neighbors
+		double    boost_parallax = 5;
+		boolean exit_loop = debugLevel < 1000;
+		boolean next_run = false;
+		boolean read_pars = true;
+		String  parameters_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/debug/vegetation/lma/parameters_vector_data.tiff";
+		do {
+			String par_path = (read_pars && !next_run)? parameters_path : null;
 			int num_samples = vegetationLMA.prepareLMA(
+					next_run,      // final boolean            keep_parameters,
 					woi50,         // final Rectangle   woi,
 					min_scenes,    // final int         min_scenes, // minimal number of scenes (inside woi) vegetation pixel must influence
 					default_alpha, // final double      default_alpha,
-				scene_weights, // final double []   scene_weights, // null or per-scene weights (higher for larger offsets)?
 					reg_weights,   // final double             reg_weights,        // fraction of the total weight used for regularization
 					alpha_loss,    // final double             alpha_loss,         // quadratic loss when alpha reaches -1.0 or 2.0 
 					alpha_offset,  // final double             alpha_offset,         // quadratic loss when alpha reaches -1.0 or 2.0
+					alpha_lpf,     // final double             alpha_lpf,           // pull to average of 4 neighbors
+					boost_parallax, // final double             boost_parallax, // increase weight of scene with maximal parallax relative to the reference scene
+					par_path,       // final String             parameters_read_path, 
 					debugLevel);   // final int         debugLevel);
-		
-		double  lambda =            0.1;
+			if (debugLevel >-2) {
+				vegetationLMA.showYfX(
+						null, // double [] vector,
+						"reconstructed_model"); // String title)
+			}
+			next_run = true;
+			double  lambda =            5.0; // 0.1;
 			double  lambda_scale_good = 0.5;
 			double  lambda_scale_bad =  8.0;
 			double  lambda_max =      1000;
 			boolean last_run =       false;
-		double  rms_diff =          0.0001;
+			double  rms_diff =          1e-8; // 0.0001; virtually forever
 			int     num_iter = 100;
 			vegetationLMA.debug_index = 0;
 			vegetationLMA.debug_image = new double [num_iter][];
@@ -732,6 +746,12 @@ public class VegetationModel {
 					last_run,         // boolean last_run,
 					null,             // String dbg_prefix,
 					debugLevel);      // int    debug_level)
+		} while (!exit_loop);
+		if (debugLevel >-2) {
+			vegetationLMA.showYfX(
+					null, // double [] vector,
+					"reconstructed_model_adjusted"); // String title)
+		}
 		return; // 
 		
 	}