Commit ab8d3318 authored by Andrey Filippov's avatar Andrey Filippov

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

SfM
parent 3f84a242
...@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener { ...@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
System.out.println("=== IMS ==="); System.out.println("=== IMS ===");
QUAD_CLT_AUX.showQuatCorr(); QUAD_CLT_AUX.showQuatCorr();
System.out.println("=== IMU ==="); System.out.println("=== IMU ===");
QUAD_CLT_AUX.showPimuOffsets(); QUAD_CLT_AUX.showPimuOffsets(CLT_PARAMETERS); //
@SuppressWarnings("unused") @SuppressWarnings("unused")
QuadCLT dbg_QUAD_CLT = QUAD_CLT; QuadCLT dbg_QUAD_CLT = QUAD_CLT;
@SuppressWarnings("unused") @SuppressWarnings("unused")
......
...@@ -295,7 +295,7 @@ public class ElphelTiffWriter { ...@@ -295,7 +295,7 @@ public class ElphelTiffWriter {
private static IIOMetadataNode createTimeStamp(LocalDateTime dt, int digits_after) { // 3 private static IIOMetadataNode createTimeStamp(LocalDateTime dt, int digits_after) { // 3
int denom = 1; int denom = 1;
for (int i = 0; i < digits_after; i++) denom *= 10; 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_rationals = new IIOMetadataNode(TIFF_RATIONALS_TAG);
IIOMetadataNode node_hrs = new IIOMetadataNode(TIFF_RATIONAL_TAG); IIOMetadataNode node_hrs = new IIOMetadataNode(TIFF_RATIONAL_TAG);
......
...@@ -23,8 +23,10 @@ ...@@ -23,8 +23,10 @@
*/ */
package com.elphel.imagej.tileprocessor; package com.elphel.imagej.tileprocessor;
import java.text.SimpleDateFormat;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Calendar;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation; import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
...@@ -508,6 +510,11 @@ public class Interscene { ...@@ -508,6 +510,11 @@ public class Interscene {
double sfm_fracall = clt_parameters.imp.sfm_fracall; // 0.3; // minimal relative area of the SfM-e 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 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 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 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 // offset is too small
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
...@@ -680,6 +687,10 @@ public class Interscene { ...@@ -680,6 +687,10 @@ public class Interscene {
if (fmg_initial_en && !fpn_list.isEmpty()) { if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too // 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) //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( int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list, fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist, fmg_distance, // double fpn_mitigate_dist,
...@@ -689,6 +700,15 @@ public class Interscene { ...@@ -689,6 +700,15 @@ public class Interscene {
avg_z, // double avg_z, avg_z, // double avg_z,
last_index, // ref_index, // int ref_index, // >= earliest_scene last_index, // ref_index, // int ref_index, // >= earliest_scene
earliest_index); // int 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) { for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) { if (debugLevel > -4) {
System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+ System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
...@@ -701,7 +721,7 @@ public class Interscene { ...@@ -701,7 +721,7 @@ public class Interscene {
use_lma_dsi, // clt_parameters.imp.use_lma_dsi, use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers, 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 fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
...@@ -755,6 +775,23 @@ public class Interscene { ...@@ -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; return 0;
} }
...@@ -1161,6 +1198,9 @@ public class Interscene { ...@@ -1161,6 +1198,9 @@ public class Interscene {
if (fmg_initial_en && !fpn_list.isEmpty()) { if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too // 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) 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( int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list, fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist, fmg_distance, // double fpn_mitigate_dist,
...@@ -1170,6 +1210,15 @@ public class Interscene { ...@@ -1170,6 +1210,15 @@ public class Interscene {
avg_z, // double avg_z, avg_z, // double avg_z,
ref_index, // int ref_index, // >= earliest_scene ref_index, // int ref_index, // >= earliest_scene
earliest_scene); // int 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) { for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) { if (debugLevel > -4) {
System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+ System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
...@@ -1182,7 +1231,7 @@ public class Interscene { ...@@ -1182,7 +1231,7 @@ public class Interscene {
use_lma_dsi, // clt_parameters.imp.use_lma_dsi, use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers, 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 fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
...@@ -1249,6 +1298,17 @@ public class Interscene { ...@@ -1249,6 +1298,17 @@ public class Interscene {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer 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 null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); 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; return earliest_scene;
} }
...@@ -1919,6 +1979,7 @@ public class Interscene { ...@@ -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_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 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 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; // int avg_len = clt_parameters.imp.avg_len;
// Set up velocities from known coordinates, use averaging // Set up velocities from known coordinates, use averaging
...@@ -2091,6 +2152,28 @@ public class Interscene { ...@@ -2091,6 +2152,28 @@ public class Interscene {
last_scene); // int last_index) last_scene); // int last_index)
// } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) { // } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
} else if (readjust_xy_ims) { } 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( double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs, quadCLTs, // final QuadCLT[] quadCLTs,
...@@ -2146,6 +2229,7 @@ public class Interscene { ...@@ -2146,6 +2229,7 @@ public class Interscene {
avg_z); // double avg_z avg_z); // double avg_z
} }
} }
*/
// old version // old version
/* /*
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities( scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
...@@ -2287,6 +2371,18 @@ public class Interscene { ...@@ -2287,6 +2371,18 @@ public class Interscene {
System.out.println("test_adjust = "+test_adjust); 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) { for (int nscene:scene_seq) {
if (nscene == debug_scene) { if (nscene == debug_scene) {
...@@ -2351,6 +2447,26 @@ public class Interscene { ...@@ -2351,6 +2447,26 @@ public class Interscene {
if (est_shift < min_max[0]) { if (est_shift < min_max[0]) {
fail_reason[0]=FAIL_REASON_MIN; fail_reason[0]=FAIL_REASON_MIN;
} else { } 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 scenes_xyzatr[nscene] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, //CLTParameters clt_parameters, clt_parameters, //CLTParameters clt_parameters,
use_lma_dsi, //,boolean use_lma_dsi, use_lma_dsi, //,boolean use_lma_dsi,
...@@ -2371,7 +2487,7 @@ public class Interscene { ...@@ -2371,7 +2487,7 @@ public class Interscene {
scenes_xyzatr[ref_index],// double [][] scene0_xyzatr, scenes_xyzatr[ref_index],// double [][] scene0_xyzatr,
scenes_xyzatr[nscene], // double [][] scene1_xyzatr, scenes_xyzatr[nscene], // double [][] scene1_xyzatr,
avg_z, // double average_z, 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_select, // boolean[] param_select,
param_regweights, // double [] param_regweights, param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2] lma_rms, // double [] rms_out, // null or double [2]
...@@ -2381,6 +2497,22 @@ public class Interscene { ...@@ -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 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) clt_parameters.imp.debug_level); // int debugLevel)
adjust_OK = scenes_xyzatr[nscene] != null; 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 (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) { if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
...@@ -2460,6 +2592,9 @@ public class Interscene { ...@@ -2460,6 +2592,9 @@ public class Interscene {
System.out.println("num_fpn_mitigate= "+fpn_list.size()); System.out.println("num_fpn_mitigate= "+fpn_list.size());
} }
if (fmg_reorient_en && !fpn_list.isEmpty()) { if (fmg_reorient_en && !fpn_list.isEmpty()) {
if (fmg_distance < (min_max[0] + 2)) {
fmg_distance = min_max[0] + 2;
}
int [][] fpn_pairs = getFPNPairs( int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list, fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist, fmg_distance, // double fpn_mitigate_dist,
...@@ -2469,6 +2604,15 @@ public class Interscene { ...@@ -2469,6 +2604,15 @@ public class Interscene {
avg_z, // double avg_z, avg_z, // double avg_z,
last_scene, // latest_scene, // int ref_index, // >= earliest_scene last_scene, // latest_scene, // int ref_index, // >= earliest_scene
earliest_scene); // int 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; boolean test_adjust1 = debugLevel > 1000;
if (test_adjust1) { if (test_adjust1) {
int [][] fpn_pairs_dbg = new int [fpn_pairs.length+3][2]; int [][] fpn_pairs_dbg = new int [fpn_pairs.length+3][2];
...@@ -2503,7 +2647,7 @@ public class Interscene { ...@@ -2503,7 +2647,7 @@ public class Interscene {
use_lma_dsi, //,boolean use_lma_dsi, use_lma_dsi, //,boolean use_lma_dsi,
fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close fpn_disable, // boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers, // boolean disable_ers, 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 fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
...@@ -2624,6 +2768,31 @@ public class Interscene { ...@@ -2624,6 +2768,31 @@ public class Interscene {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); debugLevel+1);
// 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; return earliest_scene;
} }
...@@ -2644,11 +2813,13 @@ public class Interscene { ...@@ -2644,11 +2813,13 @@ public class Interscene {
){ ){
double dx = new_xyz[0] - cur_xyzatr[0][0]; double dx = new_xyz[0] - cur_xyzatr[0][0];
double dy = new_xyz[1] - cur_xyzatr[0][1]; 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 [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr); double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
return new_xyzatr; return new_xyzatr;
} }
// Make it the only entry point // Make it the only entry point
...@@ -5186,9 +5357,9 @@ public class Interscene { ...@@ -5186,9 +5357,9 @@ public class Interscene {
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]"); System.out.println("generateEgomotionTable(): quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]"); System.out.println("generateEgomotionTable(): ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]"); System.out.println("generateEgomotionTable(): ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
} }
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) { for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
...@@ -5544,7 +5715,7 @@ public class Interscene { ...@@ -5544,7 +5715,7 @@ public class Interscene {
} }
} }
// relative to the GPS/compass
public static double [] getQuaternionCorrection( public static double [] getQuaternionCorrection(
CLTParameters clt_parameters, CLTParameters clt_parameters,
QuadCLT [] quadCLTs, QuadCLT [] quadCLTs,
......
...@@ -118,6 +118,7 @@ public class IntersceneMatchParameters { ...@@ -118,6 +118,7 @@ public class IntersceneMatchParameters {
public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted 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_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_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 adjust_gyro = false; // adjust qyro omegas offsets
public boolean apply_gyro = true; // apply adjusted qyro omegas offsets public boolean apply_gyro = true; // apply adjusted qyro omegas offsets
public boolean adjust_accl = false; // adjust IMU velocities scales public boolean adjust_accl = false; // adjust IMU velocities scales
...@@ -386,7 +387,9 @@ public class IntersceneMatchParameters { ...@@ -386,7 +387,9 @@ public class IntersceneMatchParameters {
public double max_zoom_diff = 0; // for down-views when changing altitude (0 - ignore) 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_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 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 // 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 public boolean mov_en = true; // enable detection/removal of the moving objects during pose matching
...@@ -673,12 +676,14 @@ public class IntersceneMatchParameters { ...@@ -673,12 +676,14 @@ public class IntersceneMatchParameters {
"Minimal required number of re-calculations of the interscene-accumulated DSI."); "Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient, gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
"Adjust IMU misalignment to the camera."); "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."); "Apply IMU misalignment to the camera if adjusted.");
gd.addCheckbox ("Use translation for IMU orientation", this.orient_by_move, gd.addCheckbox ("Use translation for IMU orientation", this.orient_by_move,
"Use translation data to adjust IMU orientation ."); "Use translation data to adjust IMU orientation .");
gd.addCheckbox ("Use rotation for IMU orientation", this.orient_by_rot, gd.addCheckbox ("Use rotation for IMU orientation", this.orient_by_rot,
"Use rotation data to adjust IMU orientation."); "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, gd.addCheckbox ("Adjust gyro offsets", this.adjust_gyro,
"Adjust qyro omegas offsets."); "Adjust qyro omegas offsets.");
gd.addCheckbox ("Apply gyro offsets", this.apply_gyro, gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
...@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters { ...@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters {
"Match fpn-failed scenes to later scenes with larger difference."); "Match fpn-failed scenes to later scenes with larger difference.");
gd.addCheckbox ("Refine inversion", this.refine_invert, gd.addCheckbox ("Refine inversion", this.refine_invert,
"Refine with LMA while inverting relative poses from other reference."); "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.addMessage ("Detect and remove moving objects from pose matching");
gd.addCheckbox ("Enable movement detection/elimination", this.mov_en, gd.addCheckbox ("Enable movement detection/elimination", this.mov_en,
...@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters { ...@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters {
this.apply_imu_orient = gd.getNextBoolean(); this.apply_imu_orient = gd.getNextBoolean();
this.orient_by_move = gd.getNextBoolean(); this.orient_by_move = gd.getNextBoolean();
this.orient_by_rot = gd.getNextBoolean(); this.orient_by_rot = gd.getNextBoolean();
this.orient_combo = gd.getNextBoolean();
this.adjust_gyro = gd.getNextBoolean(); this.adjust_gyro = gd.getNextBoolean();
this.apply_gyro = gd.getNextBoolean(); this.apply_gyro = gd.getNextBoolean();
this.adjust_accl = gd.getNextBoolean(); this.adjust_accl = gd.getNextBoolean();
...@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters { ...@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters {
this.fpn_skip = gd.getNextBoolean(); this.fpn_skip = gd.getNextBoolean();
this.fpn_rematch = gd.getNextBoolean(); this.fpn_rematch = gd.getNextBoolean();
this.refine_invert = gd.getNextBoolean(); this.refine_invert = gd.getNextBoolean();
this.use_precomp = gd.getNextBoolean();
this.mov_en = gd.getNextBoolean(); this.mov_en = gd.getNextBoolean();
this.mov_sigma = gd.getNextNumber(); this.mov_sigma = gd.getNextNumber();
...@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters { ...@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"apply_imu_orient", this.apply_imu_orient+""); // boolean 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_move", this.orient_by_move+""); // boolean
properties.setProperty(prefix+"orient_by_rot", this.orient_by_rot+""); // 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+"adjust_gyro", this.adjust_gyro+""); // boolean
properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean
properties.setProperty(prefix+"adjust_accl", this.adjust_accl+""); // boolean properties.setProperty(prefix+"adjust_accl", this.adjust_accl+""); // boolean
...@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters { ...@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"fpn_skip", this.fpn_skip+""); // boolean properties.setProperty(prefix+"fpn_skip", this.fpn_skip+""); // boolean
properties.setProperty(prefix+"fpn_rematch", this.fpn_rematch+""); // boolean properties.setProperty(prefix+"fpn_rematch", this.fpn_rematch+""); // boolean
properties.setProperty(prefix+"refine_invert", this.refine_invert+""); // 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_en", this.mov_en+""); // boolean
properties.setProperty(prefix+"mov_sigma", this.mov_sigma+""); // double properties.setProperty(prefix+"mov_sigma", this.mov_sigma+""); // double
...@@ -2345,6 +2356,7 @@ public class IntersceneMatchParameters { ...@@ -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+"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_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_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+"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+"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")); if (properties.getProperty(prefix+"adjust_accl")!=null) this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
...@@ -2588,6 +2600,7 @@ public class IntersceneMatchParameters { ...@@ -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_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+"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+"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_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")); if (properties.getProperty(prefix+"mov_sigma")!=null) this.mov_sigma=Double.parseDouble(properties.getProperty(prefix+"mov_sigma"));
...@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters { ...@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters {
imp.apply_imu_orient = this.apply_imu_orient; imp.apply_imu_orient = this.apply_imu_orient;
imp.orient_by_move = this.orient_by_move; imp.orient_by_move = this.orient_by_move;
imp.orient_by_rot = this.orient_by_rot; imp.orient_by_rot = this.orient_by_rot;
imp.orient_combo = this.orient_combo;
imp.adjust_gyro = this.adjust_gyro; imp.adjust_gyro = this.adjust_gyro;
imp.apply_gyro = this.apply_gyro; imp.apply_gyro = this.apply_gyro;
imp.adjust_accl = this.adjust_accl; imp.adjust_accl = this.adjust_accl;
...@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters { ...@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters {
imp.fpn_skip = this.fpn_skip; imp.fpn_skip = this.fpn_skip;
imp.fpn_rematch = this.fpn_rematch; imp.fpn_rematch = this.fpn_rematch;
imp.refine_invert = this.refine_invert; imp.refine_invert = this.refine_invert;
imp.use_precomp = this.use_precomp;
imp.mov_en = this.mov_en; imp.mov_en = this.mov_en;
imp.mov_sigma = this.mov_sigma; imp.mov_sigma = this.mov_sigma;
......
...@@ -29,8 +29,10 @@ import java.awt.Rectangle; ...@@ -29,8 +29,10 @@ import java.awt.Rectangle;
import java.io.File; import java.io.File;
import java.io.IOException; import java.io.IOException;
import java.security.NoSuchAlgorithmException; import java.security.NoSuchAlgorithmException;
import java.text.SimpleDateFormat;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections; import java.util.Collections;
import java.util.Comparator; import java.util.Comparator;
import java.util.List; import java.util.List;
...@@ -5426,15 +5428,30 @@ public class OpticalFlow { ...@@ -5426,15 +5428,30 @@ public class OpticalFlow {
} }
} }
} }
// quadCLTs[ref_index].getSmoothGround(clt_parameters);
// later move to the right place // later move to the right place
if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) { if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
QuadCLT.adjustImuOrient( boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters, QuadCLT.adjustImuOrient(
quadCLTs, // QuadCLT[] quadCLTs, clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
ref_index, // int ref_index, orient_combo, // boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
earliest_scene, // int earliest_scene, quadCLTs, // QuadCLT[] quadCLTs,
last_index, // int last_index, ref_index, // int ref_index,
debugLevel); // int debugLevel 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 (run_ly) {
if (debugLevel > -3) { if (debugLevel > -3) {
...@@ -5548,15 +5565,20 @@ public class OpticalFlow { ...@@ -5548,15 +5565,20 @@ public class OpticalFlow {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); debugLevel+1);
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) { 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.print(sb.toString());
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]+"]");
} }
} else { } else {
if (debugLevel> -3) { if (debugLevel> -3) {
......
...@@ -27,28 +27,25 @@ package com.elphel.imagej.tileprocessor; ...@@ -27,28 +27,25 @@ package com.elphel.imagej.tileprocessor;
//import java.awt.Polygon; //import java.awt.Polygon;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation; 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 org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import java.awt.Rectangle; import java.awt.Rectangle;
import java.io.BufferedWriter;
import java.io.File; import java.io.File;
import java.io.FileInputStream; import java.io.FileInputStream;
import java.io.FileNotFoundException; import java.io.FileNotFoundException;
import java.io.FileOutputStream; import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException; import java.io.IOException;
import java.io.InputStream; import java.io.InputStream;
import java.io.OutputStream; import java.io.OutputStream;
import java.nio.charset.Charset; import java.text.SimpleDateFormat;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.nio.file.attribute.PosixFilePermission;
import java.time.LocalDateTime; import java.time.LocalDateTime;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections; import java.util.Collections;
import java.util.Comparator; import java.util.Comparator;
import java.util.Enumeration; import java.util.Enumeration;
import java.util.List;
import java.util.Properties; import java.util.Properties;
import java.util.Random; import java.util.Random;
import java.util.Set; import java.util.Set;
...@@ -99,6 +96,9 @@ import loci.formats.FormatException; ...@@ -99,6 +96,9 @@ import loci.formats.FormatException;
public class QuadCLTCPU { 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 final String [] DSI_SUFFIXES = {"-INTER-INTRA-LMA","-INTER-INTRA","-DSI_MAIN"};
public static int INDEX_INTER_LMA = 0; public static int INDEX_INTER_LMA = 0;
public static int INDEX_INTER = 1; public static int INDEX_INTER = 1;
...@@ -193,6 +193,7 @@ public class QuadCLTCPU { ...@@ -193,6 +193,7 @@ public class QuadCLTCPU {
public Did_gps_pos did_gps1_ubx_pos = null; public Did_gps_pos did_gps1_ubx_pos = null;
public String ims_last_path = null; public String ims_last_path = null;
public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames) public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames)
@Deprecated
public double [][] pimu_offsets = new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data) public double [][] pimu_offsets = new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames) // public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
...@@ -212,11 +213,11 @@ public class QuadCLTCPU { ...@@ -212,11 +213,11 @@ public class QuadCLTCPU {
public void setQuatCorr(double[] quat) { public void setQuatCorr(double[] quat) {
quat_corr = quat; quat_corr = quat;
} }
@Deprecated
public double [][] getPimuOffsets() { public double [][] getPimuOffsets() {
return pimu_offsets; return pimu_offsets;
} }
@Deprecated
public void setPimuOffsets(double[][] offsets) { // never public void setPimuOffsets(double[][] offsets) { // never
pimu_offsets = offsets; pimu_offsets = offsets;
} }
...@@ -475,11 +476,12 @@ public class QuadCLTCPU { ...@@ -475,11 +476,12 @@ public class QuadCLTCPU {
public static void adjustImuOrient( public static void adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters, CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[] quadCLTs, QuadCLT[] quadCLTs,
int ref_index, int ref_index,
int earliest_scene, int earliest_scene,
int last_index, 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_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 boolean orient_by_rot = clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
...@@ -487,13 +489,14 @@ public class QuadCLTCPU { ...@@ -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"); System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
return; 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 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 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 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 adjust_accl = clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
boolean apply_accl = clt_parameters.imp.apply_accl; // apply 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_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( double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs, quadCLTs, // final QuadCLT[] quadCLTs,
...@@ -510,8 +513,7 @@ public class QuadCLTCPU { ...@@ -510,8 +513,7 @@ public class QuadCLTCPU {
} }
double [] rms = new double[5]; double [] rms = new double[5];
double [] quat = new double[4]; 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 = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: 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 debug_lev = debugLevel; // 3; int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0); double translation_weight = 1.0 / (avg_z + 1.0);
...@@ -536,47 +538,59 @@ public class QuadCLTCPU { ...@@ -536,47 +538,59 @@ public class QuadCLTCPU {
debug_lev); // int debugLevel debug_lev); // int debugLevel
if (rotated_xyzatr != null) { if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled 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 [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat( double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr, ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q) 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) { if (apply_imu_orient) {
for (int i = 0; i < new_ims_mount_atr.length; i++) { for (int i = 0; i < new_ims_mount_atr.length; i++) {
if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) { if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
apply_imu_orient = false; apply_imu_orient = false;
if (debugLevel > -3) { sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
System.out.println ("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+" exceeds the specified limit ("+quat_max_change+")"); " exceeds the specified limit ("+quat_max_change+")\n");
System.out.println ("*** Orientation update is disabled."); sb.append("*** Orientation update is disabled.\n");
}
} }
} }
} }
if (apply_imu_orient) { if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr); clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) { sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
}
} else { } 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 [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3]; double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI; for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
System.out.println(String.format( sb.append(String.format(
"Using ATR(rad)=[%9f, %9f, %9f]", new_atr[0], new_atr[1], new_atr[2])); "Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
System.out.println(String.format( sb.append(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]", degrees[0], degrees[1], degrees[2])); "Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
double [] omega_corr = null; double [] omega_corr = null;
if (adjust_gyro) { if (adjust_gyro) {
omega_corr = getOmegaCorrections( omega_corr = getOmegaCorrections(
...@@ -593,30 +607,28 @@ public class QuadCLTCPU { ...@@ -593,30 +607,28 @@ public class QuadCLTCPU {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
new_omegas[i] = used_omegas[i] - omega_corr[i]; new_omegas[i] = used_omegas[i] - omega_corr[i];
} }
if (debugLevel > -1) { sb.append(String.format(
System.out.println(String.format( "Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]", used_omegas[0],used_omegas[1],used_omegas[2])); used_omegas[0],used_omegas[1],used_omegas[2]));
System.out.println(String.format( sb.append(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]", omega_corr[0], omega_corr[1], omega_corr[2])); "Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
System.out.println(String.format( omega_corr[0], omega_corr[1], omega_corr[2]));
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[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) { if (apply_gyro) {
clt_parameters.imp.set_pimu_omegas(new_omegas); clt_parameters.imp.set_pimu_omegas(new_omegas);
if (debugLevel > -3) { sb.append(String.format(
System.out.println(String.format( "Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2])); new_omegas[0], new_omegas[1], new_omegas[2]));
System.out.println ("*** Need to save the main configuration file ***"); sb.append("*** Need to save the main configuration file ***\n");
}
} else { } else {
if (debugLevel > -3) { sb.append(String.format(
System.out.println(String.format( "New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2])); new_omegas[0], new_omegas[1], new_omegas[2]));
}
} }
} else { } else {
System.out.println("*** Adjustment of the gyro omegas failed ***"); sb.append("*** Adjustment of the gyro omegas failed ***\n");
} }
} }
double [] acc_corr = null; double [] acc_corr = null;
...@@ -639,35 +651,41 @@ public class QuadCLTCPU { ...@@ -639,35 +651,41 @@ public class QuadCLTCPU {
new_accl_corr[i] = used_accl_corr[i] * acc_corr[i]; new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
num_corr++; num_corr++;
} }
if (debugLevel > -1) { sb.append(String.format(
System.out.println(String.format( "Used velocities scales = [%9f, %9f, %9f]\n",
"Used velocities scales = [%9f, %9f, %9f]", used_accl_corr[0],used_accl_corr[1],used_accl_corr[2])); used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
System.out.println(String.format( sb.append(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]", acc_corr[0], acc_corr[1], acc_corr[2])); "Diff.velocities scales = [%9f, %9f, %9f]\n",
System.out.println(String.format( acc_corr[0], acc_corr[1], acc_corr[2]));
"New velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_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)) { if (apply_accl && (num_corr>0)) {
clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr); clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
if (debugLevel > -3) { sb.append(String.format(
System.out.println(String.format( "Applied new velocities scales = [%9f, %9f, %9f]\n",
"Applied new velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2])); new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
System.out.println ("*** Need to save the main configuration file ***"); sb.append("*** Need to save the main configuration file ***\n");
}
} else { } else {
if (debugLevel > -3) { sb.append(String.format(
System.out.println(String.format( "New velocities scales are not applied = [%9f, %9f, %9f]\n",
"New velocities scales are not applied = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2])); new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
} }
} else { } 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 { } else {
System.out.println ("*** Failed to calculate IMS mount correction! ***"); System.out.println ("*** Failed to calculate IMS mount correction! ***");
} }
} }
public static double [] getVelocitiesCorrections( public static double [] getVelocitiesCorrections(
...@@ -806,6 +824,93 @@ public class QuadCLTCPU { ...@@ -806,6 +824,93 @@ public class QuadCLTCPU {
return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]}; 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 * Refine scene poses (now only linear) from currently adjusted poses
...@@ -1576,10 +1681,285 @@ public class QuadCLTCPU { ...@@ -1576,10 +1681,285 @@ public class QuadCLTCPU {
whs, // initialize to int[3] to return {width, height, scale reduction} whs, // initialize to int[3] to return {width, height, scale reduction}
debug_level); debug_level);
} }
}
public double [] getSmoothGround(final CLTParameters clt_parameters) {
} final boolean use_lma = true; // false, or there will be too many gaps?
public double [][] getGroundNoIms( 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, CLTParameters clt_parameters,
boolean use_lma, boolean use_lma,
boolean use_parallel_proj, boolean use_parallel_proj,
...@@ -4383,6 +4763,24 @@ public class QuadCLTCPU { ...@@ -4383,6 +4763,24 @@ public class QuadCLTCPU {
return imp; 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( public ImagePlus saveDoubleArrayInModelDirectory(
String suffix, String suffix,
...@@ -10712,15 +11110,15 @@ public class QuadCLTCPU { ...@@ -10712,15 +11110,15 @@ public class QuadCLTCPU {
System.out.println("No IMS orientation correction is defined"); System.out.println("No IMS orientation correction is defined");
} }
} }
public void showPimuOffsets() { public static void showPimuOffsets(CLTParameters clt_parameters) {
showPimuOffsets(getPimuOffsets()); showPimuOffsets( clt_parameters.imp.get_pimu_offs());
} }
public static void showPimuOffsets(double [][] pimu_offsets) { public static void showPimuOffsets(double [][] pimu_offsets) {
if (pimu_offsets != null) { 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]+"]"); 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]+"]"); pimu_offsets[1][0]+", " + pimu_offsets[1][1]+", "+ + pimu_offsets[1][2]+"]");
} else { } else {
System.out.println("No PIMU offsets are defined"); System.out.println("No PIMU offsets are defined");
......
...@@ -35,10 +35,10 @@ import Jama.Matrix; ...@@ -35,10 +35,10 @@ import Jama.Matrix;
public class QuaternionLma { public class QuaternionLma {
private final static int REGLEN = 1; // number of extra (regularization) samples private final static int REGLEN = 1; // number of extra (regularization) samples
public static final int MODE_XYZ = 0; 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_COMBO = 2;
public static final int MODE_XYZQ_LOCAL = 3; 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_COMPASS = 5;
public static final int MODE_XYZ4Q3 = 6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation public static final int MODE_XYZ4Q3 = 6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
...@@ -67,6 +67,7 @@ public class QuaternionLma { ...@@ -67,6 +67,7 @@ public class QuaternionLma {
private double [] last_ymfx = null; private double [] last_ymfx = null;
private double [][] last_jt = null; private double [][] last_jt = null;
private double [] axis = null; private double [] axis = null;
private double[] dbg_data;
public double [] getQuaternion() { public double [] getQuaternion() {
if (parameters_vector.length == 3) { if (parameters_vector.length == 3) {
...@@ -673,10 +674,10 @@ public class QuaternionLma { ...@@ -673,10 +674,10 @@ public class QuaternionLma {
jt, // final double [][] jt, // should be null or initialized with [vector.length][] jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level) debug_level); // final int debug_level)
} else { } else {
return getFxDerivs6Dof( return getFxDerivs6Dof(
vector, // double [] vector, vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][] jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level) debug_level); // final int debug_level)
} }
case MODE_COMBO:return getFxDerivsVisual( // fill change case MODE_COMBO:return getFxDerivsVisual( // fill change
vector, // double [] vector, vector, // double [] vector,
...@@ -748,7 +749,7 @@ public class QuaternionLma { ...@@ -748,7 +749,7 @@ public class QuaternionLma {
double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2); double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2);
//axis //axis
double [] fx = new double [weights.length]; 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}; double [] dq_dv = new double [] {-s/2, c*axis[0]/2, c*axis[1]/2, c*axis[2]/2};
if (jt != null) { if (jt != null) {
for (int i = 0; i < vector.length; i++) { for (int i = 0; i < vector.length; i++) {
...@@ -759,6 +760,14 @@ public class QuaternionLma { ...@@ -759,6 +760,14 @@ public class QuaternionLma {
double [][] xyz_dq; double [][] xyz_dq;
for (int i = 0; i < N; i++) { for (int i = 0; i < N; i++) {
int i3 = 3 * 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]}; final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
xyz_rot = applyTo(q, xyz); xyz_rot = applyTo(q, xyz);
System.arraycopy(xyz_rot, 0, fx, i3, 3); System.arraycopy(xyz_rot, 0, fx, i3, 3);
...@@ -922,7 +931,7 @@ public class QuaternionLma { ...@@ -922,7 +931,7 @@ public class QuaternionLma {
} }
return fx; return fx;
} }
private double [] getFxDerivs6DofMode33( private double [] getFxDerivs6DofMode33( // MODE_XYZQ_LOCAL = 3; // OK with [3]
double [] vector, // double [] vector, //
final double [][] jt, // should be null or initialized with [vector.length][] final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) { final int debug_level) {
...@@ -933,6 +942,7 @@ public class QuaternionLma { ...@@ -933,6 +942,7 @@ public class QuaternionLma {
final double q0 = getQ0(vector); final double q0 = getQ0(vector);
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3}); // final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
final double [] vector_r = new double[] { q0,q1,q2,q3}; final double [] vector_r = new double[] { q0,q1,q2,q3};
if (jt != null) { if (jt != null) {
for (int i = 0; i < vector.length; i++) { for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length]; jt[i] = new double [weights.length];
...@@ -1084,7 +1094,7 @@ public class QuaternionLma { ...@@ -1084,7 +1094,7 @@ public class QuaternionLma {
return fx; 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, // double [] vector3, //
final double [][] jt, // should be null or initialized with [vector.length][] final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) { final int debug_level) {
...@@ -1428,10 +1438,14 @@ public class QuaternionLma { ...@@ -1428,10 +1438,14 @@ public class QuaternionLma {
return fx; return fx;
} }
private double [] getFxDerivsVisualMode43( private double [] getFxDerivsVisualMode43( // tested MODE_COMBO_LOCAL = 4; // OK with [3]
double [] vector3, double [] vector3,
final double [][] jt, // should be null or initialized with [vector.length][] final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) { 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 [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
double [] fx = new double [weights.length]; double [] fx = new double [weights.length];
double [] qn = new double[4]; double [] qn = new double[4];
...@@ -1476,12 +1490,15 @@ public class QuaternionLma { ...@@ -1476,12 +1490,15 @@ public class QuaternionLma {
fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3 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 + 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 + 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) {
fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height; // Z dbg_data[i7 + 0] = comb_y[0][0]/ height;
fx[i4 + 1] = -2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3 dbg_data[i7 + 1] = comb_y[0][1]/ height;
fx[i4 + 2] = -2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height dbg_data[i7 + 2] = comb_y[0][2]/ 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 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) { if (jt != null) {
xyz_dq = applyToDQ(vector, xyz); xyz_dq = applyToDQ(vector, xyz);
double [][] xyz_dq_local = new double [xyz_dq.length][]; double [][] xyz_dq_local = new double [xyz_dq.length][];
...@@ -1719,17 +1736,21 @@ public class QuaternionLma { ...@@ -1719,17 +1736,21 @@ public class QuaternionLma {
parameters_vector, // double [] vector, parameters_vector, // double [] vector,
null, // final double [][] jt, // should be null or initialized with [vector.length][] null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level) debug_level); // final int debug_level)
debugYfX ( "fx-", // String pfx, debugYfX ( "fx-", // String pfx,
fx); // double [] data) fx); // double [] data)
if (debug_level > 1) { if (debug_level > 2) {
double delta = 1E-5; debugYfX ( "ffx-", // String pfx,
System.out.println("\n\n"); dbg_data); // double [] data)
double err = compareJT( }
parameters_vector, // double [] vector, if (debug_level > 1) {
delta); // double delta); double delta = 1E-5;
System.out.println("Maximal error = "+err); 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; return rslt[0]? iter : -1;
} }
...@@ -1773,6 +1794,11 @@ public class QuaternionLma { ...@@ -1773,6 +1794,11 @@ public class QuaternionLma {
debugYfX ( "fx0-", // String pfx, debugYfX ( "fx0-", // String pfx,
fx); // double [] data) fx); // double [] data)
} }
if (debug_level > 2) {
debugYfX ( "ffx0-", // String pfx,
dbg_data); // double [] data)
}
if (debug_level > 1) { if (debug_level > 1) {
double delta = 1E-5; double delta = 1E-5;
System.out.println("\n\n"); System.out.println("\n\n");
...@@ -1919,6 +1945,9 @@ public class QuaternionLma { ...@@ -1919,6 +1945,9 @@ public class QuaternionLma {
public void debugYfX ( public void debugYfX (
String pfx, String pfx,
double [] data) { double [] data) {
if (data == null) {
return;
}
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...] // if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) { if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) {
System.out.println(String.format("%3s"+ System.out.println(String.format("%3s"+
......
...@@ -2576,6 +2576,7 @@ public class TexturedModel { ...@@ -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; 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? // currently conditionInitialDS() zeroes disparity for blue_sky. TODO: allow some FG over blue_sky?
// gets Blue Sky from scene.dsi , not from the file! // gets Blue Sky from scene.dsi , not from the file!
double [][] ds_fg = OpticalFlow.conditionInitialDS( double [][] ds_fg = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
...@@ -3165,7 +3166,7 @@ public class TexturedModel { ...@@ -3165,7 +3166,7 @@ public class TexturedModel {
hdr_whs[0], // int width, // int tilesX, hdr_whs[0], // int width, // int tilesX,
hdr_whs[1]); // int height, // int tilesY, 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, clt_parameters, // final CLTParameters clt_parameters,
cropped_z[0], // double [] data, cropped_z[0], // double [] data,
top_left_lla, // double [] lla, // latitude, longitude, altitude (or null) 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