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
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/Interscene.java
View file @ ab8d3318
......@@ -23,8 +23,10 @@
*/
package com.elphel.imagej.tileprocessor;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
......@@ -508,6 +510,11 @@ public class Interscene {
double sfm_fracall = clt_parameters.imp.sfm_fracall; // 0.3; // minimal relative area of the SfM-e
boolean fmg_initial_en = clt_parameters.imp.fmg_initial_en; // enable IMS-based FPN mitigation for initial orientation
double fmg_distance = clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
// todo: improve to use "preferred" distance
if (fmg_distance < (min_offset + 2)) {
fmg_distance = min_offset + 2;
}
double fmg_max_quad = clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
// offset is too small
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
......@@ -680,6 +687,10 @@ public class Interscene {
if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too
//double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
if (fmg_distance < (min_max[0] + 2)) {
fmg_distance = min_max[0] + 2;
}
int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist,
......@@ -689,6 +700,15 @@ public class Interscene {
avg_z, // double avg_z,
last_index, // ref_index, // int ref_index, // >= earliest_scene
earliest_index); // int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double [] min_max_fpn = {0,min_max[1]};
for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) {
System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
......@@ -701,7 +721,7 @@ public class Interscene {
use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
min_max_fpn,// min_max,// double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
......@@ -755,6 +775,23 @@ public class Interscene {
}
}
}
// Add to log
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Finished invertInitialOrientation():\n");
sb.append("getNumOrient()= "+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("earliest_scene= "+earliest_index+"\n");
sb.append("ref_index= "+ref_index+"\n");
sb.append("last_index= "+last_index+"\n");
sb.append("old_ref_index= "+ref_old+"\n");
sb.append("Maximal RMSE= "+maximal_series_rms+"\n");
sb.append("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String suffix)
}
return 0;
}
......@@ -1161,6 +1198,9 @@ public class Interscene {
if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
if (fmg_distance < (min_max[0] + 2)) {
fmg_distance = min_max[0] + 2;
}
int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist,
......@@ -1170,6 +1210,15 @@ public class Interscene {
avg_z, // double avg_z,
ref_index, // int ref_index, // >= earliest_scene
earliest_scene); // int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double [] min_max_fpn = {0,min_max[1]};
for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) {
System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
......@@ -1182,7 +1231,7 @@ public class Interscene {
use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
min_max_fpn, // min_max,// double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
......@@ -1249,6 +1298,17 @@ public class Interscene {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
// Add to log
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Finished setInitialOrientationsIms():\n");
sb.append("getNumOrient()= "+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("earliest_scene= "+earliest_scene+"\n");
sb.append("ref_index= "+ref_index+"\n");
sb.append("Maximal RMSE= "+maximal_series_rms+"\n");
sb.append("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String suffix)
return earliest_scene;
}
......@@ -1919,6 +1979,7 @@ public class Interscene {
double fmg_distance = clt_parameters.imp.fmg_distance; // try to find other reference scene not closer than this pixels
double fmg_max_quad = clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
boolean use_precomp = clt_parameters.imp.use_precomp;// try to predict initial error from previous scenes
// int avg_len = clt_parameters.imp.avg_len;
// Set up velocities from known coordinates, use averaging
......@@ -2091,6 +2152,28 @@ public class Interscene {
last_scene); // int last_index)
// } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
} else if (readjust_xy_ims) {
// optionally run adjustment here (QuadCLT.rotateImsToCameraXYZ()? Or only
scenes_xyzatr_pull = QuadCLT.refineXYZFromIMU(
clt_parameters, // CLTParameters clt_parameters,
false, // true, // false, // boolean common_scale_only // false - individual by direction
use_Z, // boolean keepZ, // if adjusting Z, qill use its old value
quadCLTs, // QuadCLT[] quadCLTs,
scenes_xyzatr, // double [][][] xyzatr,
null, // double [][][] pimu_xyzatr, // if null - will be recalculated
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene, // int last_index,
debugLevel); //int debugLevel)
for (int nscene = earliest_scene; nscene <= last_scene; nscene++) if (scenes_xyzatr[nscene] != null) {
scenes_xyzatr_pull[nscene] = modifyATRtoXYZ(
scenes_xyzatr[nscene], // double [][] cur_xyzatr, // careful with Z - using the new one
scenes_xyzatr_pull[nscene][0], // double [] new_xyz,
avg_z); // double avg_z
}
/*
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -2146,6 +2229,7 @@ public class Interscene {
avg_z); // double avg_z
}
}
*/
// old version
/*
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
......@@ -2287,6 +2371,18 @@ public class Interscene {
System.out.println("test_adjust = "+test_adjust);
}
// Main cycle - first goes down from the center, then up, and finally processes "fpn" scenes (close to reference)
double [][] precomp_xyzatr = new double [2][3];
int last_processed_scene = -2; // none
/*
* There is still a problem with a large mismatch at start position for far off-reference scenes,
* especiall those that were not refined after inversion - reversing order of earlier processed "later"
* half (after the reference in the center) earlier processed as the bottom half of the other sub-sequence.
*
* For mitigation we'll use that both half-sequences start from the center with small mismatches. The
* precomp_xyzatr will be used to store how much LMA corrected from the original estimation and add it
* to the next scene (only when it is the next), similar to the initial orientation.
*/
for (int nscene:scene_seq) {
if (nscene == debug_scene) {
......@@ -2351,6 +2447,26 @@ public class Interscene {
if (est_shift < min_max[0]) {
fail_reason[0]=FAIL_REASON_MIN;
} else {
double [][] corr_xyzatr_pull = new double [][] {scenes_xyzatr_pull[nscene][0].clone(), scenes_xyzatr_pull[nscene][1].clone()};
/*
* scenes_xyzatr[ref_index],// double [][] scene0_xyzatr, - scene to compare to
* scenes_xyzatr[nscene], // double [][] scene1_xyzatr - previous known pose?
* scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull, - now not a pull,
* but a rigid target set as initial approximation
*/
boolean applied_precomp = false;
if (use_precomp && (last_processed_scene >= nscene-1) && (last_processed_scene <= nscene+1)) {
corr_xyzatr_pull=ErsCorrection.combineXYZATR(
scenes_xyzatr_pull[nscene],
precomp_xyzatr);
applied_precomp = true;
if (debugLevel > -2) {
System.out.println(String.format(
"Applied precompensation: [%9.6f, %9.6f, %9.6f] [%9.6f, %9.6f, %9.6f]",
precomp_xyzatr[0][0], precomp_xyzatr[0][1], precomp_xyzatr[0][2],
precomp_xyzatr[1][0], precomp_xyzatr[1][1], precomp_xyzatr[1][2]));
}
}
scenes_xyzatr[nscene] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, //CLTParameters clt_parameters,
use_lma_dsi, //,boolean use_lma_dsi,
......@@ -2371,7 +2487,7 @@ public class Interscene {
scenes_xyzatr[ref_index],// double [][] scene0_xyzatr,
scenes_xyzatr[nscene], // double [][] scene1_xyzatr,
avg_z, // double average_z,
scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull,
corr_xyzatr_pull, // scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull,
param_select, // boolean[] param_select,
param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
......@@ -2381,6 +2497,22 @@ public class Interscene {
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
clt_parameters.imp.debug_level); // int debugLevel)
adjust_OK = scenes_xyzatr[nscene] != null;
if (!adjust_OK) {
last_processed_scene = -2;
} else if (use_precomp) {
double [][] last_corr_xyzatr = ErsCorrection.combineXYZATR(
scenes_xyzatr[nscene],
ErsCorrection.invertXYZATR(corr_xyzatr_pull));
// add precomp_xyzatr if it was applied:
if (applied_precomp) {
precomp_xyzatr = ErsCorrection.combineXYZATR(
precomp_xyzatr,
last_corr_xyzatr);
} else { // just use difference
precomp_xyzatr = last_corr_xyzatr;
}
last_processed_scene = nscene;
}
}
if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments
if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
......@@ -2460,6 +2592,9 @@ public class Interscene {
System.out.println("num_fpn_mitigate= "+fpn_list.size());
}
if (fmg_reorient_en && !fpn_list.isEmpty()) {
if (fmg_distance < (min_max[0] + 2)) {
fmg_distance = min_max[0] + 2;
}
int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist,
......@@ -2469,6 +2604,15 @@ public class Interscene {
avg_z, // double avg_z,
last_scene, // latest_scene, // int ref_index, // >= earliest_scene
earliest_scene); // int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double [] min_max_fpn = {0,min_max[1]};
boolean test_adjust1 = debugLevel > 1000;
if (test_adjust1) {
int [][] fpn_pairs_dbg = new int [fpn_pairs.length+3][2];
......@@ -2503,7 +2647,7 @@ public class Interscene {
use_lma_dsi, //,boolean use_lma_dsi,
fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
min_max_fpn, // min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
......@@ -2624,6 +2768,31 @@ public class Interscene {
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);
// Add to log
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Finished reAdjustPairsLMAInterscene():\n");
sb.append("getNumOrient()= "+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("earliest_scene= "+range[0]+"\n");
sb.append("ref_index= "+ref_index+"\n");
sb.append("last_index= "+range[1]+"\n");
sb.append("Maximal RMSE= "+maximal_series_rms+"\n");
sb.append("lpf_xy= "+lpf_xy+"\n");
sb.append("readjust_xy_ims="+readjust_xy_ims+"\n");
sb.append("lma_xyzatr= "+lma_xyzatr+"\n");
sb.append("use_R= "+use_R+"\n");
sb.append("use_R= "+use_R+"\n");
sb.append("mb_max_gain= "+mb_max_gain+"\n");
sb.append("avg_z= "+avg_z+"m\n");
sb.append("reg_weight_xy= "+reg_weight_xy+"\n");
sb.append("disable_ers= "+disable_ers+"\n");
sb.append("disable_ers_y= "+disable_ers_y+"\n");
sb.append("disable_ers_r= "+disable_ers_r+"\n");
sb.append("use_precomp= "+use_precomp+"\n");
sb.append("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String suffix)
return earliest_scene;
}
......@@ -2644,11 +2813,13 @@ public class Interscene {
){
double dx = new_xyz[0] - cur_xyzatr[0][0];
double dy = new_xyz[1] - cur_xyzatr[0][1];
// System.out.println(String.format("modifyATRtoXYZ(): dx=%7.3f, dy=%7.3f", dx, dy));
double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
return new_xyzatr;
}
// Make it the only entry point
......@@ -5186,9 +5357,9 @@ public class Interscene {
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.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]+"]");
}
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
......@@ -5544,7 +5715,7 @@ public class Interscene {
}
}
// relative to the GPS/compass
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
......
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/QuadCLTCPU.java
View file @ ab8d3318
......@@ -27,28 +27,25 @@ package com.elphel.imagej.tileprocessor;
//import java.awt.Polygon;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import java.awt.Rectangle;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.charset.Charset;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.nio.file.attribute.PosixFilePermission;
import java.text.SimpleDateFormat;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections;
import java.util.Comparator;
import java.util.Enumeration;
import java.util.List;
import java.util.Properties;
import java.util.Random;
import java.util.Set;
......@@ -99,6 +96,9 @@ import loci.formats.FormatException;
public class QuadCLTCPU {
public static final String IMU_CALIB_LOGS_SUFFIX = "-IMU_CALIB.log";
public static final String ORIENTATION_LOGS_SUFFIX = "-ORIENTATION.log";
public static final String [] DSI_SUFFIXES = {"-INTER-INTRA-LMA","-INTER-INTRA","-DSI_MAIN"};
public static int INDEX_INTER_LMA = 0;
public static int INDEX_INTER = 1;
......@@ -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)
@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)
......@@ -212,11 +213,11 @@ public class QuadCLTCPU {
public void setQuatCorr(double[] quat) {
quat_corr = quat;
}
@Deprecated
public double [][] getPimuOffsets() {
return pimu_offsets;
}
@Deprecated
public void setPimuOffsets(double[][] offsets) { // never
pimu_offsets = offsets;
}
......@@ -475,11 +476,12 @@ public class QuadCLTCPU {
public static void adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[] quadCLTs,
int ref_index,
int earliest_scene,
int last_index,
int debugLevel
int debugLevel
) {
boolean orient_by_move = clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
boolean orient_by_rot = clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
......@@ -487,13 +489,14 @@ public class QuadCLTCPU {
System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
return;
}
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted
boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
boolean adjust_accl = clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
boolean apply_accl = clt_parameters.imp.apply_accl; // apply IMU velocities scales
double quat_max_change = clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
double quat_min_lin = clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
double quat_min_lin = clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -510,8 +513,7 @@ public class QuadCLTCPU {
}
double [] rms = new double[5];
double [] quat = new double[4];
// int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int quat_lma_mode = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......@@ -536,47 +538,59 @@ public class QuadCLTCPU {
debug_lev); // int debugLevel
if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
if (debugLevel > -3) {
System.out.println("Applying correction to the IMS mount orientation:");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("scale="+quat_scale);
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]+"]");
}
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
/*
saveStringInModelDirectory(
sb.tiString, // String string,
IMU_CALIB_LOGS_SUFFIX); // String suffix)
*/
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Applying correction to the IMS mount orientation:\n");
sb.append("getNumOrient()="+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("avg_z= "+avg_z+" m\n");
sb.append("translation_weight= "+translation_weight+"\n");
sb.append("quat_lma_mode= "+quat_lma_mode+"\n");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
double [] new_degrees = new double[3];
for (int i = 0; i < 3; i++) new_degrees[i]=new_ims_mount_atr[i]*180/Math.PI;
sb.append("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
sb.append("scale="+quat_scale+"\n");
sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]\n");
sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]\n");
sb.append(" new ATR(rad)=["+new_ims_mount_atr[0]+", "+new_ims_mount_atr[1]+", "+new_ims_mount_atr[2]+"]\n");
sb.append(" new ATR(deg)=["+new_degrees[0]+", "+new_degrees[1]+", "+new_degrees[2]+"]\n");
if (apply_imu_orient) {
for (int i = 0; i < new_ims_mount_atr.length; i++) {
if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
apply_imu_orient = false;
if (debugLevel > -3) {
System.out.println ("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+" exceeds the specified limit ("+quat_max_change+")");
System.out.println ("*** Orientation update is disabled.");
}
sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
" exceeds the specified limit ("+quat_max_change+")\n");
sb.append("*** Orientation update is disabled.\n");
}
}
}
if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
}
sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
} else {
System.out.println ("*** IMU mount angles calculated, but not applied ***");
sb.append("*** IMU mount angles calculated, but not applied ***\n");
}
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
System.out.println(String.format(
"Using ATR(rad)=[%9f, %9f, %9f]", new_atr[0], new_atr[1], new_atr[2]));
System.out.println(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]", degrees[0], degrees[1], degrees[2]));
sb.append(String.format(
"Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
sb.append(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
double [] omega_corr = null;
if (adjust_gyro) {
omega_corr = getOmegaCorrections(
......@@ -593,30 +607,28 @@ public class QuadCLTCPU {
for (int i = 0; i < 3; i++) {
new_omegas[i] = used_omegas[i] - omega_corr[i];
}
if (debugLevel > -1) {
System.out.println(String.format(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]", used_omegas[0],used_omegas[1],used_omegas[2]));
System.out.println(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]", omega_corr[0], omega_corr[1], omega_corr[2]));
System.out.println(String.format(
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
}
sb.append(String.format(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
used_omegas[0],used_omegas[1],used_omegas[2]));
sb.append(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
omega_corr[0], omega_corr[1], omega_corr[2]));
sb.append(String.format(
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
if (apply_gyro) {
clt_parameters.imp.set_pimu_omegas(new_omegas);
if (debugLevel > -3) {
System.out.println(String.format(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
System.out.println ("*** Need to save the main configuration file ***");
}
sb.append(String.format(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
if (debugLevel > -3) {
System.out.println(String.format(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
}
sb.append(String.format(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
}
} else {
System.out.println("*** Adjustment of the gyro omegas failed ***");
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
}
double [] acc_corr = null;
......@@ -639,35 +651,41 @@ public class QuadCLTCPU {
new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
num_corr++;
}
if (debugLevel > -1) {
System.out.println(String.format(
"Used velocities scales = [%9f, %9f, %9f]", used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
System.out.println(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]", acc_corr[0], acc_corr[1], acc_corr[2]));
System.out.println(String.format(
"New velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
sb.append(String.format(
"Used velocities scales = [%9f, %9f, %9f]\n",
used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
sb.append(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]\n",
acc_corr[0], acc_corr[1], acc_corr[2]));
sb.append(String.format(
"New velocities scales = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
if (apply_accl && (num_corr>0)) {
clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
if (debugLevel > -3) {
System.out.println(String.format(
"Applied new velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
System.out.println ("*** Need to save the main configuration file ***");
}
sb.append(String.format(
"Applied new velocities scales = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
if (debugLevel > -3) {
System.out.println(String.format(
"New velocities scales are not applied = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
sb.append(String.format(
"New velocities scales are not applied = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
} else {
System.out.println("*** Adjustment of the gyro omegas failed ***");
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
//
}
sb.append("------------------------\n\n");
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(),IMU_CALIB_LOGS_SUFFIX); // String suffix)
if (debugLevel > -3) {
System.out.print(sb.toString());
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
}
public static double [] getVelocitiesCorrections(
......@@ -806,6 +824,93 @@ public class QuadCLTCPU {
return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};
}
public static double [][][] refineXYZFromIMU(
CLTParameters clt_parameters,
boolean common_scale_only,
boolean keepZ,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] pimu_xyzatr, // if null - will be recalculated
int ref_index,
int early_index,
int last_index,
int debugLevel){
if (pimu_xyzatr == null) {
pimu_xyzatr = QuadCLT.integratePIMU( // recalculate to updated if calibration was changed
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
early_index, // final int early_index,
last_index); //(quadCLTs.length -1) // int last_index,
}
double [][][] xyzatr_pull = new double [xyzatr.length][][];
double [] sxyz1= new double[3], sxyz2=new double[3];
for (int nscene = early_index; nscene <= last_index; nscene ++) {
for (int i = 0; i < 3; i++) {
double x = xyzatr[nscene][0][i];
double px = pimu_xyzatr[nscene][0][i]; // NULL pointer
sxyz1[i] += px*px;
sxyz2[i] += px* x;
}
}
double [] k = new double [3];
if (common_scale_only) {
double s1 = 0, s2 = 0;
for (int i = 0; i < 3; i++) {
s1+= sxyz1[i];
s2+= sxyz2[i];
}
for (int i = 0; i < 3; i++) {
k[i] = s2/s1;
}
} else {
for (int i = 0; i < 3; i++) {
k[i] = sxyz2[i]/ sxyz1[i];
}
}
for (int nscene = early_index; nscene <= last_index; nscene ++) {
xyzatr_pull[nscene] = new double [][] {pimu_xyzatr[nscene][0].clone(), xyzatr[nscene][1].clone()};
for (int i = 0; i < 2; i++) {
xyzatr_pull[nscene][0][i] *= k[i];
}
if (keepZ) {
xyzatr_pull[nscene][0][2] = xyzatr[nscene][0][2];
} else {
xyzatr_pull[nscene][0][2] *= k[2];
}
}
if (debugLevel > -1) {
if (common_scale_only) {
System.out.println("refineXYZFromIMU(): Scaled IMU X,Y,Z by "+k[0]+"(common) to use as camera target values.");
} else {
System.out.println("refineXYZFromIMU(): Scaled IMU X,Y,Z by ["+k[0]+", "+k[1]+", "+k[2]+
"] to use as camera target values.");
}
}
if (debugLevel > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, ix, iy, iz
"\t%9s\t%9s\t%9s", // px, py, pz
"N","X","Y","Z","iX","iY","iZ2",
"pX","pY","pZ"));
for (int nscene = early_index; nscene <= last_index; nscene ++) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
pimu_xyzatr[nscene][0][0],pimu_xyzatr[nscene][0][1],pimu_xyzatr[nscene][0][2],
xyzatr_pull[nscene][0][0],xyzatr_pull[nscene][0][1],xyzatr_pull[nscene][0][2]));
}
}
return xyzatr_pull;
}
/**
* Refine scene poses (now only linear) from currently adjusted poses
......@@ -1576,10 +1681,285 @@ public class QuadCLTCPU {
whs, // initialize to int[3] to return {width, height, scale reduction}
debug_level);
}
}
public double [][] getGroundNoIms(
}
public double [] getSmoothGround(final CLTParameters clt_parameters) {
final boolean use_lma = true; // false, or there will be too many gaps?
final double discard_low = 0.1; // fraction of all pixels
final double discard_high = 0.5; // fraction of all pixels
// final double discard_adisp, // discard above/below this fraction of average height
// final double discard_rdisp, // discard above/below this fraction of average height
final int stile_hstep = 4;
final double stile_radius = 8.0; //
final double sigma = 2.0;
final int radius_levels = 4;
final int min_non_empty = 10; // minimal number of non-empty tiles for fitting planes
final int debugLevel = 3;
return getSmoothGround(
clt_parameters, // final CLTParameters clt_parameters,
use_lma, // final boolean use_lma, // false, or there will be too many gaps?
discard_low, // final double discard_low, // fraction of all pixels
discard_high, // final double discard_high, // fraction of all pixels
stile_hstep, // final int stile_hstep,
stile_radius, // final double stile_radius,
sigma, // final double sigma,
radius_levels, // final int radius_levels,
min_non_empty, // final int min_non_empty, // minimal number of non-empty tiles for fitting planes
debugLevel); // final int debugLevel);
}
public double [] getSmoothGround(
final CLTParameters clt_parameters,
final boolean use_lma, // false, or there will be too many gaps?
final double discard_low, // fraction of all pixels
final double discard_high, // fraction of all pixels
// final double discard_adisp, // discard above/below this fraction of average height
// final double discard_rdisp, // discard above/below this fraction of average height
final int stile_hstep,
final double stile_radius,
final double sigma,
final int radius_levels,
final int min_non_empty, // minimal number of non-empty tiles for fitting planes
final int debugLevel) {
final int stile_step = 2 * stile_hstep;
final int num_bins = 1000;
final double normal_damping = 0.001; // pull to horizontal if not enough data
final double hist_rlow = 0.5;
final double hist_rhigh = 2.0;
// allow strong/high, but not near the edges
final int tilesX = tp.getTilesX();
final int tilesY = tp.getTilesY();
double [] smooth_disparity = new double [tilesX*tilesY];
int stilesX = (tilesX-1)/stile_step + 1;
int stilesY = (tilesY-1)/stile_step + 1;
final double [][] stiles_coeff = new double [stilesX*stilesY][3];
final double [][][] wnd= new double[radius_levels][][]; // ][irad][irad];
final double [] rad = new double [radius_levels];
final int [] irada = new int [radius_levels];
for (int nlev = 0; nlev < radius_levels; nlev++ ) {
if (nlev==0) {
rad[nlev] = stile_radius;
} else {
rad[nlev] = rad[nlev - 1] * 2;
}
irada[nlev] = (int) Math.ceil(rad[nlev]);
wnd[nlev]= new double [irada[nlev]][irada[nlev]];
double ir2 = irada[nlev]*irada[nlev];
for (int i = 0; i < irada[nlev]; i++) {
for (int j = 0; j < irada[nlev]; j++) {
double r2 = (i*i + j * j)/ir2;
if (r2 <= 1.0) {
wnd[nlev][i][j] = Math.cos(Math.PI*Math.sqrt(r2)/2);
}
}
}
}
final double [][] dls = getDLS();
if (dls==null) {
return null;
}
final double [][] ds = new double [][] {dls[use_lma?1:0].clone(), dls[2]};
double sw=0, swd=0;
for (int i = 0; i < ds[0].length; i++) if (!Double.isNaN(ds[0][i])){
sw += ds[1][i];
swd += ds[0][i] * ds[1][i];
}
double disp_avg = swd/sw;
final double hist_low = hist_rlow * disp_avg;
final double hist_high = hist_rhigh * disp_avg;
double k = num_bins / (hist_high - hist_low);
final Thread[] threads = ImageDtt.newThreadArray(THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TileNeibs tn = new TileNeibs(tilesX,tilesY);
PolynomialApproximation pa = new PolynomialApproximation();
double [] damping = new double [] {normal_damping, normal_damping};
double [] hist = new double [num_bins];
for (int nstile = ai.getAndIncrement(); nstile < stiles_coeff.length; nstile = ai.getAndIncrement()) {
int stileX = nstile % stilesX;
int stileY = nstile / stilesX;
int nlev = 0;
int num_non_empty; // fill data arrays and the histogram
int tileXC = stile_hstep + stile_step * stileX; // center tiles
int tileYC = stile_hstep + stile_step * stileY;
int ntileC = tileXC + tilesX * tileYC;
for (; nlev < radius_levels; nlev++) {
int irad = irada[nlev];
int idia = 2 * irad - 1;
double [] stile_d = new double [idia*idia];
double [] stile_w = new double [idia*idia];
// Arrays.fill(stile_w, 0.0); // not all elements may be filled
Arrays.fill(hist, 0.0);
double sw=0; // , swd=0;
num_non_empty = 0; // fill data arrays and the histogram
// later make to use lma first, then non-lma if nothing exists. Or increase size? (use several ones?
for (int i = 0; i < idia; i++) {
int dy = i-irad+1;
int ady = Math.abs(dy);
for (int j = 0; j < idia; j++) {
int dx = j-irad+1;
int adx = Math.abs(dx);
int tindx = tn.getNeibIndex(ntileC, dx, dy);
if (tindx >= 0) {
int ltile = i * idia + j;
double d = ds[0][tindx];
double w = ds[1][tindx]*wnd[nlev][ady][adx];
int bin = (int) (k * (d - hist_low));
if ((w > 0) && (bin >= 0) && (bin < num_bins)) {
stile_d[ltile] = d;
stile_w[ltile] = w;
sw += w;
// swd += w * d;
hist[bin] += w;
num_non_empty++;
}
}
}
}
if ((num_non_empty < min_non_empty) || (sw==0)) {
continue; // go to the next radius level
}
// try using histogram to remove outliers
double dl = discard_low * sw;
double dh = discard_high * sw;
double sh = 0.0;
int indx = 0;
for (; indx < num_bins; indx++) {
sh+= hist[indx];
if (sh >= dl) break;
}
double shp = sh- hist[indx];
// step back from the overshoot indx
double thr_low = hist_low+ (indx - (sh - dl)/(sh-shp))/num_bins *(hist_high-hist_low);
indx = num_bins-1;
sh = 0.0;
for (; indx >= 0; indx--) {
sh += hist[indx];
if (sh >= dh) {
break;
}
}
shp = sh- hist[indx];
double thr_high = hist_low+ (indx + (sh - dh)/(sh-shp))/num_bins *(hist_high-hist_low);
int numgood = 0;
// boolean [] good = new boolean[ds[0].length];
sw = 0.0;
// swd = 0.0;
for (int i = 0; i < idia; i++) {
for (int j = 0; j < idia; j++) {
int ltile = i * idia + j;
if (stile_w[ltile] > 0) {
if ((stile_d[ltile] >= thr_low) && (stile_d[ltile] <= thr_high)) {
numgood++;
} else {
stile_w[ltile] = 0;
}
}
}
}
if (numgood < min_non_empty) {
continue; // go to the next radius level
}
// fit plane
double [][][] mdata = new double [numgood][][];
int mindx = 0;
for (int i = 0; i < idia; i++) {
int dy = i-irad+1;
for (int j = 0; j < idia; j++) {
int dx = j-irad+1;
int ltile = i * idia + j;
if (stile_w[ltile] > 0) {
mdata[mindx] = new double[3][];
mdata[mindx][0] = new double [2];
mdata[mindx][0][0] = dx;
mdata[mindx][0][1] = dy;
mdata[mindx][1] = new double [1];
mdata[mindx][1][0] = stile_d[ltile];
mdata[mindx][2] = new double [1];
mdata[mindx][2][0] = stile_w[ltile];
mindx++;
}
}
}
double[][] approx2d = pa.quadraticApproximation(
mdata,
true, // boolean forceLinear, // use linear approximation
damping, // double [] damping, null OK
-1); // debug level
stiles_coeff[nstile][0] = approx2d[0][2]; // offset
stiles_coeff[nstile][1] = approx2d[0][0]; // tiltX
stiles_coeff[nstile][2] = approx2d[0][1]; // tiltY
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// Fill planes in each stile
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TileNeibs tn = new TileNeibs(tilesX,tilesY);
for (int nstile = ai.getAndIncrement(); nstile < stiles_coeff.length; nstile = ai.getAndIncrement()) {
int stileX = nstile % stilesX;
int stileY = nstile / stilesX;
int tileXC = stile_hstep + stile_step * stileX; // center tiles
int tileYC = stile_hstep + stile_step * stileY;
int ntileC = tileXC + tilesX * tileYC;
for (int dy = -stile_hstep; dy < stile_hstep; dy++) {
for (int dx = -stile_hstep; dx < stile_hstep; dx++) {
int tindx = tn.getNeibIndex(ntileC, dx, dy);
if (tindx >= 0) {
double d = stiles_coeff[nstile][0] +
stiles_coeff[nstile][1] * dx+
stiles_coeff[nstile][2] * dy;
smooth_disparity[tindx] = d;
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// Maybe use Gaussian? to additionally smooth "seams"
double [][] dbg_img = null;
if (debugLevel > 0 ) {
dbg_img = new double[5][];
dbg_img[0] = dls[2]; // strt
dbg_img[1] = dls[0]; // disp
dbg_img[2] = dls[1]; // lma
dbg_img[3] = smooth_disparity.clone();
}
if (sigma > 0) {
(new DoubleGaussianBlur()).blurDouble(smooth_disparity, tilesX, tilesY, sigma, sigma, 0.01);
}
if (dbg_img != null) {
dbg_img[4] = smooth_disparity;
String [] dbg_titles = {"str", "disp", "lma", "smooth", "blur"};
ShowDoubleFloatArrays.showArrays(
dbg_img,
tp.getTilesX(),
tp.getTilesY(),
true,
getImageName()+"-smooth-ground.tiff ",
dbg_titles);
}
// replace strong high enough objects?
return smooth_disparity;
}
public double [][] getGroundNoIms(
CLTParameters clt_parameters,
boolean use_lma,
boolean use_parallel_proj,
......@@ -4383,6 +4763,24 @@ public class QuadCLTCPU {
return imp;
}
public boolean saveStringInModelDirectory(
String string,
String suffix) { // includes extension
String x3d_path = getX3dDirectory();
String file_name = image_name + suffix;
String file_path = x3d_path + Prefs.getFileSeparator() + file_name;
try {
BufferedWriter out = new BufferedWriter(
new FileWriter(file_path, true));
out.write(string);
out.close();
return true;
} catch (IOException e) {
// Display message when exception occurs
System.out.println("exception occurred" + e);
return false;
}
}
public ImagePlus saveDoubleArrayInModelDirectory(
String suffix,
......@@ -10712,15 +11110,15 @@ public class QuadCLTCPU {
System.out.println("No IMS orientation correction is defined");
}
}
public void showPimuOffsets() {
showPimuOffsets(getPimuOffsets());
public static void showPimuOffsets(CLTParameters clt_parameters) {
showPimuOffsets( clt_parameters.imp.get_pimu_offs());
}
public static void showPimuOffsets(double [][] pimu_offsets) {
if (pimu_offsets != null) {
System.out.println("Linear velocities offsets (m/s) = ["+pimu_offsets[0][0]+", "+
System.out.println("Linear velocities scales (from 1.0) = ["+pimu_offsets[0][0]+", "+
pimu_offsets[0][1]+", "+pimu_offsets[0][2]+"]");
System.out.println("Angular velocities offsets (rad/s) = ["+
System.out.println("Angular velocities offsets (rad/s) = ["+
pimu_offsets[1][0]+", " + pimu_offsets[1][1]+", "+ + pimu_offsets[1][2]+"]");
} else {
System.out.println("No PIMU offsets are defined");
......
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