Skip to content

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

Testing translation filtering by IMS velocities

parent a12d2254
Show whitespace changes
Inline Side-by-side
Showing with 385 additions and 178 deletions
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 7c786e91
......@@ -187,7 +187,8 @@ public class ErsCorrection extends GeometryCorrection {
DP_DVX, DP_DVY, DP_DVZ,
DP_DSVAZ, DP_DSVTL, DP_DSVRL,
DP_DSVX, DP_DSVY, DP_DSVZ};
public static final int [] DP_XYZR_INDICES = {DP_DSX,DP_DSY,DP_DSZ,DP_DSRL};
public static final int [] DP_XY_INDICES = {DP_DSX,DP_DSY};
public static final int [] DP_ZR_INDICES = {DP_DSZ,DP_DSRL};
public static final int [] DP_AT_INDICES = {DP_DSAZ,DP_DSTL};
public static final int [] DP_ATT_ERS_INDICES = {DP_DSVAZ,DP_DSVTL};
......@@ -234,17 +235,19 @@ public class ErsCorrection extends GeometryCorrection {
*/
public static boolean [] getParamSelect(
boolean use_XY,
boolean use_AT,
boolean use_ERS) {
boolean [] param_select = new boolean[DP_NUM_PARS];
for (int i:DP_XYZR_INDICES) param_select[i] = true;
for (int i:DP_ZR_INDICES) param_select[i] = true;
if (use_XY) for (int i:DP_XY_INDICES) param_select[i] = true;
if (use_AT) for (int i:DP_AT_INDICES) param_select[i] = true;
if (use_ERS) for (int i:DP_ATT_ERS_INDICES) param_select[i] = true;
return param_select;
}
public static double [] getParamRegWeights(
double reg_weight,
boolean use_AT) {
double reg_weight) {
// boolean use_AT) {
double [] reg_weights = new double[DP_NUM_PARS];
reg_weights[DP_DSX] = reg_weight;
reg_weights[DP_DSY] = reg_weight;
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 7c786e91
......@@ -614,9 +614,14 @@ public class Interscene {
" ("+getFailReason(fail_reason[0])+")");
if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA) && !got_spiral)) {
fpn_list.add(scene_index);
scenes_xyzatr[scene_index] = initial_pose;
// should not be modified for initial adjust, but just in case
quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
scenes_dxyzatr[scene_index]));
if (fpn_skip) {
System.out.println("fpn_skip is set, just using initial pose, num_fpn_mitigate="+fpn_list.size());
scenes_xyzatr[scene_index] = initial_pose;
break handle_failure; // to next scene
} else {
System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
......@@ -745,8 +750,13 @@ public class Interscene {
clt_parameters.imp.debug_level); // int debugLevel);
boolean adjust_OK = rel_xyzatr != null;
if (!adjust_OK) {
System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][1]+
" referenced from scene "+fpn_pairs[ipair][0]+". Reason = "+fail_reason[0]+
// should not be modified for initial adjust, but just in case
quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
scenes_dxyzatr[fpn_pairs[ipair][0]]));
System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
} else {
scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
......@@ -777,28 +787,6 @@ public class Interscene {
} else if (debugLevel > -4) {
System.out.println("All multi scene passes are Done. Maximal RMSE was "+maximal_series_rms);
}
/*
// Set ERS data - now old way, change to IMS data (and apply to adjustPairsLMAInterscene())
double half_run_range = clt_parameters.ilp.ilma_motion_filter; // 3.50; // make a parameter
double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
ref_index, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
dxyzatr_dt[ref_index][0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt,
dxyzatr_dt[ref_index][1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
for (int scene_index = ref_index; scene_index >= earliest_scene ; scene_index--) {
ers_reference.addScene(quadCLTs[scene_index].getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
dxyzatr_dt[scene_index][0], // ers_scene.getErsXYZ_dt(),
dxyzatr_dt[scene_index][1] // ers_scene.getErsATR_dt()
);
}
*/
quadCLTs[ref_index].set_orient(1); // first orientation
quadCLTs[ref_index].set_accum(0); // reset accumulations ("build_interscene") number
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) // null pointer
......@@ -1326,11 +1314,71 @@ public class Interscene {
}
}
// double avg_rlen,// >= 1.0
/**
* Re-adjust camera poses with LMA after initial poses are already known.
* Fitting can be done for "reliable_ref" tiles only, they may be filtered
* by SfM gain external to this method).
* Fitting can be performed with ERS for Azimuth and Tilt Rotations (other
* ERS components are preserved intact).
* By default X,Y,Z and Roll are adjusted (with or without 2 angular velocities).
* Optionally one of the 2 refinement methods is used:
* a) lpf_xy true:
* previous X and Y are low-pass filtered, by running-average with cosine window
* of avg_rlen half-width
* b) readjust_xy_ims && !lpf_xy:
* X and Y are refined with IMS linear velocities shifted (by adding ax+b to
* coordinates) to best fit previously determined coordinates (which in turn
* depended on IMS angles that are more accurate than coordinates). The result
* still has absolute AGL accuracy determined by the intrascene measurements
* that are good to ~1% at 100m if properly field-calibrated
* In both a) and b) all of X,Y,Z,A,T, and R are adjusted (with optional dA/dt
* and dT/dt as ERS), and X and Y are pulled to new calculated values (by
* averaging or IMS) with strength reg_weight_xy, other parameters (Z,A,T,R) are
* not regularized.
* Special cases:
* c) (reg_weight_xy == 0.0) && readjust_xy_ims - freeze X,Y, freely adjust
* Z,A,T,R (optional subsequent runs after X,Y are "pulled" regardless of IMU or
* not
* d) (reg_weight_xy == 0.0) && lpf_xy - freely adjust X,Y,Z, A,T,R (typical mode
* for the forward-looking applications
* @param clt_parameters configuration parameters
* @param mb_max_gain maximal motion blur gain
* @param disable_ers disable adjustment of the angular velocities through ERS.
* ERS may be disabled inside adjustDiffPairsLMAInterscene()
* if not all 4 image quadrants have enough "reliable" tiles.
* @param configured_lma Ignore calculated LMA parameter selection and regularization
* weights, use ones from the configuration parameters
* @param lpf_xy Low pass filter X and Y "pull" values (derived from the
* current ones). Overrides readjust_xy_ims as they are mutually
* exclusive.
* @param avg_rlen half-width of the LPF window, used with lpf_xy==true
* @param readjust_xy_ims readjust X and Y from current values and IMS linear velocities
* @param reg_weight_xy regularization weights for X and Y, valid with any of lpf_xy or
* readjust_xy_ims.
* @param reliable_ref tile mask in line-scan order. LMA fitting ignores data for
* unselected tiles.
* @param quadCLTs array of scene instances. Only scenes between range[0] and
* range[1] (inclusive) will be used.
* @param ref_index index of the reference frame in the scenes array
* @param range {earliest_scene_index, latest_scene_index};
* @param ers_mode 0 - keep, 1 - set from velocity, 2 - set from IMS. Now always 0
* @param test_motion_blur motion blur debugging and images
* @param debugLevel debug level
* @return earliest scene - may be higher than range[0] if LMA failed (should not happen at
* this pass, should fail during initial orientation.
*/
public static int reAdjustPairsLMAInterscene( // after combo dgi is available and preliminary poses are known
CLTParameters clt_parameters,
double mb_max_gain,
boolean disable_ers,
boolean configured_lma,
boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen,
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
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,
int ref_index,
......@@ -1340,6 +1388,15 @@ public class Interscene {
int debugLevel)
{
System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
boolean freeze_xy_pull = true; // debugging freezing xy to xy_pull
final boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
ErsCorrection.getParamSelect(
!freeze_xy_pull && (!readjust_xy_ims || (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
readjust_xy_ims || lpf_xy, // boolean use_AT,
!disable_ers); //boolean use_ERS);//clt_parameters.ilp.ilma_lma_select;
final double [] param_regweights = configured_lma? clt_parameters.ilp.ilma_regularization_weights :
ErsCorrection.getParamRegWeights(
(freeze_xy_pull? 0.0: reg_weight_xy)); // 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
......@@ -1347,7 +1404,7 @@ public class Interscene {
// offset is too small
boolean fmg_rectilinear = clt_parameters.imp.fmg_rectilinear;// use rectilinear model for scene offset estimation
int avg_len = clt_parameters.imp.avg_len;
// int avg_len = clt_parameters.imp.avg_len;
// 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};
......@@ -1411,7 +1468,6 @@ public class Interscene {
}
}
double [][] ref_pXpYD = null;
double [][] dbg_mb_img = null;
double [] mb_ref_disparity =null;
// if (test_motion_blur) {
......@@ -1419,53 +1475,92 @@ public class Interscene {
if (mb_ref_disparity == null) {
mb_ref_disparity = quadCLTs[ref_index].getDLS()[use_lma_dsi?1:0];
}
// ref_pXpYD should correspond to uniform grid (do not undo ERS of the reference scene)
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
quadCLTs[ref_index], // final QuadCLT scene_QuadClt,
quadCLTs[ref_index]); // final QuadCLT reference_QuadClt)
if (test_motion_blur) {
dbg_mb_img = new double[quadCLTs.length][];
}
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
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 = 69; // -68; // -8;
double maximal_series_rms = 0.0;
/// double [][] mb_vectors_ref = null;
/// TpTask[][] tp_tasks_ref = null;
if (debug_ers) {
System.out.println("ERS velocities scale mode = '"+dbg_ers_string+"'");
}
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
if (nscene == debug_scene) {
System.out.println("nscene = "+nscene);
System.out.println("nscene = "+nscene);
}
// just checking it is not isolated
if (quadCLTs[nscene] == null) {
earliest_scene = nscene + 1;
break;
}
if (nscene != ref_index) {
// 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] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
}
scenes_xyzatr[nscene] = ers_reference.getSceneXYZATR(ts);
scenes_xyzatr_pull[nscene] = scenes_xyzatr[nscene].clone();
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
}
switch (ers_mode) {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
break;
default: // do nothing - already read
}
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
// double avg_rlen
if (lpf_xy && (reg_weight_xy > 0.0)) {
scenes_xyzatr_pull = QuadCLT.refineFromLPF(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
scenes_xyzatr, // double [][][] xyzatr,
avg_rlen, // double avg_rlen,// >= 1.0
false, // boolean filter_rot, // false - translation. Do twice for both
true, // boolean keep_rz, // keep Z and Roll
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene); // int last_index)
} else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
scenes_xyzatr, // double [][][] xyzatr,
dxyzatr_dt, // double [][][] dxyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene); // int last_index)
}
if (freeze_xy_pull) {
System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
if (scenes_xyzatr_pull[nscene] != null) {
// only X,Y, keep Z.
scenes_xyzatr[nscene][0] = scenes_xyzatr_pull[nscene][0];
scenes_xyzatr[nscene][1] = scenes_xyzatr_pull[nscene][1];
}
}
}
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 = quadCLTs[ref_index].estimateAverageShift(
......@@ -1492,27 +1587,21 @@ public class Interscene {
avg_z, // double average_z,
true, // boolean use_rot,
true); // boolean rectilinear)
/*
System.out.println(String.format("%3d xyz: %9.5f %9.5f %9.5f atr: %9.5f %9.5f %9.5f est_pix: %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],
est_center, est_quad, est_center_rectilinear, est_quad_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));
}
}
switch (ers_mode) {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
break;
default: // do nothing - already read
}
if (debug_ers) {
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",
......@@ -1532,8 +1621,6 @@ public class Interscene {
}
}
boolean [] failed_scenes = new boolean[quadCLTs.length];
int num_failed =0;
int [] scene_seq = new int [last_scene-earliest_scene+1];
{
int indx=0;
......@@ -1549,10 +1636,6 @@ public class Interscene {
double [][] pXpYD_ref = new double [tilesX*tilesY][];
ArrayList<Integer> fpn_list = new ArrayList<Integer>();
boolean fpn_disable = false; // estimate they are too close
double [][][] scenes_pull = new double [quadCLTs.length][][];
boolean [][] scenes_param_select = new boolean[quadCLTs.length][];
final boolean[] param_select = clt_parameters.ilp.ilma_lma_select;
final double [] param_regweights = clt_parameters.ilp.ilma_regularization_weights;
final double max_rms = clt_parameters.imp.max_rms;
for (int nscene:scene_seq) {
......@@ -1587,8 +1670,7 @@ public class Interscene {
break;
}
String ts = quadCLTs[nscene].getImageName();
double [] scene_xyz_pre = ZERO3;
double [] scene_atr_pre = ZERO3;
// 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,
......@@ -1606,56 +1688,15 @@ public class Interscene {
debug_scene, // = 68 int debug_scene, // = 68
debugLevel); // int debugLevel);
}
// final boolean[] param_select = clt_parameters.ilp.ilma_lma_select;
// final double [] param_regweights = clt_parameters.ilp.ilma_regularization_weights;
// final double max_rms = clt_parameters.imp.max_rms;
if (nscene == ref_index) { // set GPU reference CLT data - should be before other scenes
ers_reference.addScene(ts,
scenes_xyzatr[nscene][0], // ref_index
scenes_xyzatr[nscene][1],
dxyzatr_dt[nscene][0],
dxyzatr_dt[nscene][1]
scenes_xyzatr[nscene], // ref_index
dxyzatr_dt[nscene]
);
} else { // if (nscene == ref_index)
scene_xyz_pre = ers_reference.getSceneXYZ(ts);
scene_atr_pre = ers_reference.getSceneATR(ts);
double [][] scene_xyzatr = new double[][] {scene_xyz_pre, scene_atr_pre};
double [][] inv_scene = ErsCorrection.invertXYZATR(scene_xyzatr);
double [] avg_weights = new double [2*avg_len+1];
int n_pre = nscene + avg_len;
int n_post = nscene - avg_len;
if (n_post < earliest_scene) n_post = earliest_scene;
if (n_pre > last_scene) n_pre = last_scene;
double s0=0, sx=0, sx2=0;
double [][] sy = new double[2][3], sxy = new double [2][3];
for (int n_other = n_post; n_other <= n_pre; n_other++) {
int ix = n_other - nscene; //+/-i
int i = ix + avg_len; // >=0
avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_len + 1));
double [][] other_xyzatr = ers_reference.getSceneXYZATR(quadCLTs[n_other].getImageName());
double [][] other_rel = (n_other != ref_index)?ErsCorrection.combineXYZATR(other_xyzatr,inv_scene):
(new double[2][3]);
double w = avg_weights[i];
s0 += w;
sx += w * ix;
sx2 += w * ix * ix;
for (int j = 0; j < other_rel.length; j++) {
for (int k = 0; k < other_rel[j].length; k++) {
sy [j][k] += w * other_rel[j][k];
sxy[j][k] += w * other_rel[j][k] * ix;
}
}
}
//ers_reference
double [][] diff_pull = new double [2][3];
for (int j = 0; j < diff_pull.length; j++) {
for (int k = 0; k < diff_pull[j].length; k++) {
diff_pull[j][k]=(sy[j][k]*sx2 - sxy[j][k]*sx) / (s0*sx2 - sx*sx);
}
}
double [][] scene_pull = ErsCorrection.combineXYZATR(scene_xyzatr,diff_pull);
double [] lma_rms = new double[2];
double [][] initial_pose = scenes_xyzatr[nscene].clone();
boolean adjust_OK = false;
if (est_shift < min_max[0]) {
fail_reason[0]=FAIL_REASON_MIN;
......@@ -1678,8 +1719,8 @@ public class Interscene {
interscene_ref_disparity,// double [] ref_disparity, // null or alternative reference disparity
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[ref_index],// double [][] scene0_xyzatr,
scene_xyzatr, // double [][] scene1_xyzatr,
scene_pull, // double [][] scene1_xyzatr_pull,
scenes_xyzatr[nscene], // double [][] scene1_xyzatr,
scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull,
param_select, // boolean[] param_select,
param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
......@@ -1705,21 +1746,18 @@ public class Interscene {
// 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])+")");
if ((fail_reason[0]==FAIL_REASON_MIN) || ((fail_reason[0]==FAIL_REASON_LMA))) {
fpn_list.add(nscene);
scenes_pull[nscene] = scene_pull; // need to clone()?
scenes_param_select[nscene] = param_select; // now not needed?
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[nscene]));
if (fpn_skip) {
System.out.println("fpn_skip is set, just skipping this scene");
failed_scenes[nscene] = true;
num_failed++;
scenes_xyzatr[nscene] = new double[][] {
scene_xyz_pre, // for now restore original, pre-adjustment
scene_atr_pre // for now restore original, pre-adjustment
};
break handle_failure;
} else {
System.out.println("fpn_skip is not set, aborting series and adjusting earliest_scene");
......@@ -1807,8 +1845,8 @@ public class Interscene {
reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
scenes_xyzatr[fpn_pairs[ipair][1]],// double [][] scene0_xyzatr,
scenes_xyzatr[fpn_pairs[ipair][0]], // double [][] scene1_xyzatr,
scenes_pull [fpn_pairs[ipair][0]], // double [][] scene1_xyzatr_pull,
scenes_param_select[fpn_pairs[ipair][0]], // boolean[] param_select,
scenes_xyzatr_pull [fpn_pairs[ipair][0]], // double [][] scene1_xyzatr_pull,
param_select, // boolean[] param_select,
param_regweights, // double [] param_regweights,
lma_rms, // double [] rms_out, // null or double [2]
max_rms, // double max_rms,
......@@ -1818,9 +1856,13 @@ public class Interscene {
clt_parameters.imp.debug_level); // int debugLevel)
boolean adjust_OK = rel_xyzatr != null;
if (!adjust_OK) {
System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][1]+
" referenced from scene "+fpn_pairs[ipair][0]+". Reason = "+fail_reason[0]+
System.out.println("LMA failed at scene pair = "+fpn_pairs[ipair][0]+
" referenced from scene "+fpn_pairs[ipair][1]+". Reason = "+fail_reason[0]+
" ("+getFailReason(fail_reason[0])+")");
quadCLTs[fpn_pairs[ipair][0]].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[fpn_pairs[ipair][0]]));
} else {
scenes_xyzatr[fpn_pairs[ipair][0]] = rel_xyzatr; // save directly, no need to combine
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 7c786e91
......@@ -5265,6 +5265,13 @@ public class OpticalFlow {
if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
}
boolean configured_lma = false;
boolean lpf_xy = false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
double avg_rlen = clt_parameters.imp.avg_len; // 3.0;
boolean readjust_xy_ims = true; // false;
double reg_weight_xy = 10.0; // 0.05; // TODO: find out reasonable values
boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
boolean ers_from_ims = true; // false; // change later
// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
......@@ -5283,6 +5290,13 @@ public class OpticalFlow {
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
disable_ers, // boolean disable_ers,
configured_lma, // boolean configured_lma,
lpf_xy, // boolean lpf_xy, // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
avg_rlen, // double avg_rlen,
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,
......@@ -8748,7 +8762,9 @@ public class OpticalFlow {
combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length];
combo_dsn_final[COMBO_DSN_INDX_DISP]= combo_dsn[COMBO_DSN_INDX_DISP].clone();
for (int i = 1; i < combo_dsn_final.length; i++) {
if (i != COMBO_DSN_INDX_SFM_GAIN) {
if ( (i != COMBO_DSN_INDX_SFM_GAIN) &&
(i != COMBO_DSN_INDX_STRENGTH) &&
(i != COMBO_DSN_INDX_STRENGTH_BG)){
Arrays.fill(combo_dsn_final[i], Double.NaN);
}
}
......@@ -9336,6 +9352,21 @@ public class OpticalFlow {
tilesY); // int height)
rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA");
// fixing NaN in strengths. It is uses to return RMS in Not needed - NaN was from Arrays.fill(combo_dsn_final[i], Double.NaN);
// ImageDtt.clt_process_tl_correlations( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
// if (use_rms) nan_slices.add(DISPARITY_STRENGTH_INDEX); // will be used for RMS if LMA succeeded
// And NaN in strength cause (at least) NaN for double avg_z = quadCLTs[ref_index].getAverageZ(true);
// public static int COMBO_DSN_INDX_STRENGTH = 1; // strength, FG
// public static int COMBO_DSN_INDX_STRENGTH_BG = 6; // background strength
for (int slice:new int[] {COMBO_DSN_INDX_STRENGTH,COMBO_DSN_INDX_STRENGTH_BG}) {
if (combo_dsn_final[slice] != null) {
for (int i = 0; i <combo_dsn_final[slice].length; i++) {
if (Double.isNaN(combo_dsn_final[slice][i])) {
combo_dsn_final[slice][i] = 0.0;
}
}
}
}
ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix,
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 7c786e91
......@@ -25,6 +25,10 @@ package com.elphel.imagej.tileprocessor;
*/
//import java.awt.Polygon;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import java.awt.Rectangle;
import java.io.File;
import java.io.FileInputStream;
......@@ -236,6 +240,7 @@ public class QuadCLTCPU {
int early_index,
int last_index
) {
boolean debug = true;
double [][] xyz_integ = new double [quadCLTs.length][3];
double [] tim = new double [quadCLTs.length];
double t00= quadCLTs[early_index].getTimeStamp();
......@@ -252,15 +257,17 @@ public class QuadCLTCPU {
double dt = (t-t0)/2; // half from previous
t0 = t;
for (int i = 0; i < 3; i++) {
double y = 0;
if (nscene > early_index) {
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene-1][0][i]);
y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
if (nscene == early_index) {
xyz_integ[nscene][i] = 0;
} else {
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene][0][i]);
}
double y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
sy[i] += y;
sxy[i] += x*y;
}
}
double denom = sx2 * s0 - sx * sx;
double [] a = new double[3], b = new double[3], ref_offs = new double[3];
for (int i = 0; i < 3; i++) {
......@@ -270,16 +277,139 @@ public class QuadCLTCPU {
}
double [][][] xyzatr_out = new double [quadCLTs.length][][];
for (int nscene = early_index; nscene <= last_index; nscene++) {
xyzatr_out[nscene] = new double[3][2];
xyzatr_out[nscene] = new double[2][3];
for (int i = 0; i < 3; i++) {
xyzatr_out[nscene][0][i] = b[i] + a[i] * tim[nscene] + xyz_integ[nscene][i] - ref_offs[i];
xyzatr_out[nscene][0][i] = b[i] + a[i] * tim[nscene] + xyz_integ[nscene][i] - ref_offs[i]; // 2 of 2
xyzatr_out[nscene][1][i] = xyzatr[nscene][1][i]; // for now - just copy old, maybe will add smth.
}
}
if (debug) {
System.out.println("refineFromImsVelocities():");
System.out.println("a= ["+a[0]+", "+a[1]+", "+a[2]+"]");
System.out.println("b= ["+b[0]+", "+b[1]+", "+b[2]+"]");
System.out.println("ref_offs=["+ref_offs[0]+", "+ref_offs[1]+", "+ref_offs[2]+"]");
System.out.println(String.format(
"%3s\t%9s\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%9s\t%9s\t%9s\t\t%s\t%9s\t%9s\t\t%9s\t%9s\t%9s",
"N","T", "X","Y","Z","X-INT","Y-INT","Z-INT","X-err","Y-err","Z-err","X-lin","Y-lin","Z-lin",
"X-corr","Y-corr","Z-corr"));
for (int nscene = early_index; nscene <= last_index; nscene++) {
System.out.println(String.format(
"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.5f\t%9.5f\t%9.5f\t\t%9.4f\t%9.4f\t%9.4f",
nscene,tim[nscene],
xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
xyz_integ[nscene][0],xyz_integ[nscene][1],xyz_integ[nscene][2],
xyzatr[nscene][0][0]-xyz_integ[nscene][0],xyzatr[nscene][0][1]-xyz_integ[nscene][1],xyzatr[nscene][0][2]-xyz_integ[nscene][2],
// b[0]+a[0]*(tim[nscene]-tim[ref_index]), b[1]+a[1]*(tim[nscene]-tim[ref_index]), b[2]+a[2]*(tim[nscene]-tim[ref_index]),
b[0]+a[0]*tim[nscene], b[1]+a[1]*tim[nscene], b[2]+a[2]*tim[nscene],
xyzatr_out[nscene][0][0]-xyzatr[nscene][0][0],
xyzatr_out[nscene][0][1]-xyzatr[nscene][0][1],
xyzatr_out[nscene][0][2]-xyzatr[nscene][0][2]
));
}
}
return xyzatr_out;
}
/**
* Refining scene poses by LPF filtering. Filters either translations or rotations
* so it may use different widths (split clt_parameters.imp.avg_len)
* repeat twice (feeding result to xyzatr) if both translation and rotation filtering
* is needed.
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence (for timestamps)
* @param xyzatr current scenes [[x,y,z],[a,t,r]] in reference scene frame
* @param avg_rlen half-radius of averaging (cosine window)
* @param filter_rot false - filter translations (xyz), true - filter rotations (atr)
* @param keep_rz do not filter 3-rd component (Z or Roll) as they can be unambiguously
* fitted
* @param ref_index reference scene index
* @param early_index earliest (lowest) scene index to use
* @param last_index last (highest) scene index to use
* @return refined array of scene poses [[x,y,z],[a,t,r]] (in reference scene frame),
* same indices as input
*/
public static double [][][] refineFromLPF(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double avg_rlen,// >= 1.0
boolean filter_rot, // false - translation. Do twice for both
boolean keep_rz,
int ref_index,
int early_index,
int last_index
) {
// int avg_len = clt_parameters.imp.avg_len;
int avg_len = (int) Math.floor(avg_rlen); // clt_parameters.imp.avg_len;
double [][][] scenes_xyzatr_pull = new double [quadCLTs.length][][];
double [] avg_weights = new double [2*avg_len+1];
for (int i = 0; i < avg_weights.length; i++) {
avg_weights[i] = Math.cos(Math.PI * (i - avg_len) / (2 * avg_rlen)); // + 1));
}
// it is incorrect for rotations - need to use quaternions!
int num_comp = filter_rot ? 4: 3;
for (int nscene = early_index; nscene <= last_index; nscene++) {
double [][] inv_scene = ErsCorrection.invertXYZATR(xyzatr[nscene]);
int n_pre = nscene + avg_len;
int n_post = nscene - avg_len;
if (n_post < early_index) n_post = early_index;
if (n_pre > last_index) n_pre = last_index;
double s0=0, sx=0, sx2=0;
double [] sy = new double[num_comp], sxy = new double [num_comp];
for (int n_other = n_post; n_other <= n_pre; n_other++) {
int ix = n_other - nscene; //+/-i
int i = ix + avg_len; // >=0
double [][] other_xyzatr = xyzatr[n_other];
double [][] other_rel = ErsCorrection.combineXYZATR(other_xyzatr,inv_scene);
double [] other;
if (filter_rot) {
other = other_rel[0];
} else {
Rotation rot = new Rotation(
RotationOrder.YXZ,
ErsCorrection.ROT_CONV,
other_rel[1][0],
other_rel[1][1],
other_rel[1][2]);
other = new double[] {
rot.getQ0(),
rot.getQ1(),
rot.getQ2(),
rot.getQ3()};
}
double w = avg_weights[i];
s0 += w;
sx += w * ix;
sx2 += w * ix * ix;
// averaging translations or rotations (quaternions)
for (int k = 0; k < other.length; k++) {
sy[k] += w * other[k];
sxy[k] += w * other[k] * ix;
}
}
double [] diff = new double[num_comp];
double [][] diff_pull = new double[2][3];
for (int k = 0; k < diff.length; k++) {
diff[k]=(sy[k]*sx2 - sxy[k]*sx) / (s0*sx2 - sx*sx);
}
if (filter_rot) {
Rotation rot = new Rotation(diff[0],diff[1],diff[2],diff[3],true);
diff_pull[1]=rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
// diff_pull[0] is {0,0,0}
} else {
diff_pull[0]=diff; // diff_pull[1] is {0,0,0}
}
// Only one component is non-zero
scenes_xyzatr_pull[nscene] = ErsCorrection.combineXYZATR(xyzatr[nscene],diff_pull);
if (keep_rz) {
scenes_xyzatr_pull[nscene][0][2] = xyzatr[nscene][0][2];
scenes_xyzatr_pull[nscene][1][2] = xyzatr[nscene][1][2];
}
}
return scenes_xyzatr_pull;
}
/**
* Get linear and angular velocities (camera frame) from
......@@ -2424,6 +2554,7 @@ public class QuadCLTCPU {
boolean silent = false;
if (use_combo) {
readComboDSI (silent);
main_dsi = this.dsi;
needs_lma = needs_lma_combo;
} else {
main_dsi = readDsiMain();
......
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