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/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;