Skip to content

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

Refactoring

parent c3faa9d7
Hide whitespace changes
Inline Side-by-side
Showing with 1668 additions and 350 deletions
src/main/java/com/elphel/imagej/ims/Did_ins_2.java
View file @ c99e2f7b
......@@ -23,7 +23,7 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
// public int hdwStatus; // uint32_t
/** Euler angles: roll, pitch, yaw in radians with respect to NED */
/** Quaternion */
public float [] qn2b = new float[4];
public float [] qn2b = new float[4]; // orientation relative to NED
/** Velocity U, V, W in meters per second. Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )". */
// public float [] uvw = new float [3];
/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
......@@ -75,10 +75,10 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
}
public double [] getQEnu () {
public double [] getQEnu () {// apply absolute orientation to ortho rotation
Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5), Math.sqrt(0.5), 0, true);
Rotation quat_rot = new Rotation(qn2b[0],qn2b[1],qn2b[2],qn2b[3],true);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned); // apply absolute orientation to ortho rotation
return new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
......
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ c99e2f7b
......@@ -189,8 +189,8 @@ public class Imx5 {
}
public static double [] applyQuaternionToQuaternion(
double[] quat,
double[] targ_quat,
double[] quat, // additional correction
double[] targ_quat, // quaternion to be corrected
boolean inverse) {
Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
......@@ -222,9 +222,9 @@ public class Imx5 {
public static double [] quaternionImsToCam(
double [] quat, // ims_to_ned
double [] ims_atr, // -> mount_to_cam
double [] quat_ort) { // -> ims_to_mount_ortho
double [] quat, // ims_to_ned // normally d2_ref.getQEnu(), // current IMS absolute orientation relative to ENU world frame
double [] ims_atr, // -> IMS_to_cam (angles)
double [] quat_ort) { // -> ims_to_mount_ortho additional 90-degree rotaions
Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ c99e2f7b
......@@ -340,7 +340,9 @@ public class IntersceneMatchParameters {
public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
public boolean orient_by_move = false; // use translation data to adjust IMU orientation
public boolean orient_by_rot = true; // use rotation data to adjust IMU orientation
public boolean orient_combo = true; // use combined rotation+orientation for IMU/camera matching
public boolean orient_combo = true;
public boolean use3 = true; // when comparing orientations for LMA, use only 3 vector components of the quaternion (false - all 4)
// use combined rotation+orientation for IMU/camera matching
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
......@@ -1139,6 +1141,8 @@ min_str_neib_fpn 0.35
public int quat_num_iter = 20;
public double quat_reg_w = 0.25;
public boolean quat_scale_y = true; // Scale XYZ before orientation correction to match lengths
public double quat_max_ratio = 3.0; // 5.0; // max derivatives ratio
public double quat_max_damping = 0.5; // maximal regularization weight
public double quat_max_change = 0.1; // radians
......@@ -1866,6 +1870,8 @@ min_str_neib_fpn 0.35
"Use rotation data to adjust IMU orientation.");
gd.addCheckbox ("Use combo mode IMU orientation", this.orient_combo,
"Use combined Z/h, R, A-X/h, T+Y/h for IMU mount-to-camera orientation correction. False - use X,Y,Z,A,T,R");
gd.addCheckbox ("Use only 3 quaternion components", this.use3,
"When comparing orientations fro LMA, use only 3 vector components of the quaternions. False - all 4");
gd.addCheckbox ("Adjust gyro offsets", this.adjust_gyro,
"Adjust qyro omegas offsets.");
gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
......@@ -3319,6 +3325,9 @@ min_str_neib_fpn 0.35
"A hard limit on LMA iterations.");
gd.addNumericField("Regularization weight", this.quat_reg_w, 3,5,"",
"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");
gd.addCheckbox ("Scale XYZ before irientation matching", this.quat_scale_y,
"Scale XYZ before orientation correction to match lengths.");
gd.addNumericField("Maximal derivatives by axes ratio", this.quat_max_ratio, 3,5,"x",
"If difference is larger, add regularization to reduce it.");
......@@ -3903,6 +3912,7 @@ min_str_neib_fpn 0.35
this.orient_by_move = gd.getNextBoolean();
this.orient_by_rot = gd.getNextBoolean();
this.orient_combo = gd.getNextBoolean();
this.use3 = gd.getNextBoolean();
this.adjust_gyro = gd.getNextBoolean();
this.apply_gyro = gd.getNextBoolean();
this.adjust_accl = gd.getNextBoolean();
......@@ -4648,6 +4658,7 @@ min_str_neib_fpn 0.35
this.quat_rms_diff = gd.getNextNumber();
this.quat_num_iter = (int) gd.getNextNumber();
this.quat_reg_w = gd.getNextNumber();
this.quat_scale_y = gd.getNextBoolean();
this.quat_max_ratio = gd.getNextNumber();
this.quat_max_damping = gd.getNextNumber();
......@@ -5174,6 +5185,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"orient_by_move", this.orient_by_move+""); // boolean
properties.setProperty(prefix+"orient_by_rot", this.orient_by_rot+""); // boolean
properties.setProperty(prefix+"orient_combo", this.orient_combo+""); // boolean
properties.setProperty(prefix+"use3", this.use3+""); // 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
......@@ -5891,6 +5903,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"quat_rms_diff", this.quat_rms_diff+""); // double
properties.setProperty(prefix+"quat_num_iter", this.quat_num_iter+""); // int
properties.setProperty(prefix+"quat_reg_w", this.quat_reg_w+""); // double
properties.setProperty(prefix+"quat_scale_y", this.quat_scale_y+""); // boolean
properties.setProperty(prefix+"quat_max_ratio", this.quat_max_ratio+""); // double
properties.setProperty(prefix+"quat_max_damping", this.quat_max_damping+""); // double
......@@ -6386,6 +6399,7 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"orient_by_move")!=null) this.orient_by_move=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_move"));
if (properties.getProperty(prefix+"orient_by_rot")!=null) this.orient_by_rot=Boolean.parseBoolean(properties.getProperty(prefix+"orient_by_rot"));
if (properties.getProperty(prefix+"orient_combo")!=null) this.orient_combo=Boolean.parseBoolean(properties.getProperty(prefix+"orient_combo"));
if (properties.getProperty(prefix+"use3")!=null) this.use3=Boolean.parseBoolean(properties.getProperty(prefix+"use3"));
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"));
......@@ -7119,7 +7133,8 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"reg_weight_xy")!=null) this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
if (properties.getProperty(prefix+"quat_num_iter")!=null) this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
if (properties.getProperty(prefix+"quat_reg_w")!=null) this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));
if (properties.getProperty(prefix+"quat_scale_y")!=null) this.quat_scale_y=Boolean.parseBoolean(properties.getProperty(prefix+"quat_scale_y"));
if (properties.getProperty(prefix+"quat_max_ratio")!=null) this.quat_max_ratio=Double.parseDouble(properties.getProperty(prefix+"quat_max_ratio"));
if (properties.getProperty(prefix+"quat_max_damping")!=null) this.quat_max_damping=Double.parseDouble(properties.getProperty(prefix+"quat_max_damping"));
if (properties.getProperty(prefix+"quat_max_change")!=null) this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
......@@ -7630,6 +7645,7 @@ min_str_neib_fpn 0.35
imp.orient_by_move = this.orient_by_move;
imp.orient_by_rot = this.orient_by_rot;
imp.orient_combo = this.orient_combo;
imp.use3 = this.use3;
imp.adjust_gyro = this.adjust_gyro;
imp.apply_gyro = this.apply_gyro;
imp.adjust_accl = this.adjust_accl;
......@@ -8327,6 +8343,7 @@ min_str_neib_fpn 0.35
imp.quat_rms_diff = this.quat_rms_diff;
imp.quat_num_iter = this.quat_num_iter;
imp.quat_reg_w = this.quat_reg_w;
imp.quat_scale_y = this.quat_scale_y;
imp.quat_max_ratio = this.quat_max_ratio;
imp.quat_max_damping = this.quat_max_damping;
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ c99e2f7b
......@@ -702,12 +702,13 @@ public class QuadCLTCPU {
int early_index,
int last_index,
double [] rms, // null or double[5]; now [7]?
double [] y_scale_p, // if not null will return y_scale
int debugLevel
) {
if (quadCLTs[ref_index].getImageName().equals("1763233239_531146")) {
System.out.println("getRotationFromXYZATRCameraIms(): quadCLTs[ref_index].getImageName().getImageName()=="+quadCLTs[ref_index].getImageName());
}
boolean use_scale_y = false;
// boolean use_scale_y = false;
// final boolean use3 = (quat_lma_mode == 3); // true;
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
......@@ -719,28 +720,48 @@ public class QuadCLTCPU {
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double quat_max_ratio = clt_parameters.imp.quat_max_ratio;
double quat_max_damping = clt_parameters.imp.quat_max_damping;
boolean use_scale_y = clt_parameters.imp.quat_scale_y;
double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
double xyz_scale = 1.0 / (avg_height + 1.0); // 02/13/2026 - scaling XYZ components of vect_x and vect_y here, using weights 0..1
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
// vect_x[nscene] = ims_xyzatr[nscene];
// vect_y[nscene] = xyzatr[nscene];
vect_x[nscene] = new double [2][];
vect_y[nscene] = new double [2][];
vect_x[nscene][1] = ims_xyzatr[nscene][1]; // rotations
vect_y[nscene][1] = xyzatr [nscene][1]; // rotations
vect_x[nscene][0] = new double [3];
vect_y[nscene][0] = new double [3];
for (int i = 0; i < 3; i++) {
vect_x[nscene][0][i] = ims_xyzatr[nscene][0][i] * xyz_scale;
vect_y[nscene][0][i] = xyzatr [nscene][0][i] * xyz_scale;
}
}
// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
double [][][] vect_y_scaled;
double y_scale = 1.0;
if (use_scale_y) {
y_scale = QuaternionLma.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= QuaternionLma.scaleXYZ(
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
new int [] {early_index, last_index}); // int [] first_last){
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;
}
if ((quat_lma_mode == QuaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
// avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
......@@ -764,7 +785,7 @@ public class QuadCLTCPU {
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
// avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
......@@ -851,6 +872,10 @@ public class QuadCLTCPU {
}
}
}
// debug by outputting y_vector, fx
return quaternionLma.getQuaternion();
}
}
......@@ -858,6 +883,7 @@ public class QuadCLTCPU {
public static double [][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters,
int quat_lma_mode,
boolean use3,// false - full4 quaternion (with scale), true - only q1,q2,q3
double avg_height,
double translation_weight, // 1.0 - pure translation, 0.0 - pure rotation
QuadCLT[] quadCLTs,
......@@ -868,13 +894,13 @@ public class QuadCLTCPU {
int last_index,
double [] rms, // null or double[5]; // now [7]?
double [] quaternion, // null or double[4]
double [] y_scale_p, // double [] y_scale_p
String suffix,
int debugLevel) {
if (quadCLTs[ref_index].getImageName().equals("1763233239_531146")) {
System.out.println("quadCLTs[ref_index].getImageName().getImageName()=="+quadCLTs[ref_index].getImageName());
}
final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
// final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
boolean use_inv = false; //
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
......@@ -889,6 +915,7 @@ public class QuadCLTCPU {
early_index, // int early_index,
last_index, // int last_index,
rms, // double [] rms, // null or double[5]; // now [7]?
y_scale_p, //double [] y_scale_p, // if not null will return y_scale
debugLevel); // int debugLevel
if (quat == null) {
return null;
......@@ -927,6 +954,7 @@ public class QuadCLTCPU {
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
rotated_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());
......@@ -939,6 +967,7 @@ public class QuadCLTCPU {
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println("y_scale= "+y_scale_p[0]);
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
......@@ -975,6 +1004,7 @@ public class QuadCLTCPU {
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];
......@@ -983,6 +1013,7 @@ public class QuadCLTCPU {
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\tX\tY\tZ\tA\tT\tR\t"+
"PIMU-X\tPIMU-Y\tPIMU-Z\tPIMU-A\tPIMU-T\tPIMU-R\t"+
......@@ -1028,7 +1059,9 @@ public class QuadCLTCPU {
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 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
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -1050,18 +1083,33 @@ public class QuadCLTCPU {
int quat_lma_mode = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: 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_agl = 1.0 / (avg_z + 1.0);
if (!orient_by_move) {
translation_agl = 0.0;
} else if (!orient_by_rot) {
translation_agl = 1.0;
}
// Apply transl_cost
double translation_weight = (translation_agl * transl_cost) / (translation_agl * transl_cost + (1-translation_agl)* (1-transl_cost));
/*
double translation_weight = 1.0 / (avg_z + 1.0);
if (!orient_by_move) {
translation_weight = 0.0;
} else if (!orient_by_rot) {
translation_weight = 1.0;
}
*/
// boolean orient_combo,
// boolean save_details
if (debugLevel > -3) {
System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", translation_agl="+translation_agl+", translation_weight="+translation_weight);
}
String suffix = save_details ? (orient_combo ? IMU_CALIB_COMBO_SUFFIX: IMU_CALIB_DETAILS_SUFFIX) : null;
double [] y_scale_p = new double[1];
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
use3, // boolean use3,// false - full4 quaternion (with scale), true - only q1,q2,q3
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
......@@ -1072,6 +1120,7 @@ public class QuadCLTCPU {
last_index, // int last_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_xyzatr != null) {
......@@ -1093,6 +1142,7 @@ public class QuadCLTCPU {
sb.append("avg_z= "+avg_z+" m\n");
sb.append("translation_weight= "+translation_weight+"\n");
sb.append("quat_lma_mode= "+quat_lma_mode+"\n");
sb.append("y_scale= "+y_scale_p[0]+"\n");
sb.append("RMSe="); //rms[0]+","+rms[1]+","+rms[2]+","+rms[3]+","+rms[4]);
for (int i = 0; i < rms.length-1; i++) sb.append(rms[i]+",");
sb.append(rms[rms.length-1]+"\n");
......@@ -1865,42 +1915,49 @@ public class QuadCLTCPU {
}
// apply correction to orientation - better not to prevent extra loops.
// And it only applies to rotations
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
ims_mount_atr,
ims_ortho);
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam( // camera absolute orientation
d2_ref.getQEnu(), // current IMS absolute orientation relative to ENU world frame
ims_mount_atr, // camera to IMS orientation
ims_ortho); // full 90-degree camera to imas orientation
if (quat_corr != null) {
cam_quat_ref_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_ref_enu, false);
cam_quat_ref_enu=Imx5.applyQuaternionToQuaternion( // corrected camera absolute orientation: correction.applyTo(original)
quat_corr,
cam_quat_ref_enu,
false);
if (debugLevel > -3) {
System.out.println("getXyzatrIms(): Applying attitude correction from quaternion ["+
quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
}
}
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu); // absolute orientation ATR
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
// double [][] last_corr_xyzatr = {ZERO3,ZERO3};
for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
QuadCLT scene = quadCLTs[nscene];
Did_ins_2 d2 = scene.did_ins_2;
// enu is East, North, Up from the reference scene to current scene
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up May have inacurate altitude
double [] cam_quat_enu =Imx5.quaternionImsToCam( //per scene rotation from enu (from lla) to camera
d2.getQEnu(),
ims_mount_atr,
ims_ortho);
if (quat_corr != null) { // with correction if exists
cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
cam_quat_enu=Imx5.applyQuaternionToQuaternion(
quat_corr,
cam_quat_enu,
false);
}
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
double [] cam_xyz_enu = Imx5.applyQuaternionTo( // offset from reference in reference frame
// Offset in the reference scene frame, not in the current scene
cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame | absolute corrected reference scene orientaion
enu, // absolute offset (East, North, Up) in meters
false);
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu); // absolute camera orientation in ATR
double [][] ims_scene_xyzatr_enu = {cam_xyz_enu, scene_abs_atr_enu }; // try with xyz?
// set initial approximation from IMS, subtract reference XYZATR
// predicted by IMU from the reference scene
double [][] pose_ims = ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu));
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu)); // {{0,0,0},{ref_absolute_orientation}}
scenes_xyzatr[nscene] = pose_ims;
}
if (debugLevel>0) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ c99e2f7b
......@@ -55,7 +55,8 @@ public class QuaternionLma {
// private boolean use_6dof = false;
private int samples = 3;
private int samples_x = 3;
private double height = 1;
// @Deprecated
// private double height = 1; // will be used to scale XYZ by the caller
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
......@@ -69,6 +70,7 @@ public class QuaternionLma {
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] axis = null;
private double [] fx_saved = null;
private double[] dbg_data;
public double [] getQuaternion() {
......@@ -125,13 +127,140 @@ public class QuaternionLma {
public double [] getX() {
return x_vector;
}
public double [] getY() {
return y_vector;
}
public double [] getW() {
return weights;
}
public double [] getLastFx(int debug_level) {
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
return fx;
}
public void saveFx(int debug_level) {
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
fx_saved = fx.clone();
}
public double [] getSavedFx() {
return fx_saved;
}
public String [] getTitles(
boolean use_atr,
int [] quat_pointers // null are int[2], will return pointer to quaternion component and number of them
) {
String [] titles = new String [samples];
for (int i = 0; i <titles.length; i++) {
titles[i] = "S-"+i; // default, if not set up below
}
boolean has_atr = false;
switch (mode) {
case MODE_XYZ:
case MODE_COMPASS:
System.arraycopy(new String[] {"X","Y","Z"}, 0, titles, 0, 3);
break;
case MODE_XYZQ:
if (samples == 6) {
titles = new String[] {"X","Y","Z","Q1","Q2","Q3"};
if (quat_pointers!= null) {quat_pointers[0]=3;quat_pointers[1]=3;}
} else if (samples == 7) {
titles = new String[] {"X","Y","Z","Q0","Q1","Q2","Q3"};
if (quat_pointers!= null) {quat_pointers[0]=3;quat_pointers[1]=4;}
}
has_atr = use_atr;
break;
case MODE_COMBO:
case MODE_COMBO_LOCAL:
System.arraycopy(new String[] {"Z","2Q3","2Q2-X","2Q1+Y"}, 0, titles, 0, 4);
break;
case MODE_XYZQ_LOCAL:
titles = new String[] {"X","Y","Z","Q0","Q1","Q2","Q3"};
has_atr = use_atr;
if (quat_pointers!= null) {quat_pointers[0]=3;quat_pointers[1]=4;}
break;
case MODE_XYZ4Q3:
titles = new String[] {"X","Y","Z","Q0","Q1","Q2","Q3"};
has_atr = use_atr;
if (quat_pointers!= null) {quat_pointers[0]=3;quat_pointers[1]=4;}
break;
}
if (has_atr) {
String [] titles_atr = new String[titles.length+3];
System.arraycopy(titles, 0, titles_atr, 0, titles.length);
System.arraycopy(new String[] {"A","T","R"}, 0, titles_atr, titles.length, 3);
return titles_atr;
}
return titles;
}
String getLmaTable(
boolean use_atr,
String sep,
int debugLevel) {
int [] quat_pointers = {-1,-1};
String [] titles = getTitles(
use_atr,
quat_pointers);
boolean q4 = quat_pointers[1] == 4;
StringBuffer sb = new StringBuffer();
double [] y = getY();
double [] fx0 = getSavedFx();
double [] fx = getLastFx(debugLevel);
boolean has_fx0= (fx0 != null);
String [] groups = {"y-", "fX0-", "fX-"};
for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
boolean last_group = (n == 2);
for (int i = 0; i < titles.length; i++) {
sb.append(groups[n]+titles[i]);
if (!last_group || (i != (titles.length - 1))) {
sb.append(sep);
} else {
sb.append("\n");
}
}
}
double [][] data = {y,fx0,fx};
for (int nscene = 0; nscene < N; nscene++) {
for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
boolean last_group = (n == 2);
double [] line = new double [titles.length];
System.arraycopy(data[n], samples * nscene, line, 0, samples);
// double [] line = data[n][nscene];
if (line.length > samples) { // add atr
// line = new double [titles.length];
// System.arraycopy(data[n], 0, line, 0, data[n].length);
double [] quat = new double [4];
System.arraycopy(line, quat_pointers[0], quat, q4 ? 0 : 1, quat_pointers[1]);
if (!q4) { // add q0 > 0;
quat[0] = Math.sqrt(1.0 -quat[1]*quat[1] -quat[2]*quat[2] -quat[3]*quat[3]);
}
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false);
double [] atr = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
System.arraycopy(atr, 0, line, samples, atr.length);
}
for (int i = 0; i < titles.length; i++) {
sb.append(line[i]);
if (!last_group || (i != (titles.length - 1))) {
sb.append(sep);
} else {
sb.append("\n");
}
}
}
}
return sb.toString();
}
public void prepareCompassLMA(
double [][] vect_x, // GPS-derived X,Y,Z relative to the reference frame
......@@ -232,7 +361,7 @@ public class QuaternionLma {
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
double [] vect_w, // one per scene
double translation_weight, // 0.0 ... 1.0;
double transl_cost, // 0.0 ... 1.0;
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
......@@ -242,7 +371,7 @@ public class QuaternionLma {
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w, // double [] vect_w, // one per scene
translation_weight, // double translation_weight, // 0.0 ... 1.0;
transl_cost, // double transl_cost, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debug_level); // final int debug_level);
......@@ -254,7 +383,7 @@ public class QuaternionLma {
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w, // double [] vect_w, // one per scene
translation_weight, // double translation_weight, // 0.0 ... 1.0;
transl_cost, // double transl_cost, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debug_level); // final int debug_level);
......@@ -275,8 +404,8 @@ public class QuaternionLma {
xyz_weight = 0;
for (int i = 0; i < N; i++) {
double sample_weight = ((vect_x[i]== null) || (vect_y[i]== null)) ? 0.0:((vect_w != null)? vect_w[i] : 1.0);
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
double tw = sample_weight * transl_cost;
double rw = sample_weight * (1.0 - transl_cost);
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
y_vector[samples * i + j] = 0.0;
......@@ -308,9 +437,9 @@ public class QuaternionLma {
x_vector[samples_x * i + 5] = rot_x.getQ2();
x_vector[samples_x * i + 6] = rot_x.getQ3();
// Rotation componets
// Rotation components
// w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
if (samples < samples_x) {
if (samples < samples_x) { // no Q0
y_vector[samples * i + 3] = rot_y.getQ1();
y_vector[samples * i + 4] = rot_y.getQ2();
y_vector[samples * i + 5] = rot_y.getQ3();
......@@ -318,12 +447,13 @@ public class QuaternionLma {
weights[samples * i + 3 + j] = rw;
sw += rw;
}
} else {
} else { // has Q0
y_vector[samples * i + 3] = rot_y.getQ0();
y_vector[samples * i + 4] = rot_y.getQ1();
y_vector[samples * i + 5] = rot_y.getQ2();
y_vector[samples * i + 6] = rot_y.getQ3();
for (int j = 1; j < 4; j++) {
// for (int j = 1; j < 4; j++) {
for (int j = 0; j < 4; j++) { // 02/14/2026 - trying q0 . Verify derivatives
weights[samples * i + 3 + j] = rw;
sw += rw;
}
......@@ -354,7 +484,7 @@ public class QuaternionLma {
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
double [] vect_w, // one per scene
double translation_weight, // 0.0 ... 1.0;
double transl_cost, // 0.0 ... 1.0;
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
......@@ -384,8 +514,8 @@ public class QuaternionLma {
} else {
// xyz
double sample_weight = (vect_w != null)? vect_w[i] : 1.0;
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
double tw = sample_weight * transl_cost;
double rw = sample_weight * (1.0 - transl_cost);
// double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
// quaternions (both normalized)
Rotation rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
......@@ -430,12 +560,12 @@ public class QuaternionLma {
}
public void prepareLMAMode3(
public void prepareLMAMode3( // MODE_XYZQ_LOCAL
int mode,
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
double [] vect_w, // one per scene
double translation_weight, // 0.0 ... 1.0;
double transl_cost, // 0.0 ... 1.0;
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
......@@ -455,8 +585,8 @@ public class QuaternionLma {
double sw = 0;
for (int i = 0; i < N; i++) {
double sample_weight = ((vect_x[i]== null) || (vect_y[i]== null)) ? 0.0:((vect_w != null)? vect_w[i] : 1.0);
double tw = sample_weight * translation_weight;
double rw = sample_weight * (1.0 - translation_weight);
double tw = sample_weight * transl_cost;
double rw = sample_weight * (1.0 - transl_cost);
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
x_vector [samples * i + j] = 0.0;
......@@ -533,17 +663,17 @@ public class QuaternionLma {
public void prepareLMA(
int mode,
double avg_height, //
// double avg_height, //
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
double [] vect_w, // one per scene
double reg_w, // regularization weight [0..1)
double [] quat0,
final int debug_level) {
if (mode != MODE_COMBO) {
if (mode != MODE_COMBO) { // MODE_COMBO_LOCAL
prepareLMAMode4(
mode, // int mode,
avg_height, // double avg_height,
// avg_height, // double avg_height,
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w, // double [] vect_w, // one per scene
......@@ -552,12 +682,12 @@ public class QuaternionLma {
debug_level); // final int debug_level);
return;
}
// MODE_COMBO
N = vect_x.length;
this.mode = mode; // 2;
samples = 4;
samples_x = 7;
height = avg_height;
// height = avg_height;
pure_weight = 1.0 - reg_w;
int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples_x * N];
......@@ -591,10 +721,10 @@ public class QuaternionLma {
x_vector[samples_x * i + 5] = rot_x.getQ2(); // Q2
x_vector[samples_x * i + 6] = rot_x.getQ3(); // Q3
y_vector[samples * i + 0] = vect_y[i][0][2]/height; // Z
y_vector[samples * i + 0] = vect_y[i][0][2]; // /height; // Z
y_vector[samples * i + 1] = 2*rot_y.getQ3(); // 2 * Q3
y_vector[samples * i + 2] = 2*rot_y.getQ2() - vect_y[i][0][0] / height; // 2 * Q2 - X / height
y_vector[samples * i + 3] = 2*rot_y.getQ1() + vect_y[i][0][1] / height; // 2 * Q1 + Y / height
y_vector[samples * i + 2] = 2*rot_y.getQ2() - vect_y[i][0][0]; // / height; // 2 * Q2 - X / height
y_vector[samples * i + 3] = 2*rot_y.getQ1() + vect_y[i][0][1]; // / height; // 2 * Q1 + Y / height
for (int j = 0; j < samples; j++) {
weights[samples * i + j] = w;
......@@ -624,7 +754,7 @@ public class QuaternionLma {
public void prepareLMAMode4( // MODE_COMBO_LOCAL
int mode,
double avg_height, //
// double avg_height, //
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
double [] vect_w, // one per scene
......@@ -636,7 +766,7 @@ public class QuaternionLma {
this.mode = mode; // 2;
samples = 4;
samples_x = 7;
height = avg_height;
// height = avg_height;
pure_weight = 1.0 - reg_w;
int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples_x * N];
......@@ -882,13 +1012,10 @@ public class QuaternionLma {
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3}); // was
final double [] vector_r = normSign(new double[] { -q0,q1,q2,q3});
// final double [] vector_r = normSign(new double[] { -q0,q2,q1,q3});
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
/// jt[i][samples * N] = 2 * vector[i];
}
}
/// fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
......@@ -1396,34 +1523,34 @@ public class QuaternionLma {
quat_rot = composeQR_Q(vector_r, quat_r);
double [] quat_rot1 = compose(vector_r, quat_r);
// combined samples
fx[i4 + 0] = xyz_rot[2] / height; // Z
fx[i4 + 0] = xyz_rot[2]; // / height; // Z
fx[i4 + 1] = 2 * quat_rot[3]; // 2 * Q3
fx[i4 + 2] = 2 * quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
fx[i4 + 2] = 2 * quat_rot[2] - xyz_rot[0]; // / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * quat_rot[1] + xyz_rot[1]; // / height; // 2 * Q1 + Y / height
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
// quat_dq = composeQR_QdQ(vector,quat_r);
quat_dq = composeQR_QdQ(vector_r,quat_r,true);
// Z
jt[0][i4 + 0] = xyz_dq[0][2] / height;
jt[1][i4 + 0] = xyz_dq[1][2] / height;
jt[2][i4 + 0] = xyz_dq[2][2] / height;
jt[3][i4 + 0] = xyz_dq[3][2] / height;
jt[0][i4 + 0] = xyz_dq[0][2]; // / height;
jt[1][i4 + 0] = xyz_dq[1][2]; // / height;
jt[2][i4 + 0] = xyz_dq[2][2]; // / height;
jt[3][i4 + 0] = xyz_dq[3][2]; // / height;
// 2 * Q3
jt[0][i4 + 1] = 2 * quat_dq[0][3];
jt[1][i4 + 1] = 2 * quat_dq[1][3];
jt[2][i4 + 1] = 2 * quat_dq[2][3];
jt[3][i4 + 1] = 2 * quat_dq[3][3];
// 2 * Q2 - X / height
jt[0][i4 + 2] = 2 * quat_dq[0][2] - xyz_dq[0][0] / height;
jt[1][i4 + 2] = 2 * quat_dq[1][2] - xyz_dq[1][0] / height;
jt[2][i4 + 2] = 2 * quat_dq[2][2] - xyz_dq[2][0] / height;
jt[3][i4 + 2] = 2 * quat_dq[3][2] - xyz_dq[3][0] / height;
jt[0][i4 + 2] = 2 * quat_dq[0][2] - xyz_dq[0][0]; // / height;
jt[1][i4 + 2] = 2 * quat_dq[1][2] - xyz_dq[1][0]; // / height;
jt[2][i4 + 2] = 2 * quat_dq[2][2] - xyz_dq[2][0]; // / height;
jt[3][i4 + 2] = 2 * quat_dq[3][2] - xyz_dq[3][0]; // / height;
// 2 * Q1 + Y / height
jt[0][i4 + 3] = 2 * quat_dq[0][1] + xyz_dq[0][1] / height;
jt[1][i4 + 3] = 2 * quat_dq[1][1] + xyz_dq[1][1] / height;
jt[2][i4 + 3] = 2 * quat_dq[2][1] + xyz_dq[2][1] / height;
jt[3][i4 + 3] = 2 * quat_dq[3][1] + xyz_dq[3][1] / height;
jt[0][i4 + 3] = 2 * quat_dq[0][1] + xyz_dq[0][1]; // / height;
jt[1][i4 + 3] = 2 * quat_dq[1][1] + xyz_dq[1][1]; // / height;
jt[2][i4 + 3] = 2 * quat_dq[2][1] + xyz_dq[2][1]; // / height;
jt[3][i4 + 3] = 2 * quat_dq[3][1] + xyz_dq[3][1]; // / height;
}
}
return fx;
......@@ -1475,10 +1602,10 @@ public class QuaternionLma {
inv_y[1], // double [] quat_src, // transformation to apply to (was reference_atr)
xyz_rot, // double [] xyz_target, // to which is applied (was scene_xyz)
quat_rot); // double [] quat_target // to which is applied (was scene_atr)
fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height; // Z
fx[i4 + 0] = comb_y[0][2]; // / height; // xyz_rot[2] / height; // Z
fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3
fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]; // / height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]; // / height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
......@@ -1494,25 +1621,25 @@ public class QuaternionLma {
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
// Z
jt[0][i4 + 0] = xyz_dq_local[0][2] / height;
jt[1][i4 + 0] = xyz_dq_local[1][2] / height;
jt[2][i4 + 0] = xyz_dq_local[2][2] / height;
jt[3][i4 + 0] = xyz_dq_local[3][2] / height;
jt[0][i4 + 0] = xyz_dq_local[0][2]; // / height;
jt[1][i4 + 0] = xyz_dq_local[1][2]; // / height;
jt[2][i4 + 0] = xyz_dq_local[2][2]; // / height;
jt[3][i4 + 0] = xyz_dq_local[3][2]; // / height;
// 2 * Q3
jt[0][i4 + 1] = 2 * quat_dq_local[0][3];
jt[1][i4 + 1] = 2 * quat_dq_local[1][3];
jt[2][i4 + 1] = 2 * quat_dq_local[2][3];
jt[3][i4 + 1] = 2 * quat_dq_local[3][3];
// 2 * Q2 - X / height
jt[0][i4 + 2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
jt[1][i4 + 2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
jt[2][i4 + 2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
jt[3][i4 + 2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
jt[0][i4 + 2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0]; // / height;
jt[1][i4 + 2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0]; // / height;
jt[2][i4 + 2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0]; // / height;
jt[3][i4 + 2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0]; // / height;
// 2 * Q1 + Y / height
jt[0][i4 + 3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
jt[1][i4 + 3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
jt[2][i4 + 3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
jt[0][i4 + 3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1]; // / height;
jt[1][i4 + 3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1]; // / height;
jt[2][i4 + 3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1]; // / height;
jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1]; // / height;
}
}
if (weights.length > N*samples) {
......@@ -1574,14 +1701,14 @@ public class QuaternionLma {
inv_y[1], // double [] quat_src, // transformation to apply to (was reference_atr)
xyz_rot, // double [] xyz_target, // to which is applied (was scene_xyz)
quat_rot); // double [] quat_target // to which is applied (was scene_atr)
fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height; // Z
fx[i4 + 0] = comb_y[0][2]; // / height; // xyz_rot[2] / height; // Z
fx[i4 + 1] = 2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3
fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
fx[i4 + 2] = 2 * comb_y[1][2] - comb_y[0][0]; // / height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * comb_y[1][1] + comb_y[0][1]; // / height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
if (dbg_out) {
dbg_data[i7 + 0] = comb_y[0][0]/ height;
dbg_data[i7 + 1] = comb_y[0][1]/ height;
dbg_data[i7 + 2] = comb_y[0][2]/ height;
dbg_data[i7 + 0] = comb_y[0][0]; // / height;
dbg_data[i7 + 1] = comb_y[0][1]; // / height;
dbg_data[i7 + 2] = comb_y[0][2]; // / height;
dbg_data[i7 + 3] = comb_y[1][0];
dbg_data[i7 + 4] = comb_y[1][1];
dbg_data[i7 + 5] = comb_y[1][2];
......@@ -1603,28 +1730,10 @@ public class QuaternionLma {
double [][] d_dQn = new double[4][4];
// Z
d_dQn[0][0] = xyz_dq_local[0][2] / height;
d_dQn[1][0] = xyz_dq_local[1][2] / height;
d_dQn[2][0] = xyz_dq_local[2][2] / height;
d_dQn[3][0] = xyz_dq_local[3][2] / height;
/*
// why all "-2"?
// 2 * Q3
d_dQn[0][1] = -2 * quat_dq_local[0][3];
d_dQn[1][1] = -2 * quat_dq_local[1][3];
d_dQn[2][1] = -2 * quat_dq_local[2][3];
d_dQn[3][1] = -2 * quat_dq_local[3][3];
// 2 * Q2 - X / height
d_dQn[0][2] = -2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
d_dQn[1][2] = -2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
d_dQn[2][2] = -2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
d_dQn[3][2] = -2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
// 2 * Q1 + Y / height
d_dQn[0][3] = -2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
d_dQn[1][3] = -2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
d_dQn[2][3] = -2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
d_dQn[3][3] = -2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
*/
d_dQn[0][0] = xyz_dq_local[0][2]; // / height;
d_dQn[1][0] = xyz_dq_local[1][2]; // / height;
d_dQn[2][0] = xyz_dq_local[2][2]; // / height;
d_dQn[3][0] = xyz_dq_local[3][2]; // / height;
// 2 * Q3
d_dQn[0][1] = 2 * quat_dq_local[0][3];
......@@ -1632,15 +1741,15 @@ public class QuaternionLma {
d_dQn[2][1] = 2 * quat_dq_local[2][3];
d_dQn[3][1] = 2 * quat_dq_local[3][3];
// 2 * Q2 - X / height
d_dQn[0][2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0] / height;
d_dQn[1][2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0] / height;
d_dQn[2][2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0] / height;
d_dQn[3][2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0] / height;
d_dQn[0][2] = 2 * quat_dq_local[0][2] - xyz_dq_local[0][0]; // / height;
d_dQn[1][2] = 2 * quat_dq_local[1][2] - xyz_dq_local[1][0]; // / height;
d_dQn[2][2] = 2 * quat_dq_local[2][2] - xyz_dq_local[2][0]; // / height;
d_dQn[3][2] = 2 * quat_dq_local[3][2] - xyz_dq_local[3][0]; // / height;
// 2 * Q1 + Y / height
d_dQn[0][3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1] / height;
d_dQn[1][3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1] / height;
d_dQn[2][3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1] / height;
d_dQn[3][3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
d_dQn[0][3] = 2 * quat_dq_local[0][1] + xyz_dq_local[0][1]; // / height;
d_dQn[1][3] = 2 * quat_dq_local[1][1] + xyz_dq_local[1][1]; // / height;
d_dQn[2][3] = 2 * quat_dq_local[2][1] + xyz_dq_local[2][1]; // / height;
d_dQn[3][3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1]; // / height;
//dQn_dQ123
double [][] d_dq = mulMat(dQn_dQ123, d_dQn);
......@@ -2233,13 +2342,13 @@ public class QuaternionLma {
if (q[0] >= 0) return q;
return new double [] {-q[0], -q[1], -q[2], -q[3]};
}
/**
* Apply quaternion q to quaternion r
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
* @param r - 4 components (scalar, vector) of the target quaternion to which to apply the first one
* @return composed quaternion
*/
public static double [] composeQR_Q(
double [] q,
double [] r) {
......@@ -2574,6 +2683,105 @@ public class QuaternionLma {
}
}
/**
* Get average length ratio (X,Y,Z are not offest)
* @param vect_x first array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param vect_y second array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param first_last null (use all data) or a pair of first, last (inclusive) indices
* @return ratio of average length of the XYZ of the vect_y to vect_x
*/
public static double getLengthRatio(
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
int [] first_last){
if (first_last == null) {
first_last = new int [] {0,vect_x.length-1};
}
double [][][] scaled_xyz = new double [vect_y.length][][];
double sx2=0, sy2=0;
for (int i = first_last[0]; i<= first_last[1]; i++) if ((vect_x[i] != null) && (vect_y[i] != null)){
double lx2=0, ly2=0;
for (int j=0; j < 3; j++) {
double x=vect_x[i][0][j];
double y=vect_y[i][0][j];
lx2 += x*x;
ly2 += y*y;
}
if (!Double.isNaN(lx2) && !Double.isNaN(ly2)) {
sx2 += lx2;
sy2 += ly2;
}
}
return Math.sqrt(sy2/sx2);
}
/**
* Scale translation components of the pose vectors array
* @param s scaler to apply (when used with getLengthRatio(), s = 1.0/getLengthRatio() )
* @param vect_y array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param first_last null (use all data) or a pair of first, last (inclusive) indices
* @return scaled (only x,y,z) of the vect_y
*/
public static double [][][] scaleXYZ(
double s,
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
int [] first_last){
if (first_last == null) {
first_last = new int [] {0,vect_y.length-1};
}
double [][][] scaled_xyz = new double [vect_y.length][][];
for (int i = first_last[0]; i<= first_last[1]; i++) if (vect_y[i] != null){
scaled_xyz[i] = new double [][] {
{s* vect_y[i][0][0],s* vect_y[i][0][1],s* vect_y[i][0][2]},
vect_y[i][1]};
}
return scaled_xyz;
}
/**
* Scale X,Y,Z coordinates of the second arrays to match lengths of the first one
* @param vect_x first array of poses: [nscene{{x,y,z},{a,t,r}}
* @param vect_y second array of poses: [nscene{{x,y,z},{a,t,r}}
* @param first_last null (use all data) or a pair of first, last (inclusive) indices
* @return scaled (only x,y,z) of the vect_y to match average lengths
*/
public static double [][][] scaleXYZ(
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
int [] first_last){
if (first_last == null) {
first_last = new int [] {0,vect_x.length-1};
}
double [][][] scaled_xyz = new double [vect_y.length][][];
double sx2=0, sy2=0;
for (int i = first_last[0]; i<= first_last[1]; i++) if ((vect_x[i] != null) && (vect_y[i] != null)){
double lx2=0, ly2=0;
for (int j=0; j < 3; j++) {
double x=vect_x[i][0][j];
double y=vect_y[i][0][j];
lx2 += x*x;
ly2 += y*y;
}
if (!Double.isNaN(lx2) && !Double.isNaN(ly2)) {
sx2 += lx2;
sy2 += ly2;
}
}
double s = Math.sqrt(sx2/sy2);
for (int i = first_last[0]; i<= first_last[1]; i++) if (vect_y[i] != null){
scaled_xyz[i] = new double [][] {
{s* vect_y[i][0][0],s* vect_y[i][0][1],s* vect_y[i][0][2]},
vect_y[i][1]};
}
return scaled_xyz;
}
/*
// old version
public static double [][][] scaleXYZ(
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
......@@ -2593,6 +2801,7 @@ public class QuaternionLma {
}
return scaled_xyz;
}
*/
public static double getDiameter(
double [][][] xyzatr,
......
src/main/java/com/elphel/imagej/tileprocessor/SeriesInfinityCorrection.java
View file @ c99e2f7b
......@@ -414,12 +414,14 @@ public class SeriesInfinityCorrection {
QuadCLT index_scene, // where to put result
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) {
System.out.println("Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera");
return null;
}
// @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
......@@ -428,10 +430,8 @@ public class SeriesInfinityCorrection {
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][][][];
......@@ -465,34 +465,24 @@ public class SeriesInfinityCorrection {
double avg_z = sum_avgz/sum_avgz_w;
double [] rms = new double[7];
double [] quat = new double[4];
int quat_lma_mode = orient_combo?QuaternionLma.MODE_COMBO_LOCAL: QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
double translation_agl = 1.0 / (avg_z + 1.0);
if (!orient_by_move) {
translation_agl = 0.0;
} else if (!orient_by_rot) {
translation_agl = 1.0;
}
// Apply transl_cost
double translation_weight = (translation_agl * transl_cost) / (translation_agl * transl_cost + (1-translation_agl)* (1-transl_cost));
// boolean orient_combo,
// boolean save_details
if (debugLevel > -3) {
System.out.println("adjustImuOrient(): transl_cost="+transl_cost+", translation_agl="+translation_agl+", translation_weight="+translation_weight);
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,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
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
......@@ -502,19 +492,12 @@ public class SeriesInfinityCorrection {
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
/*
saveStringInModelDirectory(
sb.tiString, // String string,
IMU_CALIB_LOGS_SUFFIX); // String suffix)
*/
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("getNumOrient()="+quadCLTs[ref_index].getNumOrient()+" of "+clt_parameters.imp.min_num_orient+"\n");
/// sb.append("getNumAccum()= "+quadCLTs[ref_index].getNumAccum()+ " of "+clt_parameters.imp.min_num_interscene+"\n");
sb.append("avg_z= "+avg_z+" m\n");
sb.append("translation_weight= "+translation_weight+"\n");
sb.append("quat_lma_mode= "+quat_lma_mode+"\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");
......@@ -657,31 +640,43 @@ public class SeriesInfinityCorrection {
public static double [][][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight, // 1.0 - pure translation, 0.0 - pure rotation
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) {
final boolean use3 = false; // true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
boolean use_inv = false; //
boolean use_inv = true; // false; //
double [][][] xyzatr = flattenPoses(xyzatrs);
double [][][] ims_xyzatr = flattenPoses(ims_xyzatrs);
/*
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
use3, // boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
avg_height, // double avg_height,
translation_weight, // double translation_weight,
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;
}
......@@ -689,29 +684,30 @@ public class SeriesInfinityCorrection {
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 [][][][] rotated_xyzatrs = new double [num_refs][][][];
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;
rotated_xyzatrs[nref] = new double [num_scenes][][]; // orientation from camera, xyz from rotated IMS
double [] rotated_xyz = new double[3];
Rotation rot_invert = rot.revert();
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], rotated_xyz);
// ???
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]);
Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
// 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 rotation_atr,rotation_atr2;
Rotation rot_corr_ims,rotation_atr2;
if (use_inv) { // should not be used
rotation_atr = rot_invert.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
rot_corr_ims = rot_invert.applyTo(rot_ims);
rotation_atr2 = rot_corr_ims.applyTo(rot); // applyInverseTo?
} else {
rotation_atr = rot.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
rot_corr_ims = rot.applyTo(rot_ims); // applying correction to the IMS orientation
rotation_atr2 = rot_corr_ims.applyTo(rot_invert); // applyInverseTo?
}
rotated_xyzatrs[nref][nscene] = new double [][] {rotated_xyz.clone(),
rot_ims_xyzatrs[nref][nscene] = new double [][] {rot_ims_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
}
......@@ -722,13 +718,14 @@ public class SeriesInfinityCorrection {
rot, // Rotation rot,
xyzatrs, // double [][][] xyzatr,
ims_xyzatrs, // double [][][] ims_xyzatr,
rotated_xyzatrs, // double [][][] rotated_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 rotated_xyzatrs;
return rot_ims_xyzatrs;
}
public static void saveRotateImsMultiDetails(
......@@ -736,6 +733,7 @@ public class SeriesInfinityCorrection {
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];
......@@ -744,10 +742,11 @@ public class SeriesInfinityCorrection {
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\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");
"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;
......@@ -790,22 +789,21 @@ public class SeriesInfinityCorrection {
* @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 componets
* @return quaternion components
*/
@Deprecated
public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters,
int quat_lma_mode,
boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
double avg_height,
double translation_weight,
QuadCLT[][] quadCLTss,
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[5];
double [] rms, // null or double[7];
double [] y_scale_p, // if not null will return y_scale
int debugLevel) {
boolean use_scale_y = false;
// final boolean use3 = (quat_lma_mode == 3); // true;
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;
......@@ -816,115 +814,258 @@ public class SeriesInfinityCorrection {
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 [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
// double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
// double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
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] = ims_xyzatrs[nref][nscene];
vect_y[vindx++] = xyzatrs[nref][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++;
}
}
// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
double [][][] vect_y_scaled;
double y_scale = 1.0;
if (use_scale_y) {
vect_y_scaled= QuaternionLma.scaleXYZ(
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
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 ((quat_lma_mode == QuaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
0.0, // reg_w, // double reg_w, // regularization weight [0..1)
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
double max_to_min = Math.sqrt(mn_mx_diag[1]/mn_mx_diag[0]);
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("max_to_min = "+max_to_min+", quat_max_ratio="+quat_max_ratio);
System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
// 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/quat0.length);
if (debugLevel > -3) {
System.out.println("====1 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
reg_w, // double reg_w, // regularization weight [0..1)
quat0, // double [] quat0,
debugLevel); // int debug_level)
}
} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) ||
(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) ||
(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
xyzQLma.prepareLMA(
vect_x, // double [][][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
translation_weight, // 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,
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)
double [] mn_mx_diag = quaternionLma.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 (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 (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/quat0.length);
if (debugLevel > -3) {
System.out.println("====2 Calculated reg_w = "+reg_w+", reg_w_old="+reg_w_old+", a="+a);
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];
}
}
}
}
quaternionLma.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
translation_weight, // 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
quat0, // double [] quat0,
debugLevel); // int debug_level)
}
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 {
quaternionLma.prepareLMA(
vect_x, // quat_lma_xyz, // double [][] vect_x,
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
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)
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)
}
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
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
......@@ -936,12 +1077,20 @@ public class SeriesInfinityCorrection {
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 = quaternionLma.getLastRms();
double [] last_rms = xyzQLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = quaternionLma.getInitialRms();
double [] initial_rms = xyzQLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
......@@ -955,38 +1104,11 @@ public class SeriesInfinityCorrection {
}
}
}
return quaternionLma.getQuaternion();
return xyzQLma.getQuaternion();
}
}
/*
SetChannels [] set_channels=quadCLT_main.setChannels(debugLevel);
QuadCLT [] quadCLTs = new QuadCLT [set_channels.length];
for (int i = 0; i < quadCLTs.length; i++) {
quadCLTs[i] = (QuadCLT) quadCLT_main.spawnQuadCLT(
set_channels[i].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel);
quadCLTs[i].setDSRBG(
clt_parameters, // CLTParameters clt_parameters,
threadsMax, // int threadsMax, // maximal number of threads to launch
updateStatus, // boolean updateStatus,
debugLevel); // int debugLevel)
}
if (ref_index < 0) {
ref_index += quadCLTs.length;
}
*
*
public static double getImgImsScale( // correctInfinityFromIMS(
QuadCLT [] quadCLTs,
QuadCLT master_CLT,
int earliest_scene,
int latest_scene) { // -1 will use quadCLTs.length -1;
*/
}
src/main/java/com/elphel/imagej/tileprocessor/XyzQLma.java
View file @ c99e2f7b
package com.elphel.imagej.tileprocessor;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Calendar;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.ims.Imx5;
import Jama.Matrix;
public class XyzQLma {
private final static int REGLEN = 1; // number of extra (regularization) samples
/*
* Terminology and frame-model notes for this fitter:
*
* Scene:
* A set of simultaneously captured images (currently 16 for LWIR-16), identified
* by one timestamp used in file names as seconds_microseconds.
*
* Scene sequence:
* A continuous capture run (currently ~60 Hz, typically ~496-498 scenes). In older
* notes this may also be called scene series.
*
* Segment:
* A contiguous subset of a scene sequence built around one reference scene so each
* scene in the segment has sufficient overlap with that reference. Segments are used
* for robust inter-scene pose estimation and for collecting wide-baseline target
* views. Segments may overlap.
*
* Reference scene:
* The scene defining the segment frame. Its pose is normally {x,y,z,a,t,r}=0 and
* all other scene poses in the segment are represented relative to it.
*
* Data sources:
* 1) IMS-derived poses (global accuracy good, local/subpixel weaker)
* 2) Image-correlation camera poses (deep-subpixel local accuracy)
*
* This class estimates the small rotational misalignment between IMS and camera
* frames. Input arrays are per-segment relative poses:
* ims_xyzatrs[segment][scene] and xyzatrs[segment][scene]
* flattened before fitting.
*
* Why orientation uses Q*R*Q':
* Input rotations are differential (relative to each segment reference scene). The
* correction quaternion Q must therefore be applied consistently to both ends of the
* relative rotation transform, giving conjugation Q*R*Q'. Using only Q*R is for an
* absolute-frame rotation model and is incorrect here.
*
* Cost weighting:
* The global LMA cost mixes translation and orientation terms by translation_weight.
* Translation contributes to constrain two axes strongly in near-linear flight,
* while orientation often contributes stronger observability for the remaining axis.
*/
// private final static int REGLEN = 1; // number of extra (regularization) samples
private int N = 0;
private int samples = 3;
......@@ -17,7 +67,6 @@ public class XyzQLma {
private double [] parameters_vector = null;
private double [] x_vector = null;
private double [] y_vector = null;
private double [] y_inv_vector = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private double xyz_weight; // weight of all xyz, samples (weight of rotations - pure_weight- xyz_weight
......@@ -42,7 +91,7 @@ public class XyzQLma {
x_vector = new double [samples_x * N];
y_vector = new double [samples * N + extra_samples];
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
parameters_vector = new double[] {quat0[1], quat0[2], quat0[3]}; // optimize only q1,q2,q3
double sw = 0;
xyz_weight = 0;
for (int i = 0; i < N; i++) {
......@@ -123,7 +172,7 @@ public class XyzQLma {
}
public double [] getQuaternion() {
return parameters_vector;
return vectorToQ(parameters_vector);
}
private double [] getRms4(double [] rms3) { // rms, rms_pure, xyz_rms, atr_rms
......@@ -272,6 +321,15 @@ public class XyzQLma {
parameters_vector, // double [] vector,
last_jt, // final double [][] jt
debug_level); // final int debug_level
if (debug_level > 5) { // remove?
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
parameters_vector, // double [] vector,
delta); // double delta);
System.out.println("Maximal error = "+err);
}
last_ymfx = getYminusFxWeighted(
fx0, // final double [] fx
last_rms, // final double [] rms_fp
......@@ -287,6 +345,10 @@ public class XyzQLma {
lambda, // double lambda
this.last_jt // double[][] jt
));
if (debug_level > 2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv;
try {
jtjl_inv = wjtjlambda.inverse();
......@@ -294,14 +356,26 @@ public class XyzQLma {
rslt[1] = true;
return rslt;
}
if (debug_level > 2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level > 2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level > 2) {
System.out.println("mdelta");
mdelta.print(18, 10);
}
double [] delta = mdelta.getColumnPackedCopy();
double [] new_vector = parameters_vector.clone();
for (int i = 0; i < parameters_vector.length; i++) {
new_vector[i] += delta[i];
}
new_vector = normalizeQ(new_vector); // keep parameterized quaternion normalized
new_vector = normalizeQ123(new_vector); // keep q1..q3 valid for q0=sqrt(1-q1^2-q2^2-q3^2)
double [] fx = getFxDerivs(
new_vector, // double [] vector,
......@@ -358,6 +432,14 @@ public class XyzQLma {
if (rslt == null) {
return -1;
}
if (debug_level > 1) {
double [] good_or_bad4 = getGoodOrBadRms();
double [] initial_rms4 = getInitialRms();
System.out.println("LMA step " + iter + ": {" + rslt[0] + "," + rslt[1] + "} full RMS= " + good_or_bad_rms[0] +
" (" + initial_rms[0] + "), pure RMS=" + good_or_bad_rms[1] + " (" + initial_rms[1] + "), " +
"XYZ RMS=" + good_or_bad4[2] + " (" + initial_rms4[2] + "), ATR RMS=" + good_or_bad4[3] + " (" + initial_rms4[3] + "), " +
"lambda=" + lambda);
}
if (rslt[1]) {
break;
}
......@@ -373,8 +455,26 @@ public class XyzQLma {
if (!rslt[0]) {
if ((last_rms != null) && (initial_rms != null) && (last_rms[0] < initial_rms[0])) {
rslt[0] = true;
if (debug_level > 1) {
System.out.println("Step " + iter + ": Failed to converge, but result improved over initial");
}
} else if (debug_level > 1) {
System.out.println("Step " + iter + ": Failed to converge");
}
} else if (debug_level > 1) {
if (iter >= num_iter) {
System.out.println("Step " + iter + ": Improved, but number of steps exceeded maximal");
} else {
System.out.println("Step " + iter + ": LMA: Success");
}
}
if (debug_level > 0 && last_rms != null && initial_rms != null) {
double [] last_rms4 = getLastRms();
double [] initial_rms4 = getInitialRms();
System.out.println("LMA: full RMS=" + last_rms[0] + " (" + initial_rms[0] + "), pure RMS=" + last_rms[1] + " (" + initial_rms[1] + "), " +
"XYZ RMS=" + last_rms4[2] + " (" + initial_rms4[2] + "), ATR RMS=" + last_rms4[3] + " (" + initial_rms4[3] + "), " +
"lambda=" + lambda);
}
return rslt[0] ? iter : -1;
}
......@@ -383,6 +483,7 @@ public class XyzQLma {
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
double [] q4 = vectorToQ(vector);
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
......@@ -402,31 +503,27 @@ public class XyzQLma {
};
double [] f7 = new double[7];
if (jt != null) {
double [][] jt7 = new double[4][7];
applyQCore(xyzQ, vector, f7, jt7);
double [][] jt7 = getJT3(xyzQ, q4);
applyQCore(xyzQ, q4, f7, null);
for (int p = 0; p < vector.length; p++) {
for (int k = 0; k < 7; k++) {
jt[p][bof + k] = jt7[p][k];
}
}
} else {
applyQCore(xyzQ, vector, f7, null);
applyQCore(xyzQ, q4, f7, null);
}
System.arraycopy(f7, 0, fx, bof, 7);
}
// Optional regularization columns at the end:
// 3 extras -> pull q1..q3 to 0; if 4 extras, keep first one (q0) at 0 and pull q1..q3.
// Optional regularization columns at the end: pull q1..q3 to 0.
int extra = weights.length - samples * N;
int base = samples * N;
if (extra >= 3) {
int start = (extra == 4) ? 1 : 0;
for (int j = 1; j <= 3; j++) {
int col = base + start + (j - 1);
for (int j = 0; j < 3; j++) {
int col = base + j;
if (col < fx.length) {
fx[col] = vector[j];
if (jt != null) {
jt[j][col] = 1.0;
}
if (jt != null) jt[j][col] = 1.0;
}
}
}
......@@ -519,6 +616,120 @@ public class XyzQLma {
double [] r = mulQ(mulQ(q, vq), conjQ(q));
return new double[] {r[1], r[2], r[3]};
}
private static double [] normalizeQ123(double [] v3) {
double [] u = v3.clone();
double s2 = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
if (s2 >= 1.0) {
double k = (1.0 - 1.0e-12) / Math.sqrt(s2);
u[0] *= k;
u[1] *= k;
u[2] *= k;
}
return u;
}
private static double [] vectorToQ(double [] v3) {
double [] u = normalizeQ123(v3);
double s2 = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
double q0 = Math.sqrt(Math.max(0.0, 1.0 - s2));
return new double[] {q0, u[0], u[1], u[2]};
}
private static double [][] getJT3(
double [] xyzQ,
double [] q4) {
double [][] jt3 = new double[3][7];
double [][] dfdq = getDfdqNormalizedQ(xyzQ, q4);
double q0 = q4[0];
double q1 = q4[1];
double q2 = q4[2];
double q3 = q4[3];
double[] u = new double[] {q1, q2, q3};
for (int i = 0; i < 3; i++) {
double dq0du = (q0 > 1.0e-12) ? (-u[i] / q0) : 0.0;
for (int k = 0; k < 7; k++) {
jt3[i][k] = dfdq[k][i + 1] + dfdq[k][0] * dq0du;
}
}
return jt3;
}
private double compareJT(
double [] vector,
double delta) {
double [] errors = new double[vector.length];
double [][] jt = new double[vector.length][];
System.out.print("Parameters vector = [");
for (int i = 0; i < vector.length; i++) {
System.out.print(vector[i]);
if (i < (vector.length - 1)) {
System.out.print(", ");
}
}
System.out.println("]");
getFxDerivs(
vector, // double [] vector,
jt, // final double [][] jt
1); // final int debug_level
double [][] jt_delta = getFxDerivsDelta(
vector, // double [] vector,
delta, // final double delta
-1); // final int debug_level
for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {
System.out.print(String.format("%3d", n));
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f", jt[i][n]));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f", jt_delta[i][n]));
}
for (int i = 0; i < vector.length; i++) {
double e = jt[i][n] - jt_delta[i][n];
System.out.print(String.format("\t%12.9f", e));
errors[i] = Math.max(errors[i], Math.abs(e));
}
System.out.println();
}
for (int i = 0; i < vector.length; i++) {
System.out.print("\t\t");
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f", errors[i]));
}
System.out.println();
double err = 0.0;
for (int i = 0; i < vector.length; i++) {
err = Math.max(err, errors[i]);
}
return err;
}
private double [][] getFxDerivsDelta(
double [] vector,
final double delta,
final int debug_level) {
double [][] jt = new double[vector.length][weights.length];
for (int nv = 0; nv < vector.length; nv++) {
double [] vp = vector.clone();
double [] vm = vector.clone();
vp[nv] += 0.5 * delta;
vm[nv] -= 0.5 * delta;
double [] fp = getFxDerivs(
vp, // double [] vector
null, // final double [][] jt
debug_level);
double [] fm = getFxDerivs(
vm, // double [] vector
null, // final double [][] jt
debug_level);
for (int i = 0; i < weights.length; i++) {
jt[nv][i] = (fp[i] - fm[i]) / delta;
}
}
return jt;
}
private static void applyQCore(
double [] xyzQ,
......@@ -541,36 +752,19 @@ public class XyzQLma {
out[0] = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * x + 2.0 * (q1 * q2 - q0 * q3) * y + 2.0 * (q1 * q3 + q0 * q2) * z;
out[1] = 2.0 * (q1 * q2 + q0 * q3) * x + (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * y + 2.0 * (q2 * q3 - q0 * q1) * z;
out[2] = 2.0 * (q1 * q3 - q0 * q2) * x + 2.0 * (q2 * q3 + q0 * q1) * y + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * z;
out[3] = q0 * r0 - q1 * r1 - q2 * r2 - q3 * r3;
out[4] = q0 * r1 + q1 * r0 + q2 * r3 - q3 * r2;
out[5] = q0 * r2 - q1 * r3 + q2 * r0 + q3 * r1;
out[6] = q0 * r3 + q1 * r2 - q2 * r1 + q3 * r0;
double [] qr = mulQ(new double[] {q0, q1, q2, q3}, new double[] {r0, r1, r2, r3});
double [] qc = new double[] {q0, -q1, -q2, -q3};
double [] qrq = mulQ(qr, qc);
out[3] = qrq[0];
out[4] = qrq[1];
out[5] = qrq[2];
out[6] = qrq[3];
}
if (jt == null) {
return;
}
// Derivatives of outputs by normalized quaternion components q0..q3
double [][] dfdq = new double[7][4];
// d(x')/dq
dfdq[0][0] = 2.0 * ( q0 * x - q3 * y + q2 * z);
dfdq[0][1] = 2.0 * ( q1 * x + q2 * y + q3 * z);
dfdq[0][2] = 2.0 * (-q2 * x + q1 * y + q0 * z);
dfdq[0][3] = 2.0 * (-q3 * x - q0 * y + q1 * z);
// d(y')/dq
dfdq[1][0] = 2.0 * ( q3 * x + q0 * y - q1 * z);
dfdq[1][1] = 2.0 * ( q2 * x - q1 * y - q0 * z);
dfdq[1][2] = 2.0 * ( q1 * x + q2 * y + q3 * z);
dfdq[1][3] = 2.0 * ( q0 * x - q3 * y + q2 * z);
// d(z')/dq
dfdq[2][0] = 2.0 * (-q2 * x + q1 * y + q0 * z);
dfdq[2][1] = 2.0 * ( q3 * x + q0 * y - q1 * z);
dfdq[2][2] = 2.0 * (-q0 * x + q3 * y - q2 * z);
dfdq[2][3] = 2.0 * ( q1 * x + q2 * y + q3 * z);
// d(q*q_or)/dq
dfdq[3][0] = r0; dfdq[3][1] = -r1; dfdq[3][2] = -r2; dfdq[3][3] = -r3;
dfdq[4][0] = r1; dfdq[4][1] = r0; dfdq[4][2] = r3; dfdq[4][3] = -r2;
dfdq[5][0] = r2; dfdq[5][1] = -r3; dfdq[5][2] = r0; dfdq[5][3] = r1;
dfdq[6][0] = r3; dfdq[6][1] = r2; dfdq[6][2] = -r1; dfdq[6][3] = r0;
double [][] dfdq = getDfdqNormalizedQ(xyzQ, new double[] {q0, q1, q2, q3});
// Chain rule: d/dQ_i = sum_j (d/dq_j) * (dq_j/dQ_i), with q = Q/|Q|
double s2 = Q[0] * Q[0] + Q[1] * Q[1] + Q[2] * Q[2] + Q[3] * Q[3];
......@@ -596,5 +790,724 @@ public class XyzQLma {
}
}
}
private static double [][] getDfdqNormalizedQ(
double [] xyzQ,
double [] q) {
double q0 = q[0];
double q1 = q[1];
double q2 = q[2];
double q3 = q[3];
double x = xyzQ[0];
double y = xyzQ[1];
double z = xyzQ[2];
double r0 = xyzQ[3];
double r1 = xyzQ[4];
double r2 = xyzQ[5];
double r3 = xyzQ[6];
double [][] dfdq = new double[7][4];
// translation part
dfdq[0][0] = 2.0 * ( q0 * x - q3 * y + q2 * z);
dfdq[0][1] = 2.0 * ( q1 * x + q2 * y + q3 * z);
dfdq[0][2] = 2.0 * (-q2 * x + q1 * y + q0 * z);
dfdq[0][3] = 2.0 * (-q3 * x - q0 * y + q1 * z);
dfdq[1][0] = 2.0 * ( q3 * x + q0 * y - q1 * z);
dfdq[1][1] = 2.0 * ( q2 * x - q1 * y - q0 * z);
dfdq[1][2] = 2.0 * ( q1 * x + q2 * y + q3 * z);
dfdq[1][3] = 2.0 * ( q0 * x - q3 * y + q2 * z);
dfdq[2][0] = 2.0 * (-q2 * x + q1 * y + q0 * z);
dfdq[2][1] = 2.0 * ( q3 * x + q0 * y - q1 * z);
dfdq[2][2] = 2.0 * (-q0 * x + q3 * y - q2 * z);
dfdq[2][3] = 2.0 * ( q1 * x + q2 * y + q3 * z);
double [] qq = new double[] {q0, q1, q2, q3};
double [] rr = new double[] {r0, r1, r2, r3};
double [] qc = new double[] {q0, -q1, -q2, -q3};
double [] qr = mulQ(qq, rr);
double [][] e = {
{1.0, 0.0, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 1.0}};
double [][] ec = {
{1.0, 0.0, 0.0, 0.0},
{0.0,-1.0, 0.0, 0.0},
{0.0, 0.0,-1.0, 0.0},
{0.0, 0.0, 0.0,-1.0}};
for (int i = 0; i < 4; i++) {
double [] term1 = mulQ(mulQ(e[i], rr), qc);
double [] term2 = mulQ(qr, ec[i]);
dfdq[3][i] = term1[0] + term2[0];
dfdq[4][i] = term1[1] + term2[1];
dfdq[5][i] = term1[2] + term2[2];
dfdq[6][i] = term1[3] + term2[3];
}
return dfdq;
}
/**
* Get average length ratio (X,Y,Z are not offest)
* @param vect_x first array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param vect_y second array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param first_last null (use all data) or a pair of first, last (inclusive) indices
* @return ratio of average length of the XYZ of the vect_y to vect_x
*/
public static double getLengthRatio(
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
int [] first_last){
if (first_last == null) {
first_last = new int [] {0,vect_x.length-1};
}
double [][][] scaled_xyz = new double [vect_y.length][][];
double sx2=0, sy2=0;
for (int i = first_last[0]; i<= first_last[1]; i++) if ((vect_x[i] != null) && (vect_y[i] != null)){
double lx2=0, ly2=0;
for (int j=0; j < 3; j++) {
double x=vect_x[i][0][j];
double y=vect_y[i][0][j];
lx2 += x*x;
ly2 += y*y;
}
if (!Double.isNaN(lx2) && !Double.isNaN(ly2)) {
sx2 += lx2;
sy2 += ly2;
}
}
return Math.sqrt(sy2/sx2);
}
/**
* Scale translation components of the pose vectors array
* @param s scaler to apply (when used with getLengthRatio(), s = 1.0/getLengthRatio() )
* @param vect_y array of poses: [nscene]{{x,y,z},{a,t,r}}
* @param first_last null (use all data) or a pair of first, last (inclusive) indices
* @return scaled (only x,y,z) of the vect_y
*/
public static double [][][] scaleXYZ(
double s,
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
int [] first_last){
if (first_last == null) {
first_last = new int [] {0,vect_y.length-1};
}
double [][][] scaled_xyz = new double [vect_y.length][][];
for (int i = first_last[0]; i<= first_last[1]; i++) if (vect_y[i] != null){
scaled_xyz[i] = new double [][] {
{s* vect_y[i][0][0],s* vect_y[i][0][1],s* vect_y[i][0][2]},
vect_y[i][1]};
}
return scaled_xyz;
}
public double [] getMinMaxDiag(
int debug_level){
double [][] jt = new double [parameters_vector.length][];
//double [] fx =
getFxDerivs(
parameters_vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
Matrix wjtjlambda = new Matrix(getWJtJlambda(
0, // *10, // temporary
jt)); // double [][] jt)
double [] mn_mx= {Double.NaN,Double.NaN};
int si = (wjtjlambda.get(0,0) == 0)? 1 : 0;
for (int i = si; i < parameters_vector.length; i++) {
double d = wjtjlambda.get(i,i);
if (!(d > mn_mx[0])) mn_mx[0] = d;
if (!(d < mn_mx[1])) mn_mx[1] = d;
}
return mn_mx;
}
String getLmaTable(
boolean use_atr,
String sep,
int debugLevel) {
int [] quat_pointers = {3,4};
String [] titles = {"X","Y","Z","Q0","Q1","Q2","Q3"};
boolean q4 = quat_pointers[1] == 4;
StringBuffer sb = new StringBuffer();
double [] y = getY();
double [] fx0 = getSavedFx();
double [] fx = getLastFx(debugLevel);
boolean has_fx0= (fx0 != null);
String [] groups = {"y-", "fX0-", "fX-"};
for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
boolean last_group = (n == 2);
for (int i = 0; i < titles.length; i++) {
sb.append(groups[n]+titles[i]);
if (!last_group || (i != (titles.length - 1))) {
sb.append(sep);
} else {
sb.append("\n");
}
}
}
double [][] data = {y,fx0,fx};
for (int nscene = 0; nscene < N; nscene++) {
for (int n = 0; n < 3; n++) if ((n != 1 ) || has_fx0) {
boolean last_group = (n == 2);
double [] line = new double [titles.length];
System.arraycopy(data[n], samples * nscene, line, 0, samples);
if (line.length > samples) { // add atr
double [] quat = new double [4];
System.arraycopy(line, quat_pointers[0], quat, q4 ? 0 : 1, quat_pointers[1]);
if (!q4) { // add q0 > 0;
quat[0] = Math.sqrt(1.0 -quat[1]*quat[1] -quat[2]*quat[2] -quat[3]*quat[3]);
}
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false);
double [] atr = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
System.arraycopy(atr, 0, line, samples, atr.length);
}
for (int i = 0; i < titles.length; i++) {
sb.append(line[i]);
if (!last_group || (i != (titles.length - 1))) {
sb.append(sep);
} else {
sb.append("\n");
}
}
}
}
return sb.toString();
}
// moved here from SeriesInfinityCorrection
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;
}
/**
* 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();
}
}
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());
}
}
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