Commit 6845c018 authored by Andrey Filippov's avatar Andrey Filippov

Preparing infrastructure for maps matching, fixed bug in initial GEO

generation (wrong Y sign)
parent 0de865d9
...@@ -287,7 +287,7 @@ public class Imx5 { ...@@ -287,7 +287,7 @@ public class Imx5 {
/* /*
* Offset Latitude, Longitude, Altitude by North, East, Up (in meters) * Offset Latitude, Longitude, Altitude by North, East, Up (in meters)
*/ */
public static double [] offsetLla(double [] lla, double [] ned) { public static double [] offsetLlaNED(double [] lla, double [] ned) {
double [] offset_lla = new double [] { double [] offset_lla = new double [] {
lla[0] + (ned[0]/EARTH_RADIUS / Math.PI * 180 / Math.cos(lla[0])), // degrees lla[0] + (ned[0]/EARTH_RADIUS / Math.PI * 180 / Math.cos(lla[0])), // degrees
lla[1] + (ned[1]/EARTH_RADIUS / Math.PI * 180), // degrees lla[1] + (ned[1]/EARTH_RADIUS / Math.PI * 180), // degrees
......
...@@ -7,6 +7,7 @@ import java.util.HashMap; ...@@ -7,6 +7,7 @@ import java.util.HashMap;
import java.util.Properties; import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters;
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;
...@@ -15,24 +16,44 @@ import com.elphel.imagej.tileprocessor.ImageDtt; ...@@ -15,24 +16,44 @@ import com.elphel.imagej.tileprocessor.ImageDtt;
import Jama.Matrix; import Jama.Matrix;
import ij.ImagePlus; import ij.ImagePlus;
import ij.ImageStack;
import ij.Prefs; import ij.Prefs;
import ij.gui.PointRoi; import ij.gui.PointRoi;
public class ComboMap { public class ComboMap {
public static boolean FIX_VERT_Y = false; // true; // temporarily fix vertical Y coordinate bug (use -GCORR in the filename?)
public static int gpu_width = 4096;
public static int gpu_height = 4096;
public static final String ALT_SUFFIX = "-ALT";
public final String name; // timestamp public final String name; // timestamp
public String path; // full path to the model directory (including /vXX?) public String path; // full path to the model directory (including /vXX?)
public double [] lla; // lat/long/alt public double [] lla; // lat/long/alt
public LocalDateTime dt; public LocalDateTime dt;
// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
// coordinates relative to vert_meters
private double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[] private double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[]
public double orig_pix_meters; public double orig_pix_meters;
public double [] vert_meters; // offset of the image vertical in meters (scale-invariant) public double [] vert_meters; // offset of the image vertical in meters (scale-invariant)
public FloatImageData orig_image; public FloatImageData orig_image;
public FloatImageData alt_image;
public int orig_zoom_level; public int orig_zoom_level;
public boolean orig_zoom_valid; public boolean orig_zoom_valid;
public double need_extra_zoom; public double need_extra_zoom;
HashMap <Integer, FloatImageData> images; HashMap <Integer, FloatImageData> images;
public static void setGPUWIdthHeight(
int width,
int height) {
gpu_width = width;
gpu_height = height;
}
// Generate ALT image path from the GEO // Generate ALT image path from the GEO
public static String getAltPath(String path) {
int p1 = path.lastIndexOf(".");
return path.substring(0,p1)+ALT_SUFFIX+".tiff";
}
public static String getNameFromPath(String path) { public static String getNameFromPath(String path) {
int p1 = path.lastIndexOf(Prefs.getFileSeparator()); int p1 = path.lastIndexOf(Prefs.getFileSeparator());
if (p1 < 0) return null; if (p1 < 0) return null;
...@@ -53,14 +74,22 @@ public class ComboMap { ...@@ -53,14 +74,22 @@ public class ComboMap {
} }
lla = ElphelTiffReader.getLLA(imp_prop); lla = ElphelTiffReader.getLLA(imp_prop);
dt = ElphelTiffReader.getLocalDateTime(imp_prop); dt = ElphelTiffReader.getLocalDateTime(imp_prop);
vert_meters = ElphelTiffReader.getXYOffsetMeters(imp_prop); vert_meters = ElphelTiffReader.getXYOffsetMeters(imp_prop);
orig_pix_meters = ElphelTiffReader.getPixelSize(imp_prop)[0]; orig_pix_meters = ElphelTiffReader.getPixelSize(imp_prop)[0];
if (FIX_VERT_Y) {
int height_pix = ElphelTiffReader.getHeight(imp_prop);
double height_meters = height_pix * orig_pix_meters;
vert_meters[1] = height_meters-vert_meters[1];
}
orig_zoom_level = FloatImageData.getZoomLevel(orig_pix_meters); orig_zoom_level = FloatImageData.getZoomLevel(orig_pix_meters);
orig_zoom_valid = FloatImageData.isZoomValid(orig_pix_meters); orig_zoom_valid = FloatImageData.isZoomValid(orig_pix_meters);
need_extra_zoom = FloatImageData.needZoomIn(orig_pix_meters); need_extra_zoom = FloatImageData.needZoomIn(orig_pix_meters);
images = new HashMap <Integer, FloatImageData>(); images = new HashMap <Integer, FloatImageData>();
} }
public String getName() {
return name;
}
public void setAffine(double [][] affine) { public void setAffine(double [][] affine) {
this.affine = affine; this.affine = affine;
} }
...@@ -71,6 +100,9 @@ public class ComboMap { ...@@ -71,6 +100,9 @@ public class ComboMap {
ImagePlus imp = new ImagePlus(path); ImagePlus imp = new ImagePlus(path);
int width = imp.getWidth(); int width = imp.getWidth();
int height = imp.getHeight(); int height = imp.getHeight();
// TODO: FIXME!
// vert_meters[1] = height-vert_meters[1];
orig_image = new FloatImageData ( orig_image = new FloatImageData (
width, // int width, width, // int width,
height, // int height, height, // int height,
...@@ -82,6 +114,20 @@ public class ComboMap { ...@@ -82,6 +114,20 @@ public class ComboMap {
} }
return true; return true;
} }
public boolean readAltData() { // assuming the same scale as main image
ImagePlus imp = new ImagePlus(getAltPath(path));
int width = imp.getWidth();
int height = imp.getHeight();
alt_image = new FloatImageData (
width, // int width,
height, // int height,
orig_zoom_level, // int zoom_lev,
vert_meters, // double [] vert, // x,y pixel offset of the point under the camera
(float[]) (imp.getProcessor().getPixels())); // data);
return true;
}
public ImagePlus getOriginalImage(boolean show_markers) { public ImagePlus getOriginalImage(boolean show_markers) {
if (orig_image != null) { if (orig_image != null) {
String full_name = path.substring(path.lastIndexOf(Prefs.getFileSeparator()) + 1); String full_name = path.substring(path.lastIndexOf(Prefs.getFileSeparator()) + 1);
...@@ -112,7 +158,7 @@ public class ComboMap { ...@@ -112,7 +158,7 @@ public class ComboMap {
ps /= 2; ps /= 2;
} }
} else { } else {
for (int i = 0; i > zoom_level; i++) { for (int i = 0; i > zoom_level; i--) {
ps *= 2; ps *= 2;
} }
} }
...@@ -120,35 +166,82 @@ public class ComboMap { ...@@ -120,35 +166,82 @@ public class ComboMap {
} }
// private double [][] affine = new double[2][3]; // private double [][] affine = new double[2][3];
// vert_meters = ElphelTiffReader.getXYOffsetMeters(imp_prop); // vert_meters = ElphelTiffReader.getXYOffsetMeters(imp_prop);
public double [][] getBoundsMeters (boolean inverse){ // inverse - from source to rectified
// orig_pix_meters
/**
* Get top-left, top-right, bottom-right, bottom-left corners coordinates relative
* to a "vertical" point. {{-,-},{+,-},{+,+},{-,+}}
* @return double [4][2] array [corner number]{x,y}
*/
public double [][] get4SourceCornersMeters(){
double width_meters = orig_image.width * orig_pix_meters; double width_meters = orig_image.width * orig_pix_meters;
double height_meters = orig_image.height * orig_pix_meters; double height_meters = orig_image.height * orig_pix_meters;
double [][] corners = new double[][] { // CW from TL return new double[][] { // CW from TL
{ vert_meters[0], vert_meters[1]}, { - vert_meters[0], - vert_meters[1]},
{width_meters - vert_meters[0], vert_meters[1]}, {width_meters - vert_meters[0], - vert_meters[1]},
{width_meters - vert_meters[0], height_meters - vert_meters[1]}, {width_meters - vert_meters[0], height_meters - vert_meters[1]},
{ vert_meters[0], height_meters - vert_meters[1]}}; { - vert_meters[0], height_meters - vert_meters[1]}};
double [][] aff_corners = new double [4][2]; }
for (int n = 0; n < aff_corners.length; n++) {
aff_corners[n][0] = affine[0][0]*corners[n][0] + affine[0][1]*corners[n][1] + affine[0][2]; /**
aff_corners[n][1] = affine[1][0]*corners[n][0] + affine[1][1]*corners[n][1] + affine[1][2]; * Get metric bounds of this image (zero point at vert_meters)
* @param rectified if true, use rectified (inverse-transformed) image, false - original
* @return rectified {{x_min, x_max},{y_min,y_max}}
*/
public double [][] getBoundsMeters (boolean rectified){
double [][] corners = get4SourceCornersMeters();
if (rectified) {
double [][] inv_aff = invertAffine(affine);
double [][] rec_corners = new double [4][2];
for (int n = 0; n < rec_corners.length; n++) {
rec_corners[n][0] = inv_aff[0][0]*corners[n][0] + inv_aff[0][1]*corners[n][1] + inv_aff[0][2];
rec_corners[n][1] = inv_aff[1][0]*corners[n][0] + inv_aff[1][1]*corners[n][1] + inv_aff[1][2];
}
corners = rec_corners;
} }
double [][] bounds = new double [2][2]; // rows: x,y. Columns: min,max. double [][] bounds = new double [2][2]; // rows: x,y. Columns: min,max.
for (int n = 0; n < 2; n++) { for (int n = 0; n < 2; n++) {
bounds[n][0] = aff_corners[0][0]; bounds[n][0] = corners[0][n];
bounds[n][1] = bounds[n][0]; bounds[n][1] = bounds[n][0];
for (int i = 1; i < aff_corners.length; i++) { for (int i = 1; i < corners.length; i++) {
bounds[n][0]=Math.min(bounds[n][0], aff_corners[i][n]); bounds[n][0]=Math.min(bounds[n][0], corners[i][n]);
bounds[n][1]=Math.max(bounds[n][1], aff_corners[i][n]); bounds[n][1]=Math.max(bounds[n][1], corners[i][n]);
} }
} }
return bounds; return bounds;
} }
/**
* Get pixel bounds of this image (zero point at vert_meters) as doubles (to be able to
* offset before converting to int.
* @param rectified if true, use rectified (inverse-transformed) image, false - original
* @return rectified {{x_min, x_max},{y_min,y_max}}
*/
public double [][] getBoundPixels(
int zoom_level,
boolean rectified){
double [][] bounds_meters = getBoundsMeters (rectified);
double pix_size = getPixelSizeMeters (zoom_level);
double [][] bounds_pix = new double[2][2];
for (int n = 0; n < bounds_pix.length; n++) {
for (int i = 0; i < 2; i++) {
bounds_pix[n][i] = bounds_meters[n][i]/pix_size;
}
}
return bounds_pix;
}
/**
* Get rectified bounds of all provided combo images relative to the origin (vertical
* point) of the first one in meters
* @param maps array of combo images
* @return {{min_x,max_x},{min_y,max_y}} bounds that include all provided maps
* relative to the origin (vertical point) of the first image
*/
public static double [][] getBoundsMeters(ComboMap [] maps){ // maps[0] as a reference public static double [][] getBoundsMeters(ComboMap [] maps){ // maps[0] as a reference
double [][] bounds = maps[0].getBoundsMeters(false); double [][] bounds = maps[0].getBoundsMeters(true);
for (int nmap = 1; nmap < maps.length; nmap++) { for (int nmap = 1; nmap < maps.length; nmap++) {
double [][] bounds_other = maps[nmap].getBoundsMeters(false); double [][] bounds_other = maps[nmap].getBoundsMeters(true);
double [] enuOffset = maps[0].enuOffsetTo(maps[nmap]); double [] enuOffset = maps[0].enuOffsetTo(maps[nmap]);
double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image
for (int n = 0; n < bounds.length; n++) { for (int n = 0; n < bounds.length; n++) {
...@@ -158,6 +251,45 @@ public class ComboMap { ...@@ -158,6 +251,45 @@ public class ComboMap {
} }
return bounds; return bounds;
} }
/**
* In preparation for the GPU correlation (zoomed out images of the fixed
* size are calculated separately and have NaN for unused pixels.
* Calculate overlap area bounds relative to the reference image origin
* (vertical point) and top-left (of the rectified overlap area) referenced
* affine matrices for both images ([2][2][3]).
* The overlap area should fit into gpu_width, gpu_hight, the affine-generated
* image coordinates will be limited during TpTask[][] generation
* @param ref_map ComboMap instance for the reference map
* @param var_map ComboMap instance for the variable map instance
* @param zoom_lev zoom level (0 - 1 pix=1cm, 1 - 1 pix = 0.5cm, -1 - 1 pix = 2 cm.
* @param var_offset additional (to affine and lla, vert_meters) x,y offset of
* the var_map (in output pixels). Used for spiral search for initial
* match.
* @param gpu_affine combined [2][2][3] array for convertion from the rectified overlap
* area to the GPU input images. Referenced to the top-left pixel
* of the rectified overlap area and top-left image pixels
* @return {{min_x,max_x}{min_y, max_y}) of the rectified area where (0,0) corresponds
* (is centered) to the reference image origin (vertical point)
*/
public static int [][] getPairOvelapBoundsAffine(
ComboMap ref_map,
ComboMap var_map,
int zoom_lev,
int [] var_offset,
double [][][] gpu_affine){
return null;
}
/**
* Get rectified bounds of all provided combo images relative to the origin (vertical
* point) of the first one in pixels at the provided zoom level. Center of the first
* image correspond to the {0,0} point from which the bounds are counted
* @param maps array of map instances
* @param zoom_level zoom level - 0 corresponds to 1pix=1cm scale, +1 - to 1pix = 0.5cm
* @return {{min_x,max_x},{min_y, max_y}} relative to the origin of the first image
*/
public static int [][] getBoundsPixels( public static int [][] getBoundsPixels(
ComboMap [] maps, ComboMap [] maps,
int zoom_level){ // maps[0] as a reference int zoom_level){ // maps[0] as a reference
...@@ -171,6 +303,14 @@ public class ComboMap { ...@@ -171,6 +303,14 @@ public class ComboMap {
return bounds_pix; return bounds_pix;
} }
/**
* Convert from the affine matrix that calculates image coordinates from the rectified one
* (relative to the origin point) to the matrix that provides rectified coordinates from the
* image ones. Both are in meters relative to the "vert_meters" point
* @param affine [2][3] array were the last column is made of the offsets (in meters)
* @return [2][3] array that converts from the image metric coordinates relative to the
* "vert_meters" point to the rectified metric coordinates relative to the same point.
*/
public static double [][] invertAffine(double [][] affine){ public static double [][] invertAffine(double [][] affine){
Matrix A = new Matrix ( Matrix A = new Matrix (
new double [][] {{affine[0][0],affine[0][1]},{affine[1][0],affine[1][1]}}); new double [][] {{affine[0][0],affine[0][1]},{affine[1][0],affine[1][1]}});
...@@ -217,8 +357,9 @@ public class ComboMap { ...@@ -217,8 +357,9 @@ public class ComboMap {
Arrays.fill(opix, Float.NaN); Arrays.fill(opix, Float.NaN);
final double [][] wnd = new double [frscale] [frscale]; final double [][] wnd = new double [frscale] [frscale];
for (int i = 0; i <frscale; i++) { for (int i = 0; i <frscale; i++) {
double w = Math.sin((i+0.5)/frscale*Math.PI);
for (int j = 0; j <frscale; j++) { for (int j = 0; j <frscale; j++) {
wnd[i][j] = Math.sin((i+0.5)/frscale*Math.PI); wnd[i][j] = w*Math.sin((j+0.5)/frscale*Math.PI);
} }
} }
final Thread[] threads = ImageDtt.newThreadArray(); final Thread[] threads = ImageDtt.newThreadArray();
...@@ -277,10 +418,12 @@ public class ComboMap { ...@@ -277,10 +418,12 @@ public class ComboMap {
System.out.println("getPaddedGPU(): failed to prepare a scaled image"); System.out.println("getPaddedGPU(): failed to prepare a scaled image");
return null; return null;
} }
FloatImageData fid = images.get(zoom_level);
final float [] padded_gpu = new float[gpu_width*gpu_height]; final float [] padded_gpu = new float[gpu_width*gpu_height];
final int swidth = orig_image.width; final int swidth = fid.width;
final int sheight = orig_image.height; final int sheight = fid.height;
final float [] spix = orig_image.data; final float [] spix = fid.data;
Arrays.fill(padded_gpu, Float.NaN); Arrays.fill(padded_gpu, Float.NaN);
final int cheight = Math.min(sheight,gpu_height); final int cheight = Math.min(sheight,gpu_height);
final int cwidth = Math.min(swidth, gpu_width); final int cwidth = Math.min(swidth, gpu_width);
...@@ -305,70 +448,137 @@ public class ComboMap { ...@@ -305,70 +448,137 @@ public class ComboMap {
return padded_gpu; return padded_gpu;
} }
public static ImagePlus renderMulti (
String title,
boolean use_alt,
boolean show_centers,
ComboMap [] maps,
int zoom_level,
int [] origin){
int [] wh = new int[2];
double [][] centers = new double [maps.length][];
float [][] multi = renderMulti (
use_alt, // boolean use_alt,
maps, // ComboMap [] maps,
zoom_level, // int zoom_level,
wh, // int [] wh,
origin, // int [] origin){ // maps[0] as a reference
centers); // double [][] centers)
String [] map_names = new String[maps.length];
for (int n = 0; n < maps.length; n++) {
map_names[n] = maps[n].getName();
}
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;
}
/**
* Rectify and render multiple images (as slices) matching vert_meters to
* their (vert_meters points) provided IMS coordinates
* @param show altitudes instead of thermal data
* @param maps array of the combo images
* @param zoom_level zoom level (0 - 1pix=1cm, 1 - 1pix=0.5cm, -1 - 1pix=2cm
* @param wh null or int [2], will return {width, height}
* @param origin - null or double [2], will return {x0, y0} in pixels
* @param centers - null or double[maps.length][] - will return image centers coordinates
* @return
*/
public static float [][] renderMulti ( public static float [][] renderMulti (
boolean use_alt,
ComboMap [] maps, ComboMap [] maps,
int zoom_level){ // maps[0] as a reference int zoom_level,
int [] wh,
int [][] bounds = getBoundsPixels( int [] origin, // maps[0] as a reference
double [][] centers){
int [][] bounds = getBoundsPixels( // should be for rectified, {-bounds[0][0], -bounds[0][1]} - exact center
maps, maps,
zoom_level); zoom_level);
int width = bounds[0][1] - bounds[0][0]; // bounds[x][0] - negative int width = bounds[0][1] - bounds[0][0]; // bounds[x][0] - negative
int height = bounds[1][1] - bounds[1][0]; int height = bounds[1][1] - bounds[1][0];
if (wh != null) {
wh[0] = width;
wh[1] = height;
}
if (origin != null) {
origin[0] = -bounds[0][0];
origin[1] = -bounds[1][0];
}
final float [][] fpixels = new float [maps.length][width * height]; final float [][] fpixels = new float [maps.length][width * height];
for (int nmap = 0; nmap< maps.length; nmap++) { for (int nmap = 0; nmap< maps.length; nmap++) {
final int fnmap = nmap; final int fnmap = nmap;
Arrays.fill(fpixels[nmap], Float.NaN); Arrays.fill(fpixels[nmap], Float.NaN);
final double scale = getPixelSizeMeters(maps[nmap].orig_zoom_level)/getPixelSizeMeters(zoom_level); final double scale = 1.0/getPixelSizeMeters(zoom_level);
double [][] inv_bounds = maps[nmap].getBoundsMeters(true); final double src_scale = 1.0/getPixelSizeMeters(maps[nmap].orig_zoom_level); // pix per meter
// metric bounds of the rectified image relative to its origin
double [][] mbounds = maps[nmap].getBoundsMeters(true);
double [] enu_offset = maps[0].enuOffsetTo(maps[nmap]); double [] enu_offset = maps[0].enuOffsetTo(maps[nmap]);
final double [] scaled_out_center = { // xy center to apply affine to final double [] scaled_out_center = { // xy center to apply affine to
-bounds[0][0]+scale * enu_offset[0], -bounds[0][0] + scale * enu_offset[0],
-bounds[1][0]+scale * enu_offset[1]}; -bounds[1][0] - scale * enu_offset[1]};
final int [][] ibounds = new int [2][2]; if (centers != null) {
centers[nmap] = 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++) { for (int n = 0; n< 2; n++) {
ibounds[n][0] = (int) Math.floor(-bounds[n][0] + scale*(inv_bounds[n][0] + enu_offset[n])); obounds[n][0] = (int) Math.floor(scaled_out_center[n] + scale*mbounds[n][0]);
ibounds[n][1] = (int) Math.ceil (-bounds[n][0] + scale*(inv_bounds[n][0] + enu_offset[n])); obounds[n][1] = (int) Math.ceil (scaled_out_center[n] + scale*mbounds[n][1]);
} }
final int local_width = ibounds[0][1] - ibounds[0][0]; // Output window size
final int local_height = ibounds[1][1] - ibounds[1][0]; final int ownd_width = obounds[0][1] - obounds[0][0];
final int local_len = local_width * local_height; final int ownd_height = obounds[1][1] - obounds[1][0];
double [][] src_bounds=maps[nmap].getBoundsMeters (false); final int ownd_len = ownd_width * ownd_height;
double [][] src_bounds=maps[nmap].getBoundsMeters (true);
final double [] src_center = {-src_bounds[0][0],-src_bounds[1][0]}; // x,y center offset in the source image final double [] src_center = {-src_bounds[0][0],-src_bounds[1][0]}; // x,y center offset in the source image
final double [][] affine = maps[nmap].affine; final double [][] affine = maps[nmap].affine;
final int src_width = maps[nmap].orig_image.width; final int src_width = use_alt? maps[nmap].alt_image.width: maps[nmap].orig_image.width;
final int src_height = maps[nmap].orig_image.height; final int src_height = use_alt? maps[nmap].alt_image.height : maps[nmap].orig_image.height;
final float [] src_img = maps[nmap].orig_image.data; final float [] src_img = use_alt? maps[nmap].alt_image.data : maps[nmap].orig_image.data;
final Thread[] threads = ImageDtt.newThreadArray(); final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0); final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) { for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() { threads[ithread] = new Thread() {
public void run() { public void run() {
for (int nPix = ai.getAndIncrement(); nPix < local_len; nPix = ai.getAndIncrement()) { for (int nPix = ai.getAndIncrement(); nPix < ownd_len; nPix = ai.getAndIncrement()) {
int opX = nPix % local_width + ibounds[0][0]; // absolute output pX, pY int opX = nPix % ownd_width + obounds[0][0]; // absolute output pX, pY
int opY = nPix / local_width + ibounds[1][0]; 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 original image scale
double dY = (opY - scaled_out_center[1]) /scale; double dY = (opY - scaled_out_center[1]) /scale;
double [] xy_src = { double [] xy_src = { // pixels of the source image
affine[0][0]*dX+affine[0][1]*dY+affine[0][2]+src_center[0], src_scale * (affine[0][0]*dX + affine[0][1]*dY + affine[0][2] + src_center[0]),
affine[1][0]*dX+affine[1][1]*dY+affine[1][2]+src_center[1]}; src_scale * (affine[1][0]*dX + affine[1][1]*dY + affine[1][2] + src_center[1])};
// limit to the source image // limit to the source image
if (xy_src[0] < 0) xy_src[0] = 0; if ((xy_src[0] >= 0) && (xy_src[0] < (src_width-1)) &&
else if (xy_src[0] >= (src_width-1)) xy_src[0] = src_width-2; // to be on the safe side (xy_src[1] >= 0) && (xy_src[1] < (src_height-1))) {
if (xy_src[1] < 0) xy_src[1] = 0; int [] ixy_src = {(int) Math.floor(xy_src[0]), (int)Math.floor(xy_src[1]) };
else if (xy_src[1] >= (src_height-1)) xy_src[1] = src_height-2; double [] kxy = {xy_src[0]-ixy_src[0], xy_src[1]-ixy_src[1]};
int [] ixy_src = {(int) Math.floor(xy_src[0]), (int)Math.floor(xy_src[1]) }; int indx00 = ixy_src[0] + ixy_src[1] * src_width;
double [] kxy = {xy_src[0]-ixy_src[0], xy_src[1]-ixy_src[1]}; double d00 = src_img[indx00];
double d00 = src_img[ixy_src[0] + ixy_src[1]*src_width]; if (!Double.isNaN(d00)) {
if (!Double.isNaN(d00)) { double d01 = src_img[indx00 + 1];
double d01 = src_img[ixy_src[0] + 1 + ixy_src[1]*src_width]; double d10 = src_img[indx00 + src_width];
double d10 = src_img[ixy_src[0] + (ixy_src[1] + 1) *src_width]; double d11 = src_img[indx00 + src_width + 1];
double d11 = src_img[ixy_src[0] + 1 + (ixy_src[1] + 1) *src_width]; double d = d00*(1.0 - kxy[0])*(1.0 - kxy[1])+
double d = d00*(1.0 - kxy[0])*(1.0 - kxy[1])+ d01* kxy[0] *(1.0 - kxy[1])+
d01* kxy[0] *(1.0 - kxy[1])+ d10*(1.0 - kxy[0])* kxy[1]+
d10*(1.0 - kxy[0])* kxy[1]+ d11* kxy[0] * kxy[1];
d11* kxy[0] * kxy[1]; fpixels[fnmap][opX + opY*width] = (float) d;
fpixels[fnmap][opX + opY*width] = (float) d; }
} }
} }
} }
...@@ -379,4 +589,37 @@ public class ComboMap { ...@@ -379,4 +589,37 @@ public class ComboMap {
return fpixels; return fpixels;
} }
public static double [][] correlateComboPair(
CLTParameters clt_parameters,
ComboMap [] combo_maps,
int first_index,
int second_index,
int zoom_lev,
double [] offset_xy_second, // on top of affine and GPS
final int debugLevel){
int [] gpu_pair = {first_index,second_index};
double [][][] bounds_out = new double [2][][];
float [][] gpu_pair_img = new float [2][];
for (int n = 0; n < gpu_pair.length; n++) {
gpu_pair_img[n] = combo_maps[gpu_pair[n]].getPaddedGPU (
zoom_lev, // int zoom_level,
gpu_width, // int gpu_width,
gpu_height); // int gpu_height)
}
if (debugLevel > 1) {
String [] map_names = {combo_maps[gpu_pair[0]].getName(),combo_maps[gpu_pair[1]].getName()};
ShowDoubleFloatArrays.showArrays(
gpu_pair_img,
gpu_width,
gpu_height,
true,
"gpu_pair-zoom"+zoom_lev+"-"+combo_maps[gpu_pair[0]].getName()+"-"+combo_maps[gpu_pair[1]].getName(),
map_names);
}
return null;
}
} }
...@@ -32,11 +32,11 @@ public class ComboMatch { ...@@ -32,11 +32,11 @@ public class ComboMatch {
public static GpuQuad GPU_QUAD_AFFINE = null; public static GpuQuad GPU_QUAD_AFFINE = null;
public static ImagePlus imp_src1 = null; public static ImagePlus imp_src1 = null;
public static ImagePlus imp_src2 = null; public static ImagePlus imp_src2 = null;
public static ImagePlus [] imp_src = new ImagePlus[2]; public static ImagePlus [] imp_src; // = new ImagePlus[2];
public static ImagePlus [] imp_alt = new ImagePlus[2]; public static ImagePlus [] imp_alt; // = new ImagePlus[2];
public static Properties [] imp_prop = new Properties[2]; public static Properties [] imp_prop; // = new Properties[2];
public static int gpu_max_width= 3008; public static int gpu_max_width= 4096;
public static int gpu_max_height= 3008; public static int gpu_max_height= 4096;
public static boolean openTestPairGps( public static boolean openTestPairGps(
CLTParameters clt_parameters, CLTParameters clt_parameters,
...@@ -66,18 +66,46 @@ public class ComboMatch { ...@@ -66,18 +66,46 @@ public class ComboMatch {
/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877415_521986/1697877415_521986-RECT-PIX0.01-FLAT_CLN-VERT-GEO-ALT.tiff /media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877415_521986/1697877415_521986-RECT-PIX0.01-FLAT_CLN-VERT-GEO-ALT.tiff
*/ */
/*
String [] image_paths_pre = { String [] image_paths_pre = {
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877410_420287/1697877410_420287-RECT-PIX0.01-FLAT_CLN-VERT-GEO", "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877410_420287/1697877410_420287-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877412_004148/1697877412_004148-RECT-PIX0.01-FLAT_CLN-VERT-GEO" "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877412_004148/1697877412_004148-RECT-PIX0.01-FLAT_CLN-VERT-GEO"
}; };
*/
String [] image_paths_pre_0 = {
// "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877410_420287/1697877410_420287-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
// "/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-b/1697877412_004148/1697877412_004148-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877409_353265/1697877409_353265-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877410_403615/1697877410_403615-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877411_987476/1697877411_987476-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877413_137859/1697877413_137859-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877414_404948/1697877414_404948-RECT-PIX0.01-FLAT_CLN-VERT-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877415_521986/1697877415_521986-RECT-PIX0.01-FLAT_CLN-VERT-GEO"
};
String [] image_paths_pre = {
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877409_353265/1697877409_353265-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877410_403615/1697877410_403615-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877411_987476/1697877411_987476-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877413_137859/1697877413_137859-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877414_404948/1697877414_404948-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO",
"/media/elphel/SSD3-4GB/lwir16-proc/berdich3/linked/linked_1697875868-1697879449-c/1697877415_521986/1697877415_521986-RECT-PIX0.01-FLAT_CLN-VERT-GCORR-GEO"
};
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 = -4; // 0; // +1 - zoom in twice, -1 - zoom out twice int zoom_lev = -4; // 0; // +1 - zoom in twice, -1 - zoom out twice
boolean use_alt = false;
boolean show_centers = true; boolean show_centers = true;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,650); GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,800);
gd.addStringField ("First image path", image_paths_pre[0], 180, "First image full path w/o ext"); for (int n = 0; n < image_paths_pre.length; n++) {
gd.addStringField ("Second image path", image_paths_pre[1], 180, "Second image full path w/o ext"); gd.addStringField ("Image path "+n, image_paths_pre[n], 180, "Image "+n+" full path w/o ext");
}
// gd.addStringField ("First image path", image_paths_pre[0], 180, "First image full path w/o ext");
// gd.addStringField ("Second image path", image_paths_pre[1], 180, "Second image full path w/o ext");
for (int n = 0; n < image_enuatr.length; n++) { for (int n = 0; n < image_enuatr.length; n++) {
gd.addMessage("image["+n+"] pose"); gd.addMessage("image["+n+"] pose");
gd.addNumericField("East", image_enuatr[n][0][0], 3,7,"m", "Move image "+n+" East."); gd.addNumericField("East", image_enuatr[n][0][0], 3,7,"m", "Move image "+n+" East.");
...@@ -94,11 +122,15 @@ public class ComboMatch { ...@@ -94,11 +122,15 @@ 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.showDialog(); gd.showDialog();
if (gd.wasCanceled()) return false; if (gd.wasCanceled()) return false;
image_paths_pre[0] = gd.getNextString(); for (int n = 0; n < image_paths_pre.length; n++) {
image_paths_pre[1] = gd.getNextString(); image_paths_pre[n] = gd.getNextString();
}
// image_paths_pre[0] = gd.getNextString();
// image_paths_pre[1] = gd.getNextString();
for (int n = 0; n < image_enuatr.length; n++) { for (int n = 0; n < image_enuatr.length; n++) {
image_enuatr[n][0][0] = gd.getNextNumber(); image_enuatr[n][0][0] = gd.getNextNumber();
image_enuatr[n][0][1] = gd.getNextNumber(); image_enuatr[n][0][1] = gd.getNextNumber();
...@@ -110,15 +142,91 @@ public class ComboMatch { ...@@ -110,15 +142,91 @@ public class ComboMatch {
zoom_lev = (int) gd.getNextNumber(); zoom_lev = (int) gd.getNextNumber();
gpu_width = (int) gd.getNextNumber(); gpu_width = (int) gd.getNextNumber();
gpu_height = (int) gd.getNextNumber(); gpu_height = (int) gd.getNextNumber();
ComboMap.setGPUWIdthHeight(gpu_width,gpu_height);
show_centers = gd.getNextBoolean(); show_centers = gd.getNextBoolean();
use_alt = gd.getNextBoolean();
ComboMap[] combo_maps = new ComboMap[imp_src.length]; ComboMap[] combo_maps = new ComboMap[image_paths_pre.length];
String [] map_names = new String[combo_maps.length];
for (int n = 0; n < combo_maps.length; n++) { for (int n = 0; n < combo_maps.length; n++) {
combo_maps[n] = new ComboMap(image_paths_pre[n]+".tiff"); combo_maps[n] = new ComboMap(image_paths_pre[n]+".tiff");
combo_maps[n].readImageData(); combo_maps[n].readImageData();
double [][] affine = new double[2][3]; // maybe later calculate from mage_enuatr if (use_alt) {
combo_maps[n].readAltData();
}
double [][] affine = {{1,0,0},{0,1,0}}; // maybe later calculate from mage_enuatr
combo_maps[n].setAffine(affine); combo_maps[n].setAffine(affine);
map_names[n] = combo_maps[n].getName();
}
int [] origin = new int[2];
ImagePlus imp_img = ComboMap.renderMulti (
"multi_"+zoom_lev, // String title,
false, // boolean use_alt,
show_centers, // boolean show_centers,
combo_maps, // ComboMap [] maps,
zoom_lev, // int zoom_level,
origin); // int [] origin){
imp_img.show();
/*
int [] wh = new int[2];
double [][] centers = new double [combo_maps.length][];
float [][] multi = ComboMap.renderMulti (
false, // boolean use_alt,
combo_maps, // ComboMap [] maps,
zoom_lev, // int zoom_level,
wh, // int [] wh,
origin, // int [] origin){ // maps[0] as a reference
centers); // double [][] centers)
ShowDoubleFloatArrays.showArrays(
multi,
wh[0],
wh[1],
true,
"multi_"+zoom_lev,
map_names);
if (use_alt) {
float [][] multi_alt = ComboMap.renderMulti (
true, // boolean use_alt,
combo_maps, // ComboMap [] maps,
zoom_lev, // int zoom_level,
wh, // int [] wh,
origin, // int [] origin){ // maps[0] as a reference
centers); // double [][] centers)
ShowDoubleFloatArrays.showArrays(
multi_alt,
wh[0],
wh[1],
true,
"multi_alt_"+zoom_lev,
map_names);
}
*/
// which piar to compare
int [] gpu_pair = {1,2};
float [][] gpu_pair_img = new float [2][];
for (int n = 0; n < gpu_pair.length; n++) {
gpu_pair_img[n] = combo_maps[gpu_pair[n]].getPaddedGPU (
zoom_lev, // int zoom_level,
gpu_width, // int gpu_width,
gpu_height); // int gpu_height)
}
ShowDoubleFloatArrays.showArrays(
gpu_pair_img,
gpu_width,
gpu_height,
true,
"gpu_pair-zoom"+zoom_lev+"-"+combo_maps[gpu_pair[0]].getName()+"-"+combo_maps[gpu_pair[1]].getName(),
map_names);
if (image_paths_pre.length > 2) {
return true;
} }
/* */ /* */
int [] widths = new int[imp_src.length]; int [] widths = new int[imp_src.length];
......
...@@ -742,6 +742,14 @@ public class ElphelTiffReader extends TiffReader{ // BaseTiffReader { ...@@ -742,6 +742,14 @@ public class ElphelTiffReader extends TiffReader{ // BaseTiffReader {
xy_pixel_in_meters[1] = unit_size/Double.parseDouble(properties.getProperty("YResolution")); xy_pixel_in_meters[1] = unit_size/Double.parseDouble(properties.getProperty("YResolution"));
return xy_pixel_in_meters; return xy_pixel_in_meters;
} }
public static int getWidth(Properties properties) {
return Integer.parseInt(properties.getProperty("ImageWidth"));
}
public static int getHeight(Properties properties) {
return Integer.parseInt(properties.getProperty("ImageLength"));
}
public static double [] getXYOffsetMeters(Properties properties) { public static double [] getXYOffsetMeters(Properties properties) {
int res_unit = Integer.parseInt(properties.getProperty("ResolutionUnit")); int res_unit = Integer.parseInt(properties.getProperty("ResolutionUnit"));
......
...@@ -928,13 +928,14 @@ public class Interscene { ...@@ -928,13 +928,14 @@ public class Interscene {
quadCLTs); //, // QuadCLT[] quadCLTs, quadCLTs); //, // QuadCLT[] quadCLTs,
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale) // quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; // double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; //
double [] enu_corr_metric = quadCLTs[ref_index].getENUCorrMetric();
// quat_corr may still be null if does not exist // quat_corr may still be null if does not exist
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation); System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation);
System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+" correct orientation offset"); System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+" correct orientation offset");
QuadCLTCPU.showQuatCorr(quat_corr); QuadCLTCPU.showQuatCorr(quat_corr, enu_corr_metric);
} }
boolean test_quat_corr = debugLevel > 1000; boolean test_quat_corr = debugLevel > 1000;
if (test_quat_corr) { if (test_quat_corr) {
...@@ -5285,13 +5286,15 @@ public class Interscene { ...@@ -5285,13 +5286,15 @@ public class Interscene {
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][]; double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
// for testing QuaternionLma // for testing QuaternionLma
double [] rms = new double [2]; double [] rms = new double [2];
double [] enu_corr = new double[3];
double [] quatCorr = getQuaternionCorrection( double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2]; rms, // double [] rms // null or double[2];
debugLevel); // int debugLevel enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel
if (debugLevel > -3) { if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):"); System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
...@@ -5301,6 +5304,8 @@ public class Interscene { ...@@ -5301,6 +5304,8 @@ public class Interscene {
System.out.println("generateEgomotionTable(): quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]"); System.out.println("generateEgomotionTable(): quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("generateEgomotionTable(): ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]"); System.out.println("generateEgomotionTable(): ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("generateEgomotionTable(): ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]"); System.out.println("generateEgomotionTable(): ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
System.out.println("generateEgomotionTable(): ENU corr=["+enu_corr[0]+", "+enu_corr[1]+", "+enu_corr[2]+"]");
} }
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) { for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
...@@ -5663,6 +5668,7 @@ public class Interscene { ...@@ -5663,6 +5668,7 @@ public class Interscene {
int ref_index, int ref_index,
int earliest_scene, int earliest_scene,
double [] rms, // null or double[2]; double [] rms, // null or double[2];
double [] enu_corr,
int debugLevel int debugLevel
) { ) {
double [] ims_ortho = clt_parameters.imp.ims_ortho; double [] ims_ortho = clt_parameters.imp.ims_ortho;
...@@ -5736,6 +5742,55 @@ public class Interscene { ...@@ -5736,6 +5742,55 @@ public class Interscene {
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad"); System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees"); System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
} }
double [][] camera_enu = quaternionLma.cameraToENU(quaternionLma.getQuaternion());
double sw = 0;
double [] swd = new double[3];
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
double w = 1.0;
sw += w;
for (int i = 0; i < swd.length; i++) {
swd[i]+=w*(camera_enu[nscene][i]-quat_lma_enu_xyz[nscene][i]);
}
}
for (int i = 0; i < swd.length; i++) {
swd[i] /= sw;
}
if (enu_corr== null) {
enu_corr=new double[3]; // will generate, just not return
}
double [] enu= Imx5.applyQuaternionTo(// metric E,N,U
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
swd,
true); // inverse
for (int i = 0; i < 3; i++) {
enu_corr[i] = enu[i];
}
if (debug_level > -3) {
System.out.println("GNSS correction in camera X,Y,Z = ["+swd[0]+"]["+swd[1]+"]["+swd[2]+"]");
System.out.println("GNSS correction in camera E,N,U = ["+enu_corr[0]+"]["+enu_corr[1]+"]["+enu_corr[2]+"]");
}
if (debug_level > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s",
"N","GNSS-X","GNSS-Y","GNSS-Z",
"CAM-X","CAM-Y","CAM-Z"));
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
quat_lma_enu_xyz[nscene][0],quat_lma_enu_xyz[nscene][1],quat_lma_enu_xyz[nscene][2],
camera_enu[nscene][0],camera_enu[nscene][1],camera_enu[nscene][2]));
}
}
return quaternionLma.getAxisQuat(); return quaternionLma.getAxisQuat();
} }
} }
......
...@@ -5555,22 +5555,26 @@ public class OpticalFlow { ...@@ -5555,22 +5555,26 @@ public class OpticalFlow {
if (calc_quat_corr) { if (calc_quat_corr) {
double [] quat_rms = new double [5]; double [] quat_rms = new double [5];
double [] enu_corr = new double[3];
double [] quatCorr = Interscene.getQuaternionCorrection( double [] quatCorr = Interscene.getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
quat_rms, // double [] rms // null or double[2]; quat_rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel debugLevel); // int debugLevel
if (quatCorr != null) { if (quatCorr != null) {
int num_iter = (int) quat_rms[4]; int num_iter = (int) quat_rms[4];
if (debugLevel> -3) { if (debugLevel> -3) {
System.out.println("LMA done on iteration "+num_iter+ System.out.println("LMA done on iteration "+num_iter+
" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")"); " full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
QuadCLTCPU.showQuatCorr(quatCorr); QuadCLTCPU.showQuatCorr(quatCorr,enu_corr);
} }
quadCLTs[ref_index].setQuatCorr(quatCorr); quadCLTs[ref_index].setQuatCorr(quatCorr);
quadCLT_main.setQuatCorr(quatCorr); quadCLT_main.setQuatCorr(quatCorr);
quadCLTs[ref_index].setENUCorrMetric(enu_corr);
quadCLT_main.setENUCorrMetric(enu_corr);
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); debugLevel+1);
...@@ -5581,9 +5585,10 @@ public class OpticalFlow { ...@@ -5581,9 +5585,10 @@ public class OpticalFlow {
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
sb.append("compass: quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]\n"); sb.append("compass: quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]\n");
sb.append("compass: ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]\n"); sb.append("compass: ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]\n");
sb.append("compass: ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]\n"); sb.append("compass: ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]\n");
sb.append("compass: ENU corr (m)=["+enu_corr[0]+", "+enu_corr[1]+", "+enu_corr[2]+"]\n");
sb.append("------------------------\n\n"); sb.append("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String suffix) quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String suffix)
if (debugLevel > -3) { if (debugLevel > -3) {
......
...@@ -193,6 +193,7 @@ public class QuadCLTCPU { ...@@ -193,6 +193,7 @@ public class QuadCLTCPU {
public Did_gps_pos did_gps1_ubx_pos = null; public Did_gps_pos did_gps1_ubx_pos = null;
public String ims_last_path = null; public String ims_last_path = null;
public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames) public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames)
public double [] enu_corr_metric = null; // GNSS correction - add metrix offset to GNSS of the reference scene
@Deprecated @Deprecated
public double [][] pimu_offsets = new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data) public double [][] pimu_offsets = new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames) // public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
...@@ -213,6 +214,16 @@ public class QuadCLTCPU { ...@@ -213,6 +214,16 @@ public class QuadCLTCPU {
public void setQuatCorr(double[] quat) { public void setQuatCorr(double[] quat) {
quat_corr = quat; quat_corr = quat;
} }
public double [] getENUCorrMetric() {
return enu_corr_metric;
}
public void setENUCorrMetric(double[] corr) {
enu_corr_metric = corr;
}
@Deprecated @Deprecated
public double [][] getPimuOffsets() { public double [][] getPimuOffsets() {
return pimu_offsets; return pimu_offsets;
...@@ -5149,7 +5160,7 @@ public class QuadCLTCPU { ...@@ -5149,7 +5160,7 @@ public class QuadCLTCPU {
} }
return lwir_offsets; return lwir_offsets;
} }
public double [] getLwirScales() { public double [] getLwirScales() {
if (lwir_scales == null) { if (lwir_scales == null) {
lwir_scales = new double [getNumSensors()]; lwir_scales = new double [getNumSensors()];
Arrays.fill(lwir_scales, 1.0); Arrays.fill(lwir_scales, 1.0);
...@@ -5403,6 +5414,10 @@ public class QuadCLTCPU { ...@@ -5403,6 +5414,10 @@ public class QuadCLTCPU {
properties.setProperty(prefix+"quat_corr", IntersceneMatchParameters.doublesToString(this.quat_corr)); properties.setProperty(prefix+"quat_corr", IntersceneMatchParameters.doublesToString(this.quat_corr));
} }
if (this.enu_corr_metric != null) {
properties.setProperty(prefix+"enu_corr_metric", IntersceneMatchParameters.doublesToString(this.enu_corr_metric));
}
if (this.pimu_offsets != null) { if (this.pimu_offsets != null) {
properties.setProperty(prefix+"pimu_offsets_lin", IntersceneMatchParameters.doublesToString(this.pimu_offsets[0])); properties.setProperty(prefix+"pimu_offsets_lin", IntersceneMatchParameters.doublesToString(this.pimu_offsets[0]));
properties.setProperty(prefix+"pimu_offsets_ang", IntersceneMatchParameters.doublesToString(this.pimu_offsets[1])); properties.setProperty(prefix+"pimu_offsets_ang", IntersceneMatchParameters.doublesToString(this.pimu_offsets[1]));
...@@ -5575,6 +5590,9 @@ public class QuadCLTCPU { ...@@ -5575,6 +5590,9 @@ public class QuadCLTCPU {
if (properties.getProperty(prefix+"quat_corr")!=null) { if (properties.getProperty(prefix+"quat_corr")!=null) {
this.quat_corr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"quat_corr"),4); this.quat_corr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"quat_corr"),4);
} }
if (properties.getProperty(prefix+"enu_corr_metric")!=null) {
this.enu_corr_metric= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"enu_corr_metric"),3);
}
if (properties.getProperty(prefix+"pimu_offsets_lin")!=null) { if (properties.getProperty(prefix+"pimu_offsets_lin")!=null) {
this.pimu_offsets[0]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_lin"),3); this.pimu_offsets[0]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_lin"),3);
} }
...@@ -11155,9 +11173,9 @@ public class QuadCLTCPU { ...@@ -11155,9 +11173,9 @@ public class QuadCLTCPU {
public void showQuatCorr() { public void showQuatCorr() {
showQuatCorr(getQuatCorr()); showQuatCorr(getQuatCorr(),getENUCorrMetric());
} }
public static void showQuatCorr(double [] quat_corr) { public static void showQuatCorr(double [] quat_corr, double [] enu_corr_metric) {
if (quat_corr != null) { if (quat_corr != null) {
System.out.println("Correction quaternion = ["+quat_corr[0]+", "+ System.out.println("Correction quaternion = ["+quat_corr[0]+", "+
quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]"); quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
...@@ -11169,7 +11187,14 @@ public class QuadCLTCPU { ...@@ -11169,7 +11187,14 @@ public class QuadCLTCPU {
} else { } else {
System.out.println("No IMS orientation correction is defined"); System.out.println("No IMS orientation correction is defined");
} }
if (enu_corr_metric != null) {
System.out.println("Correction to ENU (meters) = ["+enu_corr_metric[0]+", "+
enu_corr_metric[1]+", "+enu_corr_metric[2]+"]");
} else {
System.out.println("No ENU correction is defined");
}
} }
public static void showPimuOffsets(CLTParameters clt_parameters) { public static void showPimuOffsets(CLTParameters clt_parameters) {
showPimuOffsets( clt_parameters.imp.get_pimu_offs()); showPimuOffsets( clt_parameters.imp.get_pimu_offs());
} }
......
...@@ -140,7 +140,7 @@ public class QuaternionLma { ...@@ -140,7 +140,7 @@ public class QuaternionLma {
if (debug_level > 0) { if (debug_level > 0) {
debugYfX ( "", // String pfx, debugYfX ( "", // String pfx,
y_vector); // double [] data) y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx, debugYfX ( "GNSS-", // String pfx,
x_vector); // double [] data) x_vector); // double [] data)
} }
...@@ -783,6 +783,35 @@ public class QuaternionLma { ...@@ -783,6 +783,35 @@ public class QuaternionLma {
} }
return fx; return fx;
} }
/**
* Rotate camera X,Y,Z to ENU to reduce GNSS noise for georeferencing of the sequence
* Camera X,Y,Z are in y_vector
* @param vector single-element angle from fitting GNSS to camera (will rotate in opposite direction)
* @return [nsample]{e,n,u}
*/
public double [][] cameraToENU(
double [] vector) {
double c = Math.cos(-vector[0]/2), s = Math.sin(-vector[0]/2); // inverse
//axis
double [][] camera_enu = new double [N][];
final double [] q = new double [] { c, s*axis[0], s*axis[1], s*axis[2]};
// double [] dq_dv = new double [] {-s/2, c*axis[0]/2, c*axis[1]/2, c*axis[2]/2};
for (int i = 0; i < N; i++) {
int i3 = 3 * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i3+j] > 0) {
break has_data;
}
}
continue; // nothing to process for this scene
}
double [] xyz = new double [] {y_vector[i3 + 0],y_vector[i3 + 1],y_vector[i3 + 2]};
camera_enu[i] = applyTo(q, xyz);
}
return camera_enu;
}
private double [] getFxDerivs6Dof( private double [] getFxDerivs6Dof(
double [] vector, double [] vector,
......
...@@ -24,9 +24,11 @@ package com.elphel.imagej.tileprocessor; ...@@ -24,9 +24,11 @@ package com.elphel.imagej.tileprocessor;
//import java.awt.Point; //import java.awt.Point;
import java.awt.Rectangle; import java.awt.Rectangle;
import java.io.IOException; import java.io.IOException;
import java.text.SimpleDateFormat;
import java.time.LocalDateTime; import java.time.LocalDateTime;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections; import java.util.Collections;
import java.util.Comparator; import java.util.Comparator;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
...@@ -3130,7 +3132,7 @@ public class TexturedModel { ...@@ -3130,7 +3132,7 @@ public class TexturedModel {
discard_rdisp, // double discard_rdisp // discard above/below this fraction of average height discard_rdisp, // double discard_rdisp // discard above/below this fraction of average height
pix_size, // double pix_size, // in meters pix_size, // double pix_size, // in meters
max_image_width, // int max_image_width // increase pixel size as a power of 2 until image fits max_image_width, // int max_image_width // increase pixel size as a power of 2 until image fits
hdr_x0y0, // double [] x0y0, // initialize to double[2] to return width, height hdr_x0y0, // double [] x0y0, // left, bottom
hdr_whs, // int [] whs, // initialize to int[3] to return {width, height, scale reduction} hdr_whs, // int [] whs, // initialize to int[3] to return {width, height, scale reduction}
debugLevel); // int debug_level debugLevel); // int debug_level
if (to_ground_xyzatr == null) { if (to_ground_xyzatr == null) {
...@@ -3139,26 +3141,26 @@ public class TexturedModel { ...@@ -3139,26 +3141,26 @@ public class TexturedModel {
double scaled_pixel_size = pix_size * hdr_whs[2]; double scaled_pixel_size = pix_size * hdr_whs[2];
if (use_parallel_proj) { if (use_parallel_proj) {
if (update_range ) { if (update_range ) {
double [][] bounds = Render3D.getBounds( double [][] bounds = Render3D.getBounds( // Y- up
tri_meshes, // final ArrayList<TriMesh> tri_meshes, tri_meshes, // final ArrayList<TriMesh> tri_meshes,
to_ground_xyzatr, // final double [][] xyzatr_toground, to_ground_xyzatr, // final double [][] xyzatr_toground,
debugLevel); // int debugLevel) // debug level debugLevel); // int debugLevel) // debug level
hdr_x0y0[0] = bounds[0][0]; hdr_x0y0[0] = bounds[0][0];
hdr_x0y0[1] = bounds[1][0]; hdr_x0y0[1] = bounds[1][0];
hdr_whs[0] = (int) Math.ceil((bounds[0][1]-bounds[0][0])/scaled_pixel_size); hdr_whs[0] = (int) Math.ceil((bounds[0][1]-bounds[0][0])/scaled_pixel_size);
hdr_whs[1] = (int) Math.ceil((bounds[1][1]-bounds[1][0])/scaled_pixel_size); hdr_whs[1] = (int) Math.ceil((bounds[1][1]-bounds[1][0])/scaled_pixel_size);
if (debugLevel > -2) { if (debugLevel > -2) {
System.out.println("Updated parameters for rendering:top left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]"); System.out.println("Updated parameters for rendering:BOTTOM left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]");
System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]); System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]);
System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm"); System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm");
} }
} else { } else {
if (debugLevel > -2) { if (debugLevel > -2) {
System.out.println("Keeping conservative parameters for rendering:top left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]"); System.out.println("Keeping conservative parameters for rendering:top left corner=["+hdr_x0y0[0]+"m, "+hdr_x0y0[1]+"m]");
System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]); System.out.println(" : width="+hdr_whs[0]+"pix, height="+hdr_whs[1]+"pix, scale level="+hdr_whs[2]);
System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm"); System.out.println(" : pixel size: ="+(1000*scaled_pixel_size)+"mm");
} }
} }
} }
if ((hdr_whs[0]<0) || (hdr_whs[1]<0)) { if ((hdr_whs[0]<0) || (hdr_whs[1]<0)) {
...@@ -3170,7 +3172,7 @@ public class TexturedModel { ...@@ -3170,7 +3172,7 @@ public class TexturedModel {
scenes[ref_index], // QuadCLT ref_scene, // all coordinates relative to this scene scenes[ref_index], // QuadCLT ref_scene, // all coordinates relative to this scene
to_ground_xyzatr, // double [][] plane_xyzatr, // projection plane center relative to reference scene to_ground_xyzatr, // double [][] plane_xyzatr, // projection plane center relative to reference scene
scaled_pixel_size, // double pixel_size, // in meters scaled_pixel_size, // double pixel_size, // in meters
hdr_x0y0, // double [] x0_y0, // usually negative - top-left point of the output render hdr_x0y0, // double [] x0_y0, // usually negative - BOTTOM-left point of the output render
hdr_whs[0], // int out_width, // output rendered image width in pixels hdr_whs[0], // int out_width, // output rendered image width in pixels
hdr_whs[1]); // int out_height); // output rendered image height in pixels hdr_whs[1]); // int out_height); // output rendered image height in pixels
boolean last_is_alpha = true; // last channel in textures slices is alpha boolean last_is_alpha = true; // last channel in textures slices is alpha
...@@ -3216,7 +3218,7 @@ public class TexturedModel { ...@@ -3216,7 +3218,7 @@ public class TexturedModel {
hdr_whs[0] = ltwh[2]; // cropped width hdr_whs[0] = ltwh[2]; // cropped width
hdr_whs[1] = ltwh[3]; // cropped height hdr_whs[1] = ltwh[3]; // cropped height
hdr_x0y0[0] +=ltwh[0]* scaled_pixel_size; // meters corrected by pixels hdr_x0y0[0] +=ltwh[0]* scaled_pixel_size; // meters corrected by pixels
hdr_x0y0[1] +=ltwh[1]* scaled_pixel_size; // meters corrected by pixels hdr_x0y0[1] +=ltwh[1]* scaled_pixel_size; // meters corrected by pixels BOTTOM!
scenes[ref_index].saveDoubleArrayInModelDirectory( // save with Z scenes[ref_index].saveDoubleArrayInModelDirectory( // save with Z
suffix+"-CROP", // String suffix, suffix+"-CROP", // String suffix,
null, // String [] labels, // or null null, // String [] labels, // or null
...@@ -3244,13 +3246,41 @@ public class TexturedModel { ...@@ -3244,13 +3246,41 @@ public class TexturedModel {
double [] top_left_lla_offset_ned = new double[3]; // use center double [] top_left_lla_offset_ned = new double[3]; // use center
if (gmap_gnss_vert) { if (gmap_gnss_vert) {
suffix+="-VERT"; suffix+="-VERT";
} else { // top left } else { // top left hdr_x0y0[*] are normally negative, bottom-left corner
top_left_lla_offset_ned = new double[] { top_left_lla_offset_ned = new double[] {
hdr_x0y0[1], // North // hdr_x0y0 - in meters ! hdr_whs[1] * scaled_pixel_size + hdr_x0y0[1], // North // hdr_x0y0 - in meters !
hdr_x0y0[0], // East hdr_x0y0[0], // East
0}; // no altitude offset here 0}; // no altitude offset here
} }
double [] corrected_lla = Imx5.offsetLla(ref_lla,top_left_lla_offset_ned); double [] enu_corr_metric = scenes[ref_index].getENUCorrMetric();
if (enu_corr_metric != null) { // add clt_parameters.gmap_gnss??
suffix+="-GCORR";
top_left_lla_offset_ned[0] += enu_corr_metric[1];
top_left_lla_offset_ned[1] += enu_corr_metric[0];
if (debugLevel > -3) {
System.out.println("Correcting GNSS by "+enu_corr_metric[1]+" m N and "+enu_corr_metric[0]+" m E.");
}
// no altitude offset here
}
double [] hdr_lefttop_meters_negative = {hdr_x0y0[0], -(hdr_whs[1] * scaled_pixel_size + hdr_x0y0[1])};
double [] corrected_lla = Imx5.offsetLlaNED(ref_lla,top_left_lla_offset_ned);
if (debugLevel > -3) {
System.out.println(" LLA: "+ref_lla[0]+", "+ref_lla[1]+", "+ref_lla[2]);
System.out.println("Corrected LLA: "+corrected_lla[0]+", "+corrected_lla[1]+", "+corrected_lla[2]);
}
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
if (enu_corr_metric != null) {
sb.append("Correcting GNSS by "+enu_corr_metric[1]+" m N and "+enu_corr_metric[0]+" m E.\n");
}
sb.append(" LLA=["+ref_lla[0]+", "+ref_lla[1]+", "+ref_lla[2]+"]\n");
sb.append("Corrected LLA=["+corrected_lla[0]+", "+corrected_lla[1]+", "+corrected_lla[2]+"]\n");
sb.append("------------------------\n\n");
scenes[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String suffix)
int num_pix = 0; int num_pix = 0;
double sum_z = 0.0; double sum_z = 0.0;
for (int i = 0; i < cropped_z[0].length; i++) { for (int i = 0; i < cropped_z[0].length; i++) {
...@@ -3265,6 +3295,8 @@ public class TexturedModel { ...@@ -3265,6 +3295,8 @@ 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?
if (gmap_save_alt) { if (gmap_save_alt) {
/* /*
scenes[ref_index].saveDoubleArrayInTopModelDirectory( // save with Z scenes[ref_index].saveDoubleArrayInTopModelDirectory( // save with Z
...@@ -3278,7 +3310,7 @@ public class TexturedModel { ...@@ -3278,7 +3310,7 @@ public class TexturedModel {
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
cropped_z[0], // double [] data, cropped_z[0], // double [] data,
corrected_lla, // double [] lla, // latitude, longitude, altitude (or null) corrected_lla, // double [] lla, // latitude, longitude, altitude (or null)
hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters) hdr_lefttop_meters_negative, // hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters)
dt, // LocalDateTime dt, // local date/time or null dt, // LocalDateTime dt, // local date/time or null
scaled_pixel_size, // double pix_in_meters, scaled_pixel_size, // double pix_in_meters,
hdr_whs[0], // int width, hdr_whs[0], // int width,
...@@ -3291,7 +3323,7 @@ public class TexturedModel { ...@@ -3291,7 +3323,7 @@ public class TexturedModel {
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
img_cropped[0], // double [] data, img_cropped[0], // double [] data,
corrected_lla, // double [] lla, // latitude, longitude, altitude (or null) corrected_lla, // double [] lla, // latitude, longitude, altitude (or null)
hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters) hdr_lefttop_meters_negative, // hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters)
dt, // LocalDateTime dt, // local date/time or null dt, // LocalDateTime dt, // local date/time or null
scaled_pixel_size, // double pix_in_meters, scaled_pixel_size, // double pix_in_meters,
hdr_whs[0], // int width, hdr_whs[0], // int width,
...@@ -3303,20 +3335,6 @@ public class TexturedModel { ...@@ -3303,20 +3335,6 @@ public class TexturedModel {
// TODO: Move UM here, save 32-bit GEO w/o UM // TODO: Move UM here, save 32-bit GEO w/o UM
if (gmap_um) { if (gmap_um) {
double um_sigma = gmap_um_sigma / scaled_pixel_size; double um_sigma = gmap_um_sigma / scaled_pixel_size;
/*
int grow_pix = (int) (4 * um_sigma); // experimentally - UM grows by 4*sigma;
// it is needed before Gaussian blur which extends NaN
double [] um_data = TileProcessor.fillNaNs( // very slow
img_cropped[0], // final double [] data,
null, // final boolean [] prohibit,
hdr_whs[0], // int width,
// CAREFUL ! Remaining NaN is grown by unsharp mask filter ************* !
2*grow_pix, // 2*width, // 16, // final int grow,
0.7, // double diagonal_weight, // relative to ortho
100, // int num_passes,
0.03, // final double max_rchange, // = 0.01 - does not need to be accurate
THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
*/
// Not yet good, but OK for UM // Not yet good, but OK for UM
double [] um_data = TileProcessor.fillNaNs( double [] um_data = TileProcessor.fillNaNs(
img_cropped[0], // final double [] idata, img_cropped[0], // final double [] idata,
...@@ -3347,7 +3365,7 @@ public class TexturedModel { ...@@ -3347,7 +3365,7 @@ public class TexturedModel {
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
img_cropped[0], // double [] data, img_cropped[0], // double [] data,
corrected_lla, // double [] lla, // latitude, longitude, altitude (or null) corrected_lla, // double [] lla, // latitude, longitude, altitude (or null)
hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters) hdr_lefttop_meters_negative, //hdr_x0y0, // double [] xy_center, // X,Y of top left to vertical (negative meters)
dt, // LocalDateTime dt, // local date/time or null dt, // LocalDateTime dt, // local date/time or null
scaled_pixel_size, // double pix_in_meters, scaled_pixel_size, // double pix_in_meters,
hdr_whs[0], // int width, hdr_whs[0], // int width,
...@@ -3373,7 +3391,6 @@ public class TexturedModel { ...@@ -3373,7 +3391,6 @@ public class TexturedModel {
} }
} }
} }
} }
} }
} }
......
...@@ -66,7 +66,7 @@ public class Render3D { ...@@ -66,7 +66,7 @@ public class Render3D {
this.out_width = out_width; this.out_width = out_width;
this.out_height = out_height; this.out_height = out_height;
this.toground = toground; this.toground = toground;
this.x0_y0 = x0_y0; // used in parallel projection this.x0_y0 = x0_y0; // used in parallel projection BOTTOM-left
this.tocam = ErsCorrection.invertXYZATR(this.toground); // null this.tocam = ErsCorrection.invertXYZATR(this.toground); // null
// ground plane x0, y0 in camera coordinates // ground plane x0, y0 in camera coordinates
ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1], 0.0})); ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1], 0.0}));
...@@ -248,7 +248,7 @@ public class Render3D { ...@@ -248,7 +248,7 @@ public class Render3D {
bounds[j][1] = Math.max(bounds[j][1], mm_xyz[j][1]); bounds[j][1] = Math.max(bounds[j][1], mm_xyz[j][1]);
} }
} }
return bounds; return bounds; // y is up
} }
......
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