Initial testing of LMA for fitting pairs of ortho images

src/main/java/com/elphel/imagej/orthomosaic/ComboMatch.java

@@ -52,16 +52,19 @@ public class ComboMatch {
 		double [][][] image_enuatr = {{{0,0,0},{0,0,0}},{{0,0,0},{0,0,0}}};
 		int gpu_width=  clt_parameters.imp.rln_gpu_width; // 3008;
 		int gpu_height= clt_parameters.imp.rln_gpu_height; // 3008;
-		int zoom_lev = -4; // 0; // +1 - zoom in twice, -1 - zoom out twice
+		int zoom_lev = -3; // 0; // +1 - zoom in twice, -1 - zoom out twice
 		boolean use_alt = false;
 		boolean show_centers = true;
-		boolean use_saved_collection = false;
+		boolean use_saved_collection = true; // false;
 		boolean save_collection = true;
+		boolean process_correlation = true; // use false to save new version of data
 		GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,800);
 		gd.addStringField ("Image list full path",  files_list_path, 180, "Image list full path.");
 		gd.addStringField ("Maps collection save path",  orthoMapsCollection_path, 180, "Save path for serialized map collection data.");
 		gd.addCheckbox    ("Use saved maps collection", use_saved_collection, "If false - use files list.");
-		gd.addCheckbox    ("Save maps collection", save_collection, "If false - use files list.");
+		gd.addCheckbox    ("Save maps collection", save_collection, "Save maps collection to be able to restore.");
+		gd.addCheckbox    ("Process correlations", process_correlation, "false to skip to just regenerate new save file.");
+		//
 //		for (int n = 0; n < image_paths_pre.length; n++) {
 //			gd.addStringField ("Image path "+n,  image_paths_pre[n], 180, "Image "+n+" full path w/o ext");		
 //		}
@@ -92,6 +95,7 @@ public class ComboMatch {
 		orthoMapsCollection_path = gd.getNextString();
 		use_saved_collection =     gd.getNextBoolean();
 		save_collection =          gd.getNextBoolean();
+		process_correlation=       gd.getNextBoolean();
 
 		for (int n = 0; n < image_enuatr.length; n++) {
 			image_enuatr[n][0][0] = gd.getNextNumber();
@@ -142,40 +146,41 @@ public class ComboMatch {
     				origin);           // int []      origin){
     		imp_alt.show();
         }
-        
-		System.out.println("Setting up GPU");
-		if (GPU_QUAD_AFFINE == null) {
-			try {
-				GPU_QUAD_AFFINE = new GpuQuad(//
-						GPU_TILE_PROCESSOR,   // GPUTileProcessor gpuTileProcessor,
-						gpu_max_width,        // final int        max_width,
-						gpu_max_height,       // final int        max_height,
-						1,                    // final int        num_colors, // normally 1?
-						clt_parameters.gpu_debug_level);
-			} catch (Exception e) {
-				System.out.println("Failed to initialize GpuQuad class");
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-				return false;
-			} // final int debugLevel);
-		}
-        
-        double [][] affine0 = {{1,0,0},{0,1,0}}; // will always stay the same
-        double [][] affine1 = {{1,0,0},{0,1,0}}; // will be variable  // here in meters, relative to vertical point
-        double [][][] affines = {affine0,affine1};
-        double [][] corr_pair_rslt = maps_collection.correlateOrthoPair(
-        		clt_parameters, // CLTParameters    clt_parameters,
-        		gpu_spair,      // String []        gpu_spair,
-        		affines,        // double [][][]    affines, // on top of GPS offsets
-        		zoom_lev,       // int              zoom_lev,
-        		debugLevel);    // final int        debugLevel)       
+        if (process_correlation) {
+        	System.out.println("Setting up GPU");
+        	if (GPU_QUAD_AFFINE == null) {
+        		try {
+        			GPU_QUAD_AFFINE = new GpuQuad(//
+        					GPU_TILE_PROCESSOR,   // GPUTileProcessor gpuTileProcessor,
+        					gpu_max_width,        // final int        max_width,
+        					gpu_max_height,       // final int        max_height,
+        					1,                    // final int        num_colors, // normally 1?
+        					clt_parameters.gpu_debug_level);
+        		} catch (Exception e) {
+        			System.out.println("Failed to initialize GpuQuad class");
+        			// TODO Auto-generated catch block
+        			e.printStackTrace();
+        			return false;
+        		} // final int debugLevel);
+        	}
+
+        	double [][] affine0 = {{1,0,0},{0,1,0}}; // will always stay the same
+        	double [][] affine1 = {{1,0,0},{0,1,0}}; // will be variable  // here in meters, relative to vertical point
+        	double [][][] affines = {affine0,affine1};
+        	double [][] corr_pair_rslt = maps_collection.correlateOrthoPair(
+        			clt_parameters, // CLTParameters    clt_parameters,
+        			gpu_spair,      // String []        gpu_spair,
+        			affines,        // double [][][]    affines, // on top of GPS offsets
+        			zoom_lev,       // int              zoom_lev,
+        			debugLevel);    // final int        debugLevel)
+        }
         if (save_collection) {
         	try {
-				maps_collection.writeOrthoMapsCollection(orthoMapsCollection_path);
-			} catch (IOException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
-			}
+        		maps_collection.writeOrthoMapsCollection(orthoMapsCollection_path);
+        	} catch (IOException e) {
+        		// TODO Auto-generated catch block
+        		e.printStackTrace();
+        	}
         }
         
         
