Commit 5a110e1d authored by Andrey Filippov's avatar Andrey Filippov

Added linear velocities scale adjustment

parent 54e00356
...@@ -2101,7 +2101,8 @@ public class Interscene { ...@@ -2101,7 +2101,8 @@ public class Interscene {
); );
double [] rms = new double[5]; double [] rms = new double[5];
double [] quat = new double[4]; 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; int debug_lev = debugLevel; // 3;
// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters // double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0); double translation_weight = 1.0 / (avg_z + 1.0);
......
...@@ -467,6 +467,10 @@ public class QuadCLTCPU { ...@@ -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 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 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 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( double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs, quadCLTs, // final QuadCLT[] quadCLTs,
...@@ -486,7 +490,7 @@ public class QuadCLTCPU { ...@@ -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 quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3; int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters 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( double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode, quat_lma_mode, // int quat_lma_mode,
...@@ -519,6 +523,17 @@ public class QuadCLTCPU { ...@@ -519,6 +523,17 @@ public class QuadCLTCPU {
ims_mount_atr, // double [] ims_atr, ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q) quat); // double [] corr_q)
if (apply_imu_orient) { 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); clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***"); System.out.println ("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***");
...@@ -575,11 +590,133 @@ public class QuadCLTCPU { ...@@ -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 { } else {
System.out.println ("*** Failed to calculate IMS mount correction! ***"); 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( public static double [] getOmegaCorrections(
CLTParameters clt_parameters, // CLTParameters clt_parameters, CLTParameters clt_parameters, // CLTParameters clt_parameters,
double [][][] rotated_xyzatr, double [][][] rotated_xyzatr,
...@@ -881,7 +1018,7 @@ public class QuadCLTCPU { ...@@ -881,7 +1018,7 @@ public class QuadCLTCPU {
double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}}; 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? if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?
for (int i = 0; i < 3; i++) { 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]; 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