Skip to content

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

Added linear velocities scale adjustment

parent 54e00356
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 188 additions and 5 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 5a110e1d
......@@ -2101,7 +2101,8 @@ public class Interscene {
);
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
// int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 5a110e1d
......@@ -467,6 +467,10 @@ public class QuadCLTCPU {
boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted
boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
boolean adjust_accl = clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
boolean apply_accl = clt_parameters.imp.apply_accl; // apply IMU velocities scales
double quat_max_change = clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
double quat_min_lin = clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -486,7 +490,7 @@ public class QuadCLTCPU {
int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
double translation_weight = 0.0; // 1.0 / (avg_z + 1.0);
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
......@@ -519,6 +523,17 @@ public class QuadCLTCPU {
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
if (apply_imu_orient) {
for (int i = 0; i < new_ims_mount_atr.length; i++) {
if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
apply_imu_orient = false;
if (debugLevel > -3) {
System.out.println ("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+" exceeds the specified limit ("+quat_max_change+")");
System.out.println ("*** Orientation update is disabled.");
}
}
}
}
if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
......@@ -575,11 +590,133 @@ public class QuadCLTCPU {
}
}
double [] acc_corr = null;
if (adjust_accl) {
acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
rotated_xyzatr, // double [][][] rotated_xyzatr,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
quat_min_lin, // double min_range,
debugLevel); // int debugLevel
if (acc_corr != null) {
double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
double [] new_accl_corr = used_accl_corr.clone();
int num_corr=0;
for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
num_corr++;
}
if (debugLevel > -1) {
System.out.println(String.format(
"Used velocities scales = [%9f, %9f, %9f]", used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
System.out.println(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]", acc_corr[0], acc_corr[1], acc_corr[2]));
System.out.println(String.format(
"New velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
if (apply_accl && (num_corr>0)) {
clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
if (debugLevel > -3) {
System.out.println(String.format(
"Applied new velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
System.out.println ("*** Need to save the main configuration file ***");
}
} else {
if (debugLevel > -3) {
System.out.println(String.format(
"New velocities scales are not applied = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
}
} else {
System.out.println("*** Adjustment of the gyro omegas failed ***");
}
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
}
public static double [] getVelocitiesCorrections(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
double [][][] rotated_xyzatr,
QuadCLT[] quadCLTs,
int ref_index,
int early_index,
int last_index,
double min_range,
int debugLevel
) {
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
double [] timestamps = new double [quadCLTs.length];
double [][][] data = new double[3][last_index-early_index+1][2];
// double ts_ref = quadCLTs[ref_index].getTimeStamp();
double [][] xyz_minmax = new double [3][2]; // zeros OK as it is for ref scene
for (int nscene = early_index; nscene <= last_index; nscene++) {
timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
xyzatr[nscene] = ers_ref.getSceneXYZATR(quadCLTs[nscene].getImageName());
for (int i = 0; i < 3; i++) {
// double d = xyzatr[nscene][0][i] - rotated_xyzatr[nscene][0][i];
xyz_minmax[i][0] = Math.min(xyz_minmax[i][0], xyzatr[nscene][0][i]);
xyz_minmax[i][1] = Math.max(xyz_minmax[i][1], xyzatr[nscene][0][i]);
}
}
boolean [] dir_ok = new boolean[3];
for (int i = 0; i < 3; i++) {
dir_ok[i] = (xyz_minmax[i][1]-xyz_minmax[i][0]) > min_range;
}
for (int nscene = early_index; nscene <= last_index; nscene++) {
// double rel_ts = timestamps[nscene] - ts_ref;
for (int i = 0; i < 3; i++) if (dir_ok[i] ){
data[i][nscene-early_index][0] = xyzatr[nscene][0][i]; // for all xyz the same
data[i][nscene-early_index][1] = rotated_xyzatr[nscene][0][i];
}
}
PolynomialApproximation pa= new PolynomialApproximation(-1); // debugLevel);
double [][] coeffs = new double [3][2];
for (int i = 0; i < 3; i++) {
if (dir_ok[i] ){
coeffs[i] = pa.polynomialApproximation1d(data[i], 1); // linear
} else {
coeffs[i] = new double [] {Double.NaN,Double.NaN};
}
}
if (debugLevel > -1) {
System.out.println("Velocity X correction: " +coeffs[0][1]+" ("+coeffs[0][0]+")");
System.out.println("Velocity Y correction: " +coeffs[1][1]+" ("+coeffs[1][0]+")");
System.out.println("Velocity Z correction: " +coeffs[2][1]+" ("+coeffs[2][0]+")");
if (debugLevel > 0) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
"N","X","Y","Z","IMU-X","IMU-Y","IMU-Z","lX","lY","lZ"));
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%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
nscene,
xyzatr[nscene][0][0],
xyzatr[nscene][0][1],
xyzatr[nscene][0][2],
rotated_xyzatr[nscene][0][0],
rotated_xyzatr[nscene][0][1],
rotated_xyzatr[nscene][0][2],
coeffs[0][0]+coeffs[0][1] * xyzatr[nscene][0][0],
coeffs[1][0]+coeffs[1][1] * xyzatr[nscene][0][1],
coeffs[2][0]+coeffs[2][1] * xyzatr[nscene][0][2]));
}
}
}
return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};
}
public static double [] getOmegaCorrections(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
double [][][] rotated_xyzatr,
......@@ -881,7 +1018,7 @@ public class QuadCLTCPU {
double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?
for (int i = 0; i < 3; i++) {
dxyzatr[0][i] -= pimu_offsets[0][i];
dxyzatr[0][i] /= pimu_offsets[0][i];
dxyzatr[1][i] -= pimu_offsets[1][i];
}
}
......
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