Skip to content

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

Next snapshot, testing with longer sequences

parent 47bf5840
Hide whitespace changes
Inline Side-by-side
Showing with 685 additions and 141 deletions
src/main/java/com/elphel/imagej/cameras/CLTParameters.java
View file @ 793b2588
......@@ -482,6 +482,7 @@ public class CLTParameters {
public double gmap_pix_size = 0.005; // hdr_x0y0, // in meters
public int gmap_max_image_width = 4000; // 3200; // increase pixel size as a power of 2 until image fits
public double gmap_min_sfm = 10.0; // minimal SfM gain to keep ground map
public double gmap_frac_sfm = 0.25; // Disregard SfM mask if lower good SfM area
public boolean gmap_crop_empty = true;
public int gmap_crop_extra = 20;
public int [] gmap_tex_pals = {0,1,2};
......@@ -1638,6 +1639,7 @@ public class CLTParameters {
properties.setProperty(prefix+"gmap_pix_size", this.gmap_pix_size+""); // double
properties.setProperty(prefix+"gmap_max_image_width", this.gmap_max_image_width+""); // int
properties.setProperty(prefix+"gmap_min_sfm", this.gmap_min_sfm+""); // double
properties.setProperty(prefix+"gmap_frac_sfm", this.gmap_frac_sfm+""); // double
properties.setProperty(prefix+"gmap_crop_empty", this.gmap_crop_empty+""); // boolean
properties.setProperty(prefix+"gmap_crop_extra", this.gmap_crop_extra+""); // int
properties.setProperty(prefix+"gmap_tex_pals", CLTParameters.arr_to_str(this.gmap_tex_pals)); // int[]
......@@ -2669,6 +2671,7 @@ public class CLTParameters {
if (properties.getProperty(prefix+"gmap_pix_size")!=null) this.gmap_pix_size=Double.parseDouble(properties.getProperty(prefix+ "gmap_pix_size"));
if (properties.getProperty(prefix+"gmap_max_image_width")!=null) this.gmap_max_image_width=Integer.parseInt(properties.getProperty(prefix+ "gmap_max_image_width"));
if (properties.getProperty(prefix+"gmap_min_sfm")!=null) this.gmap_min_sfm=Double.parseDouble(properties.getProperty(prefix+ "gmap_min_sfm"));
if (properties.getProperty(prefix+"gmap_frac_sfm")!=null) this.gmap_frac_sfm=Double.parseDouble(properties.getProperty(prefix+ "gmap_frac_sfm"));
if (properties.getProperty(prefix+"gmap_crop_empty")!=null) this.gmap_crop_empty=Boolean.parseBoolean(properties.getProperty(prefix+ "gmap_crop_empty"));
if (properties.getProperty(prefix+"gmap_crop_extra")!=null) this.gmap_crop_extra=Integer.parseInt(properties.getProperty(prefix+ "gmap_crop_extra"));// int
if (properties.getProperty(prefix+"gmap_tex_pals")!=null) this.gmap_tex_pals=CLTParameters.str_to_iarr(properties.getProperty(prefix+"gmap_tex_pals"));
......@@ -3947,6 +3950,8 @@ public class CLTParameters {
"Maximal output image size in pixels. Increase pixel size in the power-of-two ratio if calculated image width exceeds this value.");
gd.addNumericField("Minimal SfM gain", this.gmap_min_sfm, 4,6,"x",
"Minimal SfM gain to keep ground map, lower makes texture transparent.");
gd.addNumericField("Minimal SfM fraction", this.gmap_frac_sfm, 4,6,"x",
"Disregard SfM mask if available SfM tiles fraction is below.");
gd.addCheckbox ("Crop empty map areas", this.gmap_crop_empty, // true; // enable change FG pixel to opaque from transparent
"Crop the map output from four directions while only undefined pixels are removed.");
gd.addNumericField("Crop maps extra", this.gmap_crop_extra, 0,3,"pix", // minimal neighbors to keep alpha
......@@ -5191,6 +5196,7 @@ public class CLTParameters {
this.gmap_pix_size= gd.getNextNumber();
this.gmap_max_image_width= (int) gd.getNextNumber();
this.gmap_min_sfm= gd.getNextNumber();
this.gmap_frac_sfm= gd.getNextNumber();
this.gmap_crop_empty= gd.getNextBoolean();
this.gmap_crop_extra= (int) gd.getNextNumber();
this.gmap_tex_pals = CLTParameters.str_to_iarr(gd.getNextString());
......
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 793b2588
......@@ -228,11 +228,12 @@ public class ErsCorrection extends GeometryCorrection {
for (int i:DP_ZR_INDICES) param_select[i] = true;
if (use_XY) for (int i:DP_XY_INDICES) param_select[i] = true;
if (use_AT) for (int i:DP_AT_INDICES) param_select[i] = true;
// if (use_ERS) for (int i:DP_ATT_ERS_INDICES) param_select[i] = true;
if (use_ERS) param_select[DP_DSVAZ] = true;
if (use_ERS && use_ERS_tilt) param_select[DP_DSVTL] = true;
return param_select;
}
public static double [] getParamRegWeights(
double reg_weight) {
// boolean use_AT) {
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 793b2588
......@@ -43,6 +43,7 @@ import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.Imx5;
import ij.ImagePlus;
import ij.Prefs;
import ij.text.TextWindow;
public class Interscene {
......@@ -160,6 +161,7 @@ public class Interscene {
final int threadsMax, // int threadsMax,
final boolean updateStatus,
final int debugLevel) {
boolean readjust = clt_parameters.imp.refine_invert; // false; //true; // re-run LMA for the later part
// go twice, then inverse 1-st part.
// when going first - do not go beyond center (center by IMS coordinates) or just index?
// Index is easier, no need to change
......@@ -194,14 +196,29 @@ public class Interscene {
}
break;
}
if (adjusted_scene_index < cent_index) {
if (debugLevel > -3) {
System.out.println("Overlap mode, enough scenes for half of the remaining number"+
(ref_index - adjusted_scene_index - 1));
}
break;
}
} // split cycles to remove output clutter
adjusted_scene_index++;
if ((ref_index - adjusted_scene_index) >= min_num_scenes_half) {
if (debugLevel > -3) {
System.out.println("Overlap mode, reusing previous scenes adjustment scenes, number of scenes = "+
(ref_index - adjusted_scene_index));
if (adjusted_scene_index > cent_index) {
if (debugLevel > -3) {
System.out.println("Overlap mode, reusing previous scenes adjustment scenes, number of scenes = "+
(ref_index - adjusted_scene_index));
}
cent_index = adjusted_scene_index;
} else {
if (debugLevel > -3) {
System.out.println("Overlap mode, partially reusing previous scenes adjustment scenes, number of scenes = "+
(ref_index - cent_index)+ " of "+(ref_index - adjusted_scene_index));
}
}
cent_index = adjusted_scene_index;
reused_overlap = true;
} else {
if (debugLevel > -3) {
......@@ -233,6 +250,24 @@ public class Interscene {
return cent_index;
}
}
boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
if (generate_egomotion) {
String ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego-first_half.csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
cent_index, // earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
// create new dsi for quadCLTs[earliest_scene1]
OpticalFlow.buildRefDSI( // returned is a different instance than input -FIXED
clt_parameters, // CLTParameters clt_parameters,
......@@ -288,8 +323,20 @@ public class Interscene {
return earliest_scene2; // cent_index;
}
// invert first half, reference to the cent_index, add to cent_index map, generate ref_index ponter and cent_index,
invertInitialOrientation(
clt_parameters, // final CLTParameters clt_parameters,
batch_mode, // final boolean batch_mode,
readjust, // final boolean readjust,
quadCLTs, //final QuadCLT[] quadCLTs, //
ref_index, // final int ref_index,
cent_index, // final int cent_index,
earliest_scene2, // int earliest_index//
debugLevel); // final int debugLevel
String cent_ts = quadCLTs[cent_index].getImageName();
// write config for both ref_index and cent_index scenes
// ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
/*
ErsCorrection ers_center = quadCLTs[cent_index].getErsCorrection();
String cent_ts = quadCLTs[cent_index].getImageName();
double [][] center_xyzatr = new double [][] {ers_reference.getSceneXYZ(cent_ts), ers_reference.getSceneATR(cent_ts)};
......@@ -311,13 +358,12 @@ public class Interscene {
dxyzatr_dt // ers_scene.getErsXYZATR_dt(),
);
}
*/
// set pointers
quadCLTs[ref_index].setRefPointer(cent_ts); // write pointer to center scene to reference scene
quadCLTs[cent_index].setFirstLastPointers(quadCLTs[earliest_scene2],quadCLTs[ref_index]);// set first/last to center scene
// set quadCLTs[ref_index].set_orient(0); // ? as it has only half?
// quadCLTs[ref_index].set_orient(1); // first orientation // applicable to the center?
// quadCLTs[ref_index].set_accum(0); // reset accumulations ("build_interscene") number
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
......@@ -334,6 +380,321 @@ public class Interscene {
return earliest_scene2;
}
public static int invertInitialOrientation(
final CLTParameters clt_parameters,
final boolean batch_mode,
final boolean readjust,
final QuadCLT[] quadCLTs, //
final int last_index, // original ref_index (last)
final int ref_index, // new reference index (center)
final int earliest_index,
final int debugLevel
) {
ErsCorrection ers_old_reference = quadCLTs[last_index].getErsCorrection();
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
String cent_ts = quadCLTs[ref_index].getImageName();
double [][] center_xyzatr = new double [][] {ers_old_reference.getSceneXYZ(cent_ts), ers_old_reference.getSceneATR(cent_ts)};
double [][] inv_cent_xyzatr = ErsCorrection.invertXYZATR(center_xyzatr);
// Invert half-sequence to reference cent_index
for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
double [][] scene_xyzatr,dxyzatr_dt;
if (scene_index == last_index) {
scene_xyzatr = new double [2][3];
dxyzatr_dt = ers_old_reference.getErsXYZATR_dt();
} else {
String ts = quadCLTs[scene_index].getImageName();
scene_xyzatr = ers_old_reference.getSceneXYZATR(ts);
dxyzatr_dt = ers_old_reference.getSceneErsXYZATR_dt(ts);
}
double [][] scene_cent_xyzatr = ErsCorrection.combineXYZATR(scene_xyzatr, inv_cent_xyzatr);
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scene_cent_xyzatr,
dxyzatr_dt // ers_scene.getErsXYZATR_dt(),
);
}
if (readjust) {
boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always
true, // boolean use_XY
false, // boolean use_AT,
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
double maximal_series_rms = 0.0;
double [][][] scenes_xyzatr = new double [last_index+1][][];
double [][][] dxyzatr_dt = new double [last_index+1][][];
for (int nscene = earliest_index; nscene <= ref_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
scenes_xyzatr[nscene] = ers_reference.getSceneXYZATR(ts);
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
}
double [] min_max = {0, 10000, 0.0} ; // {min, max, actual rms)
// set reference
double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
double min_ref_frac= clt_parameters.imp.min_ref_frac;
boolean sfm_filter = clt_parameters.imp.sfm_filter; //true; // use SfM filtering if available
double sfm_minmax = clt_parameters.imp.sfm_minmax; //10.0; // minimal value of the SfM gain maximum to consider available
double sfm_fracmax = clt_parameters.imp.sfm_fracmax; // 0.75; // minimal fraction of the SfM maximal gain
double sfm_fracall = clt_parameters.imp.sfm_fracall; // 0.3; // minimal relative area of the SfM-e
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
int tilesY = quadCLTs[ref_index].getTileProcessor().getTilesY();
int tile_size = quadCLTs[ref_index].getTileProcessor().getTileSize();
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_max_quad = clt_parameters.imp.fmg_max_quad; // estimate offset by 4 points (rooll-aware, 25% from center) if center
// offset is too small
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
boolean debug2 = !batch_mode; // false; // true;
boolean [] reliable_ref = null;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
double [] reduced_strength = new double[1];
double avg_z = quadCLTs[ref_index].getAverageZ(true); // use lma
double [] lma_rms = new double[2];
// use combo if second pass?
if (min_ref_str > 0.0) {
reliable_ref = quadCLTs[ref_index].getReliableTiles( // will be null if does not exist.
false, // boolean use_combo,
min_ref_str, // double min_strength,
min_ref_frac, // double min_ref_frac,
ref_need_lma, // boolean needs_lma);
true, // ref_need_lma_combo, // boolean needs_lma_combo);
sfm_filter, // boolean sfm_filter, // use SfM filtering if available
sfm_minmax, // double sfm_minmax, // minimal value of the SfM gain maximum to consider available
sfm_fracmax, // double sfm_fracmax,// minimal fraction of the SfM maximal gain
sfm_fracall, // double sfm_fracall,// minimal relative area of the SfM-enabled tiles (do not apply filter if less)
reduced_strength, // if not null will return >0 if had to reduce strength (no change if did not reduce)
debugLevel); // int debugLevel)
if (reduced_strength[0] > 0) {
use_lma_dsi = false; // too few points
}
if (debug2) {
double [] dbg_img = new double [reliable_ref.length];
for (int i = 0; i < dbg_img.length; i++) {
dbg_img[i] = reliable_ref[i]?1:0;
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
quadCLTs[ref_index].getTileProcessor().getTilesX(),
quadCLTs[ref_index].getTileProcessor().getTilesY(),
"reliable_ref");
}
}
// Trying new version with motion blur and single-setting of the reference frame
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
double mb_max_gain = clt_parameters.imp.mb_max_gain_inter; // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
TpTask[][] tp_tasks_ref = new TpTask[2][];
double [][] pXpYD_ref = new double [tilesX*tilesY][];
ArrayList<Integer> fpn_list = new ArrayList<Integer>();
double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
boolean fpn_skip = true;
double [][] last_corr_xyzatr = {ZERO3,ZERO3};
for (int scene_index = ref_index; scene_index <= last_index; scene_index++) { // include cent_index itself to the map
double [][] scene_xyzatr_old; // ,dxyzatr_dt;
if (scene_index == last_index) {
scene_xyzatr_old = new double [2][3];
dxyzatr_dt[scene_index] = ers_old_reference.getErsXYZATR_dt();
} else {
String ts = quadCLTs[scene_index].getImageName();
scene_xyzatr_old = ers_old_reference.getSceneXYZATR(ts);
dxyzatr_dt[scene_index] = ers_old_reference.getSceneErsXYZATR_dt(ts);
}
double [][] initial_pose = ErsCorrection.combineXYZATR(scene_xyzatr_old, inv_cent_xyzatr);
initial_pose = ErsCorrection.combineXYZATR(
last_corr_xyzatr, // current correction
initial_pose); // previously known pose
// Will be used in prepareLMA()
quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[scene_index])); // cam_dxyzatr));
// Trying new version with motion blur and single-setting of the reference frame
double est_shift = quadCLTs[ref_index].estimateAverageShift(
new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
initial_pose, // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
fmg_rectilinear); // boolean fmg_rectilinear)
if (est_shift < fmg_max_quad) {
est_shift = quadCLTs[ref_index].estimateAverageShift(
new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
initial_pose, // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
fmg_rectilinear); // boolean rectilinear)
}
if (debugLevel > -4) {
System.out.println("nscene="+scene_index+": est offset "+
est_shift+"pix");
}
// double [][]scene_xyzatr = new double[][] {initial_pose[0].clone(),initial_pose[1].clone()};
boolean adjust_OK = false;
if (est_shift < min_max[0]) {
fail_reason[0]=FAIL_REASON_MIN;
} else {
// Trying new version with motion blur and single-setting of the reference frame
scenes_xyzatr[scene_index] = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
tp_tasks_ref, // TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
pXpYD_ref, // double [][] pXpYD_ref, // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null
ref_index, // int nscene0, // may be == ref_index
scene_index, // int nscene1, // compares to nscene0
null, // double [] ref_disparity, // null or alternative reference disparity
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[ref_index], // double [][] scene0_xyzatr,,
initial_pose, // double [][] scene1_xyzatr,
Double.NaN, // double average_z,
initial_pose, // double [] scene1_xyzatr_pull, // if both are not null, specify target values to pull to
param_select, // clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
reg_weights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms,
mb_en, // boolean mb_en,
mb_tau, // double mb_tau, // 0.008; // time constant, sec
mb_max_gain, // double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
clt_parameters.imp.debug_level); // int debugLevel);
adjust_OK = scenes_xyzatr[scene_index] != null;
}
if (!adjust_OK) {
System.out.println("LMA failed at nscene = "+scene_index+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
fpn_list.add(scene_index);
scenes_xyzatr[scene_index] = initial_pose;
// should not be modified for initial adjust, but just in case
quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[scene_index]));
continue;
}
// update last_corr_xyzatr, // current correction
double [][] corr_diff = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index], // new
ErsCorrection.invertXYZATR(initial_pose)); //old
last_corr_xyzatr = ErsCorrection.combineXYZATR (
last_corr_xyzatr,
corr_diff);
// not used here with IMS: after_spiral == false
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
));
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),
adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()
);
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
}
if (debugLevel > -3) {
System.out.println("invertInitialOrientation(): scene "+scene_index+" (of "+ quadCLTs.length+") "+
quadCLTs[ref_index].getImageName() + "/" + quadCLTs[scene_index].getImageName()+
" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
}
} // for (int scene_index = ref_index; scene_index <= last_index; scene_index++) {
if (debugLevel > -4) {
System.out.println("num_fpn_mitigate= "+fpn_list.size());
}
if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too
//double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist,
fmg_rectilinear, // boolean rectilinear,
quadCLTs, // QuadCLT [] quadCLTs,
scenes_xyzatr, // double [][][] scenes_xyzatr,
avg_z, // double avg_z,
ref_index, // int ref_index, // >= earliest_scene
earliest_index); // int earliest_scene)
for (int ipair = 0; ipair < fpn_pairs.length; ipair++) if (fpn_pairs[ipair][1] >= 0) {
if (debugLevel > -4) {
System.out.println("Mitigating FPN for scene "+fpn_pairs[ipair][0]+
" being too close to reference "+ref_index+
", using scene "+fpn_pairs[ipair][1]+" as a reference");
}
TpTask[][] tp_tasks_rel_ref = new TpTask[2][];
double [][] rel_xyzatr = adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
clt_parameters, // CLTParameters clt_parameters,
use_lma_dsi, // clt_parameters.imp.use_lma_dsi,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
true, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
tp_tasks_rel_ref, // TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
pXpYD_ref, // double [][] pXpYD_ref, // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null
fpn_pairs[ipair][1], // ref_index, // int nscene0, // may be == ref_index
fpn_pairs[ipair][0], // int nscene1, // compares to nscene0
null, // double [] ref_disparity, // null or alternative reference disparity
reliable_ref, //boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[fpn_pairs[ipair][1]], // double [][] scene0_xyzatr,,
scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose, // double [][] scene1_xyzatr,
Double.NaN, // double average_z,
scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose, // double [] scene1_xyzatr_pull, // if both are not null, specify target values to pull to
param_select, // clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
reg_weights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
2*clt_parameters.imp.max_rms, // double max_rms,
mb_en, // boolean mb_en,
mb_tau, // double mb_tau, // 0.008; // time constant, sec
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);
boolean adjust_OK = rel_xyzatr != null;
if (!adjust_OK) {
// should not be modified for initial adjust, but just in case
quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[fpn_pairs[ipair][0]]));
System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
} else {
scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().getErsXYZATR_dt());
ers_reference.addScene(quadCLTs[fpn_pairs[ipair][0]].getImageName(),
scenes_xyzatr[fpn_pairs[ipair][0]][0],
scenes_xyzatr[fpn_pairs[ipair][0]][1],
adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),
adjusted_xyzatr_dt[1]); // ZERO3 // ers_scene.getErsATR_dt()
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
}
if (debugLevel > -3) {
System.out.println("Pass multi scene "+fpn_pairs[ipair][0]+" (of "+ quadCLTs.length+") "+
quadCLTs[fpn_pairs[ipair][1]].getImageName() + "/" + quadCLTs[fpn_pairs[ipair][0]].getImageName()+
" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
}
}
}
}
}
return 0;
}
public static int setInitialOrientationsIms(
final CLTParameters clt_parameters,
final boolean compensate_ims_rotation,
......@@ -388,7 +749,13 @@ public class Interscene {
double max_zoom_diff = clt_parameters.imp.max_zoom_diff;
boolean fpn_skip = clt_parameters.imp.fpn_skip; // if false - fail as before
boolean fpn_rematch = clt_parameters.imp.fpn_rematch; // if false - keep previous
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
boolean[] param_select =
ErsCorrection.getParamSelect( // ZR - always
true, // boolean use_XY
false, // boolean use_AT,
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--) {
// to include ref scene photometric calibration
......@@ -449,8 +816,8 @@ public class Interscene {
// boolean scale_ims_velocities = false; // true;
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // does not depend on correction
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
quadCLTs); //, // QuadCLT[] quadCLTs,
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; //
// quat_corr may still be null if does not exist
......@@ -474,6 +841,7 @@ public class Interscene {
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
0, // final int early_index,
(quadCLTs.length -1) // int last_index,
// (new double[2][3]) //final double [][] pimu_offsets // null - will not subtract from velocities
......@@ -488,7 +856,7 @@ public class Interscene {
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
boolean after_spiral = false;
// boolean after_spiral = false;
boolean got_spiral = false;
double [][] last_corr_xyzatr = {ZERO3,ZERO3};
ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
......@@ -510,7 +878,7 @@ public class Interscene {
double [][] pXpYD_ref = new double [tilesX*tilesY][];
ArrayList<Integer> fpn_list = new ArrayList<Integer>();
double [] reg_weights = clt_parameters.ilp.ilma_regularization_weights;
min_max[1] = max_offset;
// min_max[1] = max_offset;
for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
if ((ref_index - scene_index) >= max_num_scenes){
earliest_scene = scene_index + 1;
......@@ -541,6 +909,7 @@ public class Interscene {
// Refine with LMA
//boost_zoom_short
double max_z_change_scaled = max_z_change;
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
if ((ref_index - scene_index) < min_num_scenes) {
min_max[1] = boost_max_short * max_offset;
max_z_change_scaled = max_z_change * boost_zoom_short;
......@@ -609,7 +978,7 @@ public class Interscene {
initial_pose, // double [][] scene1_xyzatr,
Double.NaN, // double average_z,
initial_pose, // double [] scene1_xyzatr_pull, // if both are not null, specify target values to pull to
clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
param_select, // clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
reg_weights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms,
......@@ -682,25 +1051,6 @@ public class Interscene {
last_corr_xyzatr,
corr_diff);
// not used here with IMS: after_spiral == false
if (after_spiral && (scene_index < (ref_index-1))) { // need to interpolate skipped scenes
// here we expect very small translations/angles, so linear scaling is OK
double s = 1.0 / (ref_index - scene_index);
for (int interp_index = ref_index-1; interp_index > scene_index; interp_index--) {
scenes_xyzatr[interp_index] = new double[2][3];
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
scenes_xyzatr[interp_index][i][j] =
scenes_xyzatr[scene_index][i][j] * s *(interp_index - scene_index);
}
}
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr[interp_index][0],
scenes_xyzatr[interp_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3 // ers_scene.getErsATR_dt()
);
}
}
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
......@@ -721,6 +1071,8 @@ public class Interscene {
quadCLTs[ref_index].getImageName() + "/" + scene_QuadClt.getImageName()+
" Done. RMS="+lma_rms[0]+", maximal so far was "+maximal_series_rms);
}
} // for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--)
if ((ref_index - earliest_scene + 1) < min_num_scenes) {
System.out.println("Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
......@@ -735,6 +1087,8 @@ public class Interscene {
System.out.println("num_fpn_mitigate= "+fpn_list.size());
}
if (fmg_initial_en && !fpn_list.isEmpty()) {
// here max_offset is not critical, min_offset can be 0 too
double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
int [][] fpn_pairs = getFPNPairs(
fpn_list, // ArrayList<Integer> fpn_list,
fmg_distance, // double fpn_mitigate_dist,
......@@ -769,9 +1123,9 @@ public class Interscene {
reliable_ref, //boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[fpn_pairs[ipair][1]], // double [][] scene0_xyzatr,,
scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose, // double [][] scene1_xyzatr,
Double.NaN, // double average_z,
Double.NaN, // double average_z,
scenes_xyzatr[fpn_pairs[ipair][0]], // initial_pose, // double [] scene1_xyzatr_pull, // if both are not null, specify target values to pull to
clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
param_select, // clt_parameters.ilp.ilma_lma_select, // boolean[] param_select,
reg_weights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
clt_parameters.imp.max_rms, // double max_rms,
......@@ -841,14 +1195,15 @@ public class Interscene {
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
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,
quadCLTs[ref_index].getPimuOffsets());
quadCLTs); //,
// quadCLTs[ref_index].getPimuOffsets());
double [] timestamps = new double [quadCLTs.length];
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
......@@ -1465,10 +1820,12 @@ public class Interscene {
boolean test_motion_blur,
int debugLevel)
{
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain+
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y);
boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = false; // true;
final boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
boolean restore_imu = false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect(
!freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
readjust_xy_ims || lpf_xy, // boolean use_AT,
......@@ -1481,7 +1838,6 @@ public class Interscene {
boolean fmg_reorient_en = clt_parameters.imp.fmg_reorient_en; // enable IMS-based FPN mitigation for readjustmnet orientation
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
// offset is too small
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
// int avg_len = clt_parameters.imp.avg_len;
......@@ -1615,6 +1971,24 @@ public class Interscene {
break;
default: // do nothing - already read
}
if (restore_imu) { // resore omegas from IMU
double [][][] dxyzatr_dt_pimu = QuadCLTCPU.getDxyzatrPIMU( // does not depend on correction
clt_parameters, // CLTParameters clt_parameters,
quadCLTs); // // QuadCLT[] quadCLTs,
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
for (int i = 0; i < dxyzatr_dt.length; i++) {
if ((dxyzatr_dt[i]!=null) && (dxyzatr_dt[i][1]!=null) && (dxyzatr_dt_pimu[i]!=null) && (dxyzatr_dt_pimu[i][1]!=null)) {
dxyzatr_dt[i][1][0] = dxyzatr_dt_pimu[i][1][0];
dxyzatr_dt[i][1][1] = dxyzatr_dt_pimu[i][1][1];
}
}
param_select = ErsCorrection.getParamSelect(
true, // boolean use_XY
true, // boolean use_AT,
false, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false); // boolean use_ERS_tilt);
}
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
// double avg_rlen
......@@ -4657,9 +5031,11 @@ public class Interscene {
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
String header_testpimu = "\timsX\timsY\timsZ\timsA\timsT\timsR"+
"\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR"+
"\tpimuX-C\tpimuY-C\tpimuZ-C\tpimuA-C\tpimuT-C\tpimuR-C";
String header = header_ts+(use_processed?header_img:"")+header_ins1+
header_ins2+header_ins2_extra+header_pimu+header_camv;
header_ins2+header_ins2_extra+header_pimu+header_camv+header_testpimu;
StringBuffer sb = new StringBuffer();
double [][][] dxyzatr_dt = null;
......@@ -4673,9 +5049,6 @@ public class Interscene {
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
}
/// double [] quat_ims_cam = Imx5.quaternionImsToCam(
/// ims_mount_atr, // new double[] {0, 0.13, 0},
/// ims_ortho);
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
......@@ -4692,8 +5065,27 @@ public class Interscene {
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // velocities and omegas from IMU
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
quadCLTs); // , // QuadCLT[] quadCLTs,
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
null, // double [] quat_corr, // only applies to rotations - verify!
-1); // debugLevel) ; // int debugLevel)
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
0, // final int early_index,
(quadCLTs.length -1) // int last_index,
);
int cent_index = earliest_scene + (quadCLTs.length - earliest_scene) / 2;
double [][] cent_xyzatr = pimu_xyzatr[cent_index];
// int ref_index,
// int earliest_scene,
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
......@@ -4751,8 +5143,6 @@ public class Interscene {
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
// String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
......@@ -4772,21 +5162,15 @@ public class Interscene {
d2_ref.getQn2b(), // double[] quat_ned,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
/*
double [] cam_xyz_enu0 = test_xyz_enu(
Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
*/
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
double [] quat_lma_enu_xyz = Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false);
double [] quat_lma_enu_xyz = (quatCorr != null) ?
Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
cam_xyz_enu;
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
......@@ -4807,32 +5191,40 @@ public class Interscene {
double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu))[1];
// "\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
sb.append("\t"+scene_abs_atr[0]+ "\t"+scene_abs_atr[1]+ "\t"+scene_abs_atr[2]); //
sb.append("\t"+scene_rel_atr[0]+ "\t"+scene_rel_atr[1]+ "\t"+scene_rel_atr[2]); //
// "\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"
sb.append("\t"+scene_abs_atr_enu[0]+ "\t"+scene_abs_atr_enu[1]+ "\t"+scene_abs_atr_enu[2]); //
sb.append("\t"+scene_rel_atr_enu[0]+ "\t"+scene_rel_atr_enu[1]+ "\t"+scene_rel_atr_enu[2]); //
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
/*
double [] omegas=new double[] {d3.theta[0]/d3.dt, d3.theta[1]/d3.dt, d3.theta[2]/d3.dt};
double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
sb.append("\t"+cam_dxyz[0]+ "\t"+cam_dxyz[1]+ "\t"+cam_dxyz[2]);
// a (around Y),t (around X), r (around Z)
sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]);
*/
// double [][] cam_dxyzatr = scene.getDxyzatrIms(clt_parameters, false); // raw, do not scale
// sb.append("\t"+cam_dxyzatr[0][0]+ "\t"+cam_dxyzatr[0][1]+ "\t"+cam_dxyzatr[0][2]);
// sb.append("\t"+cam_dxyzatr[1][0]+ "\t"+cam_dxyzatr[1][1]+ "\t"+cam_dxyzatr[1][2]);
// String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
// velocities and omegas from IMU
sb.append("\t"+scenes_dxyzatr[nscene][0][0]+ "\t"+scenes_dxyzatr[nscene][0][1]+ "\t"+scenes_dxyzatr[nscene][0][2]);
sb.append("\t"+scenes_dxyzatr[nscene][1][0]+ "\t"+scenes_dxyzatr[nscene][1][1]+ "\t"+scenes_dxyzatr[nscene][1][2]);
//scenes_dxyzatr
sb.append(
// getXyzatrIms () "\timsX\timsY\timsZ\timsA\timsT\timsR"
"\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]+
// integratePIMU() "\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR";
"\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]);
double [][] cent_diff = ErsCorrection.combineXYZATR(
pimu_xyzatr[nscene],
ErsCorrection.invertXYZATR(cent_xyzatr));
sb.append(
// "\tpimuX-C\tpimuY-C\tpimuZ-C\tpimuA-C\tpimuT-C\tpimuR-C" // difference to the center. XYZ - wrong (fix it)?
"\t"+cent_diff[0][0]+ "\t"+cent_diff[0][1]+ "\t"+cent_diff[0][2]+
"\t"+cent_diff[1][0]+ "\t"+cent_diff[1][1]+ "\t"+cent_diff[1][2]);
// add lpf
sb.append("\n");
}
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneLmaParameters.java
View file @ 793b2588
......@@ -38,7 +38,7 @@ public class IntersceneLmaParameters {
public boolean ilma_ers_quads = true; // Disable ERS fitting if not enough data
public double ilma_gap_frac = 0.25; // remove this center fraction of width/height when evaluating quads
public int ilma_min_quad_tiles = 10; // minimal number of defined tiles in each quad for using ERS
public double ilma_min_quad_weight = 0.01; // minimal fraction of total tiles weight in each quad for using ERS
public double ilma_min_quad_weight = 0.003; // minimal fraction of total tiles weight in each quad for using ERS
public boolean ilma_thread_invariant = true; // Do not use DoubleAdder, provide results not dependent on threads
public boolean [] ilma_lma_select = new boolean [ErsCorrection.DP_NUM_PARS]; // first three will not be used
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 793b2588
......@@ -41,6 +41,13 @@ public class IntersceneMatchParameters {
public double [] ims_ortho = {0.5, -0.5, 0.5, -0.5}; // approximate (90-deg) IMS to camera
public double [] ims_mount_atr = {0, 0, 0}; // IMS mount fine correction (A,T,R around camera axes)
public double [] ims_mount_xyz = {0, 0, 0}; // IMS center in camera coordinates
// gyros offsets in camera ATR order
// Azimuth -> -pimu_omega[2] rad/s, average did_pimu.theta[2]/did_pimu.dt
// Tilt -> -pimu_omega[1] rad/s, average did_pimu.theta[1]/did_pimu.dt
// Roll -> +pimu_omega[0] rad/s, average did_pimu.theta[0]/did_pimu.dt
public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418};
@Deprecated
public double [] ims_scale_xyz = {1.0,1.0,1.0}; // {1.1, 1.1, 1.1};
@Deprecated
......@@ -79,6 +86,11 @@ public class IntersceneMatchParameters {
public boolean sfm_average_neibs = false; // Average neighbors disparity among them
public boolean sfm_fill_weak = false; // Fill too weak tiles from nearest neighbors
public boolean sfm_extrapolate = false; // Extrapolate tiles with no SfM data (near edges)
// debug
public boolean sfm_save_seq = true; // Save SfM sequence image in model directory
public boolean sfm_show_seq = true; // Show SfM sequence
public boolean sfm_show_corr_ind = false; // Show SfM for each pairs group, each refinement run
public boolean sfm_show_corr = false; // Show combined SfM for each refinement run
public double [] getImsMountATR() {
return new double [] {
......@@ -351,13 +363,14 @@ public class IntersceneMatchParameters {
public double fpn_radius = 0.75; // pix - zero around center
public boolean fpn_ignore_border = false; // only if fpn_mask != null - ignore tile if maximum touches fpn_mask
public double min_offset = 2.5; // pixels - minimal average pixel offset between images to consider interscene matching
public double min_offset = 3.0; // pixels - minimal average pixel offset between images to consider interscene matching
public double max_pull_jump = 3.0; // pixels - maximal offset from the current pose to "pull" pose.
public double max_rel_offset = 0.5; // maximal interscene offset as a fraction of image width
public double max_rel_offset = 0.25; // maximal interscene offset as a fraction of image width
public double max_roll_deg = 5.0; // maximal interscene roll to consider matching
public double max_zoom_diff = 0; // for down-views when changing altitude (0 - ignore)
public boolean fpn_skip = true; // skip too close scenes (false - abort, previous behavior)
public boolean fpn_rematch = true; // match fpn-failed scenes to later scenes with larger difference
public boolean fpn_skip = true; // skip too close scenes (false - abort, previous behavior)
public boolean fpn_rematch = true; // match fpn-failed scenes to later scenes with larger difference
public boolean refine_invert = true; // Refine with LMA while inverting relative poses from other reference
// 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
......@@ -472,6 +485,11 @@ public class IntersceneMatchParameters {
}
public double [][] get_pimu_offs(){
// TODO: use ims_ortho, ims_mount_atr
return new double [][] {new double[3],
{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
}
public void dialogQuestions(GenericJTabbedDialog gd) {
// gd.addMessage ("Scene parameters selection");
// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
......@@ -492,6 +510,9 @@ public class IntersceneMatchParameters {
gd.addStringField ("IMS mount XYZ correction (m)", IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
"MS center (X,Y,Z m) in camera coordinates.");
gd.addStringField ("IMU angular corrections (rad/s)", IntersceneMatchParameters.doublesToString(pimu_gyr_offs), 80,
"Average from did_pimu.theta[]/did_pimu.dt when not moving.");
gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
......@@ -556,7 +577,15 @@ public class IntersceneMatchParameters {
"Fill too weak tiles from nearest neighbors.");
gd.addCheckbox ("Extrapolate no SfM data", this.sfm_extrapolate,
"Extrapolate tiles with no SfM data (near edges).");
gd.addMessage ("SfM debug images");
gd.addCheckbox ("Save SfM sequence image", this.sfm_save_seq,
"Save SfM sequence image in model directory.");
gd.addCheckbox ("Show SfM sequence", this.sfm_show_seq,
"Show SfM sequence (one slice per refinement run).");
gd.addCheckbox ("Show SfM details per run", this.sfm_show_corr_ind,
"Show SfM for each pairs group, each refinement run.");
gd.addCheckbox ("Show SfM combo per run", this.sfm_show_corr,
"Show combined SfM for each refinement run.");
gd.addTab ("Scene Series", "Processing series of scenes and multi-series sets");
gd.addMessage ("Build series options");
......@@ -1065,7 +1094,9 @@ public class IntersceneMatchParameters {
gd.addCheckbox ("Skip too close to reference scenes", this.fpn_skip,
"Skip too close to reference scenes (false - abort, previous behavior)");
gd.addCheckbox ("Match FPN-failed with other scenes", this.fpn_rematch,
"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,
"Refine with LMA while inverting relative poses from other reference.");
gd.addMessage ("Detect and remove moving objects from pose matching");
gd.addCheckbox ("Enable movement detection/elimination", this.mov_en,
......@@ -1269,6 +1300,7 @@ public class IntersceneMatchParameters {
this.ims_ortho = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 4);
this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.fmg_initial_en = gd.getNextBoolean();
......@@ -1300,6 +1332,11 @@ public class IntersceneMatchParameters {
this.sfm_fill_weak = gd.getNextBoolean();
this.sfm_extrapolate = gd.getNextBoolean();
this.sfm_save_seq = gd.getNextBoolean();
this.sfm_show_seq = gd.getNextBoolean();
this.sfm_show_corr_ind = gd.getNextBoolean();
this.sfm_show_corr = gd.getNextBoolean();
this.center_reference = gd.getNextBoolean();
this.overlap_sequences = gd.getNextBoolean();
this.reset_photometric = gd.getNextBoolean();
......@@ -1529,6 +1566,7 @@ public class IntersceneMatchParameters {
this.fpn_skip = gd.getNextBoolean();
this.fpn_rematch = gd.getNextBoolean();
this.refine_invert = gd.getNextBoolean();
this.mov_en = gd.getNextBoolean();
this.mov_sigma = gd.getNextNumber();
......@@ -1672,6 +1710,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"ims_ortho", IntersceneMatchParameters.doublesToString(this.ims_ortho));
properties.setProperty(prefix+"ims_mount_atr", IntersceneMatchParameters.doublesToString(this.ims_mount_atr));
properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
properties.setProperty(prefix+"pimu_gyr_offs", IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs));
properties.setProperty(prefix+"ims_scale_xyz", IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
properties.setProperty(prefix+"ims_scale_atr", IntersceneMatchParameters.doublesToString(this.ims_scale_atr));
properties.setProperty(prefix+"fmg_initial_en", this.fmg_initial_en + ""); // boolean
......@@ -1702,6 +1741,10 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"sfm_average_neibs", this.sfm_average_neibs + ""); // boolean
properties.setProperty(prefix+"sfm_fill_weak", this.sfm_fill_weak + ""); // boolean
properties.setProperty(prefix+"sfm_extrapolate", this.sfm_extrapolate + ""); // boolean
properties.setProperty(prefix+"sfm_save_seq", this.sfm_save_seq + ""); // boolean
properties.setProperty(prefix+"sfm_show_seq", this.sfm_show_seq + ""); // boolean
properties.setProperty(prefix+"sfm_show_corr_ind", this.sfm_show_corr_ind + ""); // boolean
properties.setProperty(prefix+"sfm_show_corr", this.sfm_show_corr + ""); // boolean
properties.setProperty(prefix+"center_reference", this.center_reference + ""); // boolean
properties.setProperty(prefix+"overlap_sequences", this.overlap_sequences + ""); // boolean
......@@ -1943,6 +1986,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"max_zoom_diff", this.max_zoom_diff+""); // double
properties.setProperty(prefix+"fpn_skip", this.fpn_skip+""); // boolean
properties.setProperty(prefix+"fpn_rematch", this.fpn_rematch+""); // boolean
properties.setProperty(prefix+"refine_invert", this.refine_invert+""); // boolean
properties.setProperty(prefix+"mov_en", this.mov_en+""); // boolean
properties.setProperty(prefix+"mov_sigma", this.mov_sigma+""); // double
......@@ -2039,6 +2083,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"ims_ortho")!=null) this.ims_ortho= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_ortho"),4);
if (properties.getProperty(prefix+"ims_mount_atr")!=null) this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
if (properties.getProperty(prefix+"ims_mount_xyz")!=null) this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3);
if (properties.getProperty(prefix+"ims_scale_xyz")!=null) this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
if (properties.getProperty(prefix+"ims_scale_atr")!=null) this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
if (properties.getProperty(prefix+"fmg_initial_en")!=null) this.fmg_initial_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_initial_en"));
......@@ -2069,7 +2114,11 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"sfm_average_neibs")!=null) this.sfm_average_neibs=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_average_neibs"));
if (properties.getProperty(prefix+"sfm_fill_weak")!=null) this.sfm_fill_weak=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_fill_weak"));
if (properties.getProperty(prefix+"sfm_extrapolate")!=null) this.sfm_extrapolate=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_extrapolate"));
if (properties.getProperty(prefix+"sfm_save_seq")!=null) this.sfm_save_seq=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_save_seq"));
if (properties.getProperty(prefix+"sfm_show_seq")!=null) this.sfm_show_seq=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_show_seq"));
if (properties.getProperty(prefix+"sfm_show_corr_ind")!=null) this.sfm_show_corr_ind=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_show_corr_ind"));
if (properties.getProperty(prefix+"sfm_show_corr")!=null) this.sfm_show_corr=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_show_corr"));
if (properties.getProperty(prefix+"center_reference")!=null) this.center_reference=Boolean.parseBoolean(properties.getProperty(prefix+"center_reference"));
if (properties.getProperty(prefix+"overlap_sequences")!=null) this.overlap_sequences=Boolean.parseBoolean(properties.getProperty(prefix+"overlap_sequences"));
if (properties.getProperty(prefix+"reset_photometric")!=null) this.reset_photometric=Boolean.parseBoolean(properties.getProperty(prefix+"reset_photometric"));
......@@ -2316,6 +2365,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"max_zoom_diff")!=null) this.max_zoom_diff=Double.parseDouble(properties.getProperty(prefix+"max_zoom_diff"));
if (properties.getProperty(prefix+"fpn_skip")!=null) this.fpn_skip=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_skip"));
if (properties.getProperty(prefix+"fpn_rematch")!=null) this.fpn_rematch=Boolean.parseBoolean(properties.getProperty(prefix+"fpn_rematch"));
if (properties.getProperty(prefix+"refine_invert")!=null) this.refine_invert=Boolean.parseBoolean(properties.getProperty(prefix+"refine_invert"));
if (properties.getProperty(prefix+"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"));
......@@ -2432,6 +2482,7 @@ public class IntersceneMatchParameters {
imp.ims_ortho = this.ims_ortho.clone();
imp.ims_mount_atr = this.ims_mount_atr.clone();
imp.ims_mount_xyz = this.ims_mount_xyz.clone();
imp.pimu_gyr_offs = this.pimu_gyr_offs.clone();
imp.ims_scale_xyz = this.ims_scale_xyz.clone();
imp.ims_scale_atr = this.ims_scale_atr.clone();
imp.fmg_initial_en = this.fmg_initial_en;
......@@ -2462,6 +2513,10 @@ public class IntersceneMatchParameters {
imp.sfm_average_neibs = this.sfm_average_neibs;
imp.sfm_fill_weak = this.sfm_fill_weak;
imp.sfm_extrapolate = this.sfm_extrapolate;
imp.sfm_save_seq = this.sfm_save_seq;
imp.sfm_show_seq = this.sfm_show_seq;
imp.sfm_show_corr_ind = this.sfm_show_corr_ind;
imp.sfm_show_corr = this.sfm_show_corr;
imp.center_reference = this.center_reference;
imp.overlap_sequences = this.overlap_sequences;
......@@ -2700,6 +2755,7 @@ public class IntersceneMatchParameters {
imp.max_zoom_diff = this.max_zoom_diff;
imp.fpn_skip = this.fpn_skip;
imp.fpn_rematch = this.fpn_rematch;
imp.refine_invert = this.refine_invert;
imp.mov_en = this.mov_en;
imp.mov_sigma = this.mov_sigma;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 793b2588
......@@ -5016,6 +5016,23 @@ public class OpticalFlow {
} else {
earliest_scene = start_ref_pointers[0];
}
if (generate_egomotion) {
String ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego-"+quadCLTs[ref_index].getNumOrient()+".csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
} else if (ims_use) {
earliest_scene = Interscene.setInitialOrientationsIms(
clt_parameters, // final CLTParameters clt_parameters,
......@@ -5052,6 +5069,7 @@ public class OpticalFlow {
if (!OK) {
return null;
}
// export csv
} else {// if (build_orientations) {
if (!reuse_video) { // reuse_video only uses reference scene
// if (ref_index)
......@@ -5132,6 +5150,11 @@ public class OpticalFlow {
// below ref_index is not necessary the last (fix all where it is supposed to be the last
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection(); // only used in ml_export
while (!reuse_video && ((quadCLTs[ref_index].getNumOrient() < min_num_orient) || (quadCLTs[ref_index].getNumAccum() < min_num_interscene))) {
if (debugLevel > -3) {
System.out.println("quadCLTs["+ref_index+"].getNumOrient()="+quadCLTs[ref_index].getNumOrient()+
" of "+min_num_orient+", quadCLTs["+ref_index+"].getNumAccum()="+quadCLTs[ref_index].getNumAccum()+
" of "+min_num_interscene);
}
if ((quadCLTs[ref_index].getNumAccum() < min_num_interscene) &&
((quadCLTs[ref_index].getNumAccum() < quadCLTs[ref_index].getNumOrient())||
(quadCLTs[ref_index].getNumOrient() >= min_num_orient))) {
......@@ -5308,6 +5331,22 @@ public class OpticalFlow {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
if (generate_egomotion) {
String ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego-"+quadCLTs[ref_index].getNumOrient()+".csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
}
}
if (run_ly) {
......@@ -6314,6 +6353,7 @@ public class OpticalFlow {
}
if (start_ref_pointers != null) {
start_ref_pointers[0] = earliest_scene;
start_ref_pointers[1] = ref_index;
}
System.out.println("buildSeries(): DONE"); //
return quadCLTs[ref_index].getX3dTopDirectory();
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 793b2588
......@@ -217,7 +217,7 @@ public class QuadCLTCPU {
return pimu_offsets;
}
public void setPimuOffsets(double[][] offsets) {
public void setPimuOffsets(double[][] offsets) { // never
pimu_offsets = offsets;
}
......@@ -431,12 +431,11 @@ public class QuadCLTCPU {
* 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 pimu_offsets linear and angular velocities offsets - subtract from IMU outputs
* @return linear and angular velocities in camera frame
*/
protected double [][] getDxyzatrIms(
CLTParameters clt_parameters,
final double [][] pimu_offsets) {
CLTParameters clt_parameters) {
final double [][] pimu_offsets = clt_parameters.imp.get_pimu_offs();
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] quat_ims_cam = Imx5.quaternionImsToCam(
......@@ -449,10 +448,10 @@ public class QuadCLTCPU {
double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
// a (around Y),t (around X), r (around Z)
double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
if (pimu_offsets != null) {
if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?
for (int i = 0; i < 3; i++) {
dxyzatr[0][i] -= pimu_offsets[0][i];
dxyzatr[1][i] -= pimu_offsets[0][i];
dxyzatr[1][i] -= pimu_offsets[1][i];
}
}
return dxyzatr;
......@@ -464,18 +463,15 @@ public class QuadCLTCPU {
* @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 pimu_offsets - corrections to velocities and omegas, subtract from the raw IMU outputs
* @return linear and angular velocities in camera frame for each scene that has IMS data
*/
public static double [][][] getDxyzatrPIMU(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
final double [][] pimu_offsets){
QuadCLT[] quadCLTs){ //,
double [][][] dxyzatr = new double [quadCLTs.length][][];
for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
dxyzatr[i] = quadCLTs[i].getDxyzatrIms(
clt_parameters,
pimu_offsets);
clt_parameters); //
}
return dxyzatr;
}
......@@ -484,46 +480,38 @@ public class QuadCLTCPU {
final CLTParameters clt_parameters,
final QuadCLT[] quadCLTs,
final int ref_index,
double [][][] dxyzatr,
final int early_index,
final int last_index//,
// final double [][] pimu_offsets // null - will not subtract from velocities
){
final int last_index){
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,
quadCLTs[ref_index].getPimuOffsets());
/*
if (pimu_offsets != null) {
for (int nscene = early_index; nscene <= last_index; nscene++) {
for (int i = 0; i < pimu_offsets.length; i++) {
for (int j = 0; j < pimu_offsets[0].length; j++) {
dxyzatr[nscene][i][j] -= pimu_offsets[i][j];
}
}
}
if (dxyzatr == null) {
dxyzatr = getDxyzatrPIMU(
clt_parameters,
quadCLTs);
}
*/
double [][][] xyzatr = new double [quadCLTs.length][][];
xyzatr[ref_index] = new double [2][3];
double [] vel_local_half = new double[3];
double [][] vel_inertial_half = new double[2][3];
for (int dir = -1; dir <= 1; dir += 2) {
double [][] vel_prev = dxyzatr[ref_index];
double [][] vel_prev = dxyzatr[ref_index]; // for reference vel local == vel inertial
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
// https://math.stackexchange.com/questions/773902/integrating-body-angular-velocity
// https://stackoverflow.com/questions/46908345/integrate-angular-velocity-as-quaternion-rotation
// https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity
double [] omega= new double[3]; // average omega
double omega_l2 = 0;
for (int i = 0; i < 3; i++) {
......@@ -539,11 +527,22 @@ public class QuadCLTCPU {
}
// Swap A and T !
Rotation qr = new Rotation(a, v[1], v[0], v[2], true);
// swap qr & rot?
Rotation qn = qr.applyTo(rot);
// Rotation qn = rot.applyTo(qr); inverted order
if (renormalize ) {
qn= new Rotation(qn.getQ0(),qn.getQ1(),qn.getQ2(),qn.getQ3(),true);
}
xyzatr[nscene][1]= qn.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
// linear integrate
for (int i = 0; i < 3; i++) {
vel_local_half[i] = 0.5 * (vel_prev[0][i] + vel_this[0][i]);
}
rot.applyTo(vel_local_half, vel_inertial_half[0]);
qn.applyTo(vel_local_half, vel_inertial_half[1]);
for (int i = 0; i < 3; i++) {
xyzatr[nscene][0][i] = xyzatr[prev_scene][0][i] + dt_half*(vel_inertial_half[0][i] + vel_inertial_half[1][i]);
}
rot = qn;
vel_prev = vel_this;
prev_scene = nscene;
......@@ -1208,6 +1207,7 @@ public class QuadCLTCPU {
return null;
}
double min_sfm_gain = clt_parameters.gmap_min_sfm ; //10.0 minimal SfM gain to keep ground map
double min_sfm_frac = clt_parameters.gmap_frac_sfm ; // 0.25; // Disregard SfM mask if lower good SfM area
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
......@@ -1234,11 +1234,24 @@ public class QuadCLTCPU {
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] dls = getDLS();
if ((min_sfm_gain> 0) && (dls[3] != null)) {
dls[2]=dls[2].clone();
for (int i = 0; i <dls[2].length; i++) {
if (dls[3][i] < min_sfm_gain) {
dls[2][i] = 0; // zero strength if low SfM gain
int num_sfm = 0;
for (int i = 0; i < dls[2].length; i++) if (dls[3][i] >= min_sfm_gain){
num_sfm++;
}
double sfm_frac = (1.0 * num_sfm)/dls[3].length;
if (sfm_frac >= min_sfm_frac) {
dls[2]=dls[2].clone();
for (int i = 0; i <dls[2].length; i++) {
if (dls[3][i] < min_sfm_gain) {
dls[2][i] = 0; // zero strength if low SfM gain
}
}
if (debug_level > -3) {
System.out.println("getGroundIms(): Fraction of SfM tiles = "+sfm_frac+" >= "+min_sfm_frac+", removing low SfM gain tiles.");
}
} else if (debug_level > -3) {
System.out.println("getGroundIms(): Fraction of SfM tiles = "+sfm_frac+" < "+min_sfm_frac+", disregarding SfM filtering.");
}
}
if (dls==null) {
......
src/main/java/com/elphel/imagej/tileprocessor/TexturedModel.java
View file @ 793b2588
......@@ -2467,6 +2467,7 @@ public class TexturedModel {
final double pix_size = clt_parameters.gmap_pix_size ; //0.005; // hdr_x0y0, // in meters
final int max_image_width = clt_parameters.gmap_max_image_width ; //4000; // 3200; // increase pixel size as a power of 2 until image fits
final double min_sfm_gain = clt_parameters.gmap_min_sfm ; //10.0 minimal SfM gain to keep ground map
final double min_sfm_frac = clt_parameters.gmap_frac_sfm ; // 0.25; // Disregard SfM mask if lower good SfM area
final boolean crop_empty = clt_parameters.gmap_crop_empty ; //true;
final int crop_extra = clt_parameters.gmap_crop_extra ; //20;
final int [] tex_pals = clt_parameters.gmap_tex_pals ; //{0,1,2};
......@@ -2598,11 +2599,25 @@ public class TexturedModel {
scenes[ref_index], // QuadCLT scene,
debugLevel); // int debug_level)
if (sfm_gain != null) {
for (int i = 0; i < ds_fg[0].length; i++) if (sfm_gain[i] < min_sfm_gain){
ds_fg[0][i] = Double.NaN;
ds_bg[0][i] = Double.NaN;
ds_fg[1][i] = 0.0;
ds_bg[1][i] = 0.0;
int num_sfm = 0;
for (int i = 0; i < ds_fg[0].length; i++) if (sfm_gain[i] >= min_sfm_gain){
num_sfm++;
}
double sfm_frac = (1.0 * num_sfm)/ds_fg[0].length;
if (sfm_frac >= min_sfm_frac) {
for (int i = 0; i < ds_fg[0].length; i++) if (sfm_gain[i] < min_sfm_gain){
ds_fg[0][i] = Double.NaN;
ds_bg[0][i] = Double.NaN;
ds_fg[1][i] = 0.0;
ds_bg[1][i] = 0.0;
}
if (debugLevel > -3) {
System.out.println("output3d(): Fraction of SfM tiles = "+sfm_frac+" >= "+min_sfm_frac+", removing low SfM gain tiles.");
}
} else {
if (debugLevel > -3) {
System.out.println("output3d(): Fraction of SfM tiles = "+sfm_frac+" < "+min_sfm_frac+", disregarding SfM filtering.");
}
}
}
......@@ -3017,7 +3032,7 @@ public class TexturedModel {
hdr_x0y0, // double [] x0y0, // initialize to double[2] to return width, height
hdr_whs, // int [] whs, // initialize to int[3] to return {width, height, scale reduction}
debugLevel); // int debug_level
if (to_ground_xyzatr == null) {
if (to_ground_xyzatr == null) {
System.out.println("***** Failed to detect flat ground surface, skipping rendering and output files generation *****");
} else {
double scaled_pixel_size = pix_size * hdr_whs[2];
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 793b2588
......@@ -8609,7 +8609,7 @@ if (debugLevel > -100) return true; // temporarily !
String [][] video_list = new String[1][];
int [][] widths_list = new int [1][];
int ref_index = -1; // -1 - last
int [] start_ref_pointers = new int[2];
int [] start_ref_pointers = new int[2]; //{earlist, reference} - reference may be center
boolean first_in_series = true;
while ((ref_index < 0) || ((ref_index + 1) >= min_num_scenes)) {
String model_directory = opticalFlow.buildSeries(
......@@ -8642,7 +8642,7 @@ if (debugLevel > -100) return true; // temporarily !
continue; // and go to the to next scene sequence from the list
}
first_in_series = false; // at least once success in this series
System.out.println("adding to video_sets_list start="+start_ref_pointers[0]+", end="+start_ref_pointers[1]);
System.out.println("adding to video_sets_list start="+start_ref_pointers[0]+", end="+ref_index); // start_ref_pointers[1]);
for (int i= 0; i < video_list[0].length; i++) {
System.out.println(i+": "+video_list[0][i]+", "+widths_list[0][i]);
}
......@@ -8650,7 +8650,7 @@ if (debugLevel > -100) return true; // temporarily !
video_list[0], // String [] paths,
widths_list[0], // int [] stereo_widths,
start_ref_pointers[0], // int earliest_scene,
start_ref_pointers[1])); // int reference_scene);
ref_index)); // start_ref_pointers[1])); // int reference_scene);
String series_action = (start_ref_pointers[0] < (min_num_scenes-1))?"is FINISHED ":("will continue down from scene "+(start_ref_pointers[0]));
System.out.println("\n ----- PROCESSING SCENE SEQUENCE "+nseq+" (last is "+(num_seq-1)+") "+series_action+" in "+
......
src/main/java/com/elphel/imagej/tileprocessor/sfm/StructureFromMotion.java
View file @ 793b2588
......@@ -798,13 +798,10 @@ public class StructureFromMotion {
final double mb_max_gain,
final boolean batch_mode,
final int debugLevel) {
boolean show_disp_corr_ind = false; //true;
boolean show_disp_corr = false; //true;
boolean show_disp_seq = true;
show_disp_corr_ind &= !batch_mode;
show_disp_corr &= !batch_mode;
show_disp_seq &= !batch_mode;
boolean save_disp_seq = clt_parameters.imp.sfm_save_seq; //true;
boolean show_disp_seq = clt_parameters.imp.sfm_show_seq &= !batch_mode; //true;
boolean show_disp_corr_ind = clt_parameters.imp.sfm_show_corr_ind &= !batch_mode; //false; //true;
boolean show_disp_corr = clt_parameters.imp.sfm_show_corr &= !batch_mode; //false; //true;
final int dbg_tile = -( 52+58*80);
final double sfm_min_base = clt_parameters.imp.sfm_min_base;
......@@ -901,7 +898,7 @@ public class StructureFromMotion {
interscene_ref_disparity =ref_scene.getDLS()[use_lma_dsi?1:0];
}
final double [] ref_disparity = interscene_ref_disparity;
double [][] disp_adj = show_disp_seq? (new double [num_readjust+1][]):null;
double [][] disp_adj = (show_disp_seq || save_disp_seq)? (new double [num_readjust+2][]):null;
if (disp_adj != null) {
disp_adj[0] = ref_disparity.clone();
}
......@@ -982,6 +979,7 @@ public class StructureFromMotion {
if (debugLevel > -3) {
System.out.println("sfmPairsSet(): No good sets with sufficient base found, bailing out from SfM ranging.");
}
return null;
}
for (int ntry = 0; ntry < num_readjust; ntry++) {
final SfmCorr [] sfmCorrCombo = new SfmCorr [tilesX*tilesY];
......@@ -1263,13 +1261,36 @@ public class StructureFromMotion {
}
} // ntry
if (disp_adj != null) {
ShowDoubleFloatArrays.showArrays(
disp_adj,
tilesX,
tilesY,
true,
"SfM-disparity-multi-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
scene_pairs_sets[0][num_set_pairs[0]-1][0].getImageName()+"-"+num_set_pairs.length);
disp_adj[disp_adj.length-1] = ref_sfm_gain;
String [] tiltes = new String[disp_adj.length];
tiltes[0] = "orig";
tiltes[disp_adj.length-1] = "gain";
for (int ntry = 0; ntry < num_readjust; ntry++) {
tiltes[ntry + 1]="pass-"+ntry;
}
// String dbg_title = "SfM-disparity-multi-"+scene_pairs_sets[0][num_set_pairs[0]-1][1].getImageName()+"-"+
// scene_pairs_sets[0][num_set_pairs[0]-1][0].getImageName()+"-"+num_set_pairs.length;
//ref_scene.
String suffix = String.format("-SfM%02d-%02d", num_set_pairs.length, ref_scene.getNumAccum());
String title = ref_scene.getImageName()+suffix;
if (show_disp_seq) {
ShowDoubleFloatArrays.showArrays(
disp_adj,
tilesX,
tilesY,
true,
title,
tiltes);
}
if (save_disp_seq) {
ref_scene.saveDoubleArrayInModelDirectory( // error
suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsn_final, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
}
}
// Re-calc BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP] = ref_disparity.clone();
......
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