@@ -367,6 +372,7 @@ public class ComboMatch {
 				gpu_width,      // final int                    img_width,
 				null,           // Rectangle                    woi, // if null, use full GPU window
 				affine,         // final double [][][]          affine,  // [2][2][3] affine coefficients to translate common to 2 images
+				null,           // TpTask [][]                  tp_tasks_o,
 				false,          // final boolean                batch_mode,
 				debugLevel);    // final int                    debugLevel);
 //		renderFromTD (
@@ -395,6 +401,7 @@ public class ComboMatch {
 			final int                    img_width,
 			Rectangle                    woi, // if null, use full GPU window
 			final double [][][]          affine,  // [2][2][3] affine coefficients to translate common to 2 images
+			TpTask [][]                  tp_tasks_o,
 			final boolean                batch_mode,
 			final int                    debugLevel) {
 		int [] wh = {img_width, fpixels[0].length/img_width};
@@ -403,6 +410,9 @@ public class ComboMatch {
 				img_width, // final int           img_width,
 				woi,                     // Rectangle           woi,
 				affine); // final double [][][] affine  // [2][2][3] affine coefficients to translate common to 2 images
+		if (tp_tasks_o != null) {
+			for (int i = 0; i < tp_tasks_o.length; i++) tp_tasks_o[i] = tp_tasks[i];
+		}
 		boolean is_aux =  true;
 		boolean is_mono = true;
 		boolean is_lwir = true;

src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java

@@ -62,6 +62,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	public double [][]                          affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[]
 	public double                               orig_pix_meters;
 	public double []                            vert_meters;   // offset of the image vertical in meters (scale-invariant)
+	public int                                  orig_width;
+	public int                                  orig_height;
 	public transient FloatImageData             orig_image;
 	public transient FloatImageData             alt_image;
 	public int                                  orig_zoom_level;
@@ -79,6 +81,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		// affine is not transient
 		// orig_pix_meters is not transient
 		// vert_meters is not transient
+		// orig_width is not transient
+		// orig_height is not transient
 		// orig_image does not need to be saved
 		// alt_image does not need to be saved
 		// orig_zoom_level is not transient
@@ -98,6 +102,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		// affine is not transient
 		// orig_pix_meters is not transient
 		// vert_meters is not transient
+		// orig_width is not transient
+		// orig_height is not transient
 		// orig_image was not saved
 		// alt_image was not saved
 		images = new HashMap <Integer, FloatImageData>(); // field images was not saved
@@ -117,6 +123,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		gpu_height = height;		
 	}
 
+	public int getWidth() {
+		return orig_width;
+	}
+	public int getHeight() {
+		return orig_height;
+	}
 // Generate ALT image path from the GEO
 	public static String getAltPath(String path) {
 		int p1 = path.lastIndexOf(".");
@@ -151,6 +163,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 			double height_meters = height_pix * orig_pix_meters;
 			vert_meters[1] = height_meters-vert_meters[1];
 		}
+		orig_width =  ElphelTiffReader.getWidth(imp_prop);
+		orig_height = ElphelTiffReader.getHeight(imp_prop);
 		
 		orig_zoom_level = FloatImageData.getZoomLevel(orig_pix_meters);
 		orig_zoom_valid = FloatImageData.isZoomValid(orig_pix_meters);
@@ -254,8 +268,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 			String full_name = path.substring(path.lastIndexOf(Prefs.getFileSeparator()) + 1);
 			ImagePlus imp = ShowDoubleFloatArrays.makeArrays(
 					orig_image.data, // float[] pixels,
-					orig_image.width,
-					orig_image.height,
+					getWidth(),
+					getHeight(),
 					full_name);
 			if (show_markers) {
 				PointRoi roi = new PointRoi();
@@ -296,8 +310,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	 */
 	public double [][] get4SourceCornersMeters(){
 		FloatImageData orig_image = getImageData();
-		double width_meters = orig_image.width * orig_pix_meters;
-		double height_meters = orig_image.height * orig_pix_meters;
+		double width_meters = getWidth() * orig_pix_meters;
+		double height_meters = getHeight() * orig_pix_meters;
 		return new double[][] { // CW from TL
 			{             - vert_meters[0],               - vert_meters[1]},
 			{width_meters - vert_meters[0],               - vert_meters[1]},
@@ -440,8 +454,8 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 			rscale *= 2;
 		}
 		final int frscale = rscale;
-		int swidth =  orig_image.width;
-		int sheight = orig_image.height;
+		int swidth =  getWidth();
+		int sheight = getHeight();
 		final float [] spix = orig_image.data;
 		final int width = (swidth+frscale-1)/frscale;
 		final int height = (sheight+frscale-1)/frscale;

src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java

@@ -20,6 +20,8 @@ import java.util.concurrent.atomic.AtomicInteger;
 
 import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.common.ShowDoubleFloatArrays;
+import com.elphel.imagej.gpu.GPUTileProcessor;
+import com.elphel.imagej.gpu.TpTask;
 import com.elphel.imagej.tileprocessor.ImageDtt;
 import com.elphel.imagej.tileprocessor.TDCorrTile;
 
@@ -441,6 +443,8 @@ public class OrthoMapsCollection  implements Serializable{
 			double [][][]    affines,  // here in meters, relative to vertical points
 			int              zoom_lev,
 			final int        debugLevel){
+		boolean show_gpu_img = true; // (debugLevel > 1);
+		boolean show_tile_centers = true; // (debugLevel > 1);
 		double [][] bounds_overlap_meters = getOverlapMeters(
 				gpu_pair[0],  // int ref_index,
 				gpu_pair[1],  // int other_index)
@@ -469,9 +473,9 @@ public class OrthoMapsCollection  implements Serializable{
 		tlo_rect_metric[0][0] = bounds_overlap_meters[0][0]; // relative to ref vert_meters
 		tlo_rect_metric[0][1] = bounds_overlap_meters[1][0]; // vert_meters
 		tlo_rect_metric[1][0] = bounds_overlap_meters[0][0]  // relative to other vert_meters
-				- rd[0] + ortho_maps[gpu_pair[1]].vert_meters[0]- ortho_maps[gpu_pair[0]].vert_meters[0];
+				- rd[0]; //  + ortho_maps[gpu_pair[1]].vert_meters[0]- ortho_maps[gpu_pair[0]].vert_meters[0];
 		tlo_rect_metric[1][1] = bounds_overlap_meters[1][0]
-				- rd[1] + ortho_maps[gpu_pair[1]].vert_meters[1]- ortho_maps[gpu_pair[0]].vert_meters[1];
+				- rd[1]; //  + ortho_maps[gpu_pair[1]].vert_meters[1]- ortho_maps[gpu_pair[0]].vert_meters[1];
 
 		double [][] tlo_src_metric = new double[tlo_rect_metric.length][2]; // relative to it's own vert_meters
 		for (int n=0; n <tlo_src_metric.length; n++) {
@@ -501,6 +505,17 @@ public class OrthoMapsCollection  implements Serializable{
 			}
 			gpu_pair_img[n] = ortho_maps[gpu_pair[n]].getPaddedGPU (zoom_lev); // int zoom_level,
 		}
+		if (show_gpu_img) {
+			String [] dbg_titles = {ortho_maps[gpu_pair[0]].getName(),ortho_maps[gpu_pair[1]].getName()};
+    		ShowDoubleFloatArrays.showArrays(
+    				gpu_pair_img,
+    				OrthoMap.gpu_width,
+    				OrthoMap.gpu_height,
+    				true,
+    				"gpu_img",
+    				dbg_titles);
+
+		}
 		Rectangle  woi = new Rectangle(0, 0, overlap_wh_pixel[0], overlap_wh_pixel[1]);
 		if (woi.width > OrthoMap.gpu_width) {
 			if (debugLevel > -3) {
@@ -517,6 +532,7 @@ public class OrthoMapsCollection  implements Serializable{
 		final int  gpu_width = OrthoMap.gpu_width; // static
 		// uses fixed_size gpu image size
 //		TDCorrTile [] td_corr_tiles =
+		TpTask [][] tp_tasks = new TpTask [2][];
 		double [][][] vector_field = 
 				ComboMatch.rectilinearVectorField(//rectilinearCorrelate_TD( // scene0/scene1
 				clt_parameters, // final CLTParameters          clt_parameters,
@@ -524,9 +540,69 @@ public class OrthoMapsCollection  implements Serializable{
 				gpu_width,      // final int                    img_width,
 				woi, // Rectangle                    woi, // if null, use full GPU window
 				affines_gpu,         // final double [][][]          affine,  // [2][2][3] affine coefficients to translate common to 2 images
+				tp_tasks, // TpTask [][]                  tp_tasks_o,
 				false,          // final boolean                batch_mode,
 				debugLevel);    // final int                    debugLevel);
 		// may use tl_rect_metric to remap to the original image
+		double [][] tile_centers = new double [vector_field[0].length][];
+		int tilesX = gpu_width/GPUTileProcessor.DTT_SIZE; 
+		for (TpTask task: tp_tasks[1]) {
+			int ti = task.getTileY() * tilesX + task.getTileX();
+			tile_centers[ti] = task.getDoubleCenterXY();
+		}
+		if (show_tile_centers){
+			double [][] dbg_img = new double [6][tile_centers.length];
+			String [] dbg_titles = {"cX","cY","px0","py0", "px1","py1"};
+			for (int i = 0; i< dbg_img.length;i++) Arrays.fill(dbg_img[i], Double.NaN);
+			for (int t = 0; t < tp_tasks[0].length; t++) {
+				TpTask task0 = tp_tasks[0][t];
+				TpTask task1 = tp_tasks[1][t];
+				int ti = task0.getTileY() * tilesX + task0.getTileX();
+				dbg_img[0][ti] = task0.getDoubleCenterXY()[0]; // same for task0, task1 
+				dbg_img[1][ti] = task1.getDoubleCenterXY()[1]; 
+				dbg_img[2][ti] = task0.getXY()[0][0];
+				dbg_img[3][ti] = task0.getXY()[0][1];
+				dbg_img[4][ti] = task1.getXY()[0][0];
+				dbg_img[5][ti] = task1.getXY()[0][1];
+			} // getXY()
+    		ShowDoubleFloatArrays.showArrays(
+    				dbg_img,
+    				tilesX,
+    				tile_centers.length/tilesX,
+    				true,
+    				"tile_centers",
+    				dbg_titles);
+		}
+		
+		
+		OrthoPairLMA orthoPairLMA = new OrthoPairLMA();
+		// vector_field[1] - neighbors
+		double lambda =            0.1;
+		double lambda_scale_good = 0.5;
+		double lambda_scale_bad =  8.0;
+		double lambda_max =      100;
+		double rms_diff =          0.001;
+		int    num_iter =         20;
+		boolean last_run =       false;
+		orthoPairLMA.prepareLMA(
+				// will always calculate relative affine, starting with unity
+				tilesX, // int           width,
+				vector_field[1], // double [][]   vector_XYS,    // optical flow X,Y, confidence obtained from the correlate2DIterate()
+				tile_centers, // double [][]   centers,       // tile centers (in pixels)
+				null, // double []     weights_extra, // optional, may be null 
+				true, // boolean       first_run,
+				debugLevel); // final int     debug_level)
+		int lma_rslt = orthoPairLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+				lambda,             // double lambda,           // 0.1
+				lambda_scale_good,  // double lambda_scale_good,// 0.5
+				lambda_scale_bad,   // double lambda_scale_bad, // 8.0
+				lambda_max,         // double lambda_max,       // 100
+				rms_diff,           // double rms_diff,         // 0.001
+				num_iter,           // int    num_iter,         // 20
+				last_run,           // boolean last_run,
+				debugLevel);       // int    debug_level)
+		System.out.println("LMA result = "+lma_rslt);
+// analyze result, re-run correlation
 		
 /*
 		if (show_vector_field) {