Commit 360eb591 authored by Andrey Filippov's avatar Andrey Filippov

improving fitting

parent 24a92f5c
...@@ -4509,8 +4509,8 @@ public class GpuQuad{ // quad camera description ...@@ -4509,8 +4509,8 @@ public class GpuQuad{ // quad camera description
double [][] cxy = new double [2][2]; // image number, {x,y} double [][] cxy = new double [2][2]; // image number, {x,y}
int [][] icxy = new int [2][2]; int [][] icxy = new int [2][2];
for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) { for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) {
int tileY = nTile / tiles_woi.width + tiles_woi.x; int tileY = nTile / tiles_woi.width + tiles_woi.y;
int tileX = nTile % tiles_woi.width + tiles_woi.y; int tileX = nTile % tiles_woi.width + tiles_woi.x;
double [] cxy0 = { double [] cxy0 = {
(tileX + 0.5) * GPUTileProcessor.DTT_SIZE, (tileX + 0.5) * GPUTileProcessor.DTT_SIZE,
(tileY + 0.5) * GPUTileProcessor.DTT_SIZE}; (tileY + 0.5) * GPUTileProcessor.DTT_SIZE};
......
...@@ -59,8 +59,8 @@ public class ComboMatch { ...@@ -59,8 +59,8 @@ public class ComboMatch {
// String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_08_november.data"; // String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_08_november.data";
// String files_list_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_10_short.list"; // String files_list_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_10_short.list";
// String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_10_short.data"; // String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_10_short.data";
String files_list_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_19_sep13.list"; String files_list_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_19_sep13_25-50-75-100m.list";
String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_19_sep13.data"; String orthoMapsCollection_path = "/media/elphel/SSD3-4GB/lwir16-proc/ortho_videos/maps_19_sep13_25-50-75-100m.data";
//maps_19_sep13.list //maps_19_sep13.list
//maps_09_short.list //maps_09_short.list
//maps_08_november.list //maps_08_november.list
...@@ -76,21 +76,30 @@ public class ComboMatch { ...@@ -76,21 +76,30 @@ public class ComboMatch {
// String [] gpu_spair = {"1697877465_672024", "1697877528_776377"}; // String [] gpu_spair = {"1697877465_672024", "1697877528_776377"};
// String [] gpu_spair = {"1694564245_111645", "1694564701_230240"}; // Morning Sep, 13 // String [] gpu_spair = {"1694564245_111645", "1694564701_230240"}; // Morning Sep, 13
// String [] gpu_spair = {"1694564245_111645", "1694564822_637346"}; // Morning Sep, 13 // String [] gpu_spair = {"1694564245_111645", "1694564822_637346"}; // Morning Sep, 13
String [] gpu_spair = {"1694564248_145989", "1694564819_336247"}; // Morning Sep, 13 // String [] gpu_spair = {"1694564248_145989", "1694564819_336247"}; // Morning Sep, 13
// String [] gpu_spair = {"1694564269_536448", "1694564822_637346"}; // Morning Sep, 13 50m, 75m
// // String [] gpu_spair = {"1694564269_536448", "1694565541_040640"}; // Morning Sep, 13 50m, 100m
// String [] gpu_spair = {"1694564822_637346", "1694565541_040640"}; // Morning Sep, 13 75m, 100m
// String [] gpu_spair = {"1694564263_701171", "1694564816_468625"}; // Morning Sep, 13 50m, 75m
// String [] gpu_spair = {"1694564266_568793", "1694564819_336247"}; // Morning Sep, 13 50m, 75m
// String [] gpu_spair = {"1694564270_086631", "1694564822_637346"}; // Morning Sep, 13 50m, 75m
// String [] gpu_spair = {"1694564272_770858", "1694564778_589341"}; // Morning Sep, 13 50m, 75m #13-#63
// String [] gpu_spair = {"1694564275_538447", "1694564744_411290"}; // Morning Sep, 13 50m, 75m #14-#58
String [] gpu_spair = {"1694564270_086631", "1694563011_045597"}; // Morning Sep, 13 50m, 25m #88-#68
//
double [][][] image_enuatr = {{{0,0,0},{0,0,0}},{{0,0,0},{0,0,0}}}; 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_width= clt_parameters.imp.rln_gpu_width; // 3008;
int gpu_height= clt_parameters.imp.rln_gpu_height; // 3008; int gpu_height= clt_parameters.imp.rln_gpu_height; // 3008;
int zoom_lev = -3; // 0; // +1 - zoom in twice, -1 - zoom out twice int zoom_lev = -3; // 0; // +1 - zoom in twice, -1 - zoom out twice
boolean show_combo_map = false; // true;
boolean use_alt = false; boolean use_alt = false;
boolean show_centers = true; boolean show_centers = true;
boolean use_saved_collection = true; // false; boolean use_saved_collection = true; // false;
boolean save_collection = true; boolean save_collection = true;
boolean process_correlation = true; // use false to save new version of data boolean process_correlation = true; // use false to save new version of data
boolean restore_temp = true; boolean restore_temp = true;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,800); double frac_remove = 0.25;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,900);
gd.addStringField ("Image list full path", files_list_path, 180, "Image list full path."); 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.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 ("Use saved maps collection", use_saved_collection, "If false - use files list.");
...@@ -118,9 +127,11 @@ public class ComboMatch { ...@@ -118,9 +127,11 @@ public class ComboMatch {
gd.addNumericField("GPU image height", gpu_height, 0,4,"", gd.addNumericField("GPU image height", gpu_height, 0,4,"",
"GPU image height"); "GPU image height");
gd.addCheckbox ("Show transformation centers", show_centers, "Mark verticals from the UAS on the ground."); gd.addCheckbox ("Show transformation centers", show_centers, "Mark verticals from the UAS on the ground.");
gd.addCheckbox ("Process altitude images", use_alt, "Load and process altitude maps."); gd.addCheckbox ("Show combo image map", show_combo_map, "Load and process altitude maps.");
gd.addCheckbox ("Show altitude combo image", use_alt, "Load and process altitude maps.");
gd.addStringField ("First matching timestamp", gpu_spair[0], 20, "First GPU-matching timestamp with '_' for decimal point."); gd.addStringField ("First matching timestamp", gpu_spair[0], 20, "First GPU-matching timestamp with '_' for decimal point.");
gd.addStringField ("Second matching timestamp", gpu_spair[1], 20, "First GPU-matching timestamp with '_' for decimal point."); gd.addStringField ("Second matching timestamp", gpu_spair[1], 20, "First GPU-matching timestamp with '_' for decimal point.");
gd.addNumericField("Remove fraction of worst matches", frac_remove, 3,7,"", "When fitting scenes remove this fraction of worst match.");
gd.showDialog(); gd.showDialog();
if (gd.wasCanceled()) return false; if (gd.wasCanceled()) return false;
...@@ -146,10 +157,12 @@ public class ComboMatch { ...@@ -146,10 +157,12 @@ public class ComboMatch {
OrthoMap.setGPUWidthHeight(gpu_width,gpu_height); OrthoMap.setGPUWidthHeight(gpu_width,gpu_height);
show_centers = gd.getNextBoolean(); show_centers = gd.getNextBoolean();
show_combo_map = gd.getNextBoolean();
use_alt = gd.getNextBoolean(); use_alt = gd.getNextBoolean();
gpu_spair[0] = gd.getNextString(); gpu_spair[0] = gd.getNextString();
gpu_spair[1] = gd.getNextString(); gpu_spair[1] = gd.getNextString();
frac_remove = gd.getNextNumber();
OrthoMapsCollection maps_collection=null; OrthoMapsCollection maps_collection=null;
if (use_saved_collection) { if (use_saved_collection) {
...@@ -160,7 +173,7 @@ public class ComboMatch { ...@@ -160,7 +173,7 @@ public class ComboMatch {
e.printStackTrace(); e.printStackTrace();
} }
} else { } else {
maps_collection = new OrthoMapsCollection(files_list_path); // should have ".list" extensiohn maps_collection = new OrthoMapsCollection(files_list_path); // should have ".list" extension
} }
String [] names = maps_collection.getNames(); String [] names = maps_collection.getNames();
...@@ -172,13 +185,16 @@ public class ComboMatch { ...@@ -172,13 +185,16 @@ public class ComboMatch {
// which pair to compare // which pair to compare
// String [] gpu_spair = {names[gpu_ipair[0]],names[gpu_ipair[1]]}; // String [] gpu_spair = {names[gpu_ipair[0]],names[gpu_ipair[1]]};
int [] origin = new int[2]; int [] origin = new int[2];
ImagePlus imp_img = maps_collection.renderMulti ( ImagePlus imp_img = null;
if (show_combo_map) {
imp_img = maps_collection.renderMulti (
"multi_"+zoom_lev, // String title, "multi_"+zoom_lev, // String title,
false, // boolean use_alt, false, // boolean use_alt,
show_centers, // boolean show_centers, show_centers, // boolean show_centers,
zoom_lev, // int zoom_level, zoom_lev, // int zoom_level,
origin); // int [] origin){ origin); // int [] origin){
imp_img.show(); imp_img.show();
}
ImagePlus imp_alt = null; ImagePlus imp_alt = null;
if (use_alt) { if (use_alt) {
imp_alt =maps_collection.renderMulti ( imp_alt =maps_collection.renderMulti (
...@@ -228,21 +244,34 @@ public class ComboMatch { ...@@ -228,21 +244,34 @@ public class ComboMatch {
// affine1[0][2] = 0.64; affine1[1][2] = 1.253; // {"1697877412_004148", "1697877522_274211"}; // // affine1[0][2] = 0.64; affine1[1][2] = 1.253; // {"1697877412_004148", "1697877522_274211"}; //
// (346.250-348.000)/12.5,(277.000-269.250)/12.5 = (-0.14, 0.62) // (346.250-348.000)/12.5,(277.000-269.250)/12.5 = (-0.14, 0.62)
// affine1[0][2] = -0.14; affine1[1][2] = 0.62; // {"1697877409_353265", "1697877518_773045"}; // // affine1[0][2] =-0.14; affine1[1][2] = 0.62; // {"1697877409_353265", "1697877518_773045"}; //
// affine1[0][2] = -1.44; affine1[1][2] = 1.653; //"1697877465_672024", "1697877563_587972" // affine1[0][2] =-1.44; affine1[1][2] = 1.653; //"1697877465_672024", "1697877563_587972"
// affine1[0][2] = -0.25; affine1[1][2] = -1.627; //{"1697877465_672024", "1697878995_797332"}; // affine1[0][2] =-0.25; affine1[1][2] = -1.627; //{"1697877465_672024", "1697878995_797332"};
// affine1[0][2] = -1.08; affine1[1][2] = 3.267; //{"1697877465_672024", "1697877528_776377"}; // affine1[0][2] =-1.08; affine1[1][2] = 3.267; //{"1697877465_672024", "1697877528_776377"};
// affine1[0][2] = -1.2; affine1[1][2] = -0.9066; //{"1694564245_111645", "1694564701_230240"}; // affine1[0][2] =-1.2; affine1[1][2] = -0.9066; //{"1694564245_111645", "1694564701_230240"};
// affine1[0][2] = 0.933; affine1[1][2] = -2.2667; //{"1694564245_111645", "1694564822_637346"}; // affine1[0][2] = 0.933; affine1[1][2] = -2.2667; //{"1694564245_111645", "1694564822_637346"};
affine1[0][2] = 2.08; affine1[1][2] = -0.093; //{"1694564248_145989", "1694564819_336247"}; // affine1[0][2] = 2.08; affine1[1][2] = -0.093; //{"1694564248_145989", "1694564819_336247"};
// affine1[0][2] =-1.88; affine1[1][2] = -2.71; //{"1694564269_536448", "1694564822_637346"};
// affine1[0][2] =-2.7; affine1[1][2] = -0.59; //{"1694564269_536448", "1694565541_040640"}; 50m-100m
// affine1[0][2] =-0.78; affine1[1][2] = 2.1; //{"1694564822_637346", "1694565541_040640"}; 75m-100m #75-#95
// affine1[0][2] =-0.92; affine1[1][2] = 2.28; //{"1694564263_701171", "1694564816_468625"}; 50m-75m
// affine1[0][2] =-5.37; affine1[1][2] = -1.53; //{"1694564266_568793", "1694564819_336247"}; 50m-75m
// affine1[0][2] = 1.92; affine1[1][2] = -1.89; //{"1694564270_086631", "1694564822_637346"}; 50m-75m
// affine1[0][2] = 0.52; affine1[1][2] = -2.387; //{"1694564272_770858", "1694564778_589341"}; 50m-75m #13-#63
// affine1[0][2] =-4.75; affine1[1][2] = 0.44; //{"1694564275_538447", "1694564744_411290"}; 50m-75m #14-#58
affine1[0][2] = 2.59; affine1[1][2] = 0.133; //{"1694564270_086631", "1694563011_045597"}; 50m, 25m #88-#68
// String [] gpu_spair = {"1694564270_086631", "1694563011_045597"}; // Morning Sep, 13 50m, 25m #88-#68
// "1694564275_538447","1694564744_411290"
double [][][] affines = {affine0,affine1}; double [][][] affines = {affine0,affine1};
int [] gpu_pair = new int[gpu_spair.length]; int [] gpu_pair = new int[gpu_spair.length];
for (int i = 0; i < gpu_pair.length; i++) { for (int i = 0; i < gpu_pair.length; i++) {
gpu_pair[i] = maps_collection.getIndex(gpu_spair[i]); gpu_pair[i] = maps_collection.getIndex(gpu_spair[i]);
} }
int [] zooms = {-3,-1,1000,1000}; int [] zooms = {-2,0,1000,1000};
debugLevel = 0; debugLevel = 0;
boolean batch_mode = true; boolean batch_mode = false; // true;
boolean ignore_prev_rms = true;
for (int zi = 0; zi < zooms.length; zi++) { for (int zi = 0; zi < zooms.length; zi++) {
zoom_lev = zooms[zi]; zoom_lev = zooms[zi];
if (zoom_lev >=1000) { if (zoom_lev >=1000) {
...@@ -251,12 +280,16 @@ public class ComboMatch { ...@@ -251,12 +280,16 @@ public class ComboMatch {
// will modify affines[1], later add jtj, weight, smth. else? // will modify affines[1], later add jtj, weight, smth. else?
double [][] corr_pair_rslt = maps_collection.correlateOrthoPair( double [][] corr_pair_rslt = maps_collection.correlateOrthoPair(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
frac_remove, // double frac_remove, // = 0.25
ignore_prev_rms, // boolean ignore_prev_rms,
batch_mode, // boolean batch_mode, batch_mode, // boolean batch_mode,
gpu_pair, // String [] gpu_spair, gpu_pair, // String [] gpu_spair,
affines, // double [][][] affines, // on top of GPS offsets affines, // double [][][] affines, // on top of GPS offsets
zoom_lev, // int zoom_lev, zoom_lev, // int zoom_lev,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
int render_zoom_lev = maps_collection.ortho_maps[gpu_pair[0]].getOriginalZoomLevel(); int render_zoom_lev = Math.min(
maps_collection.ortho_maps[gpu_pair[0]].getOriginalZoomLevel(),
maps_collection.ortho_maps[gpu_pair[1]].getOriginalZoomLevel());
ImagePlus imp_img_pair = maps_collection.renderMulti ( ImagePlus imp_img_pair = maps_collection.renderMulti (
"multi_"+gpu_spair[0]+"-"+gpu_spair[1]+"-zoom_"+render_zoom_lev+"_"+zoom_lev, // String title, "multi_"+gpu_spair[0]+"-"+gpu_spair[1]+"-zoom_"+render_zoom_lev+"_"+zoom_lev, // String title,
false, // boolean use_alt, false, // boolean use_alt,
...@@ -302,6 +335,38 @@ adjusted affines[1] for a pair: 1694564245_111645/1694564822_637346 ...@@ -302,6 +335,38 @@ adjusted affines[1] for a pair: 1694564245_111645/1694564822_637346
[[1.0148318094351692,0.01611699521114023,1.1534804399270817], [[1.0148318094351692,0.01611699521114023,1.1534804399270817],
[-0.025584298199020607,1.0182605131307008,-2.157143969635918]] [-0.025584298199020607,1.0182605131307008,-2.157143969635918]]
correlateOrthoPair(): adjusted affines[1] {"1694564269_536448", "1694564822_637346"}
[[1.0118405178770247,0.0243306610630595,-1.6570450812227402],
[-0.030053577777794035,1.007846539173136,-2.564940874119216]]
adjusted affines[1] for a pair: 1694564269_536448/1694565541_040640
[[1.032352933444291,0.012425600945003055,-2.6012366590312457],
[-0.032390806278202705,1.045595625691147,0.4724083642132655]]
Done
adjusted affines[1] for a pair: 1694564263_701171/1694564816_468625 (9- 73)
[[1.015002848762524,0.016906877812481423,-0.827920765016369],
[-0.016806089590596436,1.0126951556271355,2.453634727273915]]
adjusted affines[1] for a pair: 1694564266_568793/1694564819_336247
[[1.0208243680493612,-0.007571648582186553,-5.627387091202717],
[0.00716961393885021,1.0116421129852677,-1.4359711787904814]]
adjusted affines[1] for a pair: 1694564270_086631/1694564822_637346
[[1.01553064725059,-0.004358499034078345,1.9313585652985712],
[0.006088341815143438,1.0077680445468122,-1.7817699129643216]]
adjusted affines[1] for a pair: 1694564272_770858/1694564778_589341
[[1.0120132362035976,0.03369140870847741,0.7958782949644478],
[-0.03200874269606805,1.0088673434818738,-2.211451677912688]]
Bad, even with narrow WOI:
Done
adjusted affines[1] for a pair: 1694564275_538447/1694564744_411290
[[1.016644878296508,0.004929435606711805,-4.693221236458512],
[-0.0049692732545438024,1.0068633692157516,0.3968458715663825]]
* *
* *
double [] offset_xy_second = {0,0}; double [] offset_xy_second = {0,0};
...@@ -623,7 +688,7 @@ adjusted affines[1] for a pair: 1694564245_111645/1694564822_637346 ...@@ -623,7 +688,7 @@ adjusted affines[1] for a pair: 1694564245_111645/1694564822_637346
debugLevel); // final int debug_level debugLevel); // final int debug_level
} }
double [][][] vector_field = new double [corr_tiles_pd.length][][]; double [][][] vector_field = new double [corr_tiles_pd.length][][];
vector_field[0] = TDCorrTile.getMismatchVector( vector_field[0] = TDCorrTile.getMismatchVector( // full tiles in gpu (512*512)
corr_tiles_pd[0], // final double[][] tiles, corr_tiles_pd[0], // final double[][] tiles,
rln_sngl_rstr, // double rmax, rln_sngl_rstr, // double rmax,
rln_cent_radius, // final double centroid_radius, // 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad) rln_cent_radius, // final double centroid_radius, // 0 - all same weight, > 0 cosine(PI/2*sqrt(dx^2+dy^2)/rad)
......
package com.elphel.imagej.orthomosaic; package com.elphel.imagej.orthomosaic;
import java.awt.Rectangle;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.tileprocessor.ImageDtt;
public class FloatImageData { public class FloatImageData {
public int width; public int width;
public int height; public int height;
...@@ -66,12 +73,12 @@ public class FloatImageData { ...@@ -66,12 +73,12 @@ public class FloatImageData {
ze = 2.0 * pix_size_in_cm; ze = 2.0 * pix_size_in_cm;
} }
} else { // high resolution, pixel size < 1 } else { // high resolution, pixel size < 1
zl--; // zl--;
while (pix_size_in_cm <= (1.0-e)) { while (pix_size_in_cm <= (1.0-e)) {
zl++; zl++;
pix_size_in_cm *= 2; pix_size_in_cm *= 2;
}// exits with (2-2*e) >= pix_in_cm > (1-e) }// exits with (2-2*e) >= pix_in_cm > (1-e)
vz = pix_size_in_cm > (2.0 - e); vz = pix_size_in_cm < (1.0 + e);
if (!vz) { if (!vz) {
ze = pix_size_in_cm; ze = pix_size_in_cm;
} }
...@@ -82,9 +89,9 @@ public class FloatImageData { ...@@ -82,9 +89,9 @@ public class FloatImageData {
if (zoom_in_extra != null) { if (zoom_in_extra != null) {
zoom_in_extra[0] = ze; zoom_in_extra[0] = ze;
} }
return zl; return zl;
} }
// processing altitudes to remove non-flat surfaces from fitting ortho maps
} }
...@@ -23,6 +23,7 @@ import java.util.concurrent.atomic.AtomicInteger; ...@@ -23,6 +23,7 @@ import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters; import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.GenericJTabbedDialog; import com.elphel.imagej.common.GenericJTabbedDialog;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays; import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.TpTask; import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.Imx5; import com.elphel.imagej.ims.Imx5;
...@@ -618,6 +619,91 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -618,6 +619,91 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
return true; return true;
} }
/**
* Get elevation map with the same resolution as vector_field corresponding to the GPU-processed
* data. Array is organized at a tile grid (width corresponding to GPU width in tiles),source
* tp_tasks provides tile center coordinates for the current GPU zoom level, they are scaled for
* the source elevation data
* @param zoom_level zoom level of the GPU image, 0 is 1pix=1cm, -1 is 1pix=2cm
* @param tp_tasks array of non-empty tiles to be processed in the GPU. Integer tx and ty reference
* tile on a tile grid (8x8 pix), center X,Y for the only "sensor" 0 - float coordinates
* corresponding to the current GPU zoom level
* @param tilesX
* @param tilesY
* @param debugLevel
* @return
*/
public double [] getTileElevations(
final int zoom_level,
final TpTask [] tp_tasks,
final int tilesX,
final int tilesY,
final int debugLevel) {
final FloatImageData src_elev = getAltData();
if (src_elev == null) {
if (debugLevel > -4) {
System.out.println("getTileElevations(): No elevation data available for "+name);
}
return null;
}
final int src_width = src_elev.width;
final int src_height = src_elev.height;
final float [] felev = src_elev.data;
final double [] elev = new double [tilesX*tilesY];
Arrays.fill(elev, Double.NaN);
final int diff_zoom_lev = zoom_level-orig_zoom_level;
// double s = 1.0;
// int zl = diff_zoom_lev;
// for (; zl< 0; zl++) s *= 2.0;
// for (; zl> 0; zl--) s *= 0.5; // normally will not happen
final double scale = 1.0/getScale (diff_zoom_lev); // int zoom_lev);
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iTile = ai.getAndIncrement(); iTile < tp_tasks.length; iTile = ai.getAndIncrement()) {
TpTask task = tp_tasks[iTile];
float [] xy = task.getXY()[0];
double px = xy[0] * scale;
double py = xy[1] * scale;
// linear interpolate
int ipx = (int) Math.floor(px);
int ipy = (int) Math.floor(py);
double fpx = px -ipx;
double fpy = py -ipy;
if ((ipx >= 0) && (ipy >= 0) && (ipx < src_width) && (ipy < src_height)) {
int indx00 = ipy*src_width+ipx;
int indx = task.getTileY()*tilesX + task.getTileX();
elev[indx] =
(1.0-fpx)*(1.0-fpy)*felev[indx00] +
( fpx)*(1.0-fpy)*felev[indx00 + 1] +
(1.0-fpx)*( fpy)*felev[indx00 + src_width] +
( fpx)*( fpy)*felev[indx00 + src_width + 1];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return elev;
}
/**
* Get scale from zoom level: 0-> 1.0, -1 -> 0.5
* @param zoom_lev
* @return
*/
public static double getScale (
int zoom_lev) {
double s = 1.0;
int zl = zoom_lev;
for (; zl< 0; zl++) s *= 0.5;
for (; zl> 0; zl--) s *= 2.0;
return s;
}
/** /**
* Copy prepared scaled image to the top-left corner of the fixed-size * Copy prepared scaled image to the top-left corner of the fixed-size
* float[] array for the GPU input * float[] array for the GPU input
...@@ -988,4 +1074,493 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -988,4 +1074,493 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
return affine; return affine;
} }
/**
* Get planar approximation of the ground
*
* @param zoom_lev zoom level of the provided elevation data (0- 1pix=1 cm, -1: 1 pix=2cm)
* @param elev elevation data, meters ASL
* @param width pixel width corresponding to the elev[] array
* @param initial_above meters above average to ignore for the histogram.
* @param initial_below meters below average (positive value) to ignore for the histogram
* @param num_refine number of the plane generation refine passes
* @param frac_above remove high fraction of all tiles
* @param frac_below remove low fraction of all tiles
* @param debug_title Name of the debug image or null to skip
*
* @return plane parameters - all metric, relative to the vertical point:
* {tilt_x (m/m), tilt_y (m/m), offset (elevation at vertical point in meters)
*/
public double [] getPlaneMeters (
int zoom_lev, // of the data (3 levels down for tiles)
double [] elev,
int width,
double initial_above, // from average
double initial_below, // from average, // positive value
int num_refine,
double frac_above,
double frac_below,
String debug_title) {
final int num_bins = 1024;
int diff_zoom_lev = zoom_lev - orig_zoom_level;
double pix_size = orig_pix_meters / getScale(diff_zoom_lev); // zoom_lev negative, pixel larger
double [] pix_vertical = {vert_meters[0]/pix_size,vert_meters[1]/pix_size}; // image center in pixels
double avg_all = getMaskedAverage (
elev, // final double [] data,
null); // final boolean [] mask)
double abs_high= avg_all + initial_above;
double abs_low= avg_all - initial_below;
boolean [] mask = removeAbsoluteLowHigh (
elev, // final double [] data,
null, // final boolean [] mask,
abs_high, // final double threshold_high,
abs_low); // final double threshold_low),
double [] plane_pix = {0,0,avg_all,pix_vertical[0], pix_vertical[1]}; // 5 elements here
for (int ntry = 0; ntry < num_refine; ntry++) {
if (ntry > 0) {
// remove relatives, start from new mask
mask = removeRelativeLowHigh (
elev, // final double [] data,
null, // final boolean [] mask_in,
initial_above, // final double abs_high,
-initial_below, // final double abs_low,
frac_above, // final double rhigh,
frac_below, // final double rlow,
plane_pix, // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
width, // final int width, // only used with ground_plane != null;
num_bins); // final int num_bins)
}
plane_pix= getPlane(
elev, // final double [] data,
mask , // final boolean [] mask,
null, // final double [] weight,
width, // final int width,
pix_vertical); // final double [] xy0)
}
if (debug_title != null) {
String [] dbg_titles = {"elev", "plane", "diff"};
double [][] dbg_img = new double [dbg_titles.length][elev.length];
dbg_img[0] = elev;
for (int pix = 0; pix < elev.length; pix++) {
int px = pix % width;
int py = pix / width;
double x = px - plane_pix[3];
double y = py - plane_pix[4];
dbg_img[1][pix] = plane_pix[2]+ x *plane_pix[0] +y * plane_pix[1];
dbg_img[2][pix] = dbg_img[0] [pix] - dbg_img[1][pix];
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
width,
elev.length / width,
true,
debug_title,
dbg_titles);
}
return new double [] {plane_pix[0]*pix_size, plane_pix[1]*pix_size, plane_pix[2]}; // no xy0
}
/**
* Convert float[] elevation map to double[], optionally scale resolution
* down by integer value without interpolation
* @param data original float elevations ASL
* @param downscale integer downscale value
* @param orig_width original image width in pixels
* @return converted to double [] and optionally scaled down data (width
* and height scaled by integer division)
*/
public static double [] scaleElevationResolution(
final float [] data,
final int downscale,
final int orig_width) {
final int orig_height = data.length/orig_width;
final int width = orig_width / downscale; // floor
final int height = orig_height / downscale; // floor
final double [] data_out = new double [width*height];
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
if (downscale == 1) { // fast, just copy
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) {
data_out[iPix] = data[iPix];
}
}
};
}
} else {
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) {
int px = iPix % width;
int py = iPix / width;
data_out[iPix] = data[px + py*orig_width];
}
}
};
}
}
ImageDtt.startAndJoin(threads);
return data_out;
}
/**
* Calculate average of a double array, ignore masked out (!mask[i] and NaNs in the data)
* @param data double data array
* @param mask optional (may be null) boolean mask - ignore data items that have corresponding mask element false
* @return average value of the non-NaN and not disabled by mask elements of data[]
*/
private static double getMaskedAverage (
final double [] data,
final boolean [] mask) {
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
final double [] avg_arr = new double [threads.length];
final double [] npix_arr = new double [threads.length];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int thread_num = ati.getAndIncrement();
for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()) if (!Double.isNaN(data[iPix]) && ((mask == null) || (mask[iPix]))){
avg_arr[thread_num] += data[iPix];
npix_arr[thread_num] += 1.0;
}
}
};
}
ImageDtt.startAndJoin(threads);
double avg=0, num=0;
for (int i = 0; i < avg_arr.length; i++) {
avg+=avg_arr[i];
num+=npix_arr[i];
}
return avg/num;
}
/**
* Remove double [] data elements that are lower than threshold_low or greater than threshold_high, ignore masked out and NaN elements
* @param data double [] input data
* @param mask optional (may be null) mask for the data array. Will be modified if provided
* @param threshold_high remove higher elements () and NaNs in the data
* @param threshold_low remove lower elements () and NaNs in the data
* @return updated mask (same instance if not null) that disables (makes false) filtered out data [] elements
*/
private static boolean [] removeAbsoluteLowHigh (
final double [] data,
final boolean [] mask,
final double threshold_high,
final double threshold_low) {
final boolean [] mask_out = (mask == null) ? new boolean [data.length] : mask;
if (mask == null) {
Arrays.fill(mask_out, true);
}
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iPix = ai.getAndIncrement(); iPix < data.length; iPix = ai.getAndIncrement()){
double d = data[iPix];
if (!((d >= threshold_low) && (d <= threshold_high))) { // removes NaNs too
mask_out[iPix] = false;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return mask_out;
}
/**
* Create mask to disable tiles that have elevations significantly deviating from a
* flat surface and from the vertical point, so the vertical deviations may cause horizontal
* matching errors.
* @param zoom_lev zoom level of the elevation data (0- 1pix=1cm, -1 - 1pix = 2cm, etc.)
* @param elev array (decimated) of metric elevations ASL
* @param width width in pixels represented by elev array.
* @param plane_metric metric plane parameters - {tilt_x (m/m), tilt_y (m/m), offset at vertical point(m)}
* @param metric_error maximal allowed metric error (mask tiles that may potentially have larger error).
* @return boolean [] mask of the same size as elev, where false disables tiles
*/
public boolean [] removeHighElevationMismatch (
int zoom_lev, // of the data (3 levels down for tiles)
double [] elev,
int width,
double [] plane_metric, // tiltx,tilty, offs - in meters
double metric_error) {
double camera_height = lla[2]- plane_metric[2];
int diff_zoom_lev = zoom_lev - orig_zoom_level;
double pix_size = orig_pix_meters / getScale(diff_zoom_lev); // zoom_lev negative, pixel larger
double [] pix_vertical = {vert_meters[0]/pix_size,vert_meters[1]/pix_size}; // image center in pixels
double max_r_diff = metric_error * camera_height / pix_size;
double [] plane_pix =
{plane_metric[0]/pix_size,plane_metric[1]/pix_size,plane_metric[2],pix_vertical[0],pix_vertical[1]};
boolean [] mask = removeHighProjection (
elev, // final double [] data,
null, // final boolean [] mask_in, // will be modified if provided
max_r_diff, // final double max_r_diff,
plane_pix, // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
width); // final int width)
return mask;
}
/**
* Remove specified fractions (0 <= (rlow+rhigh) <= 1.0) of too low and too high values
* @param data double [] input data
* @param mask_in optional (may be null) mask for the data array. Will be modified if provided.
* @param abs_high high limit of the histogram (reasonably higher than useful range of the data[])
* @param abs_low low limit of the histogram (reasonably lower than useful range of the data[])
* @param rhigh fraction of the highest data[] elements to remove
* @param rlow fraction of the lowest data[] elements to remove
* @param ground_plane null or {tilt_x, tilt_y, offs, x0,y0}, where tilt_x and tilt_y are per pixel.
* x0, y0 are also in pixels (not meters)
* @param width - width of the data[] array. Only used if ground_plane != null
* @param num_bins number of the histogram bins
* @return boolean array of the remaining data elements. Input mask_in array (if not null) is modified too.
*/
private static boolean [] removeRelativeLowHigh (
final double [] data,
final boolean [] mask_in,
final double abs_high,
final double abs_low,
final double rhigh,
final double rlow,
final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
final int width, // only used with ground_plane != null;
final int num_bins) {
final boolean [] mask = (mask_in == null) ? new boolean [data.length] : mask_in;
if (mask_in == null) {
Arrays.fill(mask, true);
}
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
final double [][] hist2 = new double [threads.length][num_bins];
final double scale = num_bins/(abs_high - abs_low);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int thread_num = ati.getAndIncrement();
for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
double d = data[nPix];
if (!Double.isNaN(d)) {
if (ground_plane != null) {
double x = nPix % width - ground_plane[3];
double y = nPix/width - ground_plane[4];
double tilt_x = ground_plane[0];
double tilt_y = ground_plane[1];
double offs = ground_plane[2];
d -= x * tilt_x + y * tilt_y + offs;
}
int bin = Math.min(Math.max(((int) Math.round((d-abs_low)*scale)), 0), num_bins-1);
hist2[thread_num][bin] += 1.0;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
final double [] hist = new double [num_bins];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int bin = ai.getAndIncrement(); bin < num_bins; bin = ai.getAndIncrement()) {
for (int i = 0; i < hist2.length; i++) {
hist[bin]+= hist2[i][bin];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
double sw = 0;
for (int bin = 0; bin < num_bins; bin++) {
sw += hist[bin];
}
double trlow = sw * rlow;
double trhigh = sw * rhigh;
double sh = 0, shp = 0;
double threshold_high = abs_high;
for (int bin = num_bins-1; bin>=0; bin--) {
shp = sh;
sh += hist[bin];
if (sh > trhigh) {
double r = (sh-trhigh)/(sh-shp);
threshold_high = abs_low + (bin + r)/scale;
break;
}
}
sh = 0;
shp = 0;
double threshold_low = abs_low;
for (int bin = 0; bin < num_bins; bin++) {
shp = sh;
sh += hist[bin];
if (sh > trlow) {
double r = (trlow-shp)/(sh-shp);
threshold_low = abs_low + (bin + r)/scale;
break;
}
}
final double fthreshold_low = threshold_low;
final double fthreshold_high = threshold_high;
if (ground_plane == null) {
return removeAbsoluteLowHigh (
data, // final double [] data,
mask, // final boolean [] mask,
fthreshold_high, // final double threshold_high,
fthreshold_low); // final double threshold_low)
}
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
// int thread_num = ati.getAndIncrement();
for (int nPix = ai.getAndIncrement(); nPix < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
double d = data[nPix];
if (!Double.isNaN(d)) {
double x = nPix % width - ground_plane[3];
double y = nPix/width - ground_plane[4];
double tilt_x = ground_plane[0];
double tilt_y = ground_plane[1];
double offs = ground_plane[2];
d -= x * tilt_x + y * tilt_y + offs;
if (!((d >= fthreshold_low) && (d <= fthreshold_high))) {
mask[nPix] = false;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return mask;
}
/**
* Remove (mask out) tiles that have high product of distance (in pixels) from the vertiacal point
* by the metric difference between the elevation and an approximation plane
* @param data elevation data, decimated to match GPU tile grid. May have NaNs.
* @param mask_in null or boolean[] mask same dimension as the data[]/ False disables processing,
* same as NaN in data[]. Will be modified if provided;
* @param max_r_diff maximal product of pixel distance from the vertical point and elevation difference
* from the plane.
* @param ground_plane {tilt_x, tilt_y, offs, x0(pix), y0(pix)} or null tilt_x and tilt_y are measured
* in meters/pixel, offs - in meters and x0,y0 - in pixels
* @param width pixel width of the data[]
* @return boolean [] mask with disabled tiles
*/
private static boolean [] removeHighProjection (
final double [] data,
final boolean [] mask_in, // will be modified if provided
final double max_r_diff,
final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
final int width) {
final double max_r2_diff = max_r_diff*max_r_diff;
final boolean [] mask = (mask_in == null) ? new boolean [data.length] : mask_in;
if (mask_in == null) {
Arrays.fill(mask, true);
}
final Thread[] threads = ImageDtt.newThreadArray();
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 < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
double d = data[nPix];
if (!Double.isNaN(d)) {
double x = nPix % width - ground_plane[3];
double y = nPix/width - ground_plane[4];
double tilt_x = ground_plane[0];
double tilt_y = ground_plane[1];
double offs = ground_plane[2];
d -= x * tilt_x + y * tilt_y + offs;
double d2 = d*d;
double r2 = x*x + y*y;
double d2r2 = d2*r2;
if (d2r2 > max_r2_diff) {
mask[nPix] = false;
}
} else {
mask[nPix] = false;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return mask;
}
/**
* Fit a plane to the input data, relative to the specified point in Rectangle wnd
* @param data double input data array
* @param mask mask that disables somew input data (or null)
* @param weight optional weights array (same size as data and mask)
* @param width width of the rectangle
* @param xy0 a pair of x0, y0 - origin where the plane is referenced too
* @return double array of {tiltx, tilty, offset, xy0[0] and xy0[1]).
*/
private static double [] getPlane(
final double [] data,
final boolean [] mask,
final double [] weight,
final int width,
final double [] xy0) {
final double [][][] mdatai = new double [data.length][][];
AtomicInteger anum_good = new AtomicInteger(0);
final Thread[] threads = ImageDtt.newThreadArray();
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 < data.length; nPix = ai.getAndIncrement()) if (mask[nPix]){
double d = data[nPix];
if (!Double.isNaN(d)) {
int x = nPix % width;
int y = nPix / width;
double dx = x-xy0[0];
double dy = y-xy0[1];
int mindx = anum_good.getAndIncrement();
mdatai[mindx] = new double[3][];
mdatai[mindx][0] = new double [2];
mdatai[mindx][0][0] = dx;
mdatai[mindx][0][1] = dy;
mdatai[mindx][1] = new double [1];
mdatai[mindx][1][0] = d;
mdatai[mindx][2] = new double [1];
mdatai[mindx][2][0] = (weight == null)? 1.0 : weight[nPix];
}
}
}
};
}
ImageDtt.startAndJoin(threads);
final double [][][] mdata = new double [anum_good.get()][][];
System.arraycopy(mdatai, 0, mdata, 0, mdata.length);
double[][] approx2d = (new PolynomialApproximation()).quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
null, // damping, // double [] damping, null OK
-1); // debug level
if (approx2d != null) {
return new double[] {approx2d[0][0], approx2d[0][1], approx2d[0][2], xy0[0], xy0[1]}; // tiltX, tiltY, offset
}
return null;
}
} }
...@@ -267,37 +267,6 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -267,37 +267,6 @@ public class OrthoMapsCollection implements Serializable{
show_centers, // boolean show_centers, show_centers, // boolean show_centers,
zoom_level, // int zoom_level, zoom_level, // int zoom_level,
origin); // int [] origin) origin); // int [] origin)
/*
int [] wh = new int[2];
double [][] centers = new double [ortho_maps.length][];
float [][] multi = renderMulti (
use_alt, // boolean use_alt,
zoom_level, // int zoom_level,
wh, // int [] wh,
origin, // int [] origin){ // maps[0] as a reference
centers); // double [][] centers)
String [] map_names = new String[ortho_maps.length];
for (int n = 0; n < ortho_maps.length; n++) {
map_names[n] = ortho_maps[n].getName()+"_"+ortho_maps[n].getLocalDateTime().toString().replace("T","_")+"_UTC";
}
ImageStack stack = ShowDoubleFloatArrays.makeStack(
multi,
wh[0],
wh[1],
map_names,
false);
ImagePlus imp = new ImagePlus(title, stack);
if (show_centers) {
PointRoi roi = new PointRoi();
for (int i = 0; i < centers.length; i++) {
roi.addPoint(centers[i][0],centers[i][1], i+1);
}
roi.setOptions("label");
imp.setRoi(roi);
}
return imp;
*/
} }
public ImagePlus renderMulti ( public ImagePlus renderMulti (
...@@ -576,6 +545,8 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -576,6 +545,8 @@ public class OrthoMapsCollection implements Serializable{
public double [][] correlateOrthoPair( public double [][] correlateOrthoPair(
CLTParameters clt_parameters, CLTParameters clt_parameters,
double frac_remove, // = 0.25
boolean ignore_prev_rms,
boolean batch_mode, boolean batch_mode,
String [] gpu_spair, String [] gpu_spair,
double [][][] affines, // here in meters, relative to vertical points double [][][] affines, // here in meters, relative to vertical points
...@@ -587,6 +558,8 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -587,6 +558,8 @@ public class OrthoMapsCollection implements Serializable{
} }
return correlateOrthoPair( return correlateOrthoPair(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
frac_remove, // double frac_remove, // = 0.25
ignore_prev_rms, // boolean ignore_prev_rms,
batch_mode, // boolean batch_mode, batch_mode, // boolean batch_mode,
gpu_pair, // int [] gpu_pair, gpu_pair, // int [] gpu_pair,
affines, // double [][][] affines, // here in meters, relative to vertical points affines, // double [][][] affines, // here in meters, relative to vertical points
...@@ -596,6 +569,8 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -596,6 +569,8 @@ public class OrthoMapsCollection implements Serializable{
double [][] correlateOrthoPair( double [][] correlateOrthoPair(
CLTParameters clt_parameters, CLTParameters clt_parameters,
double frac_remove, // = 0.25
boolean ignore_prev_rms,
boolean batch_mode, boolean batch_mode,
int [] gpu_pair, int [] gpu_pair,
double [][][] affines, // here in meters, relative to vertical points double [][][] affines, // here in meters, relative to vertical points
...@@ -609,6 +584,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -609,6 +584,7 @@ public class OrthoMapsCollection implements Serializable{
show_gpu_img = false; // (debugLevel > 1); show_gpu_img = false; // (debugLevel > 1);
show_tile_centers = false; // true; // (debugLevel > 1); show_tile_centers = false; // true; // (debugLevel > 1);
} }
boolean show_vector_field = !batch_mode && (debugLevel>0); // true;
double [][] bounds_overlap_meters = getOverlapMeters( double [][] bounds_overlap_meters = getOverlapMeters(
gpu_pair[0], // int ref_index, gpu_pair[0], // int ref_index,
gpu_pair[1], // int other_index) gpu_pair[1], // int other_index)
...@@ -626,7 +602,6 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -626,7 +602,6 @@ public class OrthoMapsCollection implements Serializable{
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
overlap_wh_pixel[i] = ((int) Math.ceil(bounds_overlap_meters[i][1]/pix_size)) - ((int) Math.floor(bounds_overlap_meters[i][0]/pix_size)); overlap_wh_pixel[i] = ((int) Math.ceil(bounds_overlap_meters[i][1]/pix_size)) - ((int) Math.floor(bounds_overlap_meters[i][0]/pix_size));
} }
// double [][] bounds_overlap_pixels = new double[2][2];
// convert to pixels,shift top-left to [0,0] (remember offsets, limit w,h, // convert to pixels,shift top-left to [0,0] (remember offsets, limit w,h,
// change to pixels last, remember TL in meters? // change to pixels last, remember TL in meters?
// keep center where it was // keep center where it was
...@@ -646,8 +621,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -646,8 +621,7 @@ public class OrthoMapsCollection implements Serializable{
for (int i = 0; i < 2; i++) { // subtracting tl_rect_metric[n] (-1) for (int i = 0; i < 2; i++) { // subtracting tl_rect_metric[n] (-1)
tlo_src_metric[n][i] = tlo_src_metric[n][i] =
tlo_rect_metric[n][0] * affines[n][i][0] + tlo_rect_metric[n][0] * affines[n][i][0] +
tlo_rect_metric[n][1] * affines[n][i][1] + affines[n][i][2]; // - tlo_rect_metric[n][1] * affines[n][i][1] + affines[n][i][2];
// tl_rect_metric[n][i];
} }
} }
/// referenced to top-left pixel of the gpu image /// referenced to top-left pixel of the gpu image
...@@ -699,11 +673,20 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -699,11 +673,20 @@ public class OrthoMapsCollection implements Serializable{
int tilesY = gpu_height/GPUTileProcessor.DTT_SIZE; int tilesY = gpu_height/GPUTileProcessor.DTT_SIZE;
// uses fixed_size gpu image size // uses fixed_size gpu image size
// TDCorrTile [] td_corr_tiles =
TpTask [][] tp_tasks = new TpTask [2][]; TpTask [][] tp_tasks = new TpTask [2][];
int num_tries = 5; int num_tries = 5;
double prev_rms = Double.NaN; double prev_rms = Double.NaN;
double rel_improve = 1E-3; double rel_improve = 1E-3;
double [][] elevations = new double [tp_tasks.length][];
double [][] ground_planes_metric = new double [tp_tasks.length][];
boolean [][] ground_planes_masks = new boolean[2][];
double initial_above = 3; // m
double initial_below = 3; // m
int num_refine = 3;
double frac_above = 0.3;
double frac_below = 0.1;
double metric_error = 0.01;// 1 cm
for (int ntry = 0; ntry < num_tries; ntry++) { for (int ntry = 0; ntry < num_tries; ntry++) {
if (!batch_mode) { if (!batch_mode) {
if (debugLevel>-3) { if (debugLevel>-3) {
...@@ -720,13 +703,53 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -720,13 +703,53 @@ public class OrthoMapsCollection implements Serializable{
tp_tasks, // TpTask [][] tp_tasks_o, tp_tasks, // TpTask [][] tp_tasks_o,
batch_mode, // final boolean batch_mode, batch_mode, // final boolean batch_mode,
debugLevel); // final int debugLevel); debugLevel); // final int debugLevel);
// get elevations
int zoom_lev_tiles = zoom_lev-3;
double [] ground_planes_weight = null;
for (int num_elevations = 0; num_elevations < 1; num_elevations++) {
for (int n = 0; n < gpu_pair.length; n++) {
elevations[n] = ortho_maps[gpu_pair[n]]. getTileElevations(
zoom_lev, // final int zoom_level,
tp_tasks[n], // final TpTask [] tp_tasks,
tilesX, // final int tilesX,
tilesY, // final int tilesY,
debugLevel); // final int debugLevel)
String debug_title = show_vector_field? (ortho_maps[gpu_pair[n]].getName()+"_plane"):null;
ground_planes_metric[n] = ortho_maps[gpu_pair[n]].getPlaneMeters (
zoom_lev_tiles, // int zoom_lev, // of the data (3 levels down for tiles)
elevations[n], // double [] elev,
tilesX, // int width,
initial_above, // double initial_above, // from average
initial_below, // double initial_below, // from average, // positive value
num_refine, // int num_refine,
frac_above, // double frac_above,
frac_below, // double frac_below,
debug_title); // String debug_title)
ground_planes_masks[n] = ortho_maps[gpu_pair[n]].removeHighElevationMismatch (
zoom_lev_tiles, // int zoom_lev, // of the data (3 levels down for tiles)
elevations[n], // double [] elev,
tilesX, // int width,
ground_planes_metric[n], // double [] plane_metric, // tiltx,tilty, offs - in meters
metric_error); // double metric_error);
}
ground_planes_weight = new double[ ground_planes_masks[0].length];
int num_left = 0; // , num_was = tp_tasks[0].length;
for (int i = 0; i < ground_planes_weight.length; i++) {
if (ground_planes_masks[0][i] && ground_planes_masks[1][i]) {
num_left++;
ground_planes_weight[i] = 1.0;
}
}
System.out.println("correlateOrthoPair(): left "+num_left+" tiles (was "+tp_tasks[0].length+" before filtering)");
System.out.println();
}
boolean show_vector_field = !batch_mode && (debugLevel>0); // true;
Rectangle tile_woi = scaleRectangle (woi, GPUTileProcessor.DTT_SIZE); Rectangle tile_woi = scaleRectangle (woi, GPUTileProcessor.DTT_SIZE);
double max_err= 5.0; double max_err= 5.0;
int num_bins = 1000; int num_bins = 1000;
boolean ignore_strength = false; boolean ignore_strength = false;
double frac_remove = 0.25; //double frac_remove = 0.15;
double [][] vf_error2 = new double [vector_field.length][]; // may be able to skip [0] double [][] vf_error2 = new double [vector_field.length][]; // may be able to skip [0]
double [][][] vector_field_bkp = show_vector_field? new double[vector_field.length][][]:null; double [][][] vector_field_bkp = show_vector_field? new double[vector_field.length][][]:null;
if (vector_field_bkp != null) { if (vector_field_bkp != null) {
...@@ -759,7 +782,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -759,7 +782,7 @@ public class OrthoMapsCollection implements Serializable{
int dbg_width = tile_woi.x+tile_woi.width; int dbg_width = tile_woi.x+tile_woi.width;
int dbg_height = tile_woi.y+tile_woi.height; int dbg_height = tile_woi.y+tile_woi.height;
double [][][][] vf_all = {vector_field_bkp,vector_field}; double [][][][] vf_all = {vector_field_bkp,vector_field};
double [][] dbg_vf = new double [8 * vector_field.length][dbg_width * dbg_height]; double [][] dbg_vf = new double [8 * vector_field.length+5][dbg_width * dbg_height];
String [] dbg_titles = new String[dbg_vf.length]; String [] dbg_titles = new String[dbg_vf.length];
String [] prefix= {"single","single_filtered","neibs","neibs_filtered"}; String [] prefix= {"single","single_filtered","neibs","neibs_filtered"};
for (int n = 0; n < 2*vector_field.length; n++) { for (int n = 0; n < 2*vector_field.length; n++) {
...@@ -768,6 +791,12 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -768,6 +791,12 @@ public class OrthoMapsCollection implements Serializable{
dbg_titles [4*n+2] = prefix[n]+"-str"; dbg_titles [4*n+2] = prefix[n]+"-str";
dbg_titles [4*n+3] = prefix[n]+"-err"; dbg_titles [4*n+3] = prefix[n]+"-err";
} }
dbg_titles [8 * vector_field.length + 0] = "mask-elev";
dbg_titles [8 * vector_field.length + 1] = "elev-0";
dbg_titles [8 * vector_field.length + 2] = "elev-1";
dbg_titles [8 * vector_field.length + 3] = "mask-0";
dbg_titles [8 * vector_field.length + 4] = "mask-1";
for (int i = 0; i < dbg_vf.length; i++) { for (int i = 0; i < dbg_vf.length; i++) {
Arrays.fill(dbg_vf[i], Double.NaN); Arrays.fill(dbg_vf[i], Double.NaN);
...@@ -787,6 +816,11 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -787,6 +816,11 @@ public class OrthoMapsCollection implements Serializable{
} }
} }
} }
dbg_vf [8 * vector_field.length + 0][l] = ground_planes_weight[t];
dbg_vf [8 * vector_field.length + 1][l] = elevations[0][t];
dbg_vf [8 * vector_field.length + 2][l] = elevations[1][t];
dbg_vf [8 * vector_field.length + 3][l] = ground_planes_masks[0][t]?1.0:0.0;
dbg_vf [8 * vector_field.length + 4][l] = ground_planes_masks[1][t]?1.0:0.0;
} }
ShowDoubleFloatArrays.showArrays( ShowDoubleFloatArrays.showArrays(
dbg_vf, dbg_vf,
...@@ -881,16 +915,28 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -881,16 +915,28 @@ public class OrthoMapsCollection implements Serializable{
System.out.println("LMA RMSE worsened: new"+rms+" ("+ orthoPairLMA.getInitialRms()+"), prev="+prev_rms); System.out.println("LMA RMSE worsened: new"+rms+" ("+ orthoPairLMA.getInitialRms()+"), prev="+prev_rms);
} }
} }
if (ignore_prev_rms) {
if (debugLevel > -3) {
System.out.println("Will continue, as ignore_prev_rms is set");
}
} else {
break; break;
} }
}
affines_gpu[1]=OrthoMap.combineAffine(affines_gpu[1], orthoPairLMA.getAffine()); affines_gpu[1]=OrthoMap.combineAffine(affines_gpu[1], orthoPairLMA.getAffine());
double [][] jtj = orthoPairLMA.getLastJtJ(); double [][] jtj = orthoPairLMA.getLastJtJ();
if ((prev_rms - rms)/prev_rms < rel_improve) { if ((prev_rms - rms)/prev_rms < rel_improve) {
if (debugLevel > -2) { if (debugLevel > -2) {
System.out.println("LMA relative RMSE improvement = "+((prev_rms - rms)/prev_rms)+" < "+rel_improve+", exiting."); System.out.println("LMA relative RMSE improvement = "+((prev_rms - rms)/prev_rms)+" < "+rel_improve+", exiting.");
} }
if (ignore_prev_rms) {
if (debugLevel > -3) {
System.out.println("Will continue, as ignore_prev_rms is set");
}
} else {
break; break;
} }
}
prev_rms=rms; prev_rms=rms;
} // for (int ntry = 0; ntry < num_tries; ntry++) { } // for (int ntry = 0; ntry < num_tries; ntry++) {
// unrapping backwards, for "other" map only, reference is not modified. // unrapping backwards, for "other" map only, reference is not modified.
......
...@@ -3294,7 +3294,7 @@ public class TexturedModel { ...@@ -3294,7 +3294,7 @@ public class TexturedModel {
} }
double avg_z = sum_z/num_pix; double avg_z = sum_z/num_pix;
LocalDateTime dt = scenes[ref_index].getLocalDateTime(); LocalDateTime dt = scenes[ref_index].getLocalDateTime();
corrected_lla[2] = avg_z; // average altitude. Maybe keep drone altitude? // corrected_lla[2] = avg_z; // average altitude. Maybe keep drone altitude? - yes
if (gmap_save_alt) { if (gmap_save_alt) {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment