Fixing bugs, making OrthoMultiLMA run

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

@@ -345,10 +345,25 @@ public class OrthoMapsCollection  implements Serializable{
 		}
 		return bounds_pix;
 	}
+	public int [][] getBoundsPixels(
+			int           zoom_level, // maps[0] as a reference
+			int []        indices,
+			double [][][] affines) {
+		double [][] bounds_meters = (affines == null) ? getBoundsMeters(indices):getBoundsMeters(indices,affines);
+		double pix_size = OrthoMap.getPixelSizeMeters (zoom_level);
+		int [][] bounds_pix = new int[2][2];
+		for (int n = 0; n < bounds_pix.length; n++) {
+			bounds_pix[n][0] = (int) Math.floor(bounds_meters[n][0]/pix_size);
+			bounds_pix[n][1] = (int) Math.ceil (bounds_meters[n][1]/pix_size);
+		}
+		return bounds_pix;
+	}
+
 	public int [][] getBoundsPixels(
 			int zoom_level) { // maps[0] as a reference
 		return getBoundsPixels(zoom_level, null);
 	}
+	//double [][][] affines
 	/**
 	 * Get rectified bounds of all provided ortho images relative to the origin (vertical
 	 * point) of the first one in meters
@@ -403,6 +418,36 @@ public class OrthoMapsCollection  implements Serializable{
 		return bounds;
 	}
 
+	public double [][] getBoundsMeters(
+			int    [] indices,
+			double [][][] affines){ // maps[0] as a reference
+		if (indices == null) {
+			indices = new int [ortho_maps.length];
+			for (int i = 0; i < indices.length; i++) {
+				indices[i] = i;
+			}
+		}
+		if (affines == null) {
+			affines = new double [ortho_maps.length][][];
+			for (int i = 0; i < affines.length; i++) {
+				affines[i] = ortho_maps[indices[i]].affine;
+			}
+		}
+		double [][] bounds = ortho_maps[indices[0]].getBoundsMeters(true,affines[0]);
+		for (int imap = 1; imap < indices.length; imap++) {
+			int nmap = indices[imap];
+			double [][] bounds_other = ortho_maps[nmap].getBoundsMeters(true,affines[imap]);
+			double [] enuOffset = ortho_maps[indices[0]].enuOffsetTo(ortho_maps[nmap]);
+			double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
+			for (int n = 0; n < bounds.length; n++) {
+				bounds[n][0] = Math.min(bounds[n][0],bounds_other[n][0]+ rd[n]);
+				bounds[n][1] = Math.max(bounds[n][1],bounds_other[n][1]+ rd[n]);
+			}
+		}
+		return bounds;
+	}
+	
+	
 	/**
 	 * Get rectified overlap bounds of two provided ortho images relative to the origin (vertical
 	 * point) of the first one in meters. Use specified affine transforms, not saved with the orto map
@@ -783,10 +828,18 @@ public class OrthoMapsCollection  implements Serializable{
 			int []        origin, // maps[0] as a reference
 			double [][]   centers){
 		boolean use_alt = ground_planes != null;
-		final int dbg_x=2783, dbg_y=-5228;
-		int [][] bounds = getBoundsPixels( // should be for rectified, {-bounds[0][0], -bounds[0][1]} - exact center
+		final int dbg_x=707, dbg_y=615;
+		int [][] bounds;
+		if (affines == null) {
+			bounds = getBoundsPixels( // should be for rectified, {-bounds[0][0], -bounds[0][1]} - exact center
 					zoom_level,
 					bounds_to_indices? indices: null);
+		} else {
+			bounds = getBoundsPixels( // should be for rectified, {-bounds[0][0], -bounds[0][1]} - exact center
+					zoom_level,
+					bounds_to_indices? indices: null,
+							affines);
+		}
 		int width =  bounds[0][1] - bounds[0][0]; // bounds[x][0] - negative 
 		int height = bounds[1][1] - bounds[1][0];
 		if (wh != null) {
@@ -808,20 +861,22 @@ public class OrthoMapsCollection  implements Serializable{
 		for (int indx = 0; indx < indices.length; indx++) { //:indices) { // = 0; nmap< ortho_maps.length; nmap++) {
 			final int findx= indx;
 			final int nmap = indices[indx]; // nmap;
+			final double [][] affine = (affines !=null) ? affines[indx]: ortho_maps[nmap].affine; // only here use provided
 			Arrays.fill(dpixels[findx], Float.NaN);
 			final double scale = 1.0/OrthoMap.getPixelSizeMeters(zoom_level);
 			final double src_scale = 1.0/OrthoMap.getPixelSizeMeters(ortho_maps[nmap].orig_zoom_level); // pix per meter
 		// metric bounds of the rectified image relative to its origin
-			double [][] mbounds = ortho_maps[nmap].getBoundsMeters(true); // keep original bounds
+///			double [][] mbounds = ortho_maps[nmap].getBoundsMeters(true); // keep original bounds
+			double [][] mbounds = ortho_maps[nmap].getBoundsMeters(true,affine); // keep original bounds
 			double [] enu_offset = ortho_maps[indices[0]].enuOffsetTo(ortho_maps[nmap]);
-			final double [] scaled_out_center = { // xy center to apply affine to
+			final double [] scaled_out_center = { // xy center to apply affine to in output pixels
 					-bounds[0][0] + scale * enu_offset[0],
 					-bounds[1][0] - scale * enu_offset[1]};
 			if (centers != null) {
 				centers[findx] = scaled_out_center;
 			}
-			final int [][] obounds = new int [2][2]; // output (rectified, combined) image bounds, relative to thje top-left
-			for (int n = 0; n< 2; n++) {
+			final int [][] obounds = new int [2][2]; // output (rectified, combined) image bounds, relative to the top-left
+			for (int n = 0; n< 2; n++) { // output pixels
 				obounds[n][0] = (int) Math.floor(scaled_out_center[n] + scale*mbounds[n][0]);
 				obounds[n][1] = (int) Math.ceil (scaled_out_center[n] + scale*mbounds[n][1]);
 			}
@@ -829,9 +884,10 @@ public class OrthoMapsCollection  implements Serializable{
 			final int ownd_width =  obounds[0][1] - obounds[0][0];
 			final int ownd_height = obounds[1][1] - obounds[1][0];
 			final int ownd_len = ownd_width * ownd_height;
-			double [][] src_bounds=ortho_maps[nmap].getBoundsMeters (true); // using original affines
+///			final double [][] affine = (affines !=null) ? affines[indx]: ortho_maps[nmap].affine; // only here use provided
+///			double [][] src_bounds=ortho_maps[nmap].getBoundsMeters (true); // using original affines
+			double [][] src_bounds=ortho_maps[nmap].getBoundsMeters (false,affine); // using provided affines
 			final double [] src_center = {-src_bounds[0][0],-src_bounds[1][0]}; // x,y center offset in the source image
-			final double [][] affine = (affines !=null) ? affines[indx]: ortho_maps[nmap].affine; // only here use provided
 			final int src_width = use_alt? ortho_maps[nmap].getAltData().width: ortho_maps[nmap].getImageData().width;
 			final int src_height = use_alt? ortho_maps[nmap].getAltData().height : ortho_maps[nmap].getImageData().height;
 //			final float [] src_img = use_alt? ortho_maps[nmap].getAltData().data : ortho_maps[nmap].getImageData().data;
@@ -876,16 +932,16 @@ public class OrthoMapsCollection  implements Serializable{
 						for (int nPix = ai.getAndIncrement(); nPix < ownd_len; nPix = ai.getAndIncrement()) {
 							int opX = nPix % ownd_width + obounds[0][0]; // absolute output pX, pY
 							int opY = nPix / ownd_width + obounds[1][0];
-							double dX = (opX - scaled_out_center[0]) /scale; // in original image scale 
+							double dX = (opX - scaled_out_center[0]) /scale; // in meters
 							double dY = (opY - scaled_out_center[1]) /scale;
 							double [] xy_src = { // pixels of the source image
 									src_scale * (affine[0][0]*dX + affine[0][1]*dY + affine[0][2] + src_center[0]),
 									src_scale * (affine[1][0]*dX + affine[1][1]*dY + affine[1][2] + src_center[1])};
 							// limit to the source image
-							if ((warp_woi != null) && (warp_woi.contains(opX,opY))) {
-								if ((opX==dbg_x) && (opY==dbg_y)) {
+							if ((((int) opX)==dbg_x) && (((int) opY)==dbg_y)) {
 								System.out.println("opX="+opX+", opy="+opY);
 							}
+							if ((warp_woi != null) && (warp_woi.contains(opX,opY))) {
 								double [] dxy = warp.getWarp(opX,opY);
 								xy_src[0] += dxy[0];
 								xy_src[1] += dxy[1];

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

@@ -26,14 +26,18 @@
 package com.elphel.imagej.orthomosaic;
 
 import java.awt.Point;
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.concurrent.atomic.AtomicInteger;
 
 import com.elphel.imagej.cameras.CLTParameters;
+import com.elphel.imagej.common.ShowDoubleFloatArrays;
 import com.elphel.imagej.tileprocessor.ImageDtt;
 
 import Jama.Matrix;
+import ij.ImagePlus;
+import ij.gui.PointRoi;
 
 public class OrthoMultiLMA {
 	final boolean move_only;
@@ -80,6 +84,99 @@ public class OrthoMultiLMA {
 				affine1[i][j] = 0.5 * (direct.getAffine()[i][j]+E[i][j]);
 			}
 		}
+		
+		
+		
+		String [] titles = new String [indices.length];
+		for (int i = 0; i < indices.length; i++) {
+			titles[i] = indices[i]+"-"+maps_collection.ortho_maps[indices[i]].getName();
+		}
+		double [][][] affines_0d = {E,direct.getAffine()};
+		double [][][] affines_sym = {affine0, affine1};
+		
+		double [][] affine0a = new double [2][3]; // keep b[] ==0
+		for (int i = 0; i < 2; i++) {
+			for (int j = 0; j < 2; j++) {
+				affine0a[i][j] = 0.5* affine0[i][j]; 
+			}
+		}
+		
+		double [][] affine1a = PairwiseOrthoMatch.combineAffines( 
+				affine0a,           // double [][] affine0,
+				direct.getAffine(), //double [][] affine, // differential
+				rd);                // double [] rd);
+		
+		PairwiseOrthoMatch test_match = new PairwiseOrthoMatch (
+				affine0a, // double [][] affine0,
+				affine1a, // double [][] affine1,
+				rd); // double [] rd);
+		
+		double [][][] affines_a = {affine0a,affine1a};
+		// show as intended (E, diff)
+        int         zoom_level = -2;
+        int [] wh = new int[2];
+        int [] origin = new int[2];
+        double [][] centers = new double [indices.length][];
+		double [][] dmulti = maps_collection.renderMultiDouble (
+				null,       // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
+				indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
+				true,       // boolean       bounds_to_indices,
+				affines_a, // affines_0d, // affines,    // double [][][] affines, // null or [indices.length][2][3]
+				null,       // warp,       // FineXYCorr    warp,,
+				zoom_level, // int         zoom_level,
+				wh,         // int []      wh,
+				origin,     // int []      origin){ // maps[0] as a reference
+				centers);   // double [][] centers)
+		
+		ImagePlus imp_multi = ShowDoubleFloatArrays.makeArrays(
+				dmulti,
+				wh[0],
+				wh[1],
+				"scaled.tiff", //"direct_diff.tiff",				
+				titles); // test_titles,
+    	PointRoi roi = new PointRoi();
+    	for (int i = 0; i < centers.length; i++) {
+    		roi.addPoint(centers[i][0],centers[i][1], 1+i);
+    	}
+    	roi.setOptions("label");
+    	imp_multi.setRoi(roi);
+		imp_multi.show();
+        // show half_offsets
+		double [][] dmulti_sym = maps_collection.renderMultiDouble (
+				null,       // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
+				indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
+				true,       // boolean       bounds_to_indices,
+				affines_sym,// affines,    // double [][][] affines, // null or [indices.length][2][3]
+				null,       // warp,       // FineXYCorr    warp,,
+				zoom_level, // int         zoom_level,
+				wh,         // int []      wh,
+				origin,     // int []      origin){ // maps[0] as a reference
+				centers);   // double [][] centers)
+		
+		ImagePlus imp_sym = ShowDoubleFloatArrays.makeArrays(
+				dmulti_sym,
+				wh[0],
+				wh[1],
+				"direct_sym.tiff",				
+				titles); // test_titles,
+    	PointRoi roi_sym = new PointRoi();
+    	for (int i = 0; i < centers.length; i++) {
+    		roi_sym.addPoint(centers[i][0],centers[i][1], 1+i);
+    	}
+    	roi_sym.setOptions("label");
+    	imp_sym.setRoi(roi_sym);
+    	imp_sym.show();
+		
+		
+		
+		
+		
+		
+		
+		
+		
+		
+		
 		double [][][] derivs =       new double [12][][];
 		double [][][] derivs_delta = new double [12][][];
 		double delta = 1E-7;
@@ -113,10 +210,10 @@ public class OrthoMultiLMA {
 			OrthoMapsCollection maps_collection,
 			String              orthoMapsCollection_path
 			) {
-		int                 debugLevel = 3;
+		int                 debugLevel = 1;
 		boolean             move_only = false;
 		double []           val_coord = null; // 1 - valid, 0 - invalid, minimize coordinates errors
-		double              position_pull = 0.1;
+		double              position_pull = 0.001;
         boolean             use_inv = false;
         double              overlap_pow = 2.0; // match weight as overlap fraction to this power
 		int [] indices = maps_collection.getScenesSelection(
@@ -127,7 +224,7 @@ public class OrthoMultiLMA {
 		double lambda_scale_good = 0.5;
 		double lambda_scale_bad =  8.0;
 		double lambda_max =      100;
-		double rms_diff =          0.0001;
+		double rms_diff =          0.000001;
 		int    num_iter =         20;
 		boolean last_run =       false;
 		oml.prepareLMA (
@@ -155,6 +252,87 @@ public class OrthoMultiLMA {
         if (debugLevel > 0) {
         	System.out.println("LMA: full RMS="+rms[0]+" ("+initial_rms[0]+"), pure RMS="+rms[1]+" ("+initial_rms[1]+")");
         }
+        
+        //Get and apply affines
+    	//oml.updateAffines(maps_collection);
+        double [][][] affines = oml.getAffines();
+        
+        // test
+        /*
+		PairwiseOrthoMatch match = maps_collection.ortho_maps[indices[0]].getMatch(maps_collection.ortho_maps[indices[1]].getName());
+		double [][] match_affine = match.getAffine(); 
+		
+		double [] enuOffset = maps_collection.ortho_maps[indices[1]].enuOffsetTo(maps_collection.ortho_maps[indices[0]]);
+		double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
+        
+		double [][] affine1a = PairwiseOrthoMatch.combineAffines( 
+				affines[0],           // double [][] affine0,
+				match_affine, //double [][] affine, // differential
+				rd);                // double [] rd);
+		PairwiseOrthoMatch test_match = new PairwiseOrthoMatch (
+				affines[0], // double [][] affine0,
+				affines[1], // double [][] affine1,
+				rd); // double [] rd);
+        */
+        
+        int         zoom_level = -2;
+        int [] wh = new int[2];
+        int [] origin = new int[2];
+        double [][] centers = new double [indices.length][];
+		double [][] dmulti = maps_collection.renderMultiDouble (
+				null, // double [][]   ground_planes, // null - images, non-null altitudes. use new double[2][3] for old way alt
+				indices,    // int []        indices, // null or which indices to use (normally just 2 for pairwise comparison)
+				true, // boolean       bounds_to_indices,
+				affines, // null, // affines,    // double [][][] affines, // null or [indices.length][2][3]
+				null, // warp,       // FineXYCorr    warp,,
+				zoom_level, // int         zoom_level,
+				wh,         // int []      wh,
+				origin,     // int []      origin){ // maps[0] as a reference
+				centers);   // double [][] centers)
+		String [] titles = new String [dmulti.length];
+		for (int i = 0; i < dmulti.length; i++) {
+			titles[i] = indices[i]+"-"+maps_collection.ortho_maps[indices[i]].getName();
+		}
+		
+		ImagePlus imp_multi = ShowDoubleFloatArrays.makeArrays(
+				dmulti,
+				wh[0],
+				wh[1],
+				"matched_multi.tiff",				
+				titles); // test_titles,
+    	PointRoi roi = new PointRoi();
+    	for (int i = 0; i < centers.length; i++) {
+    		roi.addPoint(centers[i][0],centers[i][1], 1+i);
+    	}
+    	roi.setOptions("label");
+    	imp_multi.setRoi(roi);
+		imp_multi.show();
+		double [][] affine = 		getAffineAndDerivatives(
+				move_only,      //boolean       move_only,
+				affines[0],     // double [][]   affine00,
+				affines[1],     // double [][]   affine10,
+				oml.offsets[0], // double []     rd,
+				null);          // double [][][] derivs)
+		PairwiseOrthoMatch pom = new PairwiseOrthoMatch (
+				affines[0],
+				affines[1],
+				oml.offsets[0]);
+		
+		
+		
+		/*
+		if (orthoMapsCollection_path != null) {
+        	try {
+        		maps_collection.writeOrthoMapsCollection(orthoMapsCollection_path);
+        	} catch (IOException e) {
+        		// TODO Auto-generated catch block
+        		e.printStackTrace();
+        	}
+        	if (debugLevel > -4) {
+        		System.out.println("Saved data to "+ orthoMapsCollection_path);
+        	}
+		}
+		*/
         return 0;
 	}
 	
