Skip to content

I
imagej-elphel
Commit 6845c018 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Preparing infrastructure for maps matching, fixed bug in initial GEO 

generation (wrong Y sign)
parent 0de865d9
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 623 additions and 133 deletions
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ 6845c018
......@@ -287,7 +287,7 @@ public class Imx5 {
/*
* 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 [] {
lla[0] + (ned[0]/EARTH_RADIUS / Math.PI * 180 / Math.cos(lla[0])), // degrees
lla[1] + (ned[1]/EARTH_RADIUS / Math.PI * 180), // degrees
......
src/main/java/com/elphel/imagej/orthomosaic/ComboMatch.java
View file @ 6845c018
......@@ -32,11 +32,11 @@ public class ComboMatch {
public static GpuQuad GPU_QUAD_AFFINE = null;
public static ImagePlus imp_src1 = null;
public static ImagePlus imp_src2 = null;
public static ImagePlus [] imp_src = new ImagePlus[2];
public static ImagePlus [] imp_alt = new ImagePlus[2];
public static Properties [] imp_prop = new Properties[2];
public static int gpu_max_width= 3008;
public static int gpu_max_height= 3008;
public static ImagePlus [] imp_src; // = new ImagePlus[2];
public static ImagePlus [] imp_alt; // = new ImagePlus[2];
public static Properties [] imp_prop; // = new Properties[2];
public static int gpu_max_width= 4096;
public static int gpu_max_height= 4096;
public static boolean openTestPairGps(
CLTParameters clt_parameters,
......@@ -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
*/
/*
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/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}}};
int gpu_width= clt_parameters.imp.rln_gpu_width; // 3008;
int gpu_height= clt_parameters.imp.rln_gpu_height; // 3008;
int zoom_lev = -4; // 0; // +1 - zoom in twice, -1 - zoom out twice
boolean use_alt = false;
boolean show_centers = true;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,650);
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");
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set image pair",1200,800);
for (int n = 0; n < image_paths_pre.length; n++) {
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++) {
gd.addMessage("image["+n+"] pose");
gd.addNumericField("East", image_enuatr[n][0][0], 3,7,"m", "Move image "+n+" East.");
......@@ -94,11 +122,15 @@ public class ComboMatch {
gd.addNumericField("GPU image height", gpu_height, 0,4,"",
"GPU image height");
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();
if (gd.wasCanceled()) return false;
image_paths_pre[0] = gd.getNextString();
image_paths_pre[1] = gd.getNextString();
for (int n = 0; n < image_paths_pre.length; n++) {
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++) {
image_enuatr[n][0][0] = gd.getNextNumber();
image_enuatr[n][0][1] = gd.getNextNumber();
......@@ -110,15 +142,91 @@ public class ComboMatch {
zoom_lev = (int) gd.getNextNumber();
gpu_width = (int) gd.getNextNumber();
gpu_height = (int) gd.getNextNumber();
ComboMap.setGPUWIdthHeight(gpu_width,gpu_height);
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++) {
combo_maps[n] = new ComboMap(image_paths_pre[n]+".tiff");
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);
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];
......
src/main/java/com/elphel/imagej/readers/ElphelTiffReader.java
View file @ 6845c018
......@@ -742,6 +742,14 @@ public class ElphelTiffReader extends TiffReader{ // BaseTiffReader {
xy_pixel_in_meters[1] = unit_size/Double.parseDouble(properties.getProperty("YResolution"));
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) {
int res_unit = Integer.parseInt(properties.getProperty("ResolutionUnit"));
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 6845c018
......@@ -928,13 +928,14 @@ public class Interscene {
quadCLTs); //, // QuadCLT[] quadCLTs,
// 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
if (debugLevel > -3) {
System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation);
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;
if (test_quat_corr) {
......@@ -5285,13 +5286,15 @@ public class Interscene {
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
// for testing QuaternionLma
double [] rms = new double [2];
double [] enu_corr = new double[3];
double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2];
debugLevel); // int debugLevel
rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel
if (debugLevel > -3) {
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):");
......@@ -5301,6 +5304,8 @@ public class Interscene {
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(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++) {
......@@ -5663,6 +5668,7 @@ public class Interscene {
int ref_index,
int earliest_scene,
double [] rms, // null or double[2];
double [] enu_corr,
int debugLevel
) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
......@@ -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]*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();
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 6845c018
......@@ -5555,22 +5555,26 @@ public class OpticalFlow {
if (calc_quat_corr) {
double [] quat_rms = new double [5];
double [] enu_corr = new double[3];
double [] quatCorr = Interscene.getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
quat_rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel
if (quatCorr != null) {
int num_iter = (int) quat_rms[4];
if (debugLevel> -3) {
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]+")");
QuadCLTCPU.showQuatCorr(quatCorr);
QuadCLTCPU.showQuatCorr(quatCorr,enu_corr);
}
quadCLTs[ref_index].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, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
......@@ -5581,9 +5585,10 @@ public class OpticalFlow {
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
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: 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: 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(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");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String suffix)
if (debugLevel > -3) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 6845c018
......@@ -193,6 +193,7 @@ public class QuadCLTCPU {
public Did_gps_pos did_gps1_ubx_pos = 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 [] enu_corr_metric = null; // GNSS correction - add metrix offset to GNSS of the reference scene
@Deprecated
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)
......@@ -213,6 +214,16 @@ public class QuadCLTCPU {
public void setQuatCorr(double[] quat) {
quat_corr = quat;
}
public double [] getENUCorrMetric() {
return enu_corr_metric;
}
public void setENUCorrMetric(double[] corr) {
enu_corr_metric = corr;
}
@Deprecated
public double [][] getPimuOffsets() {
return pimu_offsets;
......@@ -5149,7 +5160,7 @@ public class QuadCLTCPU {
}
return lwir_offsets;
}
public double [] getLwirScales() {
public double [] getLwirScales() {
if (lwir_scales == null) {
lwir_scales = new double [getNumSensors()];
Arrays.fill(lwir_scales, 1.0);
......@@ -5403,6 +5414,10 @@ public class QuadCLTCPU {
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) {
properties.setProperty(prefix+"pimu_offsets_lin", IntersceneMatchParameters.doublesToString(this.pimu_offsets[0]));
properties.setProperty(prefix+"pimu_offsets_ang", IntersceneMatchParameters.doublesToString(this.pimu_offsets[1]));
......@@ -5575,6 +5590,9 @@ public class QuadCLTCPU {
if (properties.getProperty(prefix+"quat_corr")!=null) {
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) {
this.pimu_offsets[0]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_lin"),3);
}
......@@ -11155,9 +11173,9 @@ public class QuadCLTCPU {
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) {
System.out.println("Correction quaternion = ["+quat_corr[0]+", "+
quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
......@@ -11169,7 +11187,14 @@ public class QuadCLTCPU {
} else {
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) {
showPimuOffsets( clt_parameters.imp.get_pimu_offs());
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 6845c018
......@@ -140,7 +140,7 @@ public class QuaternionLma {
if (debug_level > 0) {
debugYfX ( "", // String pfx,
y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
debugYfX ( "GNSS-", // String pfx,
x_vector); // double [] data)
}
......@@ -783,6 +783,35 @@ public class QuaternionLma {
}
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(
double [] vector,
......
src/main/java/com/elphel/imagej/x3d/export/Render3D.java
View file @ 6845c018
......@@ -66,7 +66,7 @@ public class Render3D {
this.out_width = out_width;
this.out_height = out_height;
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
// ground plane x0, y0 in camera coordinates
ground_origin = new Vector3D(ErsCorrection.applyXYZATR(tocam, new double [] {x0_y0[0], x0_y0[1], 0.0}));
......@@ -248,7 +248,7 @@ public class Render3D {
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