Commit 793b2588 authored by Andrey Filippov's avatar Andrey Filippov

Next snapshot, testing with longer sequences

parent 47bf5840
......@@ -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());
......
......@@ -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) {
......
......@@ -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
......
......@@ -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();
......
......@@ -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(
if (dxyzatr == null) {
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];
}
}
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,12 +1234,25 @@ public class QuadCLTCPU {
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] dls = getDLS();
if ((min_sfm_gain> 0) && (dls[3] != null)) {
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) {
return null;
......
......@@ -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,12 +2599,26 @@ public class TexturedModel {
scenes[ref_index], // QuadCLT scene,
debugLevel); // int debug_level)
if (sfm_gain != null) {
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.");
}
}
}
double[][] ds_fg_bg = {ds_fg[0], ds_bg[0].clone()};
......
......@@ -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 "+
......
......@@ -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) {
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,
"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);
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