Skip to content

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

implementing readjust for cuas

parent 12743f36
Hide whitespace changes
Inline Side-by-side
Showing with 1787 additions and 567 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ f6017348
......@@ -266,7 +266,8 @@ public class Interscene {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
// ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
cent_index, // earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -340,7 +341,8 @@ public class Interscene {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
cent_index,// ref_indx,
// cent_index,// ref_indx,
quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -376,7 +378,8 @@ public class Interscene {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
cent_index,// ref_indx,
// cent_index,// ref_indx,
quadCLTs[cent_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -1438,8 +1441,8 @@ public class Interscene {
int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index. Set if non-null
final boolean updateStatus,
final int debugLevel) {
final int last_index = start_ref_pointers[1];
final int earliest_scene = start_ref_pointers[0];
int last_index = start_ref_pointers[1];
int earliest_scene = start_ref_pointers[0];
RMSEStats rmse_stats = new RMSEStats();
RMSEStats rmse_stats_metric = clt_parameters.imp.eig_use ? (new RMSEStats()):null;
double min_ref_str = clt_parameters.imp.min_ref_str;
......@@ -1449,6 +1452,7 @@ public class Interscene {
boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available
boolean lock_position = true; // clt_parameters.imp.lock_position;
boolean ims_use = clt_parameters.imp.ims_use;
boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
if (!ims_use) {
System.out.println("setInitialOrientationsCuas(): IMS use is disabled");
......@@ -1721,6 +1725,17 @@ public class Interscene {
}
}
}
if (!adjust_OK) {
earliest_scene = scene_index + 1;
if (debugLevel > -4) {
System.out.println("Pass multi scene "+scene_index+" (of "+ quadCLTs.length+") "+
center_CLT.getImageName() + "/" + scene_QuadClt.getImageName()+
" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
}
}
// update last_corr_xyzatr, // current correction
double [][] corr_diff = ErsCorrection.combineXYZATR(
scenes_xyzatr[scene_index], // new
......@@ -1788,6 +1803,25 @@ public class Interscene {
}
System.out.println();
}
if (generate_egomotion) {
String ego_path = center_CLT.getX3dDirectory()+Prefs.getFileSeparator()+
center_CLT.getImageName()+
"-ego-initial-cuas-"+center_CLT.getNumOrient()+".csv";
String ego_comment = null;
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
center_CLT,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
}
center_CLT.set_orient(1); // first orientation
center_CLT.set_accum(0); // reset accumulations ("build_interscene") number
center_CLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
......@@ -2490,6 +2524,110 @@ public class Interscene {
}
}
public static void readjustShowCorrected(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
double [][] dxyzatr_dt_nscene,
double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
double mb_max_gain,
double [] mb_ref_disparity,
QuadCLT ref_scene,
int nscene,
int debug_scene, // = 68
int debugLevel) {
QuadCLT scene = quadCLTs[nscene];
boolean show_corrected = false;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
int tilesX = ref_scene.getTileProcessor().getTilesX();
int tilesY = ref_scene.getTileProcessor().getTilesY();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
if (nscene == debug_scene) {
System.out.println("2.nscene = "+nscene);
System.out.println("3.nscene = "+nscene);
}
dbg_mb_img[nscene] = new double [tilesX*tilesY*2];
Arrays.fill(dbg_mb_img[nscene],Double.NaN);
String ts = quadCLTs[nscene].getImageName();
double [] mb_scene_xyz = ers_reference.getSceneXYZ(ts);
double [] mb_scene_atr = ers_reference.getSceneATR(ts);
double [][] ref_pXpYD = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
mb_ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
double [][] motion_blur = OpticalFlow.getMotionBlur( // dxyzatr_dt[][] should not be scaled here
ref_scene, // QuadCLT ref_scene,
scene, // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
mb_scene_xyz, // double [] camera_xyz,
mb_scene_atr, // double [] camera_atr,
dxyzatr_dt_nscene[0], // double [] camera_xyz_dt,
dxyzatr_dt_nscene[1], // double [] camera_atr_dt,
-1, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
for (int nTile = 0; nTile < motion_blur[0].length; nTile++) {
int tx = nTile % tilesX;
int ty = nTile / tilesX;
dbg_mb_img[nscene][tx + tilesX * (ty*2 +0)] = mb_tau * motion_blur[0][nTile];
dbg_mb_img[nscene][tx + tilesX * (ty*2 +1)] = mb_tau * motion_blur[1][nTile];
}
while (show_corrected) {
ImagePlus imp_mbc = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
false, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
null, // double [][] ref_pXpYD, // alternative to disparity_ref when reference is not uniform
// motion blur compensation
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
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
scene, // final QuadCLT scene,
ref_scene, // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
scene.getImageName()+"-MOTION_BLUR_CORRECTED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc.show();
ImagePlus imp_mbc_merged = QuadCLT.renderGPUFromDSI(
-1, // final int sensor_mask,
true, // final boolean merge_channels,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
mb_ref_disparity, // double [] disparity_ref,
null, // double [][] ref_pXpYD, // alternative to disparity_ref when reference is not uniform
// motion blur compensation
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
motion_blur, // double [][] mb_vectors, //
mb_scene_xyz, // ZERO3, // final double [] scene_xyz, // camera center in world coordinates
mb_scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
scene, // final QuadCLT scene,
ref_scene, // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB,
clt_parameters.imp.show_color_nan,
quadCLTs[nscene].getImageName()+"-MOTION_BLUR_CORRECTED-MERGED", // String suffix,
THREADS_MAX, // int threadsMax,
debugLevel); // int debugLevel)
imp_mbc_merged.show();
show_corrected = false; // manually set/reset
}
}
// double avg_rlen,// >= 1.0
/**
* Re-adjust camera poses with LMA after initial poses are already known.
......@@ -2730,7 +2868,8 @@ public class Interscene {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
// ref_index,
quadCLTs[ref_index],// QuadCLT ref_scene, // may be one of scenes or center
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
......@@ -2985,7 +3124,7 @@ public class Interscene {
dbg_mb_img, // double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
mb_max_gain, // double mb_max_gain,
mb_ref_disparity, // double [] mb_ref_disparity,
ref_index, // int ref_index,
quadCLTs[ref_index],// QuadCLT ref_scene
nscene, // int nscene,
debug_scene, // = 68 int debug_scene, // = 68
debugLevel); // int debugLevel);
......@@ -3383,124 +3522,752 @@ public class Interscene {
quadCLTs[ref_index].saveStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String suffix)
return earliest_scene;
}
/**
* Modify initial ATR to match change of the XYZ from the already adjusted pose.
* Uses the fact that movement in X is approximate equivalent to Azimuth rotation
* (positive X has the same effect as negative A multiplied by Z) and Y-movement
* has the same sign to Tilt.
* @param cur_xyzatr current (already adjusted by the image-based LMA) {{x,y,z},{a,t,r}
* @param new_xyz new (target from the IMS) {x,y,z}
* @param avg_z average altitude (Z)
* @return new pose {{x,y,z},{a,t,r} expected to have a close match between scenes
*/
public static double [][] modifyATRtoXYZ(
double [][] cur_xyzatr, // careful with Z - using the new one
double [] new_xyz,
double avg_z
){
double dx = new_xyz[0] - cur_xyzatr[0][0];
double dy = new_xyz[1] - cur_xyzatr[0][1];
double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
return new_xyzatr;
}
// Make it the only entry point
// uses *_dt from
// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()
public static double[][] adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
public static int reAdjustPairsLMAIntersceneCuas( // after combo dgi is available and preliminary poses are known
CLTParameters clt_parameters,
boolean use_lma_dsi,
boolean fpn_disable, // disable fpn filter if images are known to be too close
double mb_max_gain,
boolean use_R,
boolean disable_ers,
double [] min_max, // null or pair of minimal and maximal offsets
int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
QuadCLT [] quadCLTs,
int ref_index,
TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
double [][] pXpYD_ref, // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null
// will be recalculated when tp_tasks_ref[0] == null, but for reference frame
int nscene0, // may be == ref_index
int nscene1, // compares to nscene0
double [] ref_disparity, // null or alternative reference disparity
boolean [] reliable_ref, // null or bitmask of reliable reference tiles
boolean smooth_disparity, // smooth disparity (according to clt_parameters)+update reliable_ref if true
double [][] scene0_xyzatr,
double [][] scene1_xyzatr,
double average_z,
double [][] scene1_xyzatr_pull,
boolean[] param_select,
double [] param_regweights,
double [] rms_out, // null or double [2]
double max_rms,
boolean mb_en,
double mb_tau, // 0.008; // time constant, sec
double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
boolean disable_ers_y,
boolean disable_ers_r,
boolean lma_xyzatr,
boolean configured_lma,
double avg_rlen,
double reg_weight_xy, // regularization weight for X and Y when lpf_xy or readjust_xy_ims
boolean [] reliable_ref, // null or bitmask of reliable reference tiles
QuadCLT [] quadCLTs,
QuadCLT center_CLT, // contains center CLT and DSI. It has zeros fo coordinates, scenes are already around it
int [] range,
int ers_mode, // 0 - keep, 1 - set from velocity, 2 - set from IMS
boolean test_motion_blur,
int debugLevel)
{
double ref_sigma = clt_parameters.imp.ref_sigma; //10.0; // minimal value of the SfM gain maximum to consider available
{
System.out.println("reAdjustPairsLMAIntersceneCuas(): using mb_max_gain="+mb_max_gain+
", disable_ers="+disable_ers+", disable_ers_y="+disable_ers_y+
", use_R="+use_R);
/// boolean freeze_xy_pull = clt_parameters.imp.freeze_xy_pull; // true; // false; // true; // debugging freezing xy to xy_pull
boolean copy_pull_current = clt_parameters.imp.copy_pull_current; //false; // true;
boolean restore_imu = clt_parameters.imp.restore_imu; //false; // restore imu omega-az and omega-tl, freeze ERS, adjust X,Y,Z,Az,Tl,Rl
boolean lock_position = true; // clt_parameters.imp.lock_position;
double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain
boolean apply_nan = true;
if (clt_parameters.imp.ref_smooth_always) {
smooth_disparity=true; // **********************************************
// boolean ref_smooth = clt_parameters.imp.ref_smooth; //true; // use SfM filtering if available
boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect(
lma_xyzatr, // boolean use_Z,
lma_xyzatr || use_R, // boolean use_R,
lma_xyzatr, // false only in mode c): freeze X,Y// boolean use_XY
lma_xyzatr, // || ( readjust_xy_ims || lpf_xy), // boolean use_AT,
!disable_ers, //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
!disable_ers_y, // boolean use_ERS_tilt,
!disable_ers_r); // boolean use_ERS_roll);
if (lock_position) {
for (int i:ErsCorrection.DP_XYZ_INDICES) param_select[i] = false;
for (int i:ErsCorrection.DP_AT_INDICES) param_select[i] = true;
}
final double [] param_regweights = configured_lma? clt_parameters.ilp.ilma_regularization_weights :
ErsCorrection.getParamRegWeights(
0.0); // double reg_weight,
boolean fail_on_zoom_roll=false; // it should fail on initial adjustment
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
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
boolean use_precomp = clt_parameters.imp.use_precomp;// try to predict initial error from previous scenes
// boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
if (ref_disparity == null) {
ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
}
if (smooth_disparity) {
double [] dbg_disparity = (debugLevel > 2) ? ref_disparity.clone() : null;
boolean [] dbg_reliable = ((dbg_disparity != null) && (reliable_ref != null)) ? reliable_ref.clone(): null;
ref_disparity = quadCLTs[ref_index].smoothDisparity(
ref_disparity, // double [] disparity_in, (will not modify)
reliable_ref, // boolean [] reliable_ref, // optional
ref_sigma, // double sigma,
ref_smooth_diff, // double max_diff,
apply_nan); // boolean apply_nan)
if (dbg_disparity != null) {
String [] dbg_titles = {"ref", "smooth", "reliable_in", "reliable_out"};
String dbg_title = quadCLTs[ref_index].getImageName()+"-smooth_ref";
double [][] dbg_img = new double [dbg_titles.length][ref_disparity.length];
for (int i = 0; i < dbg_img.length; i++) {
Arrays.fill(dbg_img[i], Double.NaN);
}
dbg_img[0] = dbg_disparity;
dbg_img[1] = ref_disparity;
for (int i = 0; i < ref_disparity.length; i++) {
if (dbg_reliable != null) {
dbg_img[2][i] = dbg_reliable[i] ? 10.0: Double.NaN;
}
if (reliable_ref != null) {
dbg_img[3][i] = reliable_ref[i] ? 10.0: Double.NaN;
}
}
int dbg_width = quadCLTs[ref_index].getTileProcessor().getTilesX();
int dbg_height = quadCLTs[ref_index].getTileProcessor().getTilesY();
ShowDoubleFloatArrays.showArrays(
dbg_img,
dbg_width,
dbg_height,
true,
dbg_title,
dbg_titles);
}
// Set up velocities from known coordinates, use averaging
double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
boolean debug_ers = clt_parameters.ilp.ilma_debug_ers ; // true;
////Used in debug_ers
double avg_z = center_CLT.getAverageZ(true); // for offset estimation
if (debugLevel > -3) {
System.out.println("reAdjustPairsLMAIntersceneCuas(): avg_z="+avg_z);
}
// TODO: set reference as new version of adjustPairsLMAInterscene() assumes it set
if ((tp_tasks_ref == null) || (tp_tasks_ref[0] == null)) { // calculate and setup reference (for comparison) scene to GPU
if (tp_tasks_ref == null) {
tp_tasks_ref = new TpTask[2][];
}
int margin = clt_parameters.imp.margin;
double [][] pXpYD_ref1 = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
String dbg_ers_string =
((dbg_scale_dt[1][0] > 0)?"a":((dbg_scale_dt[1][0] < 0) ? "A":"0"))+
((dbg_scale_dt[1][1] > 0)?"t":((dbg_scale_dt[1][1] < 0) ? "T":"0"))+
((dbg_scale_dt[1][2] > 0)?"r":((dbg_scale_dt[1][2] < 0) ? "R":"0"))+
((dbg_scale_dt[0][0] > 0)?"x":((dbg_scale_dt[0][0] < 0) ? "X":"0"))+
((dbg_scale_dt[0][1] > 0)?"y":((dbg_scale_dt[0][1] < 0) ? "Y":"0"))+
((dbg_scale_dt[0][2] > 0)?"z":((dbg_scale_dt[0][2] < 0) ? "Z":"0"));
boolean mb_en = clt_parameters.imp.mb_en;
double mb_tau = clt_parameters.imp.mb_tau; // 0.008; // time constant, sec
// int margin = clt_parameters.imp.margin;
double second_margin = 1.1; // allow slightly larger offset for readjustments (to be limited by the first pass)
double boost_max_short = 5.0; // 2.0; // Some bug?
int earliest_scene = range[0];
int last_scene = range[1];
boolean use_combo_dsi = clt_parameters.imp.use_combo_dsi;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
int tilesX = center_CLT.getTileProcessor().getTilesX();
int tilesY = center_CLT.getTileProcessor().getTilesY();
int tile_size = center_CLT.getTileProcessor().getTileSize();
double min_offset = clt_parameters.imp.min_offset;
double max_offset = boost_max_short*second_margin*clt_parameters.imp.max_rel_offset * tilesX * tile_size;
double max_roll = second_margin*clt_parameters.imp.max_roll_deg*Math.PI/180.0;
double max_zoom_diff = second_margin*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)
int [] fail_reason = new int[1]; // null or int[1]: 0 - OK, 2 - LMA, 3 - min, 4 - max
System.out.println("reAdjustPairsLMAInterscene(): min_offset="+min_max[0]+", max_offset="+min_max[1]);
double [] disparity_raw = new double [tilesX * tilesY];
Arrays.fill(disparity_raw,clt_parameters.disparity);
double [][] combo_dsn_final = center_CLT.restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
double [][] dls = { // Update to use FG? Or FG/no BG?
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_DISP],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsn_final[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
center_CLT, // QuadCLT scene,
debugLevel);
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
}
double [][] dbg_mb_img = null;
double [] mb_ref_disparity =null;
// if (test_motion_blur) {
mb_ref_disparity = interscene_ref_disparity;
if (mb_ref_disparity == null) {
mb_ref_disparity = center_CLT.getDLS()[use_lma_dsi?1:0];
}
if (test_motion_blur) {
dbg_mb_img = new double[quadCLTs.length][];
}
ErsCorrection ers_reference = center_CLT.getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
double [][][] scenes_xyzatr_pull = new double [quadCLTs.length][][];
double [][][] dxyzatr_dt = new double [quadCLTs.length][][];
//// scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
// should have at least next or previous non-null
int debug_scene = 161; // 69; // -68; // -8;
RMSEStats rmse_stats = new RMSEStats();
RMSEStats rmse_stats_metric = clt_parameters.imp.eig_use ? (new RMSEStats()):null;
if (debug_ers) {
System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
}
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// just checking it is not isolated
if (quadCLTs[nscene] == null) {
earliest_scene = nscene + 1;
break;
}
// now reference scene should also be in ers_reference.getSceneXYZ(ts)
String ts = quadCLTs[nscene].getImageName();
if ((ers_reference.getSceneXYZ(ts)== null) || (ers_reference.getSceneATR(ts)== null)) {
earliest_scene = nscene + 1;
break;
}
scenes_xyzatr[nscene] = ers_reference.getSceneXYZATR(ts);
if (lock_position) {
scenes_xyzatr[nscene][0] = new double [3];
}
// scenes_xyzatr_pull[nscene] = scenes_xyzatr[nscene].clone();
scenes_xyzatr_pull[nscene] = new double[][] {scenes_xyzatr[nscene][0].clone(),scenes_xyzatr[nscene][1].clone()};
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
}
boolean set_line_time = debugLevel>1000;
if (set_line_time) {
double line_time = 27.778E-6;
System.out.println("line_time="+line_time);
for (int nscene = earliest_scene; nscene <= last_scene; nscene++) {
ErsCorrection ec = quadCLTs[nscene].getErsCorrection();
ec.line_time = line_time;
}
}
switch (ers_mode) {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
center_CLT, // quadCLTs[ref_index],// QuadCLT ref_scene, // may be one of scenes or center
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range, // double half_run_range
debugLevel); // int debugLevel);
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,
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(
!lock_position, // true, // boolean use_Z,
true, // boolean use_R,
!lock_position, // true, // boolean use_XY
true, // boolean use_AT,
false, // boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
false, // boolean use_ERS_tilt);
false); // boolean use_ERS_roll);
}
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
// double avg_rlen
// readjust_xy_ims = false;
if (debug_ers) {
System.out.println(String.format(
"%3s,%9s,%9s,%9s,,%9s,%9s,%9s,,%9s,%9s,%9s,,%s,%9s,%9s,,%9s,%9s,%9s,%9s",
"N","X","Y","Z","A","T","R","X-pull","Y-pull","Z-pull","A-pull","T-pull","R-pull",
"cent-dist","quad-dist","cent-rect","quad-rect"));
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// All 4 variants - just for testing. Normally should try center first and use
// quad if there is no significant movement
double est_center = center_CLT.estimateAverageShift(
new double[2][3], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
false); // boolean rectilinear)
double est_quad = center_CLT.estimateAverageShift(
new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
false); // boolean rectilinear)
double est_center_rectilinear = center_CLT.estimateAverageShift(
new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
true); // boolean rectilinear)
double est_quad_rectilinear = center_CLT.estimateAverageShift(
new double[2][3], // scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
true); // boolean rectilinear)
System.out.println(String.format("%3d,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.5f,%9.5f,%9.5f,,%9.4f, %9.4f, %9.4f, %9.4f",
nscene,
scenes_xyzatr[nscene][0][0], scenes_xyzatr[nscene][0][1], scenes_xyzatr[nscene][0][2],
scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2],
scenes_xyzatr_pull[nscene][0][0], scenes_xyzatr_pull[nscene][0][1], scenes_xyzatr_pull[nscene][0][2],
scenes_xyzatr_pull[nscene][1][0], scenes_xyzatr_pull[nscene][1][1], scenes_xyzatr_pull[nscene][1][2],
est_center, est_quad, est_center_rectilinear, est_quad_rectilinear));
}
System.out.println();
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
System.out.println(String.format("%3d xyz_dt: %9.5f %9.5f %9.5f atr_dt: %9.5f %9.5f %9.5f",
nscene,
dxyzatr_dt[nscene][0][0], dxyzatr_dt[nscene][0][1], dxyzatr_dt[nscene][0][2],
dxyzatr_dt[nscene][1][0], dxyzatr_dt[nscene][1][1], dxyzatr_dt[nscene][1][2]));
}
System.out.println();
}
double max_z_change = Double.NaN; // only applicable for drone images
/*
if (max_zoom_diff > 0) { // ignore if set to
max_z_change = avg_z * max_zoom_diff;
if (fail_on_zoom_roll) {
if (debugLevel > -3) {
System.out.println("Setting maximal Z-direction movement to "+ max_z_change+" m.");
}
}
}
*/
int [] scene_seq = new int [last_scene-earliest_scene+1];
{
int indx=0;
for (int i = last_scene; i >= earliest_scene; i--) {
scene_seq[indx++] = i;
}
/*
for (int i = ref_index; i >= earliest_scene; i--) {
scene_seq[indx++] = i;
}
for (int i = ref_index+1; i <= last_scene; i++) {
scene_seq[indx++] = i;
}
*/
}
TpTask[][] tp_tasks_ref = new TpTask[2][];
double [][] pXpYD_ref = new double [tilesX*tilesY][];
// ArrayList<Integer> fpn_list = new ArrayList<Integer>();
boolean fpn_disable = false; // estimate they are too close
final double max_rms = clt_parameters.imp.eig_use? clt_parameters.imp.eig_max_rms: clt_parameters.imp.max_rms;
//add test
boolean test_adjust = debugLevel > 1000;
// Main cycle - first goes down from the center, then up, and finally processes "fpn" scenes (close to reference)
// for CUAS - always down
double [][] precomp_xyzatr = new double [2][3];
int last_processed_scene = -2; // none
double [] center_disparity = mb_ref_disparity; // ??? use after filter?
// prepare reference scene GPU
float [][] fclt = new float [][] {center_CLT.getCenterClt()};
// set center tasks and CLT
double [][] pXpYD_center = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
center_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
center_CLT, // final QuadCLT scene_QuadClt,
center_CLT); // final QuadCLT reference_QuadClt)
TpTask [][] tp_tasks_center = setReferenceGPU (
clt_parameters, // CLTParameters clt_parameters,
center_CLT, // QuadCLT ref_scene,
center_disparity, // null // double [] ref_disparity, // null or alternative reference disparity
pXpYD_center, // double [][] ref_pXpYD,
fclt, // final float [][] fclt,
reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
0, // margin, // final int margin, // margin do not use tiles with centers closer than this to the edges. Measured in pixels.
// motion blur compensation
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
null, // double [][] mb_vectors, // now [2][ntiles]; // center does not move
debugLevel); // int debug_level)
/*
* There is still a problem with a large mismatch at start position for far off-reference scenes,
* especially those that were not refined after inversion - reversing order of earlier processed "later"
* half (after the reference in the center) earlier processed as the bottom half of the other sub-sequence.
*
* For mitigation we'll use that both half-sequences start from the center with small mismatches. The
* precomp_xyzatr will be used to store how much LMA corrected from the original estimation and add it
* to the next scene (only when it is the next), similar to the initial orientation.
*/
for (int nscene:scene_seq) {
if (nscene == debug_scene) {
System.out.println("nscene = "+nscene);
System.out.println("nscene = "+nscene);
}
// skipping shift for CUAS
/*
double est_shift = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
false, // boolean use_rot,
fmg_rectilinear); // boolean rectilinear)
if (est_shift < fmg_max_quad) {
est_shift = quadCLTs[ref_index].estimateAverageShift(
scenes_xyzatr[ref_index], // double [][] xyzatr0,
scenes_xyzatr[nscene], // double [][] xyzatr1,
avg_z, // double average_z,
true, // boolean use_rot,
fmg_rectilinear); // boolean rectilinear)
}
if (debugLevel > -4) {
System.out.println("nscene="+nscene+": est offset "+
est_shift+"pix");
}
*/
// just checking it is not isolated
if ((quadCLTs[nscene] == null) ||
( //// (nscene != ref_index) &&
((ers_reference.getSceneXYZ(quadCLTs[nscene].getImageName())== null) ||
(ers_reference.getSceneATR(quadCLTs[nscene].getImageName())== null)))) {
earliest_scene = nscene + 1;
break;
}
String ts = quadCLTs[nscene].getImageName();
// it will be modified inside LMA, so needs to be backuped/restored to undo
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[nscene]));
if (dbg_mb_img != null) { // fill in 1 slice of dbg_mb_img, show all at once later
readjustShowCorrected(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
dxyzatr_dt[nscene], // double [][] dxyzatr_dt_nscene,
dbg_mb_img, // double [][] dbg_mb_img, // initialize to double[quadCLTs.length][]
mb_max_gain, // double mb_max_gain,
mb_ref_disparity, // double [] mb_ref_disparity,
center_CLT, // QuadCLT ref_scene
nscene, // int nscene,
debug_scene, // = 68 int debug_scene, // = 68
debugLevel); // int debugLevel);
}
double [] lma_rms = new double[clt_parameters.imp.eig_use? 6 : 4]; // [2];
double [][] initial_pose = scenes_xyzatr[nscene].clone();
boolean adjust_OK = false;
// if (est_shift < min_max[0]) {
// fail_reason[0]=FAIL_REASON_MIN;
// } else {
double [][] corr_xyzatr_pull = new double [][] {scenes_xyzatr_pull[nscene][0].clone(), scenes_xyzatr_pull[nscene][1].clone()};
/*
* scenes_xyzatr[ref_index],// double [][] scene0_xyzatr, - scene to compare to
* scenes_xyzatr[nscene], // double [][] scene1_xyzatr - previous known pose?
* scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull, - now not a pull,
* but a rigid target set as initial approximation
*/
boolean applied_precomp = false;
if (use_precomp && (last_processed_scene >= nscene-1) && (last_processed_scene <= nscene+1)) {
corr_xyzatr_pull=ErsCorrection.combineXYZATR(
scenes_xyzatr_pull[nscene],
precomp_xyzatr);
applied_precomp = true;
if (debugLevel > -2) {
System.out.println(String.format(
"Applied precompensation: [%9.6f, %9.6f, %9.6f] [%9.6f, %9.6f, %9.6f]",
precomp_xyzatr[0][0], precomp_xyzatr[0][1], precomp_xyzatr[0][2],
precomp_xyzatr[1][0], precomp_xyzatr[1][1], precomp_xyzatr[1][2]));
}
}
double [][] mb_vectors_scene1 = null;
if (mb_en) {
double [][] scene1_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[nscene].getErsCorrection().getErsXYZATR_dt());
mb_vectors_scene1 = OpticalFlow.getMotionBlur(
center_CLT, // QuadCLT ref_scene,
quadCLTs[nscene], // QuadCLT scene, // can be the same as ref_scene
pXpYD_center, // double [][] ref_pXpYD, // here it is scene, not reference!
initial_pose[0], // scene1_xyzatr[0], // double [] camera_xyz,
initial_pose[1], // double [] camera_atr,
scene1_xyzatr_dt[0], // double [] camera_xyz_dt,
scene1_xyzatr_dt[1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
scenes_xyzatr[nscene] = adjustPairsLMAInterscene( // assumes reference GPU data is already set - once for multiple scenes
clt_parameters, // CLTParameters clt_parameters,
true, // boolean initial_adjust,
false, // boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers, // boolean disable_ers,
min_max, // double [] min_max, // null or pair of minimal and maximal offsets
fail_reason, // int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
center_CLT, // QuadCLT ref_QuadClt, // reference scene for ref_disparity, zero xyz and atr
center_disparity, // double [] ref_disparity, // null or alternative reference disparity
center_CLT, // QuadCLT first_QuadClt, // First in comparison pair
pXpYD_center, // double [][] pXpYD_ref, // pXpYD for the reference scene
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
tp_tasks_center[0], // TpTask[] tp_tasks_ref, // only (main if MB correction) tasks for FPN correction
quadCLTs[nscene], // QuadCLT scene_QuadClt,
initial_pose, // double [][] camera_xyzatr,
initial_pose, // double [][] scene_xyzatr_pull, // if both are not null, specify target values to pull to
new double[2][3], // double [][] ref_xyzatr,
param_select, // boolean[] param_select,
param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
max_rms, // double max_rms,
// motion blur compensation
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
mb_vectors_scene1, // double [][] mb_vectors, // now [2][ntiles];
debugLevel); // int debug_level)
adjust_OK = scenes_xyzatr[nscene] != null;
if (!adjust_OK) {
last_processed_scene = -2;
} else if (use_precomp) {
double [][] last_corr_xyzatr = ErsCorrection.combineXYZATR(
scenes_xyzatr[nscene],
ErsCorrection.invertXYZATR(corr_xyzatr_pull));
// add precomp_xyzatr if it was applied:
if (applied_precomp) {
precomp_xyzatr = ErsCorrection.combineXYZATR(
precomp_xyzatr,
last_corr_xyzatr);
} else { // just use difference
precomp_xyzatr = last_corr_xyzatr;
}
last_processed_scene = nscene;
}
if (adjust_OK && fail_on_zoom_roll) { // check only for initial orientation, do not check on readjustments
if (Math.abs(scenes_xyzatr[nscene][1][2]) > max_roll) {
fail_reason[0] = FAIL_REASON_ROLL;
adjust_OK = false;
}
if (max_zoom_diff > 0) { // ignore if set to
if (Math.abs(scenes_xyzatr[nscene][0][2]) > max_z_change) { // NaN OK - will not fail
fail_reason[0] = FAIL_REASON_ZOOM;
adjust_OK = false;
}
}
}
/*
// FAIL_REASON_ROLL
handle_failure: {
if (!adjust_OK) {
scenes_xyzatr[nscene] = initial_pose.clone(); // restore pre-adjust
boolean OK = false;
System.out.println("LMA failed at nscene = "+nscene+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
// all other reasons lead to failure
earliest_scene = nscene + 1;
if (debugLevel > -4) {
System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
center_CLT.getImageName() + "/" + ts+
" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
}
// set this and all previous to null
for (; nscene >= 0 ; nscene--) {
ers_reference.addScene(quadCLTs[nscene].getImageName()); // remove
quadCLTs[nscene] = null; // completely remove early scenes?
}
break;
}
}
*/
if (!adjust_OK) {
earliest_scene = nscene + 1;
if (debugLevel > -4) {
System.out.println("Pass multi scene "+nscene+" (of "+ quadCLTs.length+") "+
center_CLT.getImageName() + "/" + quadCLTs[nscene].getImageName()+
" FAILED. Setting earliest_scene to "+earliest_scene + " Failure reason: " + fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
}
}
// overwrite old data
// undo velocities->ers scaling
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[nscene].getErsCorrection().getErsXYZATR_dt(
));
ers_reference.addScene(ts,
scenes_xyzatr[nscene][0],
scenes_xyzatr[nscene][1],
adjusted_xyzatr_dt[0],
adjusted_xyzatr_dt[1]
);
rmse_stats.add(lma_rms[0]);
if ((rmse_stats_metric != null) && (lma_rms.length >=4)) {
rmse_stats_metric.add(lma_rms[4]);
}
if (debugLevel > -3) {
System.out.println("reAdjustPairsLMAInterscene "+nscene+" (of "+ quadCLTs.length+") "+
center_CLT.getImageName() + "/" + ts+
" Done. Weight = "+lma_rms[2]+", number="+lma_rms[3]);
System.out.print("RMS="+lma_rms[0]+
", maximal so far was "+rmse_stats.getMax()+", average was "+rmse_stats.getAverage());
if ((rmse_stats_metric != null) && (lma_rms.length >=6)) {
System.out.print(". Pixel RMS="+lma_rms[4]+
"pix, maximal so far was "+rmse_stats_metric.getMax()+
"pix, average was "+rmse_stats_metric.getAverage()+"pix.");
}
System.out.println();
}
} // for (int nscene = ref_index; nscene > earliest_scene; nscene--) {
// TODO: after all scenes done, see if any of scenes_xyzatr[] is null, and if fpn_rematch - rematch
if (dbg_mb_img != null) {
String [] dbg_mb_titles = new String[quadCLTs.length];
for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
dbg_mb_titles[i] = quadCLTs[i].getImageName();
}
ShowDoubleFloatArrays.showArrays(
dbg_mb_img,
tilesX * 2,
tilesY,
true,
center_CLT.getImageName()+"-MOTION_BLUR",
dbg_mb_titles);
}
if (debugLevel > -4) {
System.out.print("All multi scene passes are Done. Maximal RMSE was "+rmse_stats.getMax()+
", average was "+rmse_stats.getAverage());
if (rmse_stats_metric != null) {
System.out.print(". Maximal metric so far was "+rmse_stats_metric.getMax()+
"pix, average was "+rmse_stats_metric.getAverage()+"pix.");
}
System.out.println();
}
center_CLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
// Add to log
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Finished reAdjustPairsLMAInterscene():\n");
sb.append("getNumOrient()= "+center_CLT.getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
sb.append("getNumAccum()= "+center_CLT.getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("earliest_scene= "+range[0]+"\n");
//// sb.append("ref_index= "+ref_index+"\n");
sb.append("last_index= "+range[1]+"\n");
sb.append("Maximal RMSE= "+rmse_stats.getMax()+"\n");
sb.append("Average RMSE= "+rmse_stats.getAverage()+"\n");
if (rmse_stats_metric != null) {
sb.append("Maximal RMSE= "+rmse_stats_metric.getMax()+"pix\n");
sb.append("Average RMSE= "+rmse_stats_metric.getAverage()+"pix\n");
}
//// sb.append("lpf_xy= "+lpf_xy+"\n");
//// sb.append("readjust_xy_ims="+readjust_xy_ims+"\n");
sb.append("lma_xyzatr= "+lma_xyzatr+"\n");
sb.append("use_R= "+use_R+"\n");
sb.append("use_R= "+use_R+"\n");
sb.append("mb_max_gain= "+mb_max_gain+"\n");
sb.append("avg_z= "+avg_z+"m\n");
sb.append("reg_weight_xy= "+reg_weight_xy+"\n");
sb.append("disable_ers= "+disable_ers+"\n");
sb.append("disable_ers_y= "+disable_ers_y+"\n");
sb.append("disable_ers_r= "+disable_ers_r+"\n");
sb.append("use_precomp= "+use_precomp+"\n");
sb.append("------------------------\n\n");
center_CLT.saveStringInModelDirectory(sb.toString(), QuadCLT.ORIENTATION_LOGS_SUFFIX); // String suffix)
return earliest_scene;
}
/**
* Modify initial ATR to match change of the XYZ from the already adjusted pose.
* Uses the fact that movement in X is approximate equivalent to Azimuth rotation
* (positive X has the same effect as negative A multiplied by Z) and Y-movement
* has the same sign to Tilt.
* @param cur_xyzatr current (already adjusted by the image-based LMA) {{x,y,z},{a,t,r}
* @param new_xyz new (target from the IMS) {x,y,z}
* @param avg_z average altitude (Z)
* @return new pose {{x,y,z},{a,t,r} expected to have a close match between scenes
*/
public static double [][] modifyATRtoXYZ(
double [][] cur_xyzatr, // careful with Z - using the new one
double [] new_xyz,
double avg_z
){
double dx = new_xyz[0] - cur_xyzatr[0][0];
double dy = new_xyz[1] - cur_xyzatr[0][1];
double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
return new_xyzatr;
}
// Make it the only entry point
// uses *_dt from
// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()
public static double[][] adjustDiffPairsLMAInterscene( // compare two scenes, first may be reference, use motion blur
CLTParameters clt_parameters,
boolean use_lma_dsi,
boolean fpn_disable, // disable fpn filter if images are known to be too close
boolean disable_ers,
double [] min_max, // null or pair of minimal and maximal offsets
int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
QuadCLT [] quadCLTs,
int ref_index,
TpTask[][] tp_tasks_ref, // Should be TpTask[2][*]. If tp_tasks_ref[0] == null will calculate
// at set first scene to the GPU
double [][] pXpYD_ref, // should be set or at least double [num_tiles][] if tp_tasks_ref[0] == null
// will be recalculated when tp_tasks_ref[0] == null, but for reference frame
int nscene0, // may be == ref_index
int nscene1, // compares to nscene0
double [] ref_disparity, // null or alternative reference disparity
boolean [] reliable_ref, // null or bitmask of reliable reference tiles
boolean smooth_disparity, // smooth disparity (according to clt_parameters)+update reliable_ref if true
double [][] scene0_xyzatr,
double [][] scene1_xyzatr,
double average_z,
double [][] scene1_xyzatr_pull,
boolean[] param_select,
double [] param_regweights,
double [] rms_out, // null or double [2]
double max_rms,
boolean mb_en,
double mb_tau, // 0.008; // time constant, sec
double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
int debugLevel)
{
double ref_sigma = clt_parameters.imp.ref_sigma; //10.0; // minimal value of the SfM gain maximum to consider available
double ref_smooth_diff = clt_parameters.imp.ref_smooth_diff; // 0.75; // minimal fraction of the SfM maximal gain
boolean apply_nan = true;
if (clt_parameters.imp.ref_smooth_always) {
smooth_disparity=true; // **********************************************
}
// boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
if (ref_disparity == null) {
ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
}
if (smooth_disparity) {
double [] dbg_disparity = (debugLevel > 2) ? ref_disparity.clone() : null;
boolean [] dbg_reliable = ((dbg_disparity != null) && (reliable_ref != null)) ? reliable_ref.clone(): null;
ref_disparity = quadCLTs[ref_index].smoothDisparity(
ref_disparity, // double [] disparity_in, (will not modify)
reliable_ref, // boolean [] reliable_ref, // optional
ref_sigma, // double sigma,
ref_smooth_diff, // double max_diff,
apply_nan); // boolean apply_nan)
if (dbg_disparity != null) {
String [] dbg_titles = {"ref", "smooth", "reliable_in", "reliable_out"};
String dbg_title = quadCLTs[ref_index].getImageName()+"-smooth_ref";
double [][] dbg_img = new double [dbg_titles.length][ref_disparity.length];
for (int i = 0; i < dbg_img.length; i++) {
Arrays.fill(dbg_img[i], Double.NaN);
}
dbg_img[0] = dbg_disparity;
dbg_img[1] = ref_disparity;
for (int i = 0; i < ref_disparity.length; i++) {
if (dbg_reliable != null) {
dbg_img[2][i] = dbg_reliable[i] ? 10.0: Double.NaN;
}
if (reliable_ref != null) {
dbg_img[3][i] = reliable_ref[i] ? 10.0: Double.NaN;
}
}
int dbg_width = quadCLTs[ref_index].getTileProcessor().getTilesX();
int dbg_height = quadCLTs[ref_index].getTileProcessor().getTilesY();
ShowDoubleFloatArrays.showArrays(
dbg_img,
dbg_width,
dbg_height,
true,
dbg_title,
dbg_titles);
}
}
// TODO: set reference as new version of adjustPairsLMAInterscene() assumes it set
if ((tp_tasks_ref == null) || (tp_tasks_ref[0] == null)) { // calculate and setup reference (for comparison) scene to GPU
if (tp_tasks_ref == null) {
tp_tasks_ref = new TpTask[2][];
}
int margin = clt_parameters.imp.margin;
double [][] pXpYD_ref1 = OpticalFlow.transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
......@@ -6118,64 +6885,397 @@ public class Interscene {
null, // final boolean [] valid_tiles,
QuadCLT.THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
image_dtt.setReferenceTDMotionBlur( // tp_tasks_ref will be updated
image_dtt.setReferenceTDMotionBlur( // tp_tasks_ref will be updated
erase_clt,
null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
true, // final boolean use_reference_buffer,
tp_tasks_ref, // final TpTask[] tp_tasks,
clt_parameters.gpu_sigma_r, // final double gpu_sigma_r, // 0.9, 1.1
clt_parameters.gpu_sigma_b, // final double gpu_sigma_b, // 0.9, 1.1
clt_parameters.gpu_sigma_g, // final double gpu_sigma_g, // 0.6, 0.7
clt_parameters.gpu_sigma_m, // final double gpu_sigma_m, // = 0.4; // 0.7;
QuadCLT.THREADS_MAX, // final int threadsMax, // maximal number of threads to launch
debug_level); // final int globalDebugLevel);
} else {
tp_tasks_ref = new TpTask[1][];
tp_tasks_ref[0] = GpuQuad.setInterTasks( // "true" reference, with stereo actual reference will be offset
ref_scene.getNumSensors(),
ref_scene.getErsCorrection().getSensorWH()[0],
!ref_scene.hasGPU(), // final boolean calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
ref_pXpYD, // final double [][] pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
selection, // final boolean [] selection, // may be null, if not null do not process unselected tiles
ref_scene.getErsCorrection(), // final GeometryCorrection geometryCorrection,
disparity_corr, // final double disparity_corr,
margin, // final int margin, // do not use tiles if their centers are closer to the edges
null, // final boolean [] valid_tiles,
QuadCLT.THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
image_dtt.setReferenceTD( // tp_tasks_ref will be updated
fclt, // final float [][] fclt,
erase_clt,
null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
true, // final boolean use_reference_buffer,
tp_tasks_ref, // final TpTask[] tp_tasks,
tp_tasks_ref[0], // final TpTask[] tp_tasks,
clt_parameters.gpu_sigma_r, // final double gpu_sigma_r, // 0.9, 1.1
clt_parameters.gpu_sigma_b, // final double gpu_sigma_b, // 0.9, 1.1
clt_parameters.gpu_sigma_g, // final double gpu_sigma_g, // 0.6, 0.7
clt_parameters.gpu_sigma_m, // final double gpu_sigma_m, // = 0.4; // 0.7;
QuadCLT.THREADS_MAX, // final int threadsMax, // maximal number of threads to launch
debug_level); // final int globalDebugLevel);
}
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
return tp_tasks_ref;
}
public static void generateEgomotionTable(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
String path,
String comment,
int debugLevel) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
int num_processed = 0;
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
// for testing QuaternionLma
double [] rms = new double [2];
double [] enu_corr = new double[3];
double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
// ref_index, // int ref_index,
ref_scene, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
debugLevel); // int debugLevel
if (debugLevel > -3) {
if (quatCorr == null) {
System.out.println("generateEgomotionTable(): quatCorr=null");
} else {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("generateEgomotionTable(): quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("generateEgomotionTable(): ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("generateEgomotionTable(): ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
System.out.println("generateEgomotionTable(): ENU corr=["+enu_corr[0]+", "+enu_corr[1]+", "+enu_corr[2]+"]");
}
}
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
String ts = scene.getImageName();
scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
scenes_xyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(ts);
if (scenes_xyzatr[nscene] != null) {
num_processed++;
}
}
boolean use_processed = num_processed > 1;
String header_ts="#\ttimestamp";
String header_img="\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)"+
"\tEVx(m/s)\tEVy(m/s)\tEVz(m/s)\tEVa(rad/s)\tEVt(rad/s)\tEVr(rad/s)";
String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
"\tlat\tlong\talt\tned0\tned1\tned2";
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"+
"\tINS->X\tINS->Y\tINS->Z"+
"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
"\traw_A_enu\traw_T_enu\traw_R_enu"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
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_testpimu;
StringBuffer sb = new StringBuffer();
double [][][] dxyzatr_dt = null;
if (use_processed) {
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
//ref_index,
quadCLTs[ref_index],// QuadCLT ref_scene, // may be one of scenes or center
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr, // 5.0
clt_parameters.ofp.lpf_series, // half_run_range); // double half_run_range
debugLevel); // int debugLevel);
}
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref); //
double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // velocities and omegas from IMU
clt_parameters, // CLTParameters clt_parameters,
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,
earliest_scene, // final int early_index,
(quadCLTs.length -1), // int last_index,
debugLevel // final int debugLevel
);
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++) {
QuadCLT scene = quadCLTs[nscene];
if (scene == null) {
for (int i = 0; i < 26; i++) {
sb.append("\t---");
}
sb.append("\n");
continue;
}
double timestamp = scene.getTimeStamp();
Did_ins_1 d1 = scene.did_ins_1;
Did_ins_2 d2 = scene.did_ins_2;
Did_pimu d3 = scene.did_pimu;
sb.append(nscene+"\t"+timestamp);
if (use_processed) {
double [] nan3 = new double[] {Double.NaN, Double.NaN,Double.NaN};
double [][] nan23 = new double[][] {nan3,nan3};
if (scenes_xyzatr[nscene] != null) {
if (scenes_xyzatr[nscene][0] == null) scenes_xyzatr[nscene][0]= nan3;
if (scenes_xyzatr[nscene][1] == null) scenes_xyzatr[nscene][1]= nan3;
if (dxyzatr_dt[nscene]== null) dxyzatr_dt[nscene] = nan23;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (dxyzatr_dt[nscene][0] == null) dxyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene]== null) scenes_xyzatr_dt[nscene] = nan23;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
if (scenes_xyzatr_dt[nscene][0] == null) scenes_xyzatr_dt[nscene][0]= nan3;
// x,y,z - scene position in reference scene frame (reference scene is all 0-s)
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
// a,tilt,roll - scene orientation in reference scene frame (reference scene is all 0-s)
sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
// Vx, Vy, Vz - linear velocities calculated from coordinates by running average (now +/- 5 scenes
sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
// Va, Vt, Vr - angular velocities calculated from coordinates by running average (now +/- 5 scenes
sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
sb.append("\t"+scenes_xyzatr_dt[nscene][0][0]+"\t"+scenes_xyzatr_dt[nscene][0][1]+"\t"+scenes_xyzatr_dt[nscene][0][2]);
sb.append("\t"+scenes_xyzatr_dt[nscene][1][0]+"\t"+scenes_xyzatr_dt[nscene][1][1]+"\t"+scenes_xyzatr_dt[nscene][1][2]);
} else {
for (int i = 0; i < 12; i++) {
sb.append("\t---");
}
}
}
sb.append("\t"+d1.timeOfWeek);
sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[2]);
sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);
sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);
sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);
//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
sb.append("\t"+d2.timeOfWeek);
sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.lla[0]+ "\t"+d2.lla[1]+ "\t"+d2.lla[2]);
double [] double_theta = d1.getTheta();
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
double [] double_qn2b = d2.getQn2b();
double [] double_uvw = d2.getUvw();
double [] uvw_dir = Imx5.applyQuaternionTo(double_qn2b, double_uvw, false); // bad
double [] uvw_inv = Imx5.applyQuaternionTo(double_qn2b, double_uvw, true); // good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp]
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);
//"\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"
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
double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu_raw =Imx5.quaternionImsToCam(d2.getQEnu(),
new double[3], // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
double [] cam_xyz_ned = test_xyz_ned(
Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
d2_ref.getQn2b(), // double[] quat_ned,
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 = (quatCorr != null) ?
Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
cam_xyz_enu;
//"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"
// NED without ims_mount_atr correction \tcam_X1\tcam_Y1\tcam_Z1
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
// NED with ims_mount_atr correction \tcam_X2\tcam_Y2\tcam_Z2
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
//"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"
sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); // \tned->X\tned->Y\tned->Z
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr \tenu->X\tenu->Y\tenu->Z
//"\tINS->X\tINS->Y\tINS->Z"
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr "\tINS->X\tINS->Y\tINS->Z"
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
double [] scene_rel_atr=ErsCorrection.combineXYZATR(
ims_scene_xyzatr,
ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
double [] scene_raw_atr_enu = Imx5.quatToCamAtr(cam_quat_enu_raw); // not corrected by ims_mount_atr
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]); //
// "\traw_A_enu\traw_T_enu\traw_R_enu"+
sb.append("\t"+scene_raw_atr_enu[0]+ "\t"+scene_raw_atr_enu[1]+ "\t"+scene_raw_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));
// 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");
}
// test QuaternionLMA here
// Add another data
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
sb.append("New ATR mount (rad):[\t"+new_atr[0]+"\t"+new_atr[1]+"\t"+new_atr[2]+"]\n");
sb.append("New ATR mount (deg):[\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"]\n");
// double [] quatCorr
if (quatCorr == null) {
sb.append("quatCorr = null\n");
} else {
tp_tasks_ref = new TpTask[1][];
tp_tasks_ref[0] = GpuQuad.setInterTasks( // "true" reference, with stereo actual reference will be offset
ref_scene.getNumSensors(),
ref_scene.getErsCorrection().getSensorWH()[0],
!ref_scene.hasGPU(), // final boolean calcPortsCoordinatesAndDerivatives, // GPU can calculate them centreXY
ref_pXpYD, // final double [][] pXpYD, // per-tile array of pX,pY,disparity triplets (or nulls)
selection, // final boolean [] selection, // may be null, if not null do not process unselected tiles
ref_scene.getErsCorrection(), // final GeometryCorrection geometryCorrection,
disparity_corr, // final double disparity_corr,
margin, // final int margin, // do not use tiles if their centers are closer to the edges
null, // final boolean [] valid_tiles,
QuadCLT.THREADS_MAX); // final int threadsMax) // maximal number of threads to launch
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
image_dtt.setReferenceTD( // tp_tasks_ref will be updated
fclt, // final float [][] fclt,
erase_clt,
null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
true, // final boolean use_reference_buffer,
tp_tasks_ref[0], // final TpTask[] tp_tasks,
clt_parameters.gpu_sigma_r, // final double gpu_sigma_r, // 0.9, 1.1
clt_parameters.gpu_sigma_b, // final double gpu_sigma_b, // 0.9, 1.1
clt_parameters.gpu_sigma_g, // final double gpu_sigma_g, // 0.6, 0.7
clt_parameters.gpu_sigma_m, // final double gpu_sigma_m, // = 0.4; // 0.7;
QuadCLT.THREADS_MAX, // final int threadsMax, // maximal number of threads to launch
debug_level); // final int globalDebugLevel);
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
sb.append("quatCorr=[\t"+quatCorr[0]+"\t"+quatCorr[1]+"\t"+quatCorr[2]+"\t"+quatCorr[3]+"]\n");
sb.append("ATR(rad)=[\t"+corr_angles[0]+"\t"+corr_angles[1]+"\t"+corr_angles[2]+"]\n");
sb.append("ATR(deg)=[\t"+corr_degrees[0]+"\t"+corr_degrees[1]+"\t"+corr_degrees[2]+"]\n");
}
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
CalibrationFileManagement.saveStringToFile (
path,
header+"\n"+sb.toString()+"\n"+footer);
}else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
return tp_tasks_ref;
}
public static void generateEgomotionTable(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
// int ref_index,
QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
int earliest_scene,
String path,
String comment,
int debugLevel) {
QuadCLT master_scene = ref_scene;
if (ref_scene.did_ins_2 == null) {
if (debugLevel > -3) {
System.out.println("generateEgomotionTable(): ref_scene.did_ins_2 == null, using last scene for reference");
}
ref_scene = quadCLTs[quadCLTs.length-1];
}
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
// QuadCLT ref_scene = quadCLTs[ref_index];
boolean is_center = ref_scene.hasCenterClt();
ErsCorrection ers_reference = master_scene.getErsCorrection();
int num_processed = 0;
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
......@@ -6185,7 +7285,8 @@ public class Interscene {
double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
// ref_index, // int ref_index,
ref_scene, // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
......@@ -6246,7 +7347,9 @@ public class Interscene {
if (use_processed) {
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
// ref_index,
master_scene,// QuadCLT ref_scene, // may be one of scenes or center
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr, // 5.0
......@@ -6255,7 +7358,8 @@ public class Interscene {
}
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
Did_ins_2 d2_ref = ref_scene.did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
......@@ -6274,15 +7378,27 @@ public class Interscene {
// quadCLTs[ref_index].getPimuOffsets()); // boolean scale)
double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
double [][][] ims_xyzatr = master_scene.getXyzatrIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
null, // double [] quat_corr, // only applies to rotations - verify!
-1); // debugLevel) ; // int debugLevel)
int ref_index_guess = -1;
for (int i = earliest_scene; i < quadCLTs.length; i++) {
if (quadCLTs[i] == master_scene) {
ref_index_guess = i;
break;
}
if (ref_index_guess < 0) {
ref_index_guess = quadCLTs.length - 1;
}
}
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
ref_index_guess, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
(quadCLTs.length -1), // int last_index,
......@@ -6483,6 +7599,9 @@ public class Interscene {
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
@Deprecated
// adjusts by all 3 axis rotation
public static double [] getQuaternionCorrection_Old(
......@@ -6562,8 +7681,8 @@ public class Interscene {
return quaternionLma.getQuaternion();
}
}
// relative to the GPS/compass
@Deprecated
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
......@@ -6697,6 +7816,147 @@ public class Interscene {
}
}
// relative to the GPS/compass
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
int earliest_scene,
double [] rms, // null or double[2];
double [] enu_corr,
int debugLevel
) {
if (ref_scene.did_ins_2 == null) {
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): ref_scene.did_ins_2 == null, using last scene for reference");
}
ref_scene = quadCLTs[quadCLTs.length-1];
}
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
// QuadCLT ref_scene = quadCLTs[ref_index];
// boolean is_center = ref_scene.hasCenterClt();
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][];
Did_ins_2 d2_ref = ref_scene.did_ins_2;
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (scene == ref_scene) {
quat_lma_xyz[nscene] = new double[3];
} else {
String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
}
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
}
double [] up_axis = Imx5.getUpAxis(
ims_mount_atr); // double [] ims_atr )
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
int debug_level = debugLevel;
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareCompassLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y,
null, // double [][] vect_w, all same weight
up_axis, // double [] vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
debug_level); // int debug_level)
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debug_level); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (rms != null) { // null or double[2];
double [] last_rms = quaternionLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = quaternionLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
}
}
}
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
}
double [][] camera_enu = quaternionLma.cameraToENU(quaternionLma.getQuaternion());
double sw = 0;
double [] swd = new double[3];
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
double w = 1.0;
sw += w;
for (int i = 0; i < swd.length; i++) {
swd[i]+=w*(camera_enu[nscene][i]-quat_lma_enu_xyz[nscene][i]);
}
}
for (int i = 0; i < swd.length; i++) {
swd[i] /= sw;
}
if (enu_corr== null) {
enu_corr=new double[3]; // will generate, just not return
}
double [] enu= Imx5.applyQuaternionTo(// metric E,N,U
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
swd,
true); // inverse
for (int i = 0; i < 3; i++) {
enu_corr[i] = enu[i];
}
if (debug_level > -3) {
System.out.println("GNSS correction in camera X,Y,Z = ["+swd[0]+"]["+swd[1]+"]["+swd[2]+"]");
System.out.println("GNSS correction in camera E,N,U = ["+enu_corr[0]+"]["+enu_corr[1]+"]["+enu_corr[2]+"]");
}
if (debug_level > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s",
"N","GNSS-X","GNSS-Y","GNSS-Z",
"CAM-X","CAM-Y","CAM-Z"));
for (int nscene = 0; nscene < camera_enu.length; nscene++) if (camera_enu[nscene] != null) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
quat_lma_enu_xyz[nscene][0],quat_lma_enu_xyz[nscene][1],quat_lma_enu_xyz[nscene][2],
camera_enu[nscene][0],camera_enu[nscene][1],camera_enu[nscene][2]));
}
}
return quaternionLma.getAxisQuat();
}
}
static double [] test_xyz_ned(
double [] ned,
double[] quat_ned,
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ f6017348
......@@ -4871,6 +4871,9 @@ public class OpticalFlow {
//start_index
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
scenes_xyzatr[last_index] = new double[2][3]; // all zeros
boolean ims_use = center_reference || clt_parameters.imp.ims_use; // center works with IMS only
boolean use_cuas = clt_parameters.imp.cuas_rotation & ims_use; // lock_position & ims_use; // needs
QuadCLT center_CLT = null;
// See if build_ref_dsi is needed
if (!build_ref_dsi) {
// try reading full
......@@ -4880,62 +4883,74 @@ public class OpticalFlow {
colorProcParameters, //
threadsMax,
debugLevel-2);
if (center_reference && (quadCLTs[last_index] != null)) {
quadCLTs[last_index].restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
if (use_cuas) {
center_CLT = QuadCLT.restoreCenterClt(
quadCLTs[last_index]); // ref_index]);
}
if ((center_CLT != null) && center_CLT.hasCenterClt()) {
center_CLT.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
int center_index =quadCLTs[last_index].getReferenceIndex(new QuadCLT[] {quadCLTs[last_index]}); // null); self, may return 0
int [] first_last_index = quadCLTs[last_index].getFirstLastIndex(new QuadCLT[] {quadCLTs[last_index]});
if ((center_index == -1) || (center_index == 0)) { // 0 - was self-referencing, TODO: debug, should not be
// -2 - reference somewhere, not to itself
// -1 - no reference to a center
// 0 - reference
// with overlap it may already have *-DSI_MAIN or even *-INTER-INTRA-LMA.tiff
// int [] first_last_index = quadCLTs[last_index].getFirstLastIndex(quadCLTs);
if (first_last_index == null) {
build_ref_dsi = true;
} else { // should have DSI
if (debugLevel >-2) {
System.out.println("no link to center, but this seems to be other's center "+quadCLTs[last_index].getReferenceTimestamp()+
", forcing initial orientation.");
}
force_initial_orientations = true;
}
} else { // -2 - reference somewhere, not to itself
QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnQuadCLT( // will conditionImageSet
quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
if ((try_ref_scene == null) || (try_ref_scene.getDLS() == null)) {
if (debugLevel >-2) {
System.out.println("DSI data for scene "+quadCLTs[last_index].getReferenceTimestamp()+
" does not exist, forcing initial orientation.");
// consider copyJP4src for the lower half (now it is not needed)
}
force_initial_orientations = true;
} else {
try_ref_scene.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
first_last_index = try_ref_scene.getFirstLastIndex(quadCLTs);
} else {
if (center_reference && (quadCLTs[last_index] != null)) {
quadCLTs[last_index].restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
int center_index =quadCLTs[last_index].getReferenceIndex(new QuadCLT[] {quadCLTs[last_index]}); // null); self, may return 0
int [] first_last_index = quadCLTs[last_index].getFirstLastIndex(new QuadCLT[] {quadCLTs[last_index]});
if ((center_index == -1) || (center_index == 0)) { // 0 - was self-referencing, TODO: debug, should not be
// -2 - reference somewhere, not to itself
// -1 - no reference to a center
// 0 - reference
// with overlap it may already have *-DSI_MAIN or even *-INTER-INTRA-LMA.tiff
// int [] first_last_index = quadCLTs[last_index].getFirstLastIndex(quadCLTs);
if (first_last_index == null) {
// if ((first_last_index == null) ||(first_last_index[0] < 0) || (first_last_index[1] < 0)) {
build_ref_dsi = true;
} else { // should have DSI
if (debugLevel >-2) {
System.out.println("no link to center, but this seems to be other's center "+quadCLTs[last_index].getReferenceTimestamp()+
", forcing initial orientation.");
}
force_initial_orientations = true;
}
} else { // -2 - reference somewhere, not to itself
QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnQuadCLT( // will conditionImageSet
quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
if ((try_ref_scene == null) || (try_ref_scene.getDLS() == null)) {
if (debugLevel >-2) {
System.out.println("DSI data for scene "+quadCLTs[last_index].getReferenceTimestamp()+
" does not exist, forcing initial orientation.");
// consider copyJP4src for the lower half (now it is not needed)
}
force_initial_orientations = true;
} else {
try_ref_scene.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
first_last_index = try_ref_scene.getFirstLastIndex(quadCLTs);
if (first_last_index == null) {
// if ((first_last_index == null) ||(first_last_index[0] < 0) || (first_last_index[1] < 0)) {
force_initial_orientations = true;
}
}
}
}
} else {
if ((quadCLTs[last_index] == null) || !quadCLTs[last_index].dsiExists()) {
if (debugLevel >-2) {
System.out.println("DSI data for scene "+set_channels[last_index].set_name+" does not exist, forcing its calculation.");
} else {
if ((quadCLTs[last_index] == null) || !quadCLTs[last_index].dsiExists()) {
if (debugLevel >-2) {
System.out.println("DSI data for scene "+set_channels[last_index].set_name+" does not exist, forcing its calculation.");
}
build_ref_dsi = true;
}
build_ref_dsi = true;
}
}
} // if ((center_CLT != null) && center_CLT.hasCenterClt()) {} else {
}
......@@ -5051,110 +5066,100 @@ public class OpticalFlow {
debugLevel); // int debugLevel)
if (!force_initial_orientations) {
if (center_reference) {
int center_index =quadCLTs[last_index].getReferenceIndex(null);
if (center_index == -1) {
// use master_clt? - where to look for stage. in CUAS mode build directory name and look there
if (center_CLT != null) {
if (center_CLT.getNumOrient() == 0) {
if (debugLevel >-3) {
System.out.println("center_CLT exists, but does not have egomotion data. Forcing its calculation.");
}
force_initial_orientations = true;
} else {
QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnNoModelQuadCLT( // will conditionImageSet
quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
}
} else {
if (try_ref_scene == null) {
if (center_reference) {
int center_index =quadCLTs[last_index].getReferenceIndex(null);
if (center_index == -1) {
force_initial_orientations = true;
} else {
try_ref_scene.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
int [] first_last_index = try_ref_scene.getFirstLastIndex(quadCLTs);
if ((first_last_index == null)) { // ||(first_last_index[0] < 0) || (first_last_index[1] < 0)) {
QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnNoModelQuadCLT( // will conditionImageSet
quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
if (try_ref_scene == null) {
force_initial_orientations = true;
} else {
try_ref_scene.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...) // get relative poses (98)
null, // String path, // full name with extension or null to use x3d directory
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
int [] first_last_index = try_ref_scene.getFirstLastIndex(quadCLTs);
if ((first_last_index == null)) { // ||(first_last_index[0] < 0) || (first_last_index[1] < 0)) {
force_initial_orientations = true;
}
}
}
}
} else {
if ((quadCLTs[last_index].getNumOrient() == 0) ||(!quadCLTs[last_index].propertiesContainString (".scenes_"))) {
if (debugLevel >-3) {
System.out.println("Egomotion data for scene "+set_channels[last_index].set_name+" does not exist, forcing its calculation.");
} else {
if ((quadCLTs[last_index].getNumOrient() == 0) ||(!quadCLTs[last_index].propertiesContainString (".scenes_"))) {
if (debugLevel >-3) {
System.out.println("Egomotion data for scene "+set_channels[last_index].set_name+" does not exist, forcing its calculation.");
}
force_initial_orientations = true;
}
force_initial_orientations = true;
}
}
}
} // if (center_CLT != null) {} else {
} // if (!force_initial_orientations) {
// Build initial orientations
boolean ims_use = center_reference || clt_parameters.imp.ims_use; // center works with IMS only
int ref_index = last_index; // old versions
double [][] xyzatr_ims_center = null;
boolean use_cuas = clt_parameters.imp.cuas_rotation & ims_use; // lock_position & ims_use; // needs
QuadCLT center_CLT = null;
double [] atr_ims_center = null;
if (force_initial_orientations && !reuse_video) {
if (use_cuas) {
// try to restore if exists
center_CLT = QuadCLT.restoreCenterClt(
quadCLTs[last_index]); // ref_index]);
if (center_CLT != null) { // no sense to calculate center if CLT data is not available (for now will need to manually copy)
// to this vXX version
if (debugLevel > -3) {
System.out.println("Restored center CLT in CUAS mode");
}
// quadCLTs[last_index] should be known here
// for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--) if (quadCLTs[scene_index] == null){
for (int scene_index = last_index - 1; scene_index >= 0 ; scene_index--) if (quadCLTs[scene_index] == null){
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[last_index].spawnNoModelQuadCLT(
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
} // split cycles to remove output clutter
xyzatr_ims_center = Interscene.getXyzatrImsCenter( // use [0], [1] - radii
clt_parameters, // final CLTParameters clt_parameters,
use_ims_rotation, // final boolean compensate_ims_rotation,
inertial_only, // final boolean inertial_only,
quadCLTs[last_index], // final QuadCLT refClt,
quadCLTs, //final QuadCLT[] quadCLTs, //
debugLevel); // final int debugLevel)
atr_ims_center = new double[] {xyzatr_ims_center[0][0],xyzatr_ims_center[0][1],xyzatr_ims_center[0][2]};
if (debugLevel > -3) {
System.out.println("xyzatr_ims_center[0]: A="+xyzatr_ims_center[0][0]+", T="+xyzatr_ims_center[0][1]+", R="+xyzatr_ims_center[0][2]);
}
} else {
if (debugLevel > -3) {
System.out.println("Center CLT for CUAS mode does not exist");
}
// if (use_cuas) {
// try to restore if exists
// center_CLT = QuadCLT.restoreCenterClt(
// quadCLTs[last_index]); // ref_index]);
/*
if (center_CLT != null) { // no sense to calculate center if CLT data is not available (for now will need to manually copy)
// to this vXX version
if (debugLevel > -3) {
System.out.println("Restored center CLT in CUAS mode");
}
// quadCLTs[last_index] should be known here
// for (int scene_index = ref_index - 1; scene_index >= 0 ; scene_index--) if (quadCLTs[scene_index] == null){
for (int scene_index = last_index - 1; scene_index >= 0 ; scene_index--) if (quadCLTs[scene_index] == null){
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[last_index].spawnNoModelQuadCLT(
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
} // split cycles to remove output clutter
xyzatr_ims_center = Interscene.getXyzatrImsCenter( // use [0], [1] - radii
clt_parameters, // final CLTParameters clt_parameters,
use_ims_rotation, // final boolean compensate_ims_rotation,
inertial_only, // final boolean inertial_only,
quadCLTs[last_index], // final QuadCLT refClt,
quadCLTs, //final QuadCLT[] quadCLTs, //
debugLevel); // final int debugLevel)
atr_ims_center = new double[] {xyzatr_ims_center[0][0],xyzatr_ims_center[0][1],xyzatr_ims_center[0][2]};
if (debugLevel > -3) {
System.out.println("xyzatr_ims_center[0]: A="+xyzatr_ims_center[0][0]+", T="+xyzatr_ims_center[0][1]+", R="+xyzatr_ims_center[0][2]);
}
} else {
if (debugLevel > -3) {
System.out.println("Center CLT for CUAS mode does not exist, falling back to non-CUAS mode");
}
}
*/
// }
boolean OK = false;
int es1 = -1;
/*
public static int setInitialOrientationsCuas(
final CLTParameters clt_parameters,
final double [][] center_xyzatr, // center_xyzatr[0]=ZERO3 - for this sequence
final QuadCLT center_CLT, // contains center CLT and DSI
final boolean compensate_ims_rotation,
final boolean inertial_only,
int min_num_scenes,
final ColorProcParameters colorProcParameters,
final QuadCLT[] quadCLTs, //
final QuadCLT.SetChannels [] set_channels,
final boolean batch_mode,
int earliest_scene,
int [] start_ref_pointers, // [0] - earliest valid scene, [1] ref_index. Set if non-null
final int threadsMax, // int threadsMax,
final boolean updateStatus,
final int debugLevel) {
*/
if (center_CLT != null) {
double [][] center_xyzatr = {ZERO3,atr_ims_center};
// int [] start_ref_pointers = {earliest_scene, last_index};
......@@ -5210,7 +5215,8 @@ public class OpticalFlow {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
// ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -5219,34 +5225,6 @@ public class OpticalFlow {
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
/*
// reference to earliest
String ego_path_early = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
quadCLTs[ref_index].getImageName()+
"-ego_early-"+quadCLTs[ref_index].getNumOrient()+".csv";
Interscene.invertInitialOrientation(
clt_parameters, // final CLTParameters clt_parameters,
batch_mode, // final boolean batch_mode,
false, // final boolean readjust,
true, // final int set_self, // set record for the new reference to new reference
quadCLTs, // final QuadCLT[] quadCLTs, //
quadCLTs.length-1,// final int last_index, // last to process (normally old reference)
earliest_scene, // final int ref_index, // new reference index (center)
ref_index, // final int ref_old, // original ref_index (normally == last_index)
earliest_scene, // final int earliest_index// first to process
debugLevel); // final int debugLevel
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
earliest_scene,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path_early, // String path,
ego_comment); // String comment);
if (debugLevel> -3) {
System.out.println("Egomotion referenced to earliest scene table saved to "+ego_path_early);
}
*/
}
} else if (ims_use) {
earliest_scene = Interscene.setInitialOrientationsIms(
......@@ -5286,49 +5264,49 @@ public class OpticalFlow {
}
// export csv
} else {// if (build_orientations) {
if (!reuse_video) { // reuse_video only uses reference scene
// if (ref_index)
for (int scene_index = last_index; scene_index >= earliest_scene ; scene_index--) {
if (scene_index != ref_index) {
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( // restores image data
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
if (center_CLT != null) {
if (!reuse_video) { // reuse_video only uses reference scene
System.out.println ("re-spawning with updated photogrammetric calibration of reference scene?");
for (int scene_index = last_index; scene_index >= earliest_scene ; scene_index--) {
// should we skip if already exists? Or need to re-run to apply new photometric calibration?
// Or should photogrammetric calibration be saved with center_CLT?
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( // restores image data
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
}
}
}
if (center_reference) {
ref_index =quadCLTs[last_index].getReferenceIndex(quadCLTs);
quadCLTs[ref_index].restoreInterProperties(null, false, debugLevel); //null
int [] first_last_index = quadCLTs[ref_index].getFirstLastIndex(quadCLTs);
earliest_scene = first_last_index[0]; // null pointer if does not exist
last_index = first_last_index[1];
}
// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
/*
if (!center_reference) { // for center reference it should be set during initial orientation. Or for all too?
for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
if ((!quadCLTs[ref_index].tsExists(set_channels[scene_index].set_name)) ||
((ref_index - scene_index) >= max_num_scenes)) {
earliest_scene = scene_index + 1;
} else {
if (!reuse_video) { // reuse_video only uses reference scene
// if (ref_index)
for (int scene_index = last_index; scene_index >= earliest_scene ; scene_index--) {
if (scene_index != ref_index) {
// to include ref scene photometric calibration
quadCLTs[scene_index] = quadCLTs[ref_index].spawnNoModelQuadCLT( // restores image data
set_channels[scene_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
}
}
}
if ((ref_index - earliest_scene + 1) < min_num_scenes) {
System.out.println("2.Total number of useful scenes = "+(ref_index - earliest_scene + 1)+
" < "+min_num_scenes+". Scrapping this series.");
if (start_ref_pointers != null) {
start_ref_pointers[0] = earliest_scene;
}
return null;
if (center_reference) {
ref_index =quadCLTs[last_index].getReferenceIndex(quadCLTs);
quadCLTs[ref_index].restoreInterProperties(null, false, debugLevel); //null
int [] first_last_index = quadCLTs[ref_index].getFirstLastIndex(quadCLTs);
earliest_scene = first_last_index[0]; // null pointer if does not exist
last_index = first_last_index[1];
}
}
*/
// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
}
// both after initial orientations and if loaded
if (ref_index != last_index) { // do that for reference (center) scene again (was for the last scene)
System.out.println("ref_index != last_index: ref_index="+ref_index+", last_index="+last_index);
// FIXME: Will deal with blue sky later - not needed for drones.
/// ref_blue_sky = quadCLTs[ref_index].getDoubleBlueSky();
quadCLTs[ref_index] = (QuadCLT) quadCLT_main.spawnQuadCLT( // restores dsi from "DSI-MAIN"
......@@ -5370,24 +5348,26 @@ public class OpticalFlow {
// just in case that orientations were calculated before:
// earliest_scene = getEarliestScene(quadCLTs);
// 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
QuadCLT master_CLT = (center_CLT != null)? center_CLT : quadCLTs[ref_index];
ErsCorrection ers_reference = master_CLT.getErsCorrection(); // only used in ml_export
if (!reuse_video) {
while ((quadCLTs[ref_index].getNumOrient() < min_num_orient) || (quadCLTs[ref_index].getNumAccum() < min_num_interscene)) {
while ((master_CLT.getNumOrient() < min_num_orient) || (master_CLT.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()+
System.out.println("master_CLT.getNumOrient()="+master_CLT.getNumOrient()+
" of "+min_num_orient+", master_CLT.getNumAccum()="+master_CLT.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))) {
if ((master_CLT.getNumAccum() < min_num_interscene) &&
((master_CLT.getNumAccum() < master_CLT.getNumOrient())||
(master_CLT.getNumOrient() >= min_num_orient))) {
boolean done_sfm = false;
double [][] sfm_dsn = null;
int mb_gain_index_depth = clt_parameters.imp.mb_gain_index_depth; // depth map refine pass (SfM) to switch to full mb_max_gain from mb_max_gain_inter
if (quadCLTs[ref_index].getNumAccum() > 0) {
if (!clt_parameters.imp.lock_position) {
if (master_CLT.getNumAccum() > 0) {
if (!clt_parameters.imp.lock_position) { // SFM mode, not with cuas
double mb_max_gain = clt_parameters.imp.mb_max_gain;
if (quadCLTs[ref_index].getNumOrient() < mb_gain_index_depth) { // (min_num_orient - 1)) {
if (master_CLT.getNumOrient() < mb_gain_index_depth) { // (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
}
int num_avg_pairs = clt_parameters.imp.sfm_num_pairs; // number of scene pairs to average
......@@ -5398,9 +5378,6 @@ public class OpticalFlow {
" is more than half-range. Reducing to "+num_avg_pairs);
}
}
// if (num_avg_pairs > (last_index - ref_index)) {
// num_avg_pairs = last_index - ref_index;
// }
QuadCLT[][][] scenes_seq_pairs = new QuadCLT[3][num_avg_pairs][2];
int ref_index_mod1 = Math.max(ref_index, earliest_scene + num_avg_pairs - 1);
int ref_index_mod2 = Math.min(ref_index, last_index-num_avg_pairs + 1);
......@@ -5439,6 +5416,8 @@ public class OpticalFlow {
if (!done_sfm) { // first pass or sfm failed
boolean compensate_dsi = true;
// should skip scenes w/o orientation 06/29/2022
// TODO: Implement for CUAS
combo_dsn_final = intersceneExport( // result indexed by COMBO_DSN_TITLES, COMBO_DSN_INDX_***
clt_parameters, // CLTParameters clt_parameters,
compensate_dsi, // boolean compensate_dsi,
......@@ -5458,19 +5437,19 @@ public class OpticalFlow {
debugLevel); // int debugLevel)
}
}
quadCLTs[ref_index].inc_accum();
master_CLT.inc_accum();
// save with updated num_accum
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
master_CLT.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);
}
double [] reduced_strength = new double[1];
if (quadCLTs[ref_index].getNumOrient() < min_num_orient) {
if (master_CLT.getNumOrient() < min_num_orient) {
boolean [] reliable_ref = null;
boolean show_reliable_ref = !batch_mode; // false;
double min_ref_str_sel = use_combo_reliable? min_ref_str_lma: min_ref_str;
if (min_ref_str > 0.0) {
reliable_ref = quadCLTs[ref_index].getReliableTiles( // will be null if does not exist.
reliable_ref = master_CLT.getReliableTiles( // will be null if does not exist.
use_combo_reliable, // boolean use_combo,
min_ref_str_sel, // double min_strength,
min_ref_frac, // double min_ref_frac,
......@@ -5489,12 +5468,10 @@ public class OpticalFlow {
}
ShowDoubleFloatArrays.showArrays(
dbg_img,
quadCLTs[ref_index].getTileProcessor().getTilesX(),
quadCLTs[ref_index].getTileProcessor().getTilesY(),
"reliable_ref-"+quadCLTs[ref_index].getImageName());
master_CLT.getTileProcessor().getTilesX(),
master_CLT.getTileProcessor().getTilesY(),
"reliable_ref-"+master_CLT.getImageName());
}
}
// TODO: move to config
// boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
......@@ -5507,18 +5484,13 @@ public class OpticalFlow {
int mb_ers_y_index = clt_parameters.imp.mb_ers_y_index;
int mb_ers_r_index = clt_parameters.imp.mb_ers_r_index;
int mb_all_index = clt_parameters.imp.mb_all_index;
int mb_gain_index_pose = clt_parameters.imp.mb_gain_index_pose; // pose readjust pass to switch to full mb_max_gain from mb_max_gain_inter
boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < mb_ers_index);
boolean disable_ers_y = (quadCLTs[ref_index].getNumOrient() < mb_ers_y_index);
boolean disable_ers_r = (quadCLTs[ref_index].getNumOrient() < mb_ers_r_index);
boolean lma_xyzatr = (quadCLTs[ref_index].getNumOrient() == mb_all_index);
boolean lma_use_Z = clt_parameters.imp.lma_use_Z; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean lma_use_R = clt_parameters.imp.lma_use_R; // true; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
boolean ers_from_ims = true; // false; // change later
int ers_mode = 0; // keep
// with IMS it is already set during initial orientation. In non-IMS mode
......@@ -5537,30 +5509,51 @@ public class OpticalFlow {
if (quadCLTs[ref_index].getNumOrient() < mb_gain_index_pose) { // (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
}
earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R,
disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r,
lma_xyzatr, // boolean lma_xyzatr,
configured_lma, // boolean configured_lma,
lpf_xy, // boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
avg_rlen, // double avg_rlen,
readjust_xy_ims,// boolean readjust_xy_ims, // readjust X,Y fromIMS linear velocities and full X,Y movement
// from the previous adjustment. Adjust A,T,R,Z (and optionally
// angular velocities) freely
reg_weight_xy, // double reg_weight_xy, // regularization weight for X and Y
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
new int [] {earliest_scene, last_index}, //int [] range,
ers_mode, // int ers_mode, // 0 - keep, 1 - set from velocity, 2 - set from IMS
!batch_mode, // boolean test_motion_blur,
debugLevel) ; // int debugLevel)
// TODO: Implement for CUAS **************************************************
if (center_CLT != null) {
earliest_scene = Interscene.reAdjustPairsLMAIntersceneCuas( // after combo dgi is available and preliminary poses are known
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
lma_use_R, // boolean use_R,
disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r,
lma_xyzatr, // boolean lma_xyzatr,
configured_lma, // boolean configured_lma,
avg_rlen, // double avg_rlen,
reg_weight_xy, // double reg_weight_xy, // regularization weight for X and Y
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
quadCLTs, // QuadCLT [] quadCLTs,
center_CLT, // QuadCLT center_CLT, // contains center CLT and DSI. It has zeros fo coordinates, scenes are already around it
new int [] {earliest_scene, last_index}, //int [] range,
ers_mode, // int ers_mode, // 0 - keep, 1 - set from velocity, 2 - set from IMS
!batch_mode, // boolean test_motion_blur,
debugLevel) ; // int debugLevel)
}else {
earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
lma_use_Z, // boolean use_Z,
lma_use_R, // boolean use_R,
disable_ers, // boolean disable_ers,
disable_ers_y, // boolean disable_ers_y,
disable_ers_r, // boolean disable_ers_r,
lma_xyzatr, // boolean lma_xyzatr,
configured_lma, // boolean configured_lma,
lpf_xy, // boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
avg_rlen, // double avg_rlen,
readjust_xy_ims,// boolean readjust_xy_ims, // readjust X,Y fromIMS linear velocities and full X,Y movement
// from the previous adjustment. Adjust A,T,R,Z (and optionally
// angular velocities) freely
reg_weight_xy, // double reg_weight_xy, // regularization weight for X and Y
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
new int [] {earliest_scene, last_index}, //int [] range,
ers_mode, // int ers_mode, // 0 - keep, 1 - set from velocity, 2 - set from IMS
!batch_mode, // boolean test_motion_blur,
debugLevel) ; // int debugLevel)
}
// should update earliest_scene
if ((last_index - earliest_scene + 1) < min_num_scenes) {
System.out.println("After reAdjustPairsLMAInterscene() total number of useful scenes = "+(ref_index - earliest_scene + 1)+
......@@ -5574,8 +5567,8 @@ public class OpticalFlow {
System.out.println("After reAdjustPairsLMAInterscene() not all scenes matched, earliest useful scene = "+earliest_scene+
" (total number of scenes = "+(ref_index - earliest_scene + 1)+").");
}
quadCLTs[ref_index].inc_orient();
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
master_CLT.inc_orient();
master_CLT.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) {
......@@ -5586,7 +5579,8 @@ public class OpticalFlow {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
// ref_index,// ref_indx,
master_CLT, // quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -5718,7 +5712,8 @@ public class OpticalFlow {
double [] quatCorr = Interscene.getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
// ref_index, // int ref_index,
quadCLTs[ref_index], // QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
quat_rms, // double [] rms // null or double[2];
enu_corr, //double [] enu_corr,
......@@ -5774,6 +5769,9 @@ public class OpticalFlow {
}
boolean combine_clt = clt_parameters.imp.cuas_rotation; // (cuas_atr[0] != 0) || (cuas_atr[1] != 0) || (cuas_atr[2] != 0);
// TODO: Refine center_CLT if it exists, not crete
if (combine_clt) {
// see if it exists
/*
......@@ -5833,164 +5831,7 @@ public class OpticalFlow {
imp_virtual.show();
}
}
}
/*
if (combine_clt) {
boolean apply_clt = true; // set GPU with data
boolean show_clt = true;
boolean merge_clt = true;
boolean save_clt = true;
boolean save_dsi = true; // translate each layer of *-INTER-INTRA-LMA.tiff to the virtual center view and save it
boolean save_in_ref = true; // save in reference scene directory
boolean save_in_last = true; // save in last scene directory
QuadCLT ref_clt = quadCLTs[ref_index];
int sensor_mask_clt = -1; // all
if (combo_dsn_final == null) {
combo_dsn_final =quadCLTs[ref_index].restoreComboDSI(true); // also sets quadCLTs[ref_index].dsi and blue sky
}
double [][] dls = {
combo_dsn_final[COMBO_DSN_INDX_DISP],
combo_dsn_final[COMBO_DSN_INDX_LMA],
combo_dsn_final[COMBO_DSN_INDX_STRENGTH]
};
double [][] ds_clt = conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
quadCLTs[ref_index], // QuadCLT scene,
debugLevel);
boolean save_weights = true;
final double [][] ref_pXpYD = getRefPxPyD( // Use to interpolate disparity layers
clt_parameters, // CLTParameters clt_parameters,
combine_clt, // boolean mode_cuas,
// save_weights, // final boolean save_weights, // output corresponding weights for each data
null, // Rectangle fov_tiles,
ZERO3, // double [] stereo_xyz, // offset reference camera {x,y,z}
cuas_atr, // double [] stereo_atr_in, // offset reference orientation (cuas)
ds_clt[0], // double [] ref_disparity,
ref_clt, // QuadCLT refCLT, // should be the same instance if one of quadCLTs
debugLevel); // int debugLevel)
float [][] combo_seq_clt_w = getTDComboSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
ref_pXpYD, // double [][] ref_pXpYD,
save_weights, // boolean save_weights, // output corresponding weights for each data
merge_clt, // boolean merge_all,
sensor_mask_clt, // int sensor_mask,
null, // Rectangle fov_tiles,
ZERO3, // double [] stereo_xyz, // offset reference camera {x,y,z}
cuas_atr, // double [] stereo_atr_in, // offset reference orientation (cuas)
ds_clt[0], // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
ref_clt, // QuadCLT refCLT, // should be the same instance if one of quadCLTs
debugLevel); // int debugLevel)
float [][] combo_seq_clt = save_weights? new float [combo_seq_clt_w.length/2][]: combo_seq_clt_w;
if (save_weights) { // remove second half (weights as integer numbers)
for (int i = 0; i< combo_seq_clt.length; i++) {
combo_seq_clt[i] = combo_seq_clt_w[i];
}
}
if (save_clt) {
int [] wh_c = ref_clt.getWHC(false);
//final int tile_size_td = 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE;
final int width_clt = wh_c[0] * 2; // to make image dimensions similar
final int height_clt = combo_seq_clt[0].length/width_clt;
String [] clt_titles = new String [combo_seq_clt_w.length];
for (int i = 0; i < combo_seq_clt.length; i++) {
clt_titles[i] = "chn-"+i;
if (save_weights) {
clt_titles[i+combo_seq_clt.length] = "weight-"+i;
}
}
String suffix_clt = "-clt_combo";
if (save_weights) {
suffix_clt+="_weights";
}
if (save_in_ref) {
ref_clt.saveFloatArrayInModelDirectory( // error
suffix_clt, // String suffix,
clt_titles, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_seq_clt_w, // dbg_data, // float [][] data,
width_clt, // int width,
height_clt); // int height)
}
if (save_in_last) {
quadCLTs[quadCLTs.length - 1].saveFloatArrayInModelDirectory( // error
suffix_clt, // String suffix,
clt_titles, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_seq_clt_w, // dbg_data, // float [][] data,
width_clt, // int width,
height_clt); // int height)
}
}
if (save_dsi) {
// final double [][] combo_dsi_cli = ref_clt.restoreComboDSI (true);
TileProcessor tp = quadCLTs[quadCLTs.length - 1].getTileProcessor();
final int transform_size = tp.getTileSize();
final int tilesX = tp.getTilesX();
final int tilesY = tp.getTilesY();
final double [][] combo_dsi_center = interpolateDSI(
ref_pXpYD, // final double [][] ref_pXpYD,
combo_dsn_final, // combo_dsi_cli, // final double [][] dsi_in,
transform_size, // final int transform_size,
tilesX, // final int tilesX,
tilesY); //final int tilesY);
String rslt_suffix = "-CENTER-INTER-INTRA";
// use quadCLTs[quadCLTs.length - 1].restoreComboDSI ("-CENTER",true); to restore
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
if (save_in_ref) {
ref_clt.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsi_center, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
}
if (save_in_last) {
quadCLTs[quadCLTs.length - 1].saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
OpticalFlow.COMBO_DSN_TITLES, // combo_dsn_titles_full, // null, // String [] labels, // or null
combo_dsi_center, // dbg_data, // double [][] data,
tilesX, // int width,
tilesY); // int height)
}
}
int [] whc = new int[3];
if (apply_clt) { // set GPU with data
quadCLTs[ref_index].setComboToTD(
combo_seq_clt, // final float [][] fclt,
merge_clt, // final boolean merge_channels, // duplicate same data to all selected channels
sensor_mask_clt, // final int sensor_mask, // only if merge_channels
whc, // final int [] whc, // if int[2], will return width, height
false); // final boolean use_reference);
if (show_clt) {
String suffix="-virtual";
ImagePlus imp_virtual = ref_clt.renderFromTD ( // do we need to update gpuQuad ?
sensor_mask_clt, // final int sensor_mask,
merge_clt, // boolean merge_channels,
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.getColorProcParameters(ref_clt.isAux()), //ColorProcParameters colorProcParameters,
clt_parameters.getRGBParameters(), //EyesisCorrectionParameters.RGBParameters rgbParameters,\
whc, // null, // int [] wh,
false, // toRGB, // boolean toRGB,
false, // use_reference, // boolean use_reference
suffix); // String suffix)
imp_virtual.show();
}
}
}
*/
if (generate_egomotion) {
......@@ -6002,7 +5843,8 @@ public class OpticalFlow {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
// ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -6016,7 +5858,8 @@ public class OpticalFlow {
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
// ref_index,// ref_indx,
quadCLTs[ref_index], //QuadCLT ref_scene, // may be one of quadCLTs or center_CLT
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment, // String comment);
......@@ -9042,8 +8885,18 @@ public class OpticalFlow {
double dt = scenes[nscene1].getTimeStamp() - scenes[nscene0].getTimeStamp();
double [] scene_xyz0 = ers_reference.getSceneXYZ(ts0);
double [] scene_atr0 = ers_reference.getSceneATR(ts0);
double [] scene_xyz1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneXYZ(ts1);
double [] scene_atr1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneATR(ts1);
// double [] scene_xyz1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneXYZ(ts1);
// double [] scene_atr1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneATR(ts1);
// now reference frame has its record too
double [] scene_xyz1 = ers_reference.getSceneXYZ(ts1);
double [] scene_atr1 = ers_reference.getSceneATR(ts1);
// for very old?
if (scene_xyz1 == null) {
scene_xyz1 = ZERO3;
}
if (scene_atr1 == null) {
scene_atr1 = ZERO3;
}
ers_xyzatr[nscene] = new double[2][3];
for (int i = 0; i < 3; i++) {
ers_xyzatr[nscene][0][i] = (scene_xyz1[i]-scene_xyz0[i])/dt;
......@@ -9106,6 +8959,113 @@ public class OpticalFlow {
}
return ers_xyzatr;
}
public static double [][][] getVelocitiesFromScenes(
QuadCLT [] scenes, // ordered by increasing timestamps
// int ref_index,
QuadCLT ref_scene, // may be one of scenes or center
int start_scene,
int end_scene,
double [][][] scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available
double half_run_range,
int debugLevel
){
double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][];
if (half_run_range <=0 ) {
ErsCorrection ers_reference = ref_scene.getErsCorrection();
for (int nscene = start_scene; nscene <= end_scene; nscene ++) if (scenes[nscene] != null){
int nscene0 = nscene - 1;
if ((nscene0 < start_scene) ||
(scenes[nscene0]== null)||
(ers_reference.getSceneXYZ(scenes[nscene0].getImageName())== null) ||
(ers_reference.getSceneATR(scenes[nscene0].getImageName())== null)) {
nscene0 = nscene;
}
int nscene1 = nscene + 1;
if ((nscene1 > end_scene) || (scenes[nscene1]== null)) {
nscene1 = nscene;
}
if (nscene1 > nscene0) {
String ts0 = scenes[nscene0].getImageName();
String ts1 = scenes[nscene1].getImageName();
double dt = scenes[nscene1].getTimeStamp() - scenes[nscene0].getTimeStamp();
double [] scene_xyz0 = ers_reference.getSceneXYZ(ts0);
double [] scene_atr0 = ers_reference.getSceneATR(ts0);
// double [] scene_xyz1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneXYZ(ts1);
// double [] scene_atr1 = (nscene1== ref_index)? ZERO3:ers_reference.getSceneATR(ts1);
// now reference frame has its record too
double [] scene_xyz1 = ers_reference.getSceneXYZ(ts1);
double [] scene_atr1 = ers_reference.getSceneATR(ts1);
// for very old?
if (scene_xyz1 == null) {
scene_xyz1 = ZERO3;
}
if (scene_atr1 == null) {
scene_atr1 = ZERO3;
}
ers_xyzatr[nscene] = new double[2][3];
for (int i = 0; i < 3; i++) {
ers_xyzatr[nscene][0][i] = (scene_xyz1[i]-scene_xyz0[i])/dt;
ers_xyzatr[nscene][1][i] = (scene_atr1[i]-scene_atr0[i])/dt;
}
} else {
System.out.println("**** Isoloated scene!!! skipping... now may only happen for a ref_scene****");
}
}
} else {
double [] weights = new double [(int) Math.floor(half_run_range)+1];
weights[0] = 1;
for (int i = 1; i < weights.length; i++) {
weights[i] = 0.5* (Math.cos(i*Math.PI/half_run_range) +1.0);
}
for (int nscene = start_scene; nscene <= end_scene; nscene ++) if (scenes[nscene] != null){
double tref = scenes[nscene].getTimeStamp();
double s0 = 0.0, sx = 0.0, sx2 = 0.0;
double [][] sy = new double[2][3];
double [][] sxy = new double[2][3];
int nds = 0;
for (int ds = -weights.length + 1; ds < weights.length; ds++) {
int ns = nscene + ds;
if ((ns >= 0) && (ns < scenes_xyzatr.length) && (scenes_xyzatr[ns] != null)) {
double w = (ds >= 0) ? weights[ds] : weights[-ds];
s0 += w;
double dt = scenes[ns].getTimeStamp() - tref;
double wx = w * dt;
sx += wx;
sx2 += wx * dt;
for (int m = 0; m < sy.length; m++) {
for (int d = 0; d < sy[m].length; d++) {
if (scenes_xyzatr[ns]== null ) {
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
}
}else if (scenes_xyzatr[ns][m]== null ) {
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
}
} else {
double y = scenes_xyzatr[ns][m][d];
sy [m][d] += w * y;
sxy[m][d] += wx * y;
}
}
}
nds++; // number of different timestamps used
}
}
if ((nds > 1) && (s0 > 0)) {
ers_xyzatr[nscene] = new double[2][3];
for (int m = 0; m < sy.length; m++) {
for (int d = 0; d < sy[m].length; d++) {
ers_xyzatr[nscene][m][d] = (s0*sxy[m][d]-sx*sy[m][d])/(s0*sx2 - sx*sx);
}
}
}
}
}
return ers_xyzatr;
}
@Deprecated
......
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