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
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/IntersceneMatchParameters.java
View file @ 5a110e1d
......@@ -46,6 +46,7 @@ public class IntersceneMatchParameters {
// Tilt -> -pimu_omega[1] rad/s, average did_pimu.theta[1]/did_pimu.dt
// Roll -> +pimu_omega[0] rad/s, average did_pimu.theta[0]/did_pimu.dt
public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418};
public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1)
@Deprecated
......@@ -117,6 +118,8 @@ public class IntersceneMatchParameters {
public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
public boolean adjust_gyro = false;// adjust qyro omegas offsets
public boolean apply_gyro = true; // apply adjusted qyro omegas offsets
public boolean adjust_accl = false;// adjust IMU velocities scales
public boolean apply_accl = true; // apply IMU velocities scales
public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
......@@ -459,6 +462,7 @@ public class IntersceneMatchParameters {
public double quat_max_change = 0.1; // radians
public double quat_min_transl = 5.0; // meters to adjust by translation
public double quat_min_rot = 0.1; // radians to adjust by rotation
public double quat_min_lin = 10.0; // meters, minimal distance per axis to adjust IMS velocity scale
public boolean stereo_merge = true;
public int stereo_gap = 32; // pixels between right and left frames
......@@ -528,8 +532,9 @@ public class IntersceneMatchParameters {
public double [][] get_pimu_offs(){
// TODO: use ims_ortho, ims_mount_atr
return new double [][] {new double[3],
{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
return new double [][] {
{ pimu_acc_offs[0] + 1, pimu_acc_offs[1] + 1, pimu_acc_offs[2] + 1},
{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
}
public void set_pimu_omegas(double [] atr) {
......@@ -537,6 +542,14 @@ public class IntersceneMatchParameters {
pimu_gyr_offs[1] = - atr[1];
pimu_gyr_offs[0] = atr[2];
}
public void set_pimu_velocities_scales(double [] vxyz) {
pimu_acc_offs[0] = vxyz[0] - 1;
pimu_acc_offs[1] = vxyz[1] - 1;
pimu_acc_offs[2] = vxyz[2] - 1;
}
public void dialogQuestions(GenericJTabbedDialog gd) {
// gd.addMessage ("Scene parameters selection");
// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
......@@ -560,6 +573,9 @@ public class IntersceneMatchParameters {
gd.addStringField ("IMU angular corrections (rad/s)", IntersceneMatchParameters.doublesToString(pimu_gyr_offs), 80,
"Average from did_pimu.theta[]/did_pimu.dt when not moving.");
gd.addStringField ("IMU accelerators gain", IntersceneMatchParameters.doublesToString(pimu_acc_offs), 80,
"IMU accelerators gain difference from 1. Negative values when IMU-reported velocities are smaller than actual");
gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
......@@ -661,6 +677,12 @@ public class IntersceneMatchParameters {
"Adjust qyro omegas offsets.");
gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
"Apply adjusted qyro omegas offsets.");
gd.addCheckbox ("Adjust accelerometers scales", this.adjust_accl,
"Adjust IMU linear velocities scales.");
gd.addCheckbox ("Apply accelerometers scales", this.apply_accl,
"Apply IMU linear velocities scales.");
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
"Calculate camera orientation correction from predicted by IMS .");
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
......@@ -1304,6 +1326,8 @@ public class IntersceneMatchParameters {
"Do not use translation for IMS mount adjustment if it is too small.");
gd.addNumericField("Minimal rotation for mount calibration", this.quat_min_rot, 3,5,"rad",
"Do not use rotations for IMS mount adjustment if it is too small.");
gd.addNumericField("Minimal per-axis travel", this.quat_min_lin, 3,5,"m",
"Minimal per-axis travel to adjust accelerometer sensitivity.");
gd.addTab("Stereo/Video","Parameters for stereo video generation");
gd.addMessage ("Stereo");
......@@ -1409,6 +1433,7 @@ public class IntersceneMatchParameters {
this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.pimu_acc_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.fmg_initial_en = gd.getNextBoolean();
......@@ -1457,6 +1482,10 @@ public class IntersceneMatchParameters {
this.apply_imu_orient = gd.getNextBoolean();
this.adjust_gyro = gd.getNextBoolean();
this.apply_gyro = gd.getNextBoolean();
this.adjust_accl = gd.getNextBoolean();
this.apply_accl = gd.getNextBoolean();
this.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean();
this.use_quat_corr = gd.getNextBoolean();
......@@ -1752,6 +1781,7 @@ public class IntersceneMatchParameters {
this.quat_max_change = gd.getNextNumber();
this.quat_min_transl = gd.getNextNumber();
this.quat_min_rot = gd.getNextNumber();
this.quat_min_lin = gd.getNextNumber();
if (stereo_views.length > 0) {
int i = gd.getNextChoiceIndex();
......@@ -1849,6 +1879,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"ims_mount_atr", IntersceneMatchParameters.doublesToString(this.ims_mount_atr));
properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
properties.setProperty(prefix+"pimu_gyr_offs", IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs));
properties.setProperty(prefix+"pimu_acc_offs", IntersceneMatchParameters.doublesToString(this.pimu_acc_offs));
properties.setProperty(prefix+"ims_scale_xyz", IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
properties.setProperty(prefix+"ims_scale_atr", IntersceneMatchParameters.doublesToString(this.ims_scale_atr));
properties.setProperty(prefix+"fmg_initial_en", this.fmg_initial_en + ""); // boolean
......@@ -1896,6 +1927,8 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"apply_imu_orient", this.apply_imu_orient+""); // boolean
properties.setProperty(prefix+"adjust_gyro", this.adjust_gyro+""); // boolean
properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean
properties.setProperty(prefix+"adjust_accl", this.adjust_accl+""); // boolean
properties.setProperty(prefix+"apply_accl", this.apply_accl+""); // boolean
properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
......@@ -2201,6 +2234,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"quat_max_change", this.quat_max_change+""); // double
properties.setProperty(prefix+"quat_min_transl", this.quat_min_transl+""); // double
properties.setProperty(prefix+"quat_min_rot", this.quat_min_rot+""); // double
properties.setProperty(prefix+"quat_min_lin", this.quat_min_lin+""); // double
properties.setProperty(prefix+"stereo_merge", this.stereo_merge+""); // boolean
properties.setProperty(prefix+"stereo_gap", this.stereo_gap+""); // int
......@@ -2253,6 +2287,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"ims_mount_atr")!=null) this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
if (properties.getProperty(prefix+"ims_mount_xyz")!=null) this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3);
if (properties.getProperty(prefix+"pimu_acc_offs")!=null) this.pimu_acc_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_acc_offs"),3);
if (properties.getProperty(prefix+"ims_scale_xyz")!=null) this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
if (properties.getProperty(prefix+"ims_scale_atr")!=null) this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
if (properties.getProperty(prefix+"fmg_initial_en")!=null) this.fmg_initial_en=Boolean.parseBoolean(properties.getProperty(prefix+"fmg_initial_en"));
......@@ -2300,6 +2335,11 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"apply_imu_orient")!=null) this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
if (properties.getProperty(prefix+"adjust_gyro")!=null) this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
if (properties.getProperty(prefix+"apply_gyro")!=null) this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
if (properties.getProperty(prefix+"adjust_accl")!=null) this.adjust_accl=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_accl"));
if (properties.getProperty(prefix+"apply_accl")!=null) this.apply_accl=Boolean.parseBoolean(properties.getProperty(prefix+"apply_accl"));
if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
......@@ -2612,6 +2652,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"quat_max_change")!=null) this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
if (properties.getProperty(prefix+"quat_min_transl")!=null) this.quat_min_transl=Double.parseDouble(properties.getProperty(prefix+"quat_min_transl"));
if (properties.getProperty(prefix+"quat_min_rot")!=null) this.quat_min_rot=Double.parseDouble(properties.getProperty(prefix+"quat_min_rot"));
if (properties.getProperty(prefix+"quat_min_lin")!=null) this.quat_min_lin=Double.parseDouble(properties.getProperty(prefix+"quat_min_lin"));
if (properties.getProperty(prefix+"stereo_merge")!=null) this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
if (properties.getProperty(prefix+"stereo_gap")!=null) this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
......@@ -2684,6 +2725,7 @@ public class IntersceneMatchParameters {
imp.ims_mount_atr = this.ims_mount_atr.clone();
imp.ims_mount_xyz = this.ims_mount_xyz.clone();
imp.pimu_gyr_offs = this.pimu_gyr_offs.clone();
imp.pimu_acc_offs = this.pimu_acc_offs.clone();
imp.ims_scale_xyz = this.ims_scale_xyz.clone();
imp.ims_scale_atr = this.ims_scale_atr.clone();
imp.fmg_initial_en = this.fmg_initial_en;
......@@ -2731,6 +2773,8 @@ public class IntersceneMatchParameters {
imp.apply_imu_orient = this.apply_imu_orient;
imp.adjust_gyro = this.adjust_gyro;
imp.apply_gyro = this.apply_gyro;
imp.adjust_accl = this.adjust_accl;
imp.apply_accl = this.apply_accl;
imp.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr;
imp.use_quat_corr = this.use_quat_corr;
......@@ -3032,6 +3076,7 @@ public class IntersceneMatchParameters {
imp.quat_max_change = this.quat_max_change;
imp.quat_min_transl = this.quat_min_transl;
imp.quat_min_rot = this.quat_min_rot;
imp.quat_min_lin = this.quat_min_lin;
imp.stereo_merge = this.stereo_merge;
imp.stereo_gap = this.stereo_gap;
......
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