Added bicubic, fixed map inversion

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

@@ -4,6 +4,8 @@ import java.awt.Rectangle;
 import java.util.Arrays;
 import java.util.concurrent.atomic.AtomicInteger;
 
+import org.apache.commons.math3.analysis.interpolation.PiecewiseBicubicSplineInterpolatingFunction;
+
 import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.common.ShowDoubleFloatArrays;
 import com.elphel.imagej.tileprocessor.ErsCorrection;
@@ -228,21 +230,34 @@ public class VegetationModel {
 			
 		}
 		int dbg_scene = -64;
+		boolean use_bicubic = true;
 		double [][][] terrain_pix =    new double [num_scenes][][];
 		double [][][] vegetation_pix = new double [num_scenes][][];
 		for (int nscene = 0; nscene < num_scenes; nscene++) {
 			if (nscene == dbg_scene) {
 				System.out.println("test_vegetation(): nscene="+nscene);
 			}
-			terrain_pix[nscene] =  interpolatePxPyDBilinear(
-					terrain_diff[nscene],    // final double [][] pXpYD_tile,
-					tilesX,                  // final int         tilesX,
-					tileSize);               // final int         tile_size)
-			vegetation_pix[nscene] =  interpolatePxPyDBilinear(
-					vegetation_diff[nscene], // final double [][] pXpYD_tile,
-					tilesX,                  // final int         tilesX,
-					tileSize);               // final int         tile_size)
+			if (use_bicubic) {
+				terrain_pix[nscene] =  interpolatePxPyDBicubic(
+						terrain_diff[nscene],    // final double [][] pXpYD_tile,
+						tilesX,                  // final int         tilesX,
+						tileSize);               // final int         tile_size)
+				vegetation_pix[nscene] =  interpolatePxPyDBicubic(
+						vegetation_diff[nscene], // final double [][] pXpYD_tile,
+						tilesX,                  // final int         tilesX,
+						tileSize);               // final int         tile_size)
+			} else {
+				terrain_pix[nscene] =  interpolatePxPyDBilinear(
+						terrain_diff[nscene],    // final double [][] pXpYD_tile,
+						tilesX,                  // final int         tilesX,
+						tileSize);               // final int         tile_size)
+				vegetation_pix[nscene] =  interpolatePxPyDBilinear(
+						vegetation_diff[nscene], // final double [][] pXpYD_tile,
+						tilesX,                  // final int         tilesX,
+						tileSize);               // final int         tile_size)
+			}
 		}
+
 		
 		if (show_debug) {
 			String [] titles_frame = {"terr-pX","veg-pX","terr-pY","veg-pY","terr-D","veg-D"};
@@ -273,10 +288,11 @@ public class VegetationModel {
 					}
 				}
 			}
+			String title = "terrain_vegetation_pix"+ (use_bicubic?"-bicubic":"-bilinear")+".tiff";
 			ShowDoubleFloatArrays.showArraysHyperstack(
 					data_dbg,                 // double[][][] pixels, 
 					tilesX*tileSize,          // int          width, 
-					"terrain_vegetation_pix", // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
+					title,                    // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
 					titles_scene,             // String []    titles, // all slices*frames titles or just slice titles or null
 					titles_frame,             // String []    frame_titles, // frame titles or null
 					true);                    // boolean      show)
@@ -315,7 +331,7 @@ public class VegetationModel {
 		}
 		/* */
 		double [][][] veg_to_terr =    new double [num_scenes][][];
-		Rectangle   window1 = new Rectangle(0,0,640,480);
+		Rectangle   window1 = new Rectangle(0,0,640,512);
 		Rectangle   window2 = out_window;
 		boolean     map_diff1 = true;
 		boolean     map_diff2 = out_diff; // true;
@@ -381,7 +397,7 @@ public class VegetationModel {
 					tilesX*tileSize,                   // final int           width,
 					quadCLTs,                          // QuadCLT []          quadCLTs, // just for names
 					"combo_offset.tiff"); // String              title) { // with .tiff
