From a0c4b51321b3b98c670148f93d2e554bbc871149 Mon Sep 17 00:00:00 2001 From: AndreyFilippov Date: Fri, 23 Oct 2020 00:16:50 -0600 Subject: [PATCH] bug fixes in multiple scene fitting --- .../imagej/tileprocessor/ErsCorrection.java | 12 +- .../tileprocessor/GeometryCorrection.java | 1 - .../imagej/tileprocessor/OpticalFlow.java | 279 ++++++++++++------ 3 files changed, 201 insertions(+), 91 deletions(-) diff --git a/src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java b/src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java index f6dd294..eb65e96 100644 --- a/src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java +++ b/src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java @@ -434,7 +434,7 @@ public class ErsCorrection extends GeometryCorrection { this.xyz = xyz; this.atr = atr; this.ers_xyz_dt = ers_xyz_dt; - this.ers_atr_dt = ers_xyz_dt; + this.ers_atr_dt = ers_atr_dt; this.ers_xyz_d2t = new double[3]; this.ers_atr_d2t = new double[3]; } @@ -774,6 +774,16 @@ public class ErsCorrection extends GeometryCorrection { setupERS(); } + public void printVectors( + double [] xyz, + double [] atr) + { + if (xyz != null) printAngle(" XYZ",xyz); + if (atr != null) printAngle(" ATR",atr); + printAngle( "ERS XYZ_dt",ers_wxyz_center_dt); + printAngle( "ERS ATR_dt",ers_watr_center_dt); + } + public void setupERS() { double ers_sign = 1.0; // -1.0; // invert all corrections to opposite? diff --git a/src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java b/src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java index 6032962..4b2ae86 100644 --- a/src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java +++ b/src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java @@ -58,7 +58,6 @@ public class GeometryCorrection { public int debugLevel = 0; public double line_time = 26.5E-6; // duration of sensor scan line (for ERS) Wrong, 36.38us (change and re-run ERS - public int pixelCorrectionWidth=2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2) public int pixelCorrectionHeight=1936; diff --git a/src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java b/src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java index c5a69b9..f4d0354 100644 --- a/src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java +++ b/src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java @@ -1999,7 +1999,9 @@ public class OpticalFlow { QuadCLT scene_QuadCLT, int iscale) // 8 { + String title = reference_QuadCLT.getImageName()+"-"+scene_QuadCLT.image_name+suffix; double [][] dsrbg = transformCameraVew( // shifts previous image correctly (right) + title, // final String title, camera_xyz0, // double [] camera_xyz, // camera center in world coordinates camera_atr0, //double [] camera_atr, // camera orientation relative to world frame scene_QuadCLT, // QuadCLT camera_QuadClt, @@ -2008,10 +2010,11 @@ public class OpticalFlow { double [][] dsrbg_ref; if (blur_reference) { dsrbg_ref = transformCameraVew( // shifts previous image correctly (right) - ZERO3, // camera_xyz0, // double [] camera_xyz, // camera center in world coordinates - ZERO3, // camera_atr0, // double [] camera_atr, // camera orientation relative to world frame - reference_QuadCLT, // scene_QuadCLT, // QuadCLT camera_QuadClt, - reference_QuadCLT, // reference_QuadCLT, // reference + title+"-reference", // final String title, + ZERO3, // camera_xyz0, // double [] camera_xyz, // camera center in world coordinates + ZERO3, // camera_atr0, // double [] camera_atr, // camera orientation relative to world frame + reference_QuadCLT, // scene_QuadCLT, // QuadCLT camera_QuadClt, + reference_QuadCLT, // reference_QuadCLT, // reference iscale); } else { dsrbg_ref= reference_QuadCLT.getDSRBG(); @@ -2024,22 +2027,21 @@ public class OpticalFlow { String [] dsrbg_titles = {"d", "s", "r", "b", "g"}; // combine this scene with warped previous one - String [] rtitles = new String[2* dsrbg_titles.length]; - double [][] dbg_rslt = new double [rtitles.length][]; - for (int i = 0; i < dsrbg_titles.length; i++) { - rtitles[2*i] = dsrbg_titles[i]+"0"; - rtitles[2*i+1] = dsrbg_titles[i]; - dbg_rslt[2*i] = pair[0][i]; - dbg_rslt[2*i+1] = pair[1][i]; - } - String title = reference_QuadCLT.getImageName()+"-"+scene_QuadCLT.image_name+suffix; - (new ShowDoubleFloatArrays()).showArrays( - dbg_rslt, - tilesX, - tilesY, - true, - title, - rtitles); + String [] rtitles = new String[2* dsrbg_titles.length]; + double [][] dbg_rslt = new double [rtitles.length][]; + for (int i = 0; i < dsrbg_titles.length; i++) { + rtitles[2*i] = dsrbg_titles[i]+"0"; + rtitles[2*i+1] = dsrbg_titles[i]; + dbg_rslt[2*i] = pair[0][i]; + dbg_rslt[2*i+1] = pair[1][i]; + } + (new ShowDoubleFloatArrays()).showArrays( + dbg_rslt, + tilesX, + tilesY, + true, + title, + rtitles); } @@ -2052,16 +2054,17 @@ public class OpticalFlow { int iscale) // 8 { int nscenes = scenes.length; - int indx_ref = nscenes - 1; + int indx_ref = nscenes - 1; + String title = "previous_frames_matching"+suffix; double [][][] dsrbg = new double [nscenes][][]; String [] time_stamps = new String[nscenes]; // [0] - last scene before the reference one for (int i = 0; i < nscenes; i++) { int indx = dsrbg.length - i - 1; + time_stamps[i] = scenes[indx].getImageName(); if ((i == 0) && !blur_reference) { dsrbg[0]= scenes[indx_ref].getDSRBG(); } else { - time_stamps[i] = scenes[indx].getImageName(); ErsCorrection ers_scene = scenes[indx].getErsCorrection(); double [] ers_scene_original_xyz_dt = ers_scene.getErsXYZ_dt(); double [] ers_scene_original_atr_dt = ers_scene.getErsATR_dt(); @@ -2070,10 +2073,11 @@ public class OpticalFlow { scene_ers_dt[indx][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt); ers_scene.setupERS(); dsrbg[i] = transformCameraVew( // shifts previous image correctly (right) - scene_xyzatr[indx][0], // double [] camera_xyz, // camera center in world coordinates - scene_xyzatr[indx][1], //double [] camera_atr, // camera orientation relative to world frame - scenes[indx], // QuadCLT camera_QuadClt, - scenes[indx_ref], // reference + title, // final String title, + scene_xyzatr[indx][0], // double [] camera_xyz, // camera center in world coordinates + scene_xyzatr[indx][1], //double [] camera_atr, // camera orientation relative to world frame + scenes[indx], // QuadCLT camera_QuadClt, + scenes[indx_ref], // reference iscale); ers_scene.setErsDt( ers_scene_original_xyz_dt, // double [] ers_xyz_dt, @@ -2089,23 +2093,22 @@ public class OpticalFlow { int nslices = dsrbg_titles.length; // combine this scene with warped previous one - String [] rtitles = new String[nscenes * nslices]; - double [][] dbg_rslt = new double [rtitles.length][]; - for (int nslice = 0; nslice < nslices; nslice++) { - for (int nscene = 0; nscene < nscenes; nscene++) { - rtitles[nscenes * nslice + nscene] = dsrbg_titles[nslice]+"-"+time_stamps[nscene]; - dbg_rslt[nscenes * nslice + nscene] = dsrbg[nscene][nslice]; - } + String [] rtitles = new String[nscenes * nslices]; + double [][] dbg_rslt = new double [rtitles.length][]; + for (int nslice = 0; nslice < nslices; nslice++) { + for (int nscene = 0; nscene < nscenes; nscene++) { + rtitles[nscenes * nslice + nscene] = dsrbg_titles[nslice]+"-"+time_stamps[nscene]; + dbg_rslt[nscenes * nslice + nscene] = dsrbg[nscene][nslice]; } - String title = "previous_frames_matching"+suffix; - (new ShowDoubleFloatArrays()).showArrays( - dbg_rslt, - tilesX, - tilesY, - true, - title, - rtitles); - + } + (new ShowDoubleFloatArrays()).showArrays( + dbg_rslt, + tilesX, + tilesY, + true, + title, + rtitles); + } @@ -2220,6 +2223,7 @@ public class OpticalFlow { /** * Transform scene view to visually match with a reference scene. It is not accurate as it uses resampling and * related low pass filtering. + * @param title image title to pring * @param scene_xyz Scene X (right),Y (up), Z (negative away form camera) in the reference camera coordinates * or null to use scene instance coordinates. * @param scene_atr Scene azimuth, tilt and roll (or null to use scene instance). @@ -2229,6 +2233,7 @@ public class OpticalFlow { * @return Per-tile array of resampled {disparity,strength,red,blue,green} values (or nulls). */ public double [][] transformCameraVew( + final String title, final double [] scene_xyz, // camera center in world coordinates final double [] scene_atr, // camera orientation relative to world frame final QuadCLT scene_QuadClt, @@ -2261,11 +2266,26 @@ public class OpticalFlow { final ErsCorrection ersSceneCorrection = scene_QuadClt.getErsCorrection(); ersReferenceCorrection.setupERS(); // just in case - setUP using instance paRAMETERS ersSceneCorrection.setupERS(); + System.out.println("\ntransformCameraVew(): >> "+title +" <<"); + System.out.println("Reference scene ("+reference_QuadClt.getImageName()+"):"); + ersReferenceCorrection.printVectors(null, null); + System.out.println("Target scene ("+scene_QuadClt.getImageName()+"):"); + ersSceneCorrection.printVectors (scene_xyz, scene_atr); final Thread[] threads = ImageDtt.newThreadArray(threadsMax); final AtomicInteger ai = new AtomicInteger(0); - final double [] zbuffer = new double [tiles]; -// DoubleAccumulator admax = new DoubleAccumulator (Double::max, Double.NEGATIVE_INFINITY); -// DoubleAccumulator admax = new DoubleAccumulator (Double::max, Double.NEGATIVE_INFINITY); +// final double [] zbuffer = new double [tiles]; + DoubleAccumulator [] azbuffer = new DoubleAccumulator[tiles]; + for (int ithread = 0; ithread < threads.length; ithread++) { + threads[ithread] = new Thread() { + public void run() { + for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) { + azbuffer[nTile] = new DoubleAccumulator (Double::max, Double.NEGATIVE_INFINITY); + } + } + }; + } + ImageDtt.startAndJoin(threads); + ai.set(0); for (int ithread = 0; ithread < threads.length; ithread++) { threads[ithread] = new Thread() { public void run() { @@ -2297,12 +2317,15 @@ public class OpticalFlow { int spx = (int) Math.round(pXpYD[0]*scale); int spy = (int) Math.round(pXpYD[1]*scale); if ((px >= 0) && (py >= 0) && (px < tilesX) & (py < tilesY)) { + double d = pXpYD[2]; + azbuffer[nTile].accumulate(pXpYD[2]); //Z-buffer - if (!(pXpYD[2] < zbuffer[px + py* tilesX])) { - zbuffer[px + py* tilesX] = pXpYD[2]; +// if (!(d < zbuffer[px + py* tilesX])) { +// zbuffer[px + py* tilesX] = d; + if (!(d < azbuffer[nTile].get())) { if ((spx >= 0) && (spy >= 0) && (spx < stilesX) & (spy < stilesY)) { int sTile = spx + spy* stilesX; - ds[QuadCLT.DSRBG_DISPARITY][sTile] = pXpYD[2]; //reduce* + ds[QuadCLT.DSRBG_DISPARITY][sTile] = d; // pXpYD[2]; //reduce* for (int i = QuadCLT.DSRBG_STRENGTH; i < dsrbg_camera.length; i++) { ds[i][sTile] = dsrbg_camera[i][nTile]; // reduce * } @@ -2316,14 +2339,118 @@ public class OpticalFlow { }; } ImageDtt.startAndJoin(threads); + ai.set(0); + for (int ithread = 0; ithread < threads.length; ithread++) { + threads[ithread] = new Thread() { + public void run() { + for (int i = ai.getAndIncrement(); i < ds.length; i = ai.getAndIncrement()) { + ds[i] = (new DoubleGaussianBlur()).blurWithNaN( + ds[i], // double[] pixels, + null, // double [] in_weight, // or null + stilesX, // int width, + stilesY, // int height, + sigma, // double sigmaX, + sigma, // double sigmaY, + 0.01); // double accuracy); + } + } + }; + } + ImageDtt.startAndJoin(threads); + ai.set(0); + + final double [][] dsrbg_out = new double [dsrbg_camera.length][tiles]; + final int [][] num_non_nan = new int [dsrbg_out.length] [tiles]; + for (int ithread = 0; ithread < threads.length; ithread++) { + threads[ithread] = new Thread() { + public void run() { + for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) if (dsrbg_camera[QuadCLT.DSRBG_STRENGTH][nTile] > 0.0) { + int tileY = nTile / tilesX; + int tileX = nTile % tilesX; + int tile = tileX + tileY * tilesX; + int stileY0 = tileY * iscale; + int stileY1 = stileY0 + iscale; + int stileX0 = tileX * iscale; + int stileX1 = stileX0 + iscale; + for (int stileY = stileY0; stileY < stileY1; stileY++) { + for (int stileX = stileX0; stileX < stileX1; stileX++) { + int stile = stileX + stileY * stilesX; + for (int i = 0; i < dsrbg_out.length; i++) { + double d = ds[i][stile]; + if (!Double.isNaN(d)) { + num_non_nan[i][tile] ++; + dsrbg_out[i][tile] += d; + } + } + } + } + } + } + }; + } + ImageDtt.startAndJoin(threads); + for (int i = 0; i < dsrbg_out.length; i++) { + for (int j = 0; j < tiles; j++) { + if (num_non_nan[i][j] == 0) { + dsrbg_out[i][j] = Double.NaN; + } else { + dsrbg_out[i][j]/=num_non_nan[i][j]; + } + } + } + + if (num_passes > 0) { + for (int i = 0; i < dsrbg_out.length; i++) { + dsrbg_out[i] = tp.fillNaNs( + dsrbg_out[i], // double [] data, + tilesX, //int width, + 2 * num_passes, // int grow, + 0.5 * Math.sqrt(2.0), // double diagonal_weight, // relative to ortho + num_passes * rel_num_passes, // int num_passes, + threadsMax); // final int threadsMax) // maximal number of threads to launch + } + } + return dsrbg_out; + } + + + public double [][] transformCameraVewSingle( + double [] scene_xyz, // camera center in world coordinates + double [] scene_atr, // camera orientation relative to world frame + QuadCLT scene_QuadClt, + QuadCLT reference_QuadClt, + int iscale) + { + TileProcessor tp = reference_QuadClt.getTileProcessor(); + int tilesX = tp.getTilesX(); + int tilesY = tp.getTilesY(); + int tiles = tilesX*tilesY; + int transform_size = tp.getTileSize(); + int rel_num_passes = 10; + int num_passes = transform_size; // * 2; + + int stilesX = iscale*tilesX; + int stilesY = iscale*tilesY; + int stiles = stilesX*stilesY; + double sigma = 0.5 * iscale; + double scale = 1.0 * iscale/transform_size; + double [][] dsrbg_camera = scene_QuadClt.getDSRBG(); +/// double [][] dsrbg_reference = reference_QuadClt.getDSRBG(); + double [][] ds = new double [dsrbg_camera.length][stiles]; + for (int i = 0; i -1) { - System.out.println("Pass 1 scene "+i+" (of "+ scenes.length+") "+ - reference_QuadClt.getImageName() + "/" + scene_QuadClt.getImageName()+" Done."); - } ers_reference.addScene(scene_QuadClt.getImageName(), scenes_xyzatr[i][0], scenes_xyzatr[i][1], @@ -2736,7 +2862,7 @@ public class OpticalFlow { int tilesY = tp.getTilesY(); if (clt_parameters.ofp.enable_debug_images && (debug_level > 0)) { compareRefSceneTiles( - "before_LMA", // String suffix, + "-before_LMA", // String suffix, blur_reference, // boolean blur_reference, camera_xyz0, // double [] camera_xyz0, camera_atr0, // double [] camera_atr0, @@ -2868,7 +2994,7 @@ public class OpticalFlow { if (clt_parameters.ofp.enable_debug_images && (debug_level == 1)) { /// if (!clt_parameters.ofp.enable_debug_images || (clt_parameters.ofp.enable_debug_images && (debug_level == 1))) { compareRefSceneTiles( - "after_lma", // String suffix, + "-after_lma", // String suffix, blur_reference, // boolean blur_reference, camera_xyz0, // double [] camera_xyz0, camera_atr0, // double [] camera_atr0, @@ -2876,33 +3002,7 @@ public class OpticalFlow { scene_QuadCLT, // QuadCLT scene_QuadCLT, iscale); // int iscale) // 8 } - /* - double [] wxyz_center_dt_prev1 = ersCorrectionPrev.ers_wxyz_center_dt; - double [] watr_center_dt_prev1 = ersCorrectionPrev.ers_watr_center_dt; // is twice omega! - double [] wxyz_delta1 = new double[3]; - double [] watr_delta1 = new double[3]; - for (int i = 0; i <3; i++) { - wxyz_delta1[i] = dt * (k_prev * wxyz_center_dt_prev1[i] + (1.0-k_prev) * ersCorrection.ers_wxyz_center_dt[i]); - watr_delta1[i] = 0.5 * dt * (k_prev * watr_center_dt_prev1[i] + (1.0-k_prev) * ersCorrection.ers_watr_center_dt[i]); - } - watr_delta1[0] = -watr_delta1[0]; /// TESTING! - watr_delta1[2] = -watr_delta1[2]; /// TESTING! - if (debug_level > 0) { - System.out.println(IntersceneLma.printNameV3("ATR from ERS", watr_delta1)); - System.out.println(IntersceneLma.printNameV3("XYZ from ERS", wxyz_delta1)); - System.out.println("Number of full corr+LMA runs = "+(nlma+1)); - } - */ - /* - reference_QuadCLT.getErsCorrection().addScene(scene_QuadCLT.getImageName(), camera_xyz0,camera_atr0); - - reference_QuadCLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) - null, // String path, // full name with extension or w/o path to use x3d directory - debug_level); - */ return new double [][] {camera_xyz0, camera_atr0}; - -// return null; //pair; } @@ -3187,11 +3287,12 @@ public class OpticalFlow { int tilesX = tp.getTilesX(); int tilesY = tp.getTilesY(); String [] dsrbg_titles = {"d", "s", "r", "b", "g"}; - + String title = this_image_name+"-"+scene_QuadCLT.image_name+"-dt"+dt; double [][] dsrbg = transformCameraVew( // shifts previous image correctly (right) - camera_xyz0, // double [] camera_xyz, // camera center in world coordinates - camera_atr0, //double [] camera_atr, // camera orientation relative to world frame - scene_QuadCLT, // QuadCLT camera_QuadClt, + title, // final String title, + camera_xyz0, // double [] camera_xyz, // camera center in world coordinates + camera_atr0, //double [] camera_atr, // camera orientation relative to world frame + scene_QuadCLT, // QuadCLT camera_QuadClt, reference_QuadCLT, // reference iscale); double [][][] pair = {reference_QuadCLT.getDSRBG(),dsrbg}; @@ -3223,7 +3324,7 @@ public class OpticalFlow { dbg_rslt[2*i] = pair[0][i]; dbg_rslt[2*i+1] = pair[1][i]; } - String title = this_image_name+"-"+scene_QuadCLT.image_name+"-dt"+dt; +// String title = this_image_name+"-"+scene_QuadCLT.image_name+"-dt"+dt; (new ShowDoubleFloatArrays()).showArrays( dbg_rslt, tilesX, -- 2.18.1