Skip to content

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

Implemented IMU-only scene sequence orientation

parent ba487fc2
Hide whitespace changes
Inline Side-by-side
Showing with 243 additions and 35 deletions
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ 8406092c
......@@ -5086,6 +5086,8 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
}
QUAD_CLT_AUX.showExtrinsicCorr("aux");// show_fine_corr("aux");
QUAD_CLT_AUX.geometryCorrection.showRig();// show_fine_corr("aux");
System.out.println("=== IMS ===");
QUAD_CLT_AUX.showQuatCorr();
@SuppressWarnings("unused")
QuadCLT dbg_QUAD_CLT = QUAD_CLT;
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 8406092c
......@@ -444,13 +444,34 @@ public class Interscene {
}
boolean scale_ims_velocities = false; // true;
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // does not depend on correction
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
scale_ims_velocities); // boolean scale)
double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; //
// quat_corr may still be null if does not exist
if (debugLevel > -3) {
System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation);
System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+" correct orientation offset");
QuadCLTCPU.showQuatCorr(quat_corr);
}
boolean test_quat_corr = debugLevel > 1000;
if (test_quat_corr) {
testPIMU (
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
debugLevel); // final int debugLevel);
/*
quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
null, // double [] quat_corr, // only applies to rotations - verify!
debugLevel) ; // int debugLevel)
*/
}
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
......@@ -797,7 +818,50 @@ public class Interscene {
return earliest_scene;
}
public static void testPIMU(
final CLTParameters clt_parameters,
final QuadCLT[] quadCLTs,
final int ref_index,
final int debugLevel
) {
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
null, // double [] quat_corr, // only applies to rotations - verify!
debugLevel) ; // int debugLevel)
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
0, // final int early_index,
(quadCLTs.length -1), // int last_index,
(new double[2][3]) //final double [][] offsets // null - will not subtract from velocities
);
double [][][] dxyzatr = QuadCLT.getDxyzatrPIMU(
clt_parameters,
quadCLTs,
false);
double [] timestamps = new double [quadCLTs.length];
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
}
System.out.println("N\tTimestamp\timsX\timsY\timsZ\timsA\timsT\timsR"+
"\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR"+
"\tvelX\tvelY\tvelZ\tomegaA\tomegaT\tomegaR");
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
System.out.println(
nscene+"\t"+timestamps[nscene]+"\t"+
ims_xyzatr[nscene][0][0]+ "\t"+ims_xyzatr[nscene][0][1]+ "\t"+ims_xyzatr[nscene][0][2]+"\t"+
ims_xyzatr[nscene][1][0]+ "\t"+ims_xyzatr[nscene][1][1]+ "\t"+ims_xyzatr[nscene][1][2]+"\t"+
pimu_xyzatr[nscene][0][0]+"\t"+pimu_xyzatr[nscene][0][1]+"\t"+pimu_xyzatr[nscene][0][2]+"\t"+
pimu_xyzatr[nscene][1][0]+"\t"+pimu_xyzatr[nscene][1][1]+"\t"+pimu_xyzatr[nscene][1][2]+"\t"+
dxyzatr[nscene][0][0]+ "\t"+dxyzatr[nscene][0][1]+ "\t"+dxyzatr[nscene][0][2]+"\t"+
dxyzatr[nscene][1][0]+ "\t"+dxyzatr[nscene][1][1]+ "\t"+dxyzatr[nscene][1][2]+"\t");
}
System.out.println();
}
public static int [][] getFPNPairs(
ArrayList<Integer> fpn_list,
double fpn_mitigate_dist,
......@@ -1413,7 +1477,9 @@ public class Interscene {
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
boolean debug_ers = clt_parameters.ilp.ilma_debug_ers ; // true;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // for offset estimation
if (debugLevel > -3) {
System.out.println("reAdjustPairsLMAInterscene(): avg_z="+avg_z);
}
String dbg_ers_string =
((dbg_scale_dt[1][0] > 0)?"a":((dbg_scale_dt[1][0] < 0) ? "A":"0"))+
((dbg_scale_dt[1][1] > 0)?"t":((dbg_scale_dt[1][1] < 0) ? "T":"0"))+
......@@ -2042,11 +2108,30 @@ public class Interscene {
while (pull_offset > clt_parameters.imp.max_pull_jump) {
// so far assuming only X and Y to be modified. Z - won't harm, angles are not used now,
// but if yes - quaternions are needed. Note, that angles are updated and will not now match pull
// here updtae all parameters that are NOT adjusted. For angles use individual angles, just avoid
// here update all parameters that are NOT adjusted. For angles use individual angles, just avoid
// crossing 2pi
/// XYZ ErsCorrection.DP_DSX + i
for (int i = 0; i < 3; i++) if (!param_select[ErsCorrection.DP_DSX + i]){
scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
}
// Az [-pi,+pi), Tl [-pi/2,+pi/2), Rl[-pi.+pi], but only roll can cross +/-pi
for (int i = 0; i < 3; i++) if (!param_select[ErsCorrection.DP_DSAZ + i]){
if (i==2) {
double da = scene1_xyzatr_pull[1][i] - scene1_xyzatr[1][i];
if (da > Math.PI) da -= 2 * Math.PI;
else if (da < -Math.PI) da += 2 * Math.PI;
scene1_xyzatr[0][i] += da * clt_parameters.imp.max_pull_jump / pull_offset;
if (scene1_xyzatr[1][i] >= Math.PI) scene1_xyzatr[1][i] -= 2 * Math.PI;
else if (scene1_xyzatr[1][i] < -Math.PI) scene1_xyzatr[1][i] += 2 * Math.PI;
} else {
scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
}
}
/*
for (int i = 0; i < 2; i++) {
scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
}
*/
// apply small scene1_xyzatr jump towards scene1_xyzatr_pull, run adjustPairsLMAInterscene, return if failed
double [][] new_xyzatr = adjustPairsLMAInterscene( // assumes reference GPU data is already set - once for multiple scenes
clt_parameters, // CLTParameters clt_parameters,
......@@ -2086,8 +2171,16 @@ public class Interscene {
if (new_pull_offset < pull_offset) {
scene1_xyzatr = new_xyzatr;
// later - watch which parameters are updated by LMA, update those
scene1_xyzatr_pull[1] = new_xyzatr[1]; // update adjusted orientation
scene1_xyzatr_pull[0][2] = new_xyzatr[0][2];// update Z
for (int i = 0; i < 3; i++) {
if (param_select[ErsCorrection.DP_DSX + i]){
scene1_xyzatr_pull[0][i] = scene1_xyzatr[0][i];
}
if (param_select[ErsCorrection.DP_DSAZ + i]){
scene1_xyzatr_pull[1][i] = scene1_xyzatr[1][i];
}
}
// scene1_xyzatr_pull[1] = new_xyzatr[1]; // update adjusted orientation
// scene1_xyzatr_pull[0][2] = new_xyzatr[0][2];// update Z
pull_offset = new_pull_offset;
if (debugLevel > -3) {
System.out.println("adjustDiffPairsLMAInterscene(): new pull_offset="+pull_offset);
......@@ -4442,7 +4535,7 @@ public class Interscene {
ims_ortho);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms( // velocities and omegas from IMU
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // velocities and omegas from IMU
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
false); // boolean scale)
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 8406092c
......@@ -57,15 +57,15 @@ public class IntersceneMatchParameters {
public double sfm_min_gain = 5.0; // Minimal SfM gain to apply SfM to the depth map
public double sfm_min_frac = 0.5; // Minimal fraction of defined tiles to have SfM correction
public int sfm_num_pairs = 16; // desired number of SfM pairs to average
public int sfm_num_pairs = 32; // desired number of SfM pairs to average
public double sfp_tolerance = 0.05; // average SfM pairs if their baselines differ less
public int sfm_readjust = 5; // number of SfM readjustment cycles
public double sfm_prev_frac = 0.6; // update if new sfm gain > this fraction of the old one
public double sfm_same_weight = 0.8; // correction weight when new SfM gain is the same as the old one
public int sfm_shrink = 2; // shrink sfm gain area before applying sfm_fade_sigma
public double sfm_fade_sigma = 3.0; // fade SfM gains at the edges
public double sfm_min_str1 = 0.2; // update if correction strength exceeds
public double sfm_min_str16 = 0.4; // update if correction strength exceeds (for >=16 pairs)
public double sfm_min_str1 = 0.15; // 0.22; // update if correction strength exceeds
public double sfm_min_str16 = 0.22; // 0.4; // update if correction strength exceeds (for >=16 pairs)
public boolean sfm_use_neibs = true; // use neighbors if individual corr is too weak
public double sfm_neib_too_str1= 0.3; // do not count neighbors stronger than that
public double sfm_neib_too_str16= 0.4; // do not count neighbors stronger than that (for >=16 pairs)
......@@ -95,6 +95,7 @@ public class IntersceneMatchParameters {
public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
public boolean use_quat_corr = true; // Use orientation correction everywhere if available
public boolean generate_egomotion = true; // generate egomotion table (image-based and ims)
public boolean generate_mapped = true; // generate any of the sequences - Tiff, video, mono or stereo
......@@ -569,10 +570,12 @@ public class IntersceneMatchParameters {
"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
"Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
"Calculate camera orientation correction from predicted by IMS .");
gd.addCheckbox ("Apply IMS orientation correction", this.apply_quat_corr,
"Apply camera orientation correction from predicted by IMS.");
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
"Apply camera orientation correction from predicted by IMS when generating output files.");
gd.addCheckbox ("Use IMS orientation correction internally", this.use_quat_corr,
"Use camera orientation correction from predicted by IMS internally from the previous calculations.");
gd.addMessage ("Generate/show scene sequences");
gd.addCheckbox ("Generate egomotion table", this.generate_egomotion,
......@@ -1288,6 +1291,7 @@ public class IntersceneMatchParameters {
this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
this.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean();
this.use_quat_corr = gd.getNextBoolean();
this.generate_egomotion = gd.getNextBoolean();
this.generate_mapped = gd.getNextBoolean();
this.reuse_video = gd.getNextBoolean();
......@@ -1683,6 +1687,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean
properties.setProperty(prefix+"generate_mapped", this.generate_mapped+""); // boolean
properties.setProperty(prefix+"reuse_video", this.reuse_video+""); // boolean
......@@ -2042,6 +2047,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
if (properties.getProperty(prefix+"generate_egomotion")!=null) this.generate_egomotion=Boolean.parseBoolean(properties.getProperty(prefix+"generate_egomotion"));
if (properties.getProperty(prefix+"generate_mapped")!=null) this.generate_mapped=Boolean.parseBoolean(properties.getProperty(prefix+"generate_mapped"));
if (properties.getProperty(prefix+"reuse_video")!=null) this.reuse_video=Boolean.parseBoolean(properties.getProperty(prefix+"reuse_video"));
......@@ -2428,6 +2434,7 @@ public class IntersceneMatchParameters {
imp.min_num_interscene = this.min_num_interscene;
imp.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr;
imp.use_quat_corr = this.use_quat_corr;
imp.generate_egomotion = this.generate_egomotion;
imp.generate_mapped = this.generate_mapped;
imp.reuse_video = this.reuse_video;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 8406092c
......@@ -4655,8 +4655,10 @@ public class OpticalFlow {
save_mapped_mono_color[1] || gen_avi_mono_color[1] || show_mono_color[1]} ;
// skip completely if no color or mono, tiff or video
boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
// apply_quat_corr used inside getGroundIns() - used when generating output
// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
boolean use_ims_rotation = clt_parameters.imp.use_quat_corr; // use internally
boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
boolean generate_mapped = clt_parameters.imp.generate_mapped &&
(gen_seq_mono_color[0] || gen_seq_mono_color[1]); // generate sequences - Tiff and/or video
......@@ -4665,6 +4667,7 @@ public class OpticalFlow {
boolean [] annotate_mono_color = {clt_parameters.imp.annotate_mono,clt_parameters.imp.annotate_color};
boolean annotate_transparent_mono = clt_parameters.imp.annotate_transparent_mono;
boolean [] generate_modes3d = {
clt_parameters.imp.generate_raw,
clt_parameters.imp.generate_inf,
......@@ -4980,14 +4983,13 @@ public class OpticalFlow {
boolean ims_use = center_reference || clt_parameters.imp.ims_use; // center works with IMS only
/// int center_index=ref_index;
int ref_index = last_index; // old versions
boolean compensate_ims_rotation = false;
if (force_initial_orientations && !reuse_video) {
boolean OK = false;
int es1 = -1;
if (center_reference) {
es1 = Interscene.setInitialOrientationsCenterIms(
clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
use_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters,
debayerParameters, // EyesisCorrectionParameters.DebayerParameters debayerParameters,
......@@ -5014,7 +5016,7 @@ public class OpticalFlow {
} else if (ims_use) {
earliest_scene = Interscene.setInitialOrientationsIms(
clt_parameters, // final CLTParameters clt_parameters,
compensate_ims_rotation, // final boolean compensate_ims_rotation,
use_ims_rotation, // final boolean compensate_ims_rotation,
min_num_scenes, // int min_num_scenes,
colorProcParameters, // final ColorProcParameters colorProcParameters,
quadCLTs, // final QuadCLT[] quadCLTs, //
......@@ -5265,16 +5267,15 @@ public class OpticalFlow {
if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
}
boolean configured_lma = false;
boolean lpf_xy = false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen = clt_parameters.imp.avg_len; // 3.0;
// TODO: move to config
// boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
boolean configured_lma = false;
boolean lpf_xy = false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen = clt_parameters.imp.avg_len; // 3.0;
boolean readjust_xy_ims = true; // false;
double reg_weight_xy = 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
double reg_weight_xy = 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
boolean ers_from_ims = true; // false; // change later
// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
boolean ers_from_ims = true; // false; // change later
int ers_mode = 0; // keep
// with IMS it is already set during initial orientation. In non-IMS mode
if (!ers_from_ims && (quadCLTs[ref_index].getNumOrient() < 2)) {
......@@ -5292,7 +5293,7 @@ public class OpticalFlow {
disable_ers, // boolean disable_ers,
configured_lma, // boolean configured_lma,
lpf_xy, // boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
avg_rlen, // double avg_rlen,
avg_rlen, // double avg_rlen,
readjust_xy_ims,// boolean readjust_xy_ims, // readjust X,Y fromIMS linear velocities and full X,Y movement
// from the previous adjustment. Adjust A,T,R,Z (and optionally
// angular velocities) freely
......@@ -5427,8 +5428,7 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("LMA done on iteration "+num_iter+
" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
System.out.println("Correction quaternion = ["+quatCorr[0]+", "+
quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
QuadCLTCPU.showQuatCorr(quatCorr);
}
quadCLTs[ref_index].setQuatCorr(quatCorr);
quadCLT_main.setQuatCorr(quatCorr);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 8406092c
......@@ -193,6 +193,7 @@ public class QuadCLTCPU {
public Did_gps_pos did_gps1_ubx_pos = null;
public String ims_last_path = null;
public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
// latest (or any? first also?) scene in a sequence has a timestamp of the reference scene
public String timestamp_reference = null;
......@@ -289,14 +290,15 @@ public class QuadCLTCPU {
System.out.println("b= ["+b[0]+", "+b[1]+", "+b[2]+"]");
System.out.println("ref_offs=["+ref_offs[0]+", "+ref_offs[1]+", "+ref_offs[2]+"]");
System.out.println(String.format(
"%3s\t%9s\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%s\t%9s\t%9s\t\t%9s\t%9s\t%9s",
"N","T", "X","Y","Z","X-INT","Y-INT","Z-INT","X-err","Y-err","Z-err","X-lin","Y-lin","Z-lin",
"%3s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%s\t%9s\t%9s\t\t%9s\t%9s\t%9s",
"N","T", "X","Y","Z","A","T","R","X-INT","Y-INT","Z-INT","X-err","Y-err","Z-err","X-lin","Y-lin","Z-lin",
"X-corr","Y-corr","Z-corr"));
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\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.4f\t%9.4f\t%9.4f",
"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.4f\t%9.4f\t%9.4f",
nscene,tim[nscene],
xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
xyz_integ[nscene][0],xyz_integ[nscene][1],xyz_integ[nscene][2],
xyzatr[nscene][0][0]-xyz_integ[nscene][0],xyzatr[nscene][0][1]-xyz_integ[nscene][1],xyzatr[nscene][0][2]-xyz_integ[nscene][2],
// b[0]+a[0]*(tim[nscene]-tim[ref_index]), b[1]+a[1]*(tim[nscene]-tim[ref_index]), b[2]+a[2]*(tim[nscene]-tim[ref_index]),
......@@ -414,6 +416,7 @@ public class QuadCLTCPU {
/**
* Get linear and angular velocities (camera frame) from
* this scene IMS data
* Reorders [0][0] and [0][1], because A - around Y, T - around X, R - around Z
* @param clt_parameters configuration parameters (including
* scales from IMS to linear for linear and angular velocities
* @param scale if true, multiply by scales from he configuration parameters)
......@@ -445,14 +448,14 @@ public class QuadCLTCPU {
/**
* Get linear and angular velocities (camera frame) from
* scenes' IMS data
* scenes' DID_PIMU data
* @param clt_parameters configuration parameters (including
* scales from IMS to linear for linear and angular velocities
* @param quadCLTs array of scenes with IMS data loaded
* @param scale if true, multiply by scales from he configuration parameters)
* @return linear and angular velocities in camera frame for each scene that has IMS data
*/
public static double [][][] getDxyzatrIms(
public static double [][][] getDxyzatrPIMU(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
boolean scale) {
......@@ -465,6 +468,75 @@ public class QuadCLTCPU {
return dxyzatr;
}
public static double [][][] integratePIMU(
final CLTParameters clt_parameters,
final QuadCLT[] quadCLTs,
final int ref_index,
final int early_index,
final int last_index,
final double [][] offsets // null - will not subtract from velocities
){
boolean renormalize = true;
double [] timestamps = new double [quadCLTs.length];
for (int nscene = early_index; nscene <= last_index; nscene++) {
timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
}
double [][][] dxyzatr = getDxyzatrPIMU(
clt_parameters,
quadCLTs,
false);
if (offsets != null) {
for (int nscene = early_index; nscene <= last_index; nscene++) {
for (int i = 0; i < offsets.length; i++) {
for (int j = 0; j < offsets[0].length; j++) {
dxyzatr[nscene][i][j] -= offsets[i][j];
}
}
}
}
double [][][] xyzatr = new double [quadCLTs.length][][];
xyzatr[ref_index] = new double [2][3];
for (int dir = -1; dir <= 1; dir += 2) {
double [][] vel_prev = dxyzatr[ref_index];
int prev_scene = ref_index;
Rotation rot = new Rotation(1.0, 0.0, 0.0,0.0, false);
for (int nscene = ref_index + dir; (nscene >= early_index) && (nscene <= last_index); nscene += dir) {
xyzatr[nscene] = new double[2][3];
double dt_half = 0.5* (timestamps[nscene]-timestamps[prev_scene]);
double [][] vel_this = dxyzatr[nscene];
// linear integrate
for (int i = 0; i < 3; i++) {
xyzatr[nscene][0][i] = xyzatr[prev_scene][0][i] + dt_half*(vel_prev[0][i] + vel_this[0][i]);
}
// quaternion_integrate
// https://math.stackexchange.com/questions/1693067/differences-between-quaternion-integration-methods
double [] omega= new double[3]; // average omega
double omega_l2 = 0;
for (int i = 0; i < 3; i++) {
omega[i] = 0.5 * (vel_this[1][i] + vel_prev[1][i]);
omega_l2 += omega[i]*omega[i];
}
double omega_l = Math.sqrt(omega_l2);
double a = Math.cos(omega_l * dt_half);
double b = Math.sin(omega_l * dt_half);
double [] v = new double[3];
for (int i = 0; i < 3; i++) {
v[i] = b * omega[i] / omega_l;
}
// Swap A and T !
Rotation qr = new Rotation(a, v[1], v[0], v[2], true);
Rotation qn = qr.applyTo(rot);
if (renormalize ) {
qn= new Rotation(qn.getQ0(),qn.getQ1(),qn.getQ2(),qn.getQ3(),true);
}
xyzatr[nscene][1]= qn.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
rot = qn;
vel_prev = vel_this;
prev_scene = nscene;
}
}
return xyzatr;
}
/**
* Get offset and rotation of each scene from the sequence relative to this
......@@ -529,6 +601,19 @@ public class QuadCLTCPU {
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu));
scenes_xyzatr[nscene] = pose_ims;
}
if (debugLevel>0) {
System.out.println(String.format(
"%3s\t%9s\t%9s\t%9s\t%9s\t%9s",
"N","X","Y","Z","A","T","R"));
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
System.out.println(String.format(
"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
nscene,
scenes_xyzatr[nscene][0][0],scenes_xyzatr[nscene][0][1],scenes_xyzatr[nscene][0][2],
scenes_xyzatr[nscene][1][0],scenes_xyzatr[nscene][1][1],scenes_xyzatr[nscene][1][2]
));
}
}
return scenes_xyzatr;
}
......@@ -9959,6 +10044,24 @@ public class QuadCLTCPU {
System.out.println(geometryCorrection.getCorrVector().toString());
}
}
public void showQuatCorr() {
showQuatCorr(getQuatCorr());
}
public static void showQuatCorr(double [] quat_corr) {
if (quat_corr != null) {
System.out.println("Correction quaternion = ["+quat_corr[0]+", "+
quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
double [] angles = Imx5.quatToCamAtr(quat_corr);
System.out.println("Correction angles: "+
(angles[0]*180/Math.PI)+"deg, "+
(angles[1]*180/Math.PI)+"deg, "+
(angles[2]*180/Math.PI)+"deg");
} else {
System.out.println("No IMS orientation correction is defined");
}
}
public boolean editExtrinsicCorr() // not used in lwir
{
......
src/main/java/com/elphel/imagej/tileprocessor/sfm/StructureFromMotion.java
View file @ 8406092c
......@@ -815,7 +815,7 @@ public class StructureFromMotion {
final double neib_too_strong1 = clt_parameters.imp.sfm_neib_too_str1; // 0.4; // do not count neighbors stronger than that
final double neib_too_strong16 = clt_parameters.imp.sfm_neib_too_str16; // 0.4; // do not count neighbors stronger than that
final int sfm_shrink = clt_parameters.imp.sfm_shrink; // 5;
final double fade_sigma = clt_parameters.imp. sfm_fade_sigma; // 3.0; // fade SfM gains at the edges
final double fade_sigma = clt_parameters.imp.sfm_fade_sigma; // 3.0; // fade SfM gains at the edges
final boolean use_neibs = clt_parameters.imp.sfm_use_neibs; // true;
final double min_neib_strength1= clt_parameters.imp.sfm_neib_str1; // 0.5; // update if no-individual and neibs correction strength exceeds
final double min_neib_strength16= clt_parameters.imp.sfm_neib_str16; // 0.5; // update if no-individual and neibs correction strength exceeds
......@@ -1257,6 +1257,9 @@ public class StructureFromMotion {
if (disp_adj != null) {
disp_adj[ntry + 1] = ref_disparity.clone();
}
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): Finished pass "+(ntry + 1)+" (of "+ num_readjust+")");
}
} // ntry
if (disp_adj != null) {
ShowDoubleFloatArrays.showArrays(
......
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