Skip to content

I
imagej-elphel
Commit 38a3b797 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

More refactoring: moving orientation-related static methods from 

SeriesInfinityCorrection to XyzQLma class
parent c99e2f7b
Hide whitespace changes
Inline Side-by-side
Showing with 181 additions and 879 deletions
src/main/java/com/elphel/imagej/tileprocessor/SeriesInfinityCorrection.java
View file @ 38a3b797
......@@ -22,181 +22,8 @@ import com.elphel.imagej.ims.Imx5;
public class SeriesInfinityCorrection {
public static final String SUFFIX_SERIES_INFINITY = "-series_infiniti.csv";
public static double [] adjustImuOrientMulti(
CLTParameters clt_parameters,
QuadCLT [] refCLTs,
QuadCLT index_CLT, // normally - just the last in quadCLTs
boolean save_sequence_orient,
boolean process_segments,
double [][] orient_rslt,
final int debugLevel) {
// boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
for (QuadCLT ref_scene:refCLTs) {
ref_scene.restoreAnyDSI(debugLevel);
}
// get all scenes used in at least one segment.
HashSet<String> scene_set = new HashSet<String>();
HashMap<String,QuadCLT> ref_map = new HashMap<String,QuadCLT>();
for (QuadCLT ref_scene:refCLTs) {
ref_map.put(ref_scene.getImageName(), ref_scene);
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
for (String s:names)scene_set.add(s);
}
String [] all_scene_names = scene_set.toArray(new String[0]);
Arrays.sort(all_scene_names);
HashMap<String,QuadCLT> scene_map = new HashMap<String,QuadCLT>();
String models_dir = index_CLT.correctionsParameters.x3dDirectory;
final String suffix = index_CLT.correctionsParameters.imsSuffix; // assuming common for all
final QuadCLT[] scenes = new QuadCLT[all_scene_names.length];
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iScene = ai.getAndIncrement(); iScene < scenes.length; iScene = ai.getAndIncrement()) {
String scene_name = all_scene_names[iScene];
QuadCLT rscene = ref_map.get(scene_name);
// create new if did not exist,
if (rscene == null) {
scenes[iScene] = new QuadCLT(index_CLT,scene_name);
} else {
scenes[iScene] = rscene;
}
Path ims_path = Paths.get(models_dir).resolve(scene_name).resolve(scene_name+suffix);
scenes[iScene].restoreIms(
clt_parameters, // // CLTParameters clt_parameters,
ims_path.toString(), // String ims_path,
true, // boolean create,
debugLevel-1); // debugLevelInner); // int debugLevel);
}
}
};
}
ImageDtt.startAndJoin(threads);
for (int nscene = 0; nscene < all_scene_names.length; nscene++) { // scene_name:all_scene_names) {
scene_map.put(all_scene_names[nscene], scenes[nscene]);
}
// Now iterate through the reference scenes, using IMS data from the scene_map scenes.
// initially will use earliest and latest scenes only. Improve for the future non-linear flying multicopters
// and account for sync loss (far samples with random coordinates)
double [] img_scales = new double[refCLTs.length];
double [] dist_visual = new double[refCLTs.length];
double [] dist_ims2 = new double[refCLTs.length];
double [] disp_corrs= new double[refCLTs.length];
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());
}
// 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,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
0, // int earliest_scene,
quadCLTs.length - 1, // int last_index,
save_sequence_orient, // boolean save_details,
debugLevel); // int debugLevel);
}
continue; // just to put a break point
}
// adjusting for all series
double [] quat = adjustImuOrient(
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
save_sequence_orient, // boolean save_details,
debugLevel+1); // int debugLevel)
// save per-segment quaternion and angles, and the result one
if (save_sequence_orient && (quats != null)) {
saveQuternionCorrection(
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(
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];
allCLTs[0] = index_CLT;
quats_all[0] = quat;
for (int i = 0; i < quats.length; i++) {
allCLTs[i+1] = refCLTs[i];
quats_all[i+1] = quats[i];
}
StringBuffer sb = new StringBuffer();
sb.append("timestamp\tQ0\tQ1\tQ2\tQ3\tscale\tA\tT\tR\tA(deg)\tT(deg)\tR(deg)\n");
for (int nscene = 0; nscene < quats_all.length; nscene++) {
double [] q = quats_all[nscene];
Rotation rot = new Rotation(q[0],q[1],q[2],q[3], false); // no normalization - see if can be scaled
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
double quat_scale = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
for (int i = 0; i < 3; i++) degrees[i] = angles[i]*180/Math.PI;
sb.append(allCLTs[nscene].getImageName()+"\t");
sb.append(rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\t");
sb.append(quat_scale+"\t");
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;
Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
CalibrationFileManagement.saveStringToFile (
path.toString(),
sb.toString());
if (debug) {
System.out.println("Summary orientation correction data saved to "+path.toString());
}
}
public static double getInfinityMuliSeries(
......@@ -405,710 +232,7 @@ public class SeriesInfinityCorrection {
}
return;
}
public static double[] adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
QuadCLT[][] quadCLTss,
int [] ref_indices,
QuadCLT index_scene, // where to put result
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
int num_refs = ref_indices.length;
double [][][][] pimu_xyzatrs = new double[num_refs][][][];
double [][][][] xyzatrs = new double[num_refs][][][];
ErsCorrection [] ers_refs = new ErsCorrection[num_refs];
// calculate average Z for the sequence (does not need to be very accurate
double sum_avgz_w = 0,sum_avgz=0;
for (int nref = 0; nref < num_refs; nref++) {
int last_index = quadCLTss[nref].length-1;
QuadCLT[] quadCLTs = quadCLTss[nref];
int ref_index = ref_indices[nref];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
pimu_xyzatrs[nref] = 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,
last_index, // last_index, //(quadCLTs.length -1) // int last_index,
debugLevel); // final int debugLevel
xyzatrs[nref] = new double [quadCLTss[nref].length][][];
for (int nscene = 0; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatrs[nref][nscene] = ers_ref.getSceneXYZATR(ts);
}
ers_refs[nref] = ers_ref;
double w = 1.0;
sum_avgz_w += w;
sum_avgz += w * quadCLTs[ref_index].getAverageZ(true); // in meters
}
double avg_z = sum_avgz/sum_avgz_w;
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);
}
String suffix = save_details ? (orient_combo ? QuadCLTCPU.IMU_CALIB_COMBO_SUFFIX: 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,
quadCLTss, // QuadCLT[][] quadCLTss,
xyzatrs, // double [][][][] xyzatrs,
pimu_xyzatrs, // double [][][][] ims_xyzatrs,
ref_indices, // int [] ref_indices,
rms, // double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
y_scale_p, // double [] y_scale_p
index_scene, // QuadCLT index_scene, // where to put result
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");
index_scene.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(
CLTParameters clt_parameters,
double avg_height,
double transl_cost, // 1.0 - pure translation, 0.0 - pure rotation
QuadCLT[][] quadCLTss,
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
int [] ref_indices,
double [] rms, // null or double[5];
double [] quaternion, // null or double[4]
double [] y_scale_p,
QuadCLT index_scene, // where to put result
String suffix,
int debugLevel) {
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
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_refs = ims_xyzatrs.length;
double [][][][] rot_ims_xyzatrs = new double [num_refs][][][]; // ims poses rotated by the new correction to match camera ones
for (int nref = 0; nref < num_refs; nref++) {
int num_scenes = ims_xyzatrs[nref].length;
rot_ims_xyzatrs[nref] = 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_xyzatrs[nref][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_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][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_xyzatrs[nref][nscene] = new double [][] {rot_ims_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
}
if (suffix != null) {
Path orient_path = Paths.get(index_scene.getX3dDirectory(true)).
resolve(index_scene.getImageName()+suffix); // IMU_CALIB_DETAILS_SUFFIX);
saveRotateImsMultiDetails(
rot, // Rotation rot,
xyzatrs, // double [][][] xyzatr,
ims_xyzatrs, // double [][][] ims_xyzatr,
rot_ims_xyzatrs, // 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_xyzatrs;
}
public static void saveRotateImsMultiDetails(
Rotation rot,
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
double [][][][] rotated_xyzatrs,
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_refs = xyzatrs.length;
for (int nref = 0; nref < num_refs; nref++) {
int num_scenes = xyzatrs[nref].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,
xyzatrs[nref][nscene][0][0],xyzatrs[nref][nscene][0][1],xyzatrs[nref][nscene][0][2],
xyzatrs[nref][nscene][1][0],xyzatrs[nref][nscene][1][1],xyzatrs[nref][nscene][1][2],
ims_xyzatrs[nref][nscene][0][0],ims_xyzatrs[nref][nscene][0][1],ims_xyzatrs[nref][nscene][0][2],
ims_xyzatrs[nref][nscene][1][0],ims_xyzatrs[nref][nscene][1][1],ims_xyzatrs[nref][nscene][1][2],
rotated_xyzatrs[nref][nscene][0][0],rotated_xyzatrs[nref][nscene][0][1],rotated_xyzatrs[nref][nscene][0][2],
rotated_xyzatrs[nref][nscene][1][0],rotated_xyzatrs[nref][nscene][1][1],rotated_xyzatrs[nref][nscene][1][2]));
}
}
CalibrationFileManagement.saveStringToFile (
path,
sb.toString());
}
/**
* Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS positions (currently
* only linear are used). Refined scene poses may be used as pull targets
* with free orientations (and angular velocities for ERS).
* Result poses are {0,0,0} for the reference frame.
*
* Assuming correct IMU angular velocities and offset linear ones.
*
* @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 ims_xyzatr IMU-derived (integrated) scene position and orientation in reference
* scene frame (only position is currently used)
* @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 quaternion components
*/
@Deprecated
public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters,
double avg_height, // will be used to scale XYZ on this level, QuaternionLMA.height will be set to 1 and later removed
double transl_cost, // will be relative weight of translations (0..1), rotations - 1-translation_weight
QuadCLT[][] quadCLTss, // unused
double [][][][] xyzatrs,
double [][][][] ims_xyzatrs,
double [] rms, // null or double[7];
double [] y_scale_p, // if not null will return y_scale
int debugLevel) {
boolean print_lma_table = false;
boolean print_lma_table_atr = true; // add ATR
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio; // 0.25;
double quat_max_damping = clt_parameters.imp.quat_max_damping;
boolean use_scale_y = clt_parameters.imp.quat_scale_y;
int num_samples = 0;
for (int nref = 0; nref < xyzatrs.length;nref++) {
num_samples += xyzatrs[nref].length;
}
double [][][] vect_y = new double [num_samples][][]; // camera XYZATR
double [][][] vect_x = new double [num_samples][][]; // IMS XYZATR
int vindx = 0;
double xyz_scale = 1.0 / (avg_height + 1.0);
for (int nref = 0; nref < xyzatrs.length;nref++) {
for (int nscene = 0; nscene < xyzatrs[nref].length; nscene++) {
vect_x[vindx] = new double [2][];
vect_x[vindx][1] = ims_xyzatrs[nref][nscene][1]; // will not be modified
vect_y[vindx] = new double [2][];
vect_y[vindx][1] = xyzatrs [nref][nscene][1]; // will not be modified
vect_x[vindx][0] = new double[3];
vect_y[vindx][0] = new double[3];
for (int i = 0; i < 3; i++) {
vect_x[vindx][0][i] = ims_xyzatrs[nref][nscene][0][i] * xyz_scale;
vect_y[vindx][0][i] = xyzatrs [nref][nscene][0][i] * xyz_scale;
}
vindx++;
}
}
double [][][] vect_y_scaled;
double y_scale = 1.0;
if (use_scale_y) {
y_scale = XyzQLma.getLengthRatio(
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
null); // new int [] {early_index, last_index}); // int [] first_last){ // null - all
vect_y_scaled= XyzQLma.scaleXYZ(
1.0/y_scale, // double s,
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
null); // new int [] {early_index, last_index}); // int [] first_last){ // null - all
} else {
vect_y_scaled = vect_y;
}
if (y_scale_p != null) {
y_scale_p[0] = y_scale;
}
XyzQLma xyzQLma = new XyzQLma();
xyzQLma.prepareLMA(
// quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
transl_cost, // double translation_weight, // 0.0 ... 1.0;
0.0, // reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
// quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = xyzQLma.getMinMaxDiag(debugLevel);
double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
if (debugLevel > -3) {
System.out.println("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
}
// if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
if (max_to_min > quat_max_ratio) {
reg_w = quat_max_damping * (max_to_min - quat_max_ratio) / max_to_min;
double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
double reg_w_old = a/(a + quat_max_ratio*quat_max_ratio/4); // quat0.length);
if (debugLevel > -3) {
System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
xyzQLma.prepareLMA(
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
transl_cost, // double translation_weight, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
debugLevel); // int debug_level)
}
if (print_lma_table) {
xyzQLma.saveFx(debugLevel);
}
int lma_result = xyzQLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debugLevel); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (print_lma_table) {
String lma_table= xyzQLma.getLmaTable(
print_lma_table_atr,
"\t",
debugLevel);
System.out.println(lma_table);
}
if (rms != null) { // null or double[2];
double [] last_rms = xyzQLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = xyzQLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
if ((rms.length >= 6) && (last_rms.length > 2)){
rms[5] = last_rms[2];
if ((rms.length >= 7) && (last_rms.length > 3)){
rms[6] = last_rms[3];
}
}
}
}
}
return xyzQLma.getQuaternion();
}
}
/**
* Flatten multi-segment sequences to a single segment ones. Need to apply to double [][][][] xyzatrs and
* double [][][][] ims_xyzatrs separately
* @param pose_segments array of poses to be flattened [segment_index][scene_index]{{x,y,z},{a,t,r}}
* @return flattened poses [scene_index]{{x,y,z},{a,t,r}}
*/
public static double [][][] flattenPoses(
double [][][][] pose_segments) {
int num_samples = 0;
for (int nref = 0; nref < pose_segments.length;nref++) {
num_samples += pose_segments[nref].length;
}
double [][][] poses = new double [num_samples][][];
int vindx = 0;
for (int nref = 0; nref < pose_segments.length;nref++) {
for (int nscene = 0; nscene < pose_segments[nref].length; nscene++) {
poses[vindx] = pose_segments[nref][nscene];
vindx++;
}
}
return poses;
}
/**
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence (for timestamps)
* @param avr_height for scaling translational components
* @param transl_cost relative weight of translations (0..1), rotation weighs: 1-translation_weight
* @param vect_y scenes relative (to their references) poses {{x,y,z},{a,t,r]}}
* @param vect_x IMU-derived (integrated) scene position and orientation relative to its reference
* @param rms null or a double[7] to receive rms values
* @param y_scale_p null or double[1] - will return y_scale - camera translations relative to the IMS
* @param debugLevel debug level
* @return quaternion components
*/
public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters,
double avg_height, // for scaling translational components
double transl_cost, // will be relative weight of translations (0..1), rotations - 1-translation_weight
double [][][] vect_y,
double [][][] vect_x,
double [] rms, // null or double[5];
double [] y_scale_p, // if not null will return y_scale
int debugLevel) {
boolean print_lma_table = false;
boolean print_lma_table_atr = true; // add ATR
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio; // 0.25;
double quat_max_damping = clt_parameters.imp.quat_max_damping;
boolean use_scale_y = clt_parameters.imp.quat_scale_y;
double xyz_scale = 1.0 / (avg_height + 1.0);
// scale translational components (x,y,z) to match rotational one by the "order of magnitude"
// use clone() to keep the source data intact.
for (int nscene = 0; nscene < vect_x.length; nscene++) {
vect_x[nscene] = vect_x[nscene].clone();
vect_y[nscene] = vect_y[nscene].clone();
vect_x[nscene][0] = vect_x[nscene][0].clone();
vect_y[nscene][0] = vect_y[nscene][0].clone();
for (int i = 0; i < 3; i++) {
vect_x[nscene][0][i] *= xyz_scale;
vect_y[nscene][0][i] *= xyz_scale;
}
}
double [][][] vect_y_scaled;
double y_scale = 1.0;
if (use_scale_y) {
y_scale = XyzQLma.getLengthRatio(
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
null); // new int [] {early_index, last_index}); // int [] first_last){ // null - all
vect_y_scaled= XyzQLma.scaleXYZ(
1.0/y_scale, // double s,
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
null); // new int [] {early_index, last_index}); // int [] first_last){ // null - all
} else {
vect_y_scaled = vect_y;
}
if (y_scale_p != null) {
y_scale_p[0] = y_scale;
}
XyzQLma xyzQLma = new XyzQLma();
xyzQLma.prepareLMA(
// quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
transl_cost, // double translation_weight, // 0.0 ... 1.0;
0.0, // reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
// quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = xyzQLma.getMinMaxDiag(debugLevel);
double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
if (debugLevel > -3) {
System.out.println("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
}
// if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
if (max_to_min > quat_max_ratio) {
reg_w = quat_max_damping * (max_to_min - quat_max_ratio) / max_to_min;
double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
double reg_w_old = a/(a + quat_max_ratio*quat_max_ratio/4); // quat0.length);
if (debugLevel > -3) {
System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
xyzQLma.prepareLMA(
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
transl_cost, // double translation_weight, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
debugLevel); // int debug_level)
}
if (print_lma_table) {
xyzQLma.saveFx(debugLevel);
}
int lma_result = xyzQLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debugLevel); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (print_lma_table) {
String lma_table= xyzQLma.getLmaTable(
print_lma_table_atr,
"\t",
debugLevel);
System.out.println(lma_table);
}
if (rms != null) { // null or double[2];
double [] last_rms = xyzQLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = xyzQLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
if ((rms.length >= 6) && (last_rms.length > 2)){
rms[5] = last_rms[2];
if ((rms.length >= 7) && (last_rms.length > 3)){
rms[6] = last_rms[3];
}
}
}
}
}
return xyzQLma.getQuaternion();
}
}
}
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 38a3b797
......@@ -8698,7 +8698,7 @@ if (debugLevel > -100) return true; // temporarily !
// TODO: implement
// for testing running adjustImuOrient for the last segment in this series
double [][] orient_rslt = save_series_orient ? (new double [ref_scenes.length][]) : null;
double [] quatcorr_multi = SeriesInfinityCorrection.adjustImuOrientMulti(
double [] quatcorr_multi = XyzQLma.adjustImuOrientMulti(
clt_parameters, // CLTParameters clt_parameters,
ref_scenes, // QuadCLT [] quadCLTs,
index_scenes[0], // QuadCLT index_CLT, // normally - just the last in quadCLTs
......
src/main/java/com/elphel/imagej/tileprocessor/XyzQLma.java
View file @ 38a3b797
......@@ -3,7 +3,11 @@ package com.elphel.imagej.tileprocessor;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Calendar;
import java.util.HashMap;
import java.util.HashSet;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
......@@ -1509,5 +1513,179 @@ public class XyzQLma {
sb.toString());
}
public static double [] adjustImuOrientMulti(
CLTParameters clt_parameters,
QuadCLT [] refCLTs,
QuadCLT index_CLT, // normally - just the last in quadCLTs
boolean save_sequence_orient,
boolean process_segments,
double [][] orient_rslt,
final int debugLevel) {
// boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
for (QuadCLT ref_scene:refCLTs) {
ref_scene.restoreAnyDSI(debugLevel);
}
// get all scenes used in at least one segment.
HashSet<String> scene_set = new HashSet<String>();
HashMap<String,QuadCLT> ref_map = new HashMap<String,QuadCLT>();
for (QuadCLT ref_scene:refCLTs) {
ref_map.put(ref_scene.getImageName(), ref_scene);
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
for (String s:names)scene_set.add(s);
}
String [] all_scene_names = scene_set.toArray(new String[0]);
Arrays.sort(all_scene_names);
HashMap<String,QuadCLT> scene_map = new HashMap<String,QuadCLT>();
String models_dir = index_CLT.correctionsParameters.x3dDirectory;
final String suffix = index_CLT.correctionsParameters.imsSuffix; // assuming common for all
final QuadCLT[] scenes = new QuadCLT[all_scene_names.length];
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iScene = ai.getAndIncrement(); iScene < scenes.length; iScene = ai.getAndIncrement()) {
String scene_name = all_scene_names[iScene];
QuadCLT rscene = ref_map.get(scene_name);
// create new if did not exist,
if (rscene == null) {
scenes[iScene] = new QuadCLT(index_CLT,scene_name);
} else {
scenes[iScene] = rscene;
}
Path ims_path = Paths.get(models_dir).resolve(scene_name).resolve(scene_name+suffix);
scenes[iScene].restoreIms(
clt_parameters, // // CLTParameters clt_parameters,
ims_path.toString(), // String ims_path,
true, // boolean create,
debugLevel-1); // debugLevelInner); // int debugLevel);
}
}
};
}
ImageDtt.startAndJoin(threads);
for (int nscene = 0; nscene < all_scene_names.length; nscene++) { // scene_name:all_scene_names) {
scene_map.put(all_scene_names[nscene], scenes[nscene]);
}
// Now iterate through the reference scenes, using IMS data from the scene_map scenes.
// initially will use earliest and latest scenes only. Improve for the future non-linear flying multicopters
// and account for sync loss (far samples with random coordinates)
double [] img_scales = new double[refCLTs.length];
double [] dist_visual = new double[refCLTs.length];
double [] dist_ims2 = new double[refCLTs.length];
double [] disp_corrs= new double[refCLTs.length];
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());
}
// 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,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
0, // int earliest_scene,
quadCLTs.length - 1, // int last_index,
save_sequence_orient, // boolean save_details,
debugLevel); // int debugLevel);
}
continue; // just to put a break point
}
// adjusting for all series
double [] quat = adjustImuOrient(
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
save_sequence_orient, // boolean save_details,
debugLevel+1); // int debugLevel)
// save per-segment quaternion and angles, and the result one
if (save_sequence_orient && (quats != null)) {
saveQuternionCorrection(
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(
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];
allCLTs[0] = index_CLT;
quats_all[0] = quat;
for (int i = 0; i < quats.length; i++) {
allCLTs[i+1] = refCLTs[i];
quats_all[i+1] = quats[i];
}
StringBuffer sb = new StringBuffer();
sb.append("timestamp\tQ0\tQ1\tQ2\tQ3\tscale\tA\tT\tR\tA(deg)\tT(deg)\tR(deg)\n");
for (int nscene = 0; nscene < quats_all.length; nscene++) {
double [] q = quats_all[nscene];
Rotation rot = new Rotation(q[0],q[1],q[2],q[3], false); // no normalization - see if can be scaled
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
double quat_scale = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
for (int i = 0; i < 3; i++) degrees[i] = angles[i]*180/Math.PI;
sb.append(allCLTs[nscene].getImageName()+"\t");
sb.append(rot.getQ0()+"\t"+rot.getQ1()+"\t"+rot.getQ2()+"\t"+rot.getQ3()+"\t");
sb.append(quat_scale+"\t");
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;
Path path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+suffix);
CalibrationFileManagement.saveStringToFile (
path.toString(),
sb.toString());
if (debug) {
System.out.println("Summary orientation correction data saved to "+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