Commit 7c786e91 authored by Andrey Filippov's avatar Andrey Filippov

Testing translation filtering by IMS velocities

parent a12d2254
...@@ -187,7 +187,8 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -187,7 +187,8 @@ public class ErsCorrection extends GeometryCorrection {
DP_DVX, DP_DVY, DP_DVZ, DP_DVX, DP_DVY, DP_DVZ,
DP_DSVAZ, DP_DSVTL, DP_DSVRL, DP_DSVAZ, DP_DSVTL, DP_DSVRL,
DP_DSVX, DP_DSVY, DP_DSVZ}; 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_AT_INDICES = {DP_DSAZ,DP_DSTL};
public static final int [] DP_ATT_ERS_INDICES = {DP_DSVAZ,DP_DSVTL}; public static final int [] DP_ATT_ERS_INDICES = {DP_DSVAZ,DP_DSVTL};
...@@ -234,17 +235,19 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -234,17 +235,19 @@ public class ErsCorrection extends GeometryCorrection {
*/ */
public static boolean [] getParamSelect( public static boolean [] getParamSelect(
boolean use_XY,
boolean use_AT, boolean use_AT,
boolean use_ERS) { boolean use_ERS) {
boolean [] param_select = new boolean[DP_NUM_PARS]; 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_AT) for (int i:DP_AT_INDICES) param_select[i] = true;
if (use_ERS) for (int i:DP_ATT_ERS_INDICES) param_select[i] = true; if (use_ERS) for (int i:DP_ATT_ERS_INDICES) param_select[i] = true;
return param_select; return param_select;
} }
public static double [] getParamRegWeights( public static double [] getParamRegWeights(
double reg_weight, double reg_weight) {
boolean use_AT) { // boolean use_AT) {
double [] reg_weights = new double[DP_NUM_PARS]; double [] reg_weights = new double[DP_NUM_PARS];
reg_weights[DP_DSX] = reg_weight; reg_weights[DP_DSX] = reg_weight;
reg_weights[DP_DSY] = reg_weight; reg_weights[DP_DSY] = reg_weight;
......
...@@ -5265,6 +5265,13 @@ public class OpticalFlow { ...@@ -5265,6 +5265,13 @@ public class OpticalFlow {
if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) { if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
mb_max_gain = clt_parameters.imp.mb_max_gain_inter; 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 disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
boolean ers_from_ims = true; // false; // change later boolean ers_from_ims = true; // false; // change later
// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0; // int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
...@@ -5283,6 +5290,13 @@ public class OpticalFlow { ...@@ -5283,6 +5290,13 @@ public class OpticalFlow {
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain, mb_max_gain, // double mb_max_gain,
disable_ers, // boolean disable_ers, 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 reliable_ref, // boolean [] reliable_ref, // null or bitmask of reliable reference tiles
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
...@@ -8748,7 +8762,9 @@ public class OpticalFlow { ...@@ -8748,7 +8762,9 @@ public class OpticalFlow {
combo_dsn_titles_full.length:combo_dsn_titles.length][combo_dsn[0].length]; 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(); combo_dsn_final[COMBO_DSN_INDX_DISP]= combo_dsn[COMBO_DSN_INDX_DISP].clone();
for (int i = 1; i < combo_dsn_final.length; i++) { 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); Arrays.fill(combo_dsn_final[i], Double.NaN);
} }
} }
...@@ -9336,6 +9352,21 @@ public class OpticalFlow { ...@@ -9336,6 +9352,21 @@ public class OpticalFlow {
tilesY); // int height) tilesY); // int height)
rslt_suffix = "-INTER-INTRA"; rslt_suffix = "-INTER-INTRA";
rslt_suffix += (clt_parameters.correlate_lma?"-LMA":"-NOLMA"); 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 ref_scene.saveDoubleArrayInModelDirectory( // error
rslt_suffix, // String suffix, rslt_suffix, // String suffix,
......
...@@ -25,6 +25,10 @@ package com.elphel.imagej.tileprocessor; ...@@ -25,6 +25,10 @@ package com.elphel.imagej.tileprocessor;
*/ */
//import java.awt.Polygon; //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.awt.Rectangle;
import java.io.File; import java.io.File;
import java.io.FileInputStream; import java.io.FileInputStream;
...@@ -236,6 +240,7 @@ public class QuadCLTCPU { ...@@ -236,6 +240,7 @@ public class QuadCLTCPU {
int early_index, int early_index,
int last_index int last_index
) { ) {
boolean debug = true;
double [][] xyz_integ = new double [quadCLTs.length][3]; double [][] xyz_integ = new double [quadCLTs.length][3];
double [] tim = new double [quadCLTs.length]; double [] tim = new double [quadCLTs.length];
double t00= quadCLTs[early_index].getTimeStamp(); double t00= quadCLTs[early_index].getTimeStamp();
...@@ -252,15 +257,17 @@ public class QuadCLTCPU { ...@@ -252,15 +257,17 @@ public class QuadCLTCPU {
double dt = (t-t0)/2; // half from previous double dt = (t-t0)/2; // half from previous
t0 = t; t0 = t;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
double y = 0; if (nscene == early_index) {
if (nscene > early_index) { xyz_integ[nscene][i] = 0;
xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene-1][0][i]); } else {
y = xyzatr[nscene][0][i] - xyz_integ[nscene][i]; 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; sy[i] += y;
sxy[i] += x*y; sxy[i] += x*y;
} }
} }
double denom = sx2 * s0 - sx * sx; double denom = sx2 * s0 - sx * sx;
double [] a = new double[3], b = new double[3], ref_offs = new double[3]; double [] a = new double[3], b = new double[3], ref_offs = new double[3];
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
...@@ -270,16 +277,139 @@ public class QuadCLTCPU { ...@@ -270,16 +277,139 @@ public class QuadCLTCPU {
} }
double [][][] xyzatr_out = new double [quadCLTs.length][][]; double [][][] xyzatr_out = new double [quadCLTs.length][][];
for (int nscene = early_index; nscene <= last_index; nscene++) { 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++) { 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. 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; 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 * Get linear and angular velocities (camera frame) from
...@@ -2424,6 +2554,7 @@ public class QuadCLTCPU { ...@@ -2424,6 +2554,7 @@ public class QuadCLTCPU {
boolean silent = false; boolean silent = false;
if (use_combo) { if (use_combo) {
readComboDSI (silent); readComboDSI (silent);
main_dsi = this.dsi;
needs_lma = needs_lma_combo; needs_lma = needs_lma_combo;
} else { } else {
main_dsi = readDsiMain(); 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