@@ -205,7 +383,8 @@ public class OrthoMultiLMA {
 		for (int np = 0; np < num_pairs; np++) {
 			Point pair = pairs_list.get(np);
 			pairs[np] = new int[] {pair.x, pair.y};
-			double [] enuOffset = maps_collection.ortho_maps[indices[pair.x]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.y]]);
+//			double [] enuOffset = maps_collection.ortho_maps[indices[pair.x]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.y]]);
+			double [] enuOffset = maps_collection.ortho_maps[indices[pair.y]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.x]]);
 			offsets[np] = new double[] {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
 		}
 		pairs_ni = createNonIntersectingPairs(pairs);
@@ -272,6 +451,37 @@ public class OrthoMultiLMA {
 		return weights.length;
 	}
 
+	public void  updateAffines(OrthoMapsCollection maps_collection) {
+		for (int n = 0; n < num_scenes; n++) {
+			double [][] affine = maps_collection.ortho_maps[indices[n]].getAffine();
+			if (move_only) {
+				System.arraycopy(parameters_vector, 2*n,     affine[0], 2,  1);
+				System.arraycopy(parameters_vector, 2*n + 1, affine[1], 2,  1);
+			} else {
+				System.arraycopy(parameters_vector, 6*n,     affine[0], 0,  3);
+				System.arraycopy(parameters_vector, 6*n + 3, affine[1], 0,  3);
+			}
+		}
+	}
+	public double [][][] getAffines(){
+		double [][][] affines = new double [num_scenes][2][3];
+		for (int n = 0; n < num_scenes; n++) {
+			if (move_only) {
+				affines[n][0][0] = 1.0;
+				affines[n][1][1] = 1.0;
+				System.arraycopy(parameters_vector, 2*n,     affines[n][0], 2,  1);
+				System.arraycopy(parameters_vector, 2*n + 1, affines[n][1], 2,  1);
+
+			} else {
+				System.arraycopy(parameters_vector, 6*n,     affines[n][0], 0,  3);
+				System.arraycopy(parameters_vector, 6*n + 3, affines[n][1], 0,  3);
+			}
+		}
+		return affines;
+	}
+	
+	
+	
 	public static double [][] getAffineAndDerivetives_delta(
 			boolean       move_only,
 			double        delta,

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

@@ -172,6 +172,24 @@ public class PairwiseOrthoMatch implements Serializable {
 //				rms = Double.NaN; // double rms,
 //				zoom_lev = 0; // int zoom_lev)
 	}
+	public static double [][] combineAffines( 
+			double [][] affine0,
+			double [][] affine, // differential
+			double [] rd) {
+		Matrix A0 = new Matrix (
+				new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
+		Matrix B0 = new Matrix(new double [][] {{affine0[0][2]},{affine0[1][2]}});
+		Matrix A = new Matrix (
+				new double [][] {{affine[0][0],affine[0][1]},{affine[1][0],affine[1][1]}});
+		Matrix B = new Matrix(new double [][] {{affine[0][2]},{affine[1][2]}});
+		Matrix V =  new Matrix(new double [][] {{-rd[0]},{-rd[1]}});
+		Matrix A1 = A.times(A0);
+		Matrix B1 = B.minus(A.times(V.minus(B0).minus(A0.times(V))));
+		double[][] affine1 = new double[][] {
+			{A1.get(0,0),A1.get(0,1), B1.get(0,0)},
+			{A1.get(1,0),A1.get(1,1), B1.get(1,0)}};
+		return affine1;
+	}	
 	
 	public double [][] getAffine(){
 		return affine;