Skip to content

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

Working on IMU-to-camera calibration

parent 4e4edacb
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 694 additions and 261 deletions
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ eed35990
......@@ -361,6 +361,7 @@ public class ErsCorrection extends GeometryCorrection {
}
public void setPropertiesScenes(String prefix, Properties properties){
String [] timestamps = getScenes();
// TODO: use getScenes(first, last) as there may be stray entries in getScenes() caused by earlier runs
for (String k : timestamps) {
if (getScene(k) != null) {
String [] s_scenes = getScene(k).toStrings(); // null pointer
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ eed35990
......@@ -6201,6 +6201,7 @@ public class OpticalFlow {
if (debugLevel >-3) {
System.out.println("===== All adjustment cycles are done, considering one-time processing, such as lazy eye calibration. =====");
}
boolean orient_save_details = true;
if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
QuadCLT.adjustImuOrient(
......@@ -6210,6 +6211,7 @@ public class OpticalFlow {
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
orient_save_details, // boolean save_details,
debugLevel); // int debugLevel
// Try both orient_combo/!orient_combo for the log!
QuadCLT.adjustImuOrient(
......@@ -6219,6 +6221,7 @@ public class OpticalFlow {
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
orient_save_details, // boolean save_details,
debugLevel); // int debugLevel
}
......@@ -11434,6 +11437,7 @@ public class OpticalFlow {
System.out.println("Scale = "+scale);
if (compensate_dsi) {
ErsCorrection ers_reference = ref_scene.getErsCorrection();
// TODO: use getScenes(first_ts, last_ts) to remove stray data
String [] timestamps = ers_reference.getScenes();
for (String ts:timestamps) {
double [][] xyzatr = ers_reference.getSceneXYZATR(ts);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ eed35990
......@@ -6137,24 +6137,40 @@ if (debugLevel < -100) {
Path x3d_path=Paths.get(correctionsParameters.x3dDirectory).resolve(scene_name).resolve(correctionsParameters.x3dModelVersion);
Path interframe_path = x3d_path.resolve(scene_name+INTERFRAME_SUFFIX+CONFIGURATION_EXTENSION);
File interframe_file = new File(interframe_path.toString());
if (!interframe_file.exists()) {
if (debugLevel > -3) System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
return null;
if (interframe_file.exists()) { // only read if available
// read all parameters? from //fish://elphel@192.168.0.137/home/elphel/lwir16-proc/NC/linked/linked_1763232117-1763234145-v88/1763232148_284331-index/v88/1763232148_284331-SETTINGS.corr-xml
// read interscene parameters and <entry key="EYESIS_DCT_AUX.refscenes_1763232144_433048"></entry>
quadCLT.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...)
interframe_file.toString(), // path, // full name with extension or null to use x3d directory
false, // boolean all_properties,
debugLevel); // int debugLevel)
quadCLT.isPhotometricUpdatedAndReset(); // reset changed 12/25/2025
// get directory with version from name and this
// read existing parameters and inter-intra
// all data should exist, inter-intra and ims
// if (debugLevel > -3) System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
// return null;
} else {
if (debugLevel > -2) {
System.out.println("spawnQuadCLT(): "+interframe_path.toString()+" does not exist");
}
}
boolean always_read_ims = debugLevel>1000;
if (always_read_ims) {
final String ims_suffix = correctionsParameters.imsSuffix; // assuming common for all
Path ims_path = Paths.get(correctionsParameters.x3dDirectory).resolve(scene_name).resolve(scene_name+ims_suffix);
if (new File(ims_path.toString()).exists()) {
restoreIms(
clt_parameters, // // CLTParameters clt_parameters,
ims_path.toString(), // String ims_path,
true, // boolean create,
debugLevel-1); // debugLevelInner); // int debugLevel);
}
}
// read all parameters? from //fish://elphel@192.168.0.137/home/elphel/lwir16-proc/NC/linked/linked_1763232117-1763234145-v88/1763232148_284331-index/v88/1763232148_284331-SETTINGS.corr-xml
// read interscene parameters and <entry key="EYESIS_DCT_AUX.refscenes_1763232144_433048"></entry>
quadCLT.restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...)
interframe_file.toString(), // path, // full name with extension or null to use x3d directory
false, // boolean all_properties,
debugLevel); // int debugLevel)
quadCLT.isPhotometricUpdatedAndReset(); // reset changed 12/25/2025
// get directory with version from name and this
// read existing parameters and inter-intra
// all data should exist, inter-intra and ims
return quadCLT;
}
public QuadCLT spawnQuadCLT(
String set_name,
CLTParameters clt_parameters,
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ eed35990
......@@ -60,6 +60,7 @@ import java.util.concurrent.atomic.DoubleAccumulator;
import java.util.regex.Matcher;
import java.util.regex.Pattern;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.calibration.PixelMapping;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
......@@ -109,6 +110,8 @@ import loci.formats.FormatException;
public class QuadCLTCPU {
public static final String IMU_CALIB_LOGS_SUFFIX = "-IMU_CALIB.log";
public static final String IMU_CALIB_DETAILS_SUFFIX = "-IMU_CALIB_DETAILS.csv";
public static final String IMU_CALIB_COMBO_SUFFIX = "-IMU_CALIB_COMBO.csv";
public static final String ORIENTATION_LOGS_SUFFIX = "-ORIENTATION.log";
public static final String INTERFRAME_SUFFIX = "-INTERFRAME";
public static final String FIELD_CALIBRATION_SUFFIX = "-FIELD_CALIBRATION";
......@@ -837,9 +840,9 @@ public class QuadCLTCPU {
int early_index,
int last_index,
double [] rms, // null or double[5];
double [] quaternion, // null or double[4]
int debugLevel
) {
double [] quaternion, // null or double[4]
String suffix,
int debugLevel) {
final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
boolean use_inv = false; //
double [] quat = getRotationFromXYZATRCameraIms(
......@@ -883,6 +886,21 @@ public class QuadCLTCPU {
rotated_xyzatr[nscene] = new double [][] {rotated_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,
early_index, // int early_index,
last_index, // int last_index,
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
rotated_xyzatr, // double [][][] rotated_xyzatr,
orient_path.toString()); // String path) {
if (debugLevel > -3) {
System.out.println("Orientation details saved to "+orient_path.toString());
}
}
if (debugLevel > -1) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
......@@ -899,7 +917,7 @@ public class QuadCLTCPU {
"PIMU-X","PIMU-Y","PIMU-Z","PIMU-A","PIMU-T","PIMU-R",
"ROT-X","ROT-Y","ROT-Z","ROT-A","ROT-T","ROT-R"));
for (int nscene = early_index; nscene <= last_index; nscene++) {
System.out.println(String.format("%3s"+
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\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",
......@@ -915,6 +933,44 @@ public class QuadCLTCPU {
return rotated_xyzatr;
}
public static void saveRotateImsDetails(
Rotation rot,
int early_index,
int last_index,
double [][][] xyzatr,
double [][][] ims_xyzatr,
double [][][] rotated_xyzatr,
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(
"N\tX\tY\tZ\tA\tT\tR\t"+
"PIMU-X\tPIMU-Y\tPIMU-Z\tPIMU-A\tPIMU-T\tPIMU-R\t"+
"ROT-X\tROT-Y\tROT-Z\tROT-A\tROT-T\tROT-R\n");
for (int nscene = early_index; nscene <= last_index; 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 void adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
boolean orient_combo,
......@@ -922,8 +978,8 @@ public class QuadCLTCPU {
int ref_index,
int earliest_scene,
int last_index,
int debugLevel
) {
boolean save_details,
int debugLevel) {
boolean orient_by_move = clt_parameters.imp.orient_by_move; // use translation data to adjust IMU orientation
boolean orient_by_rot = clt_parameters.imp.orient_by_rot; // use rotation data to adjust IMU orientation
if (!orient_by_move && !orient_by_rot) {
......@@ -965,6 +1021,9 @@ public class QuadCLTCPU {
} else if (!orient_by_rot) {
translation_weight = 1.0;
}
// boolean orient_combo,
// boolean save_details
String suffix = save_details ? (orient_combo ? IMU_CALIB_COMBO_SUFFIX: IMU_CALIB_DETAILS_SUFFIX) : null;
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
......@@ -976,9 +1035,10 @@ public class QuadCLTCPU {
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_index, // int last_index,
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
rms, // double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
suffix, // String suffix,
debug_lev); // int debugLevel
if (rotated_xyzatr != 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
......@@ -1588,7 +1648,10 @@ public class QuadCLTCPU {
ims_ortho);
if (quat_corr != null) {
quat_ims_cam=Imx5.applyQuaternionToQuaternion(quat_corr, quat_ims_cam, false);
}
}
if (did_ins_2 == null) {
System.out.println("getDxyzatrIms(): did_ins_2=null");
}
double [] double_uvw = did_ins_2.getUvw();
if ((double_uvw[0] == 0.0) && (double_uvw[1] == 0.0) && (double_uvw[2] == 0.0)) {
double_uvw = did_ins_1.getUvw();
......
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