Skip to content

I
imagej-elphel
Commit 4354952a authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Restored single-seriest rotation offset with the new code.

parent 38a3b797
Hide whitespace changes
Inline Side-by-side
Showing with 380 additions and 56 deletions
src/main/java/com/elphel/imagej/tileprocessor/XyzQLma.java
View file @ 4354952a
......@@ -983,9 +983,8 @@ public class XyzQLma {
return sb.toString();
}
// moved here from SeriesInfinityCorrection
public static double[] adjustImuOrient(
public static double[] adjustImuOrient( // multi-segment
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[][] quadCLTss,
int [] ref_indices,
QuadCLT index_scene, // where to put result
......@@ -1046,8 +1045,8 @@ public class XyzQLma {
if (debugLevel > -3) {
System.out.println("adjustImuOrient(): transl_cost="+transl_cost);
}
String suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX) : null;
// String suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX) : null;
String suffix = save_details ? QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX : null;
double [] y_scale_p = new double[1];
double [][][][] rotated_xyzatrs = rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
......@@ -1214,7 +1213,227 @@ public class XyzQLma {
return quat;
}
public static double [][][][] rotateImsToCameraXYZ(
public static double[] adjustImuOrient( // single-segment
CLTParameters clt_parameters, // CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
int ref_index,
boolean save_details,
int debugLevel) {
// @Deprecated
// boolean orient_by_move = clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
// @Deprecated
// boolean orient_by_rot = clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
// if (!orient_by_move && !orient_by_rot) {
// System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
// return null;
// }
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
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
// boolean use3= clt_parameters.imp.use3; // false; // false - full4 quaternion (with scale), true - only q1,q2,q3
double transl_cost = clt_parameters.imp.wser_orient_transl; // AGL corrected, 0.5 - equal weights, 0.0 - rotation only, 1.0 - translation-only
// calculate average Z for the sequence (does not need to be very accurate
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double [] rms = new double[7];
double [] quat = new double[4];
int debug_lev = debugLevel; // 3;
if (debugLevel > -3) {
System.out.println("adjustImuOrient(): transl_cost="+transl_cost);
}
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
null, // quat_corr, // double [] quat_corr,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
0, // earliest_scene, // final int early_index,
quadCLTs.length -1, // last_index, //(quadCLTs.length -1) // int last_index,
debugLevel); // final int debugLevel
double [][][] xyzatr = new double [quadCLTs.length][][];
for (int nscene = 0; nscene < xyzatr.length; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
String suffix = save_details ? QuadCLTCPU.IMU_CALIB_DETAILS_SUFFIX : null;
double [] y_scale_p = new double[1];
double [][][] rotated_xyzatrs = rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
avg_z, // double avg_height,
transl_cost, // translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
rms, // double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
y_scale_p, // double [] y_scale_p
suffix, // String suffix,
debug_lev); // int debugLevel
if (rotated_xyzatrs != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
StringBuffer sb = new StringBuffer();
sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
sb.append("Applying correction to the IMS mount orientation:\n");
sb.append("avg_z= "+avg_z+" m\n");
sb.append("transl_cost= "+transl_cost+"\n");
sb.append("y_scale= "+y_scale_p[0]+"\n");
sb.append("RMSe=");
for (int i = 0; i < rms.length-1; i++) sb.append(rms[i]+",");
sb.append(rms[rms.length-1]+"\n");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
double quat_scale = Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
double [] new_degrees = new double[3];
for (int i = 0; i < 3; i++) new_degrees[i]=new_ims_mount_atr[i]*180/Math.PI;
sb.append("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
sb.append("scale="+quat_scale+"\n");
double angle = Math.sqrt(corr_angles[0]*corr_angles[0] + corr_angles[1]*corr_angles[1] + corr_angles[2]*corr_angles[2]);
sb.append("delta ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"] ("+ angle+")\n");
sb.append("delta ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"] ("+ angle*180/Math.PI+")\n");
sb.append(" new ATR(rad)=["+new_ims_mount_atr[0]+", "+new_ims_mount_atr[1]+", "+new_ims_mount_atr[2]+"]\n");
sb.append(" new ATR(deg)=["+new_degrees[0]+", "+new_degrees[1]+", "+new_degrees[2]+"]\n");
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;
sb.append("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+
" exceeds the specified limit ("+quat_max_change+")\n");
sb.append("*** Orientation update is disabled.\n");
}
}
}
if (apply_imu_orient) {
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
sb.append("*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n");
} else {
sb.append("*** IMU mount angles calculated, but not applied ***\n");
}
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
sb.append(String.format(
"Using ATR(rad)=[%9f, %9f, %9f]\n", new_atr[0], new_atr[1], new_atr[2]));
sb.append(String.format(
"Using ATR(deg)=[%9f, %9f, %9f]\n", degrees[0], degrees[1], degrees[2]));
double [] omega_corr = null;
// Work later on adjust_gyro and adjust_accl;
/*
if (adjust_gyro) {
omega_corr = getOmegaCorrections(
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,
debugLevel); // int debugLevel
if (omega_corr != null) {
double [] used_omegas = clt_parameters.imp.get_pimu_offs()[1]; // returns in atr order
double [] new_omegas = new double[3];
for (int i = 0; i < 3; i++) {
new_omegas[i] = used_omegas[i] - omega_corr[i];
}
sb.append(String.format(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
used_omegas[0],used_omegas[1],used_omegas[2]));
sb.append(String.format(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
omega_corr[0], omega_corr[1], omega_corr[2]));
sb.append(String.format(
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
if (apply_gyro) {
clt_parameters.imp.set_pimu_omegas(new_omegas);
sb.append(String.format(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
sb.append(String.format(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n",
new_omegas[0], new_omegas[1], new_omegas[2]));
}
} else {
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
}
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++;
}
sb.append(String.format(
"Used velocities scales = [%9f, %9f, %9f]\n",
used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
sb.append(String.format(
"Diff.velocities scales = [%9f, %9f, %9f]\n",
acc_corr[0], acc_corr[1], acc_corr[2]));
sb.append(String.format(
"New velocities scales = [%9f, %9f, %9f]\n",
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);
sb.append(String.format(
"Applied new velocities scales = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
sb.append("*** Need to save the main configuration file ***\n");
} else {
sb.append(String.format(
"New velocities scales are not applied = [%9f, %9f, %9f]\n",
new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
}
} else {
sb.append("*** Adjustment of the gyro omegas failed ***\n");
}
//
}
*/
sb.append("------------------------\n\n");
quadCLTs[ref_index].appendStringInModelDirectory(sb.toString(),QuadCLTCPU.IMU_CALIB_LOGS_SUFFIX); // String suffix)
if (debugLevel > -3) {
System.out.print(sb.toString());
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
return quat;
}
public static double [][][][] rotateImsToCameraXYZ( // multi-segment
CLTParameters clt_parameters,
double avg_height,
double transl_cost, // 1.0 - pure translation, 0.0 - pure rotation
......@@ -1231,18 +1450,6 @@ public class XyzQLma {
boolean use_inv = true; // false; //
double [][][] xyzatr = flattenPoses(xyzatrs);
double [][][] ims_xyzatr = flattenPoses(ims_xyzatrs);
/*
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
avg_height, // double avg_height,
transl_cost, // double transl_cost,
quadCLTss, // QuadCLT[][] quadCLTss,
xyzatrs, // double [][][][] xyzatrs,
ims_xyzatrs, // double [][][][] ims_xyzatrs,
rms, // double [] rms, // null or double[5];
y_scale_p, // double [] y_scale_p, // if not null will return y_scale
debugLevel); // int debugLevel
*/
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
avg_height, // double avg_height, // for scaling translational components
......@@ -1303,6 +1510,79 @@ public class XyzQLma {
}
return rot_ims_xyzatrs;
}
public static double [][][] rotateImsToCameraXYZ( // single-segment
CLTParameters clt_parameters,
double avg_height,
double transl_cost, // 1.0 - pure translation, 0.0 - pure rotation
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] ims_xyzatr,
int ref_index,
double [] rms, // null or double[5];
double [] quaternion, // null or double[4]
double [] y_scale_p,
String suffix,
int debugLevel) {
boolean use_inv = true; // false; //
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
avg_height, // double avg_height, // for scaling translational components
transl_cost, // double transl_cost,
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
rms, // double [] rms, // null or double[5];
y_scale_p, // double [] y_scale_p, // if not null will return y_scale
debugLevel); // int debugLevel
if (quat == null) {
return null;
}
if (quaternion != null) {
System.arraycopy(quat, 0, quaternion, 0, 4);
}
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
Rotation rot_invert = rot.revert();
int num_scenes = ims_xyzatr.length;
double [][][] rot_ims_xyzatr = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
double [] rot_ims_xyz = new double[3];
for (int nscene = 0; nscene < num_scenes; nscene++) {
rot.applyTo(ims_xyzatr[nscene][0], rot_ims_xyz); // xyz from ims rotated to match the camera
// not used
// Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
// xyzatrs[nref][nscene][1][0], xyzatrs[nref][nscene][1][1], xyzatrs[nref][nscene][1][2]);
// convert ims angles to quaternion:
Rotation rot_ims = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2]);
Rotation rot_corr_ims,rotation_atr2;
if (use_inv) { // should not be used
rot_corr_ims = rot_invert.applyTo(rot_ims);
rotation_atr2 = rot_corr_ims.applyTo(rot); // applyInverseTo?
} else {
rot_corr_ims = rot.applyTo(rot_ims); // applying correction to the IMS orientation
rotation_atr2 = rot_corr_ims.applyTo(rot_invert); // applyInverseTo?
}
rot_ims_xyzatr[nscene] = new double [][] {rot_ims_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
if (suffix != null) {
Path orient_path = Paths.get(quadCLTs[ref_index].getX3dDirectory(true)).
resolve(quadCLTs[ref_index].getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
saveRotateImsDetails(
rot, // Rotation rot,
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
rot_ims_xyzatr, // double [][][] rotated_xyzatr,
y_scale_p[0], // double y_scale,
orient_path.toString()); // String path) {
if (debugLevel > -3) {
System.out.println("Orientation details saved to "+orient_path.toString());
}
}
return rot_ims_xyzatr;
}
/**
* Flatten multi-segment sequences to a single segment ones. Need to apply to double [][][][] xyzatrs and
* double [][][][] ims_xyzatrs separately
......@@ -1472,6 +1752,7 @@ public class XyzQLma {
return xyzQLma.getQuaternion();
}
}
public static void saveRotateImsMultiDetails(
Rotation rot,
double [][][][] xyzatrs,
......@@ -1513,6 +1794,44 @@ public class XyzQLma {
sb.toString());
}
public static void saveRotateImsDetails(
Rotation rot,
double [][][] xyzatr,
double [][][] ims_xyzatr,
double [][][] rotated_xyzatr,
double y_scale,
String path) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
StringBuffer sb = new StringBuffer();
sb.append("quat\t"+rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\n");
sb.append("ATR(rad)\t"+angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\n");
sb.append("ATR(deg)\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
sb.append("y_scale=\t"+y_scale+"\n");
sb.append(
"N\tCAM-X\tCAM-Y\tCAM-Z\tCAM-A\tCAM-T\tCAM-R\t"+
"IMS-X\tIMS-Y\tIMS-Z\tIMS-A\tIMS-T\tIMS-R\t"+
"RIMS-X\tRIMS-Y\tRIMS-Z\tRIMS-A\tRIMS-T\tRIMS-R\n");
int num_scenes = xyzatr.length;
for (int nscene = 0; nscene < num_scenes; nscene++) {
sb.append(String.format("%3d"+
"\t%f\t%f\t%f\t%f\t%f\t%f"+
"\t%f\t%f\t%f\t%f\t%f\t%f"+
"\t%f\t%f\t%f\t%f\t%f\t%f\n",
nscene,
xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],
ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],
rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],
rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));
}
CalibrationFileManagement.saveStringToFile (
path,
sb.toString());
}
public static double [] adjustImuOrientMulti(
CLTParameters clt_parameters,
QuadCLT [] refCLTs,
......@@ -1582,35 +1901,36 @@ public class XyzQLma {
int [] ref_indices = new int [refCLTs.length];
double [][] quats = process_segments? (new double [refCLTs.length][]) : null;
QuadCLT[][] quadCLTss = new QuadCLT[refCLTs.length][];
for (int nref = 0; nref < refCLTs.length; nref++) {
QuadCLT ref_scene = refCLTs[nref];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
String [] fl_ts = ref_scene.getFirstLastTimestamps();
boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
String name_ref = ref_scene.getImageName();
QuadCLT[] quadCLTs = new QuadCLT[names.length];
int ref_index = -1;
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
if (names[nscene].equals(name_ref)) {
ref_index = nscene;
quadCLTs[nscene] = ref_scene;
} else {
quadCLTs[nscene] = scene_map.get(names[nscene]);
}
}
if (ref_index < 0) {
System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
continue;
}
ref_indices[nref] = ref_index;
quadCLTss[nref] = quadCLTs;
if (ref_scene.getImageName().equals("1763233239_531146")) {
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
for (int nref = 0; nref < refCLTs.length; nref++) {
QuadCLT ref_scene = refCLTs[nref];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
String [] fl_ts = ref_scene.getFirstLastTimestamps();
boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
String name_ref = ref_scene.getImageName();
QuadCLT[] quadCLTs = new QuadCLT[names.length];
int ref_index = -1;
for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
if (names[nscene].equals(name_ref)) {
ref_index = nscene;
quadCLTs[nscene] = ref_scene;
} else {
quadCLTs[nscene] = scene_map.get(names[nscene]);
}
// just for testing - adjusting per segment
if (quats != null) {
}
if (ref_index < 0) {
System.out.println("adjustImuOrientMulti(): ref_index <0 for ref_scene "+name_ref);
continue;
}
ref_indices[nref] = ref_index;
quadCLTss[nref] = quadCLTs;
if (ref_scene.getImageName().equals("1763233239_531146")) {
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
System.out.println("ref_scene.getImageName()=="+ref_scene.getImageName());
}
// just for testing - adjusting per segment
if (quats != null) {
/*
quats [nref] = QuadCLTCPU.adjustImuOrient(
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo,
......@@ -1618,16 +1938,22 @@ public class XyzQLma {
ref_index, // int ref_index,
0, // int earliest_scene,
quadCLTs.length - 1, // int last_index,
save_sequence_orient, // boolean save_details,
save_sequence_orient,// boolean save_details,
debugLevel); // int debugLevel);
}
continue; // just to put a break point
*/
quats [nref] = adjustImuOrient( // single-segment
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
save_sequence_orient, // boolean save_details,
debugLevel); // int debugLevel);
}
continue; // just to put a break point
}
// adjusting for all series
double [] quat = adjustImuOrient(
double [] quat = adjustImuOrient( // multi-segment
clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
orient_combo, // boolean orient_combo,
quadCLTss, // QuadCLT[][] quadCLTss,
ref_indices, // int [] ref_indices,
index_CLT, // QuadCLT index_scene, // where to put result
......@@ -1636,23 +1962,21 @@ public class XyzQLma {
// save per-segment quaternion and angles, and the result one
if (save_sequence_orient && (quats != null)) {
saveQuternionCorrection(
saveQuaternionCorrection(
quats, // double [][] quats,
quat, // double [] quat,
refCLTs, // QuadCLT [] refCLTs,
index_CLT, // QuadCLT index_CLT,
orient_combo, // boolean orient_combo,
(debugLevel>-3)); // boolean debug)
}
return quat;
}
public static void saveQuternionCorrection(
public static void saveQuaternionCorrection(
double [][] quats,
double [] quat,
QuadCLT [] refCLTs,
QuadCLT index_CLT,
boolean orient_combo,
boolean debug) {
double [][] quats_all = new double[quats.length+1][];
QuadCLT [] allCLTs = new QuadCLT [quats_all.length];
......@@ -1677,7 +2001,7 @@ public class XyzQLma {
sb.append(angles[0]+"\t"+angles[1]+"\t"+angles[2]+"\t");
sb.append(degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"\n");
}
String suffix = orient_combo ? QuadCLTCPU.IMU_CALIB_SUM_COMBO_SUFFIX: QuadCLTCPU.IMU_CALIB_SUMMARY_SUFFIX;
String suffix = QuadCLTCPU.IMU_CALIB_SUMMARY_SUFFIX;
Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
CalibrationFileManagement.saveStringToFile (
path.toString(),
......
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