Skip to content

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

Bug fixing to make run for many scenes. Started LPF for the ground w/o 

SfM
parent 3f84a242
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 774 additions and 138 deletions
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ ab8d3318
......@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
System.out.println("=== IMS ===");
QUAD_CLT_AUX.showQuatCorr();
System.out.println("=== IMU ===");
QUAD_CLT_AUX.showPimuOffsets();
QUAD_CLT_AUX.showPimuOffsets(CLT_PARAMETERS); //
@SuppressWarnings("unused")
QuadCLT dbg_QUAD_CLT = QUAD_CLT;
@SuppressWarnings("unused")
......
src/main/java/com/elphel/imagej/readers/ElphelTiffWriter.java
View file @ ab8d3318
......@@ -295,7 +295,7 @@ public class ElphelTiffWriter {
private static IIOMetadataNode createTimeStamp(LocalDateTime dt, int digits_after) { // 3
int denom = 1;
for (int i = 0; i < digits_after; i++) denom *= 10;
int fsec = dt.getSecond()*denom+((int) Math.round(denom * dt.getNano()*1E-9));
int fsec = dt.getSecond()*denom+((int) Math.round(denom * (dt.getNano()*1E-9)));
IIOMetadataNode node_rationals = new IIOMetadataNode(TIFF_RATIONALS_TAG);
IIOMetadataNode node_hrs = new IIOMetadataNode(TIFF_RATIONAL_TAG);
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ ab8d3318
......@@ -118,6 +118,7 @@ public class IntersceneMatchParameters {
public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
public boolean orient_by_move = false; // use translation data to adjust IMU orientation
public boolean orient_by_rot = true; // use rotation data to adjust IMU orientation
public boolean orient_combo = true; // use combined rotation+orientation for IMU/camera matching
public boolean adjust_gyro = false; // adjust qyro omegas offsets
public boolean apply_gyro = true; // apply adjusted qyro omegas offsets
public boolean adjust_accl = false; // adjust IMU velocities scales
......@@ -386,7 +387,9 @@ public class IntersceneMatchParameters {
public double max_zoom_diff = 0; // for down-views when changing altitude (0 - ignore)
public boolean fpn_skip = true; // skip too close scenes (false - abort, previous behavior)
public boolean fpn_rematch = true; // match fpn-failed scenes to later scenes with larger difference
public boolean refine_invert = false; // Refine with LMA while inverting relative poses from other reference
// still not clear why it sometimes fails without refine_invert (too large initial mismatch)
public boolean refine_invert = true; // Refine with LMA while inverting relative poses from other reference
public boolean use_precomp = false; // try to predict initial error from previous scenes
// Remove moving objects (goal is not to detect slightest movement, but to improve pose matching
public boolean mov_en = true; // enable detection/removal of the moving objects during pose matching
......@@ -673,12 +676,14 @@ public class IntersceneMatchParameters {
"Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
"Adjust IMU misalignment to the camera.");
gd.addCheckbox ("Adjust IMU orientation", this.apply_imu_orient,
gd.addCheckbox ("Apply IMU orientation", this.apply_imu_orient,
"Apply IMU misalignment to the camera if adjusted.");
gd.addCheckbox ("Use translation for IMU orientation", this.orient_by_move,
"Use translation data to adjust IMU orientation .");
gd.addCheckbox ("Use rotation for IMU orientation", this.orient_by_rot,
"Use rotation data to adjust IMU orientation.");
gd.addCheckbox ("Use combo mode IMU orientation", this.orient_combo,
"Use combined Z/h, R, A-X/h, T+Y/h for IMU mount-to-camera orientation correction. False - use X,Y,Z,A,T,R");
gd.addCheckbox ("Adjust gyro offsets", this.adjust_gyro,
"Adjust qyro omegas offsets.");
gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
......@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters {
"Match fpn-failed scenes to later scenes with larger difference.");
gd.addCheckbox ("Refine inversion", this.refine_invert,
"Refine with LMA while inverting relative poses from other reference.");
gd.addCheckbox ("Precompensate orientation readjustment", this.use_precomp,
"Guess needed initial precompensation from the previously processed scenes.");
gd.addMessage ("Detect and remove moving objects from pose matching");
gd.addCheckbox ("Enable movement detection/elimination", this.mov_en,
......@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters {
this.apply_imu_orient = gd.getNextBoolean();
this.orient_by_move = gd.getNextBoolean();
this.orient_by_rot = gd.getNextBoolean();
this.orient_combo = gd.getNextBoolean();
this.adjust_gyro = gd.getNextBoolean();
this.apply_gyro = gd.getNextBoolean();
this.adjust_accl = gd.getNextBoolean();
......@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters {
this.fpn_skip = gd.getNextBoolean();
this.fpn_rematch = gd.getNextBoolean();
this.refine_invert = gd.getNextBoolean();
this.use_precomp = gd.getNextBoolean();
this.mov_en = gd.getNextBoolean();
this.mov_sigma = gd.getNextNumber();
......@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"apply_imu_orient", this.apply_imu_orient+""); // boolean
properties.setProperty(prefix+"orient_by_move", this.orient_by_move+""); // boolean
properties.setProperty(prefix+"orient_by_rot", this.orient_by_rot+""); // boolean
properties.setProperty(prefix+"orient_combo", this.orient_combo+""); // boolean
properties.setProperty(prefix+"adjust_gyro", this.adjust_gyro+""); // boolean
properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean
properties.setProperty(prefix+"adjust_accl", this.adjust_accl+""); // boolean
......@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"fpn_skip", this.fpn_skip+""); // boolean
properties.setProperty(prefix+"fpn_rematch", this.fpn_rematch+""); // boolean
properties.setProperty(prefix+"refine_invert", this.refine_invert+""); // boolean
properties.setProperty(prefix+"use_precomp", this.use_precomp+""); // boolean
properties.setProperty(prefix+"mov_en", this.mov_en+""); // boolean
properties.setProperty(prefix+"mov_sigma", this.mov_sigma+""); // double
......@@ -2345,6 +2356,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"apply_imu_orient")!=null) this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
if (properties.getProperty(prefix+"orient_by_move")!=null) this.orient_by_move=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_move"));
if (properties.getProperty(prefix+"orient_by_rot")!=null) this.orient_by_rot=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_rot"));
if (properties.getProperty(prefix+"orient_combo")!=null) this.orient_combo=Boolean.parseBoolean(properties.getProperty(prefix+"orient_combo"));
if (properties.getProperty(prefix+"adjust_gyro")!=null) this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
if (properties.getProperty(prefix+"apply_gyro")!=null) this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
if (properties.getProperty(prefix+"adjust_accl")!=null) this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
......@@ -2588,6 +2600,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"fpn_skip")!=null) this.fpn_skip=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_skip"));
if (properties.getProperty(prefix+"fpn_rematch")!=null) this.fpn_rematch=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_rematch"));
if (properties.getProperty(prefix+"refine_invert")!=null) this.refine_invert=Boolean.parseBoolean(properties.getProperty(prefix+"refine_invert"));
if (properties.getProperty(prefix+"use_precomp")!=null) this.use_precomp=Boolean.parseBoolean(properties.getProperty(prefix+"use_precomp"));
if (properties.getProperty(prefix+"mov_en")!=null) this.mov_en=Boolean.parseBoolean(properties.getProperty(prefix+"mov_en"));
if (properties.getProperty(prefix+"mov_sigma")!=null) this.mov_sigma=Double.parseDouble(properties.getProperty(prefix+"mov_sigma"));
......@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters {
imp.apply_imu_orient = this.apply_imu_orient;
imp.orient_by_move = this.orient_by_move;
imp.orient_by_rot = this.orient_by_rot;
imp.orient_combo = this.orient_combo;
imp.adjust_gyro = this.adjust_gyro;
imp.apply_gyro = this.apply_gyro;
imp.adjust_accl = this.adjust_accl;
......@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters {
imp.fpn_skip = this.fpn_skip;
imp.fpn_rematch = this.fpn_rematch;
imp.refine_invert = this.refine_invert;
imp.use_precomp = this.use_precomp;
imp.mov_en = this.mov_en;
imp.mov_sigma = this.mov_sigma;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ ab8d3318
......@@ -29,8 +29,10 @@ import java.awt.Rectangle;
import java.io.File;
import java.io.IOException;
import java.security.NoSuchAlgorithmException;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
......@@ -5426,15 +5428,30 @@ public class OpticalFlow {
}
}
}
// quadCLTs[ref_index].getSmoothGround(clt_parameters);
// later move to the right place
if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
QuadCLT.adjustImuOrient(
clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
debugLevel); // int debugLevel
boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
QuadCLT.adjustImuOrient(
clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
debugLevel); // int debugLevel
// Try both orient_combo/!orient_combo for the log!
QuadCLT.adjustImuOrient(
clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
!orient_combo, // boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
debugLevel); // int debugLevel
}
if (run_ly) {
if (debugLevel > -3) {
......@@ -5548,15 +5565,20 @@ public class OpticalFlow {
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);
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
sb.append("Applying correction to the IMS to world orientation (rotating around IMS vertical):\n");
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("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),QuadCLT.IMU_CALIB_LOGS_SUFFIX); // String suffix)
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):");
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;
System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
System.out.print(sb.toString());
}
} else {
if (debugLevel> -3) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ ab8d3318
......@@ -35,10 +35,10 @@ import Jama.Matrix;
public class QuaternionLma {
private final static int REGLEN = 1; // number of extra (regularization) samples
public static final int MODE_XYZ = 0;
public static final int MODE_XYZQ = 1;
public static final int MODE_XYZQ = 1; // OK with [3]
public static final int MODE_COMBO = 2;
public static final int MODE_XYZQ_LOCAL = 3;
public static final int MODE_COMBO_LOCAL = 4;
public static final int MODE_COMBO_LOCAL = 4; // OK with [3]
public static final int MODE_COMPASS = 5;
public static final int MODE_XYZ4Q3 = 6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
......@@ -67,6 +67,7 @@ public class QuaternionLma {
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] axis = null;
private double[] dbg_data;
public double [] getQuaternion() {
if (parameters_vector.length == 3) {
......@@ -673,10 +674,10 @@ public class QuaternionLma {
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
} else {
return getFxDerivs6Dof(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
return getFxDerivs6Dof(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
case MODE_COMBO:return getFxDerivsVisual( // fill change
vector, // double [] vector,
......@@ -748,7 +749,7 @@ public class QuaternionLma {
double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2);
//axis
double [] fx = new double [weights.length];
final double [] q = new double [] { c/2, s*axis[0]/2, s*axis[1]/2, s*axis[2]/2};
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};
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
......@@ -759,6 +760,14 @@ public class QuaternionLma {
double [][] xyz_dq;
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
}
final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
xyz_rot = applyTo(q, xyz);
System.arraycopy(xyz_rot, 0, fx, i3, 3);
......@@ -922,7 +931,7 @@ public class QuaternionLma {
}
return fx;
}
private double [] getFxDerivs6DofMode33(
private double [] getFxDerivs6DofMode33( // MODE_XYZQ_LOCAL = 3; // OK with [3]
double [] vector, //
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
......@@ -933,6 +942,7 @@ public class QuaternionLma {
final double q0 = getQ0(vector);
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
final double [] vector_r = new double[] { q0,q1,q2,q3};
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
......@@ -1084,7 +1094,7 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations
private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations MODE_XYZQ tested
double [] vector3, //
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
......@@ -1428,10 +1438,14 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivsVisualMode43(
private double [] getFxDerivsVisualMode43( // tested MODE_COMBO_LOCAL = 4; // OK with [3]
double [] vector3,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
boolean dbg_out = debug_level>2;
if (dbg_out) {
dbg_data = new double[N * samples_x];
}
double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
double [] fx = new double [weights.length];
double [] qn = new double[4];
......@@ -1476,12 +1490,15 @@ public class QuaternionLma {
fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3
fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
/*
fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height; // Z
fx[i4 + 1] = -2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3
fx[i4 + 2] = -2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = -2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
*/
if (dbg_out) {
dbg_data[i7 + 0] = comb_y[0][0]/ height;
dbg_data[i7 + 1] = comb_y[0][1]/ height;
dbg_data[i7 + 2] = comb_y[0][2]/ height;
dbg_data[i7 + 3] = comb_y[1][0];
dbg_data[i7 + 4] = comb_y[1][1];
dbg_data[i7 + 5] = comb_y[1][2];
dbg_data[i7 + 6] = comb_y[1][3];
}
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
double [][] xyz_dq_local = new double [xyz_dq.length][];
......@@ -1719,17 +1736,21 @@ public class QuaternionLma {
parameters_vector, // double [] vector,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
debugYfX ( "fx-", // String pfx,
fx); // double [] data)
if (debug_level > 1) {
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
parameters_vector, // double [] vector,
delta); // double delta);
System.out.println("Maximal error = "+err);
}
debugYfX ( "fx-", // String pfx,
fx); // double [] data)
if (debug_level > 2) {
debugYfX ( "ffx-", // String pfx,
dbg_data); // double [] data)
}
if (debug_level > 1) {
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
parameters_vector, // double [] vector,
delta); // double delta);
System.out.println("Maximal error = "+err);
}
}
return rslt[0]? iter : -1;
}
......@@ -1773,6 +1794,11 @@ public class QuaternionLma {
debugYfX ( "fx0-", // String pfx,
fx); // double [] data)
}
if (debug_level > 2) {
debugYfX ( "ffx0-", // String pfx,
dbg_data); // double [] data)
}
if (debug_level > 1) {
double delta = 1E-5;
System.out.println("\n\n");
......@@ -1919,6 +1945,9 @@ public class QuaternionLma {
public void debugYfX (
String pfx,
double [] data) {
if (data == null) {
return;
}
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) {
System.out.println(String.format("%3s"+
......
src/main/java/com/elphel/imagej/tileprocessor/TexturedModel.java
View file @ ab8d3318
......@@ -2576,6 +2576,7 @@ public class TexturedModel {
double [] sfm_gain = (min_sfm_gain > 0.0) ? combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_SFM_GAIN] : null;
// currently conditionInitialDS() zeroes disparity for blue_sky. TODO: allow some FG over blue_sky?
// gets Blue Sky from scene.dsi , not from the file!
double [][] ds_fg = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
......@@ -3165,7 +3166,7 @@ public class TexturedModel {
hdr_whs[0], // int width, // int tilesX,
hdr_whs[1]); // int height, // int tilesY,
*/
scenes[ref_index].writeLwirGeoTiff32(
scenes[ref_index].writeLwirGeoTiff32( // Negative value supplied for TIFF_RATIONAL
clt_parameters, // final CLTParameters clt_parameters,
cropped_z[0], // double [] data,
top_left_lla, // double [] lla, // latitude, longitude, altitude (or null)
......
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