-				/*	*/
+			/*	*/
 		}
 
 		
@@ -439,7 +455,34 @@ public class VegetationModel {
 					titles_scene,                     // String []    titles, // all slices*frames titles or just slice titles or null
 					titles_frame,                     // String []    frame_titles, // frame titles or null
 					true);                            // boolean      show)
-		}		
+		}
+		/* */
+		double [][] vegetation_mapped =    new double [num_scenes][];
+		for (int nscene = 0; nscene < num_scenes; nscene++) {
+			vegetation_mapped[nscene] = applyMap(
+					terrain_render[nscene][0], // final double []   img,
+					tilesX * tileSize,         // final int         img_width,
+					veg_to_terr[nscene],       // final double [][] map,
+					window1,                   // final Rectangle   window,
+					map_diff_out);             // final boolean     map_diff) 
+		}	
+
+		if (show_debug) {
+			String [] titles_frame = {"terrain","mapped_vegetation", "vegetation"};
+			String [] titles_scene = new String [num_scenes];
+			for (int nscene = 0; nscene < num_scenes; nscene++) {
+				titles_scene[nscene] = nscene+":"+quadCLTs[nscene].getImageName();
+			}
+			double [][][] render3 = {terrain_mono, vegetation_mapped, vegetation_mono};
+			ShowDoubleFloatArrays.showArraysHyperstack(
+					render3,           // double[][][] pixels, 
+					tilesX * tileSize,                // int          width, 
+					"terrain_vegetation_mapped.tiff", // String       title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
+					titles_scene,                     // String []    titles, // all slices*frames titles or just slice titles or null
+					titles_frame,                     // String []    frame_titles, // frame titles or null
+					true);                            // boolean      show)
+		}
+		/* */
 		return;
 	}
 	
@@ -541,7 +584,88 @@ public class VegetationModel {
 		return imp;
 		
 	}
-	
+
+
+	/**
+	 * Apply a map to an image (with bi-linear interpolation) and output warped image	
+	 * @param img       source image, has NaN-s
+	 * @param img_width source image width
+	 * @param map       map (absolute or differential), where each pixel is either null or a pair or 
+	 *                  fractional source image coordinates. In differential mode it is a pair of offsets
+	 *                  from the map x,y indices. 
+	 * @param window    Rectangle with {width, height} specifying output image size and (in
+	 *                  differential mode only) {x,y} corresponds to absolute origin
+	 * @param map_diff  true for differential mode, false - for absolute.
+	 * @return          warped image in line-scan order, may have NaN-s.
+	 */
+	public static double [] applyMap(
+			final double []   img,
+			final int         img_width,
+			final double [][] map,
+			final Rectangle   window,
+			final boolean     map_diff) {
+		final int img_height = img.length /img_width;
+		final int num_pixels = window.width*window.height;
+		final double [] render_out = new double [num_pixels];
+		Arrays.fill(render_out, Double.NaN);
+		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 nPix = ai.getAndIncrement(); nPix < num_pixels; nPix = ai.getAndIncrement()) if (map[nPix] != null){
+						pix_label: {
+							int ix = nPix % window.width;
+							int iy = nPix / window.width;
+							double [] pxy = map[nPix].clone();
+							if (map_diff) {
+								pxy[0] += 0.5 + ix - window.x;
+								pxy[1] += 0.5 + iy - window.y;
+							}
+//							pxy[0] += window2.x;
+//							pxy[1] += window2.y;
+							int x0 = (int) Math.floor(pxy[0]);
+							int y0 = (int) Math.floor(pxy[1]);
+							if ((x0 < 0) || (y0 < 0) || (x0 >= (img_width -1)) || (y0 >= (img_height-1))) {
+								break pix_label; // all 4 corners should fit
+							}
+							int img_pix = x0+ y0 * img_width;
+							double [][] corners = {
+									{img[img_pix],                 img[img_pix + 1]},
+									{img[img_pix + img_width], img[img_pix + img_width + 1]}};
+							
+							for (int dy = 0; dy < 2; dy++) {
+								for (int dx = 0; dx < 2; dx++) {
+									double corner = corners[dy][dx];
+									if (Double.isNaN(corner)) {
+										break pix_label; // all 4 corners should be defined
+									}
+//									if (map_diff2) {
+//										corner[0] += x0 + dx  + 0.5 - window2.x;
+//										corner[1] += y0 + dy  + 0.5 - window2.y;
+//									}
+								}
+							}
+
+							double fx = pxy[0] - x0;
+							double fy = pxy[1] - y0;
+							render_out[nPix] = 
+										(1-fx)*(1-fy)*corners[0][0] +
+										(  fx)*(1-fy)*corners[0][1] +
+										(1-fx)*(  fy)*corners[1][0] +
+										(  fx)*(  fy)*corners[1][1];
+						}
+
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		
+		
+		
+		return render_out;
+	}
 	
 	/**
 	 * Combine maps: map1 and map2 (map2 transforms result of map1)
@@ -773,9 +897,11 @@ public class VegetationModel {
 										double u1 = 1.0 - (vp2[0]*v32[0] + vp2[1]*v32[1])/l2_32;
 										double v1 = 1.0 - (vp2[0]*v12[0] + vp2[1]*v12[1])/l2_12;
 										// Use arithmetic average as some of u0,u1,v0,v1 can be small negatives
-										double u = 0.5 * (u0 + u1);
-										double v = 0.5 * (v0 + v1);
-										
+										//double u = 0.5 * (u0 + u1);
+										//double v = 0.5 * (v0 + v1);
+										double denom = 1-(u1-u0)*(v1-v0);
+										double u = (u0 +(u1-u0)*v0)/denom;
+										double v = (v0 +(v1-v0)*u0)/denom;
 										int oindx = ox + oy*out_window.width;
 										map_out[oindx] = new double [odepth];
 										map_out[oindx][0] = ix0 + u;
@@ -844,6 +970,133 @@ public class VegetationModel {
 		
 	}
 	
+	public static double [][] interpolatePxPyDBicubic(
+			final double [][] pXpYD_tile,
+			final int         tilesX,
+			final int         tile_size){
+		final int odepth = 3; // just x,y. if 3 - will have 0 for disparity
+		final int width = tilesX * tile_size; 
+		final int htile_size = tile_size/2;
+		int num_tiles = pXpYD_tile.length;
+		int num_pixels = num_tiles * tile_size * tile_size; 
+		final int tilesY = num_tiles/tilesX;
+		final double [][] pXpYD_pixel = new double [num_pixels][];
+		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
+		final AtomicInteger ai = new AtomicInteger(0);
+		final double [][] tslices = new double [odepth][(tilesX+2)*(tilesY+2)]; // extended by 1 each of 4 sides
+		final double [][] pslices = new double [odepth][num_pixels];
+		for (int ns = 0; ns < odepth; ns++) {
+			Arrays.fill(tslices[ns], Double.NaN);
+		}
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nTile = ai.getAndIncrement(); nTile < num_tiles; nTile = ai.getAndIncrement()) if (pXpYD_tile[nTile] != null){
+						int tileX = nTile % tilesX;
+						int tileY = nTile / tilesX;
+						int nTile_ex = (tileX + 1) + (tileY + 1) * (tilesX+2); 
+						for(int ns = 0; ns < odepth; ns++) {
+							tslices[ns][nTile_ex] = pXpYD_tile[nTile][ns];
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		ai.set(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					TileNeibs tn = new TileNeibs(tilesX+2,tilesY+2);
+					for (int ns = ai.getAndIncrement(); ns < odepth; ns = ai.getAndIncrement()){
+						OrthoMap.fillNaNs(
+								tslices[ns], // double [] data,
+								tn,         // TileNeibs tn,
+								3);         // int min_neibs)
+					}
+				}
+			};
+		}
+		ImageDtt.startAndJoin(threads);
+		final double [] y = new double [tilesY+2]; // f is [col][row] ! 
+		final double [] x = new double [tilesX+2];
+		for (int i = 0; i < x.length; i++) {
+			x[i] = -htile_size + tile_size*i;
+		}
+		for (int i = 0; i < y.length; i++) {
+			y[i] = -htile_size + tile_size*i;
+		}
+		for (int nslice = 0; nslice < odepth; nslice++) {
+			final double [] tslice = tslices[nslice]; 
+			final double [] pslice = pslices[nslice];
+			final double [][] tslice2 = new double [tilesY+2][tilesX+2];
+			for (int i = 0; i < tslice2.length; i++) {
+				System.arraycopy(
+						tslice,
+						i * (tilesX+2),
+						tslice2[i],
+						0,
+						(tilesX+2));
+			}
+			final PiecewiseBicubicSplineInterpolatingFunction pbsif= new PiecewiseBicubicSplineInterpolatingFunction(y, x, tslice2);
+			ai.set(0);
+			for (int ithread = 0; ithread < threads.length; ithread++) {
+				threads[ithread] = new Thread() {
+					public void run() {
+						for (int nPix = ai.getAndIncrement(); nPix < num_pixels; nPix = ai.getAndIncrement()){
+							int pixX = nPix % width; 
+							int pixY = nPix / width;
+//							if (pbsif.isValidPoint(pixY,pixX)) { // then overwrite with bicubic
+							pslice[nPix] = pbsif.value(pixY,pixX);   
+//							}
+						}
+					}
+				};
+			}
+			ImageDtt.startAndJoin(threads);
+		}
+		
+		ai.set(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nPix = ai.getAndIncrement(); nPix < num_pixels; nPix = ai.getAndIncrement()){
+						int pixX = nPix % width;
+						int pixY = nPix / width;
+						int tileX = pixX/tile_size;
+						int tileY = pixY/tile_size;
+						if (pXpYD_tile[tileX+tileY*tilesX] != null) {
+							boolean defined = true;
+							for (int i = 0; (i < odepth) && defined; i++) {
+								defined &= !Double.isNaN(pslices[i][nPix]);
+							}
+							if (defined) {
+								pXpYD_pixel[nPix] = new double [odepth];
+								for (int i = 0; i < odepth; i++) {
+									pXpYD_pixel[nPix][i] = pslices[i][nPix];
+								}
+							}
+						}
+					}
+				}
+			};
+		}
+		ImageDtt.startAndJoin(threads);
+		/*
+		ShowDoubleFloatArrays.showArrays(
+				pslices,
+				tilesX * tile_size,
+				tilesY * tile_size,
+				true,
+				"test_bicubic",
+				new String[] {"pX","pY","D"});
+		*/
+		return pXpYD_pixel;
+	}
+	
+	
+	
+	
 	/**
 	 * Expand defined tile pXpYD so each defined tile has at least 3 consecutive neighbors: 2 ortho and diagonal between them
 	 * @param pXpYD_tile
@@ -1067,6 +1320,14 @@ public class VegetationModel {
 		return pXpYD;
 	}
 	
+	
+	
+	/**
+	 * Calculate pXpYD difference from the reference scene
+	 * @param pXpYD
+	 * @param ref_index
+	 * @return
+	 */
 	public static double [][][] diffPxPyDs(
 			final double [][][] pXpYD,
 			final int ref_index){

src/main/java/com/elphel/imagej/x3d/export/Render3D.java

@@ -649,7 +649,7 @@ public class Render3D {
 											(1.0 - fy) * (      fx) * texture[chn][indx10] + 
 											(      fy) * (1.0 - fx) * texture[chn][indx01] + 
 											(      fy) * (      fx) * texture[chn][indx11];
-									if (pbsif[tri_index[indx][0]][chn].isValidPoint(px, py)) { // tghen overwrite with bicubic
+									if (pbsif[tri_index[indx][0]][chn].isValidPoint(px, py)) { // then overwrite with bicubic
 										pix_val[chn] = pbsif[tri_index[indx][0]][chn].value(px,py);   
 									}
 								}