Skip to content

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

Implemented mode 4 for IMS-to-camera adjustment

parent 1d564879
Hide whitespace changes
Inline Side-by-side
Showing with 224 additions and 316 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 598a2074
...@@ -5437,7 +5437,7 @@ public class OpticalFlow { ...@@ -5437,7 +5437,7 @@ public class OpticalFlow {
} }
double [] rms = new double[5]; double [] rms = new double[5];
double [] quat = new double[4]; double [] quat = new double[4];
int quat_lma_mode = 3; // 2; // 1; int quat_lma_mode = 4; // 3; // 2; // 1;
int debug_lev = 3; int debug_lev = 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0); double translation_weight = 1.0 / (avg_z + 1.0);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 598a2074
...@@ -266,7 +266,7 @@ public class QuadCLTCPU { ...@@ -266,7 +266,7 @@ public class QuadCLTCPU {
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25; double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma(); QuaternionLma quaternionLma = new QuaternionLma();
if (quat_lma_mode == 2) { if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
for (int nscene = early_index; nscene <= last_index; nscene++) { for (int nscene = early_index; nscene <= last_index; nscene++) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 598a2074
...@@ -289,6 +289,19 @@ public class QuaternionLma { ...@@ -289,6 +289,19 @@ public class QuaternionLma {
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1 double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0, double [] quat0,
final int debug_level) { final int debug_level) {
if (mode != 2) {
prepareLMAMode4(
mode, // int mode,
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
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);
return;
}
N = vect_x.length; N = vect_x.length;
this.mode = mode; // 2; this.mode = mode; // 2;
samples = 4; samples = 4;
...@@ -349,307 +362,88 @@ public class QuaternionLma { ...@@ -349,307 +362,88 @@ public class QuaternionLma {
x_vector); // double [] data) x_vector); // double [] data)
} }
} }
private double [] getFxDerivsOld(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
switch (mode) {
case 1:return getFxDerivs6DofOld(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 2:return getFxDerivsVisualOld( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
double [] fx = new double [weights.length];
final double q0 = vector[0];
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
jt[i][3 * N] = 2 * vector[i];
}
}
fx[3 * N] = q0*q0 + q1*q1 + q2 * q2 + q3*q3;
for (int i = 0; i < N; i++) {
int i3 = 3 * i;
final double x = x_vector[i3 + 0];
final double y = x_vector[i3 + 1];
final double z = x_vector[i3 + 2];
final double s = q1 * x + q2 * y + q3 * z;
fx[i3 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
fx[i3 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
fx[i3 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
if (jt != null) {
jt[0][i3 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;
jt[1][i3 + 0] = 2*s + 2*q1*x;
jt[2][i3 + 0] = 2*z*q0 + 2*q1*y;
jt[3][i3 + 0] = 2*y*q0 + 2*q1*z;
jt[0][i3 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1; public void prepareLMAMode4(
jt[1][i3 + 1] = 2*z*q0 + 2*x*q2; int mode,
jt[2][i3 + 1] = 2*s + 2*y*q2; double avg_height, //
jt[3][i3 + 1] =-2*x*q0+ 2*z*q2; double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
jt[0][i3 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2; double [] vect_w, // one per scene
jt[1][i3 + 2] =-2*y*q0 + 2*x*q3; double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
jt[2][i3 + 2] = 2*x*q0 + 2*y*q3; double [] quat0,
jt[3][i3 + 2] = 2*s + 2*z*q3; final int debug_level) {
}
} N = vect_x.length;
return fx; this.mode = mode; // 2;
} samples = 4;
private double [] getFxDerivs6DofOld( samples_x = 7;
double [] vector, height = avg_height;
final double [][] jt, // should be null or initialized with [vector.length][] pure_weight = 1.0 - reg_w;
final int debug_level) { x_vector = new double [samples_x * N];
double [] fx = new double [weights.length]; y_vector = new double [samples * N + REGLEN];
final double q0 = vector[0]; y_inv_vector = new double [samples_x * N + REGLEN];
final double q1 = vector[1]; weights = new double [samples * N + REGLEN];
final double q2 = vector[2]; parameters_vector = quat0.clone();
final double q3 = vector[3]; double sw = 0;
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;
for (int i = 0; i < N; i++) {
int i7 = samples * i;
// translations
final double x = x_vector[i7 + 0];
final double y = x_vector[i7 + 1];
final double z = x_vector[i7 + 2];
final double s = q1 * x + q2 * y + q3 * z;
fx[i7 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
fx[i7 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
fx[i7 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
if (jt != null) {
jt[0][i7 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;
jt[1][i7 + 0] = 2*s + 2*q1*x;
jt[2][i7 + 0] = 2*z*q0 + 2*q1*y;
jt[3][i7 + 0] = 2*y*q0 + 2*q1*z;
jt[0][i7 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1;
jt[1][i7 + 1] = 2*z*q0 + 2*x*q2;
jt[2][i7 + 1] = 2*s + 2*y*q2;
jt[3][i7 + 1] =-2*x*q0+ 2*z*q2;
jt[0][i7 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;
jt[1][i7 + 2] =-2*y*q0 + 2*x*q3;
jt[2][i7 + 2] = 2*x*q0 + 2*y*q3;
jt[3][i7 + 2] = 2*s + 2*z*q3;
}
// rotations
final double r0 = x_vector[i7 + 3];
final double r1 = x_vector[i7 + 4];
final double r2 = x_vector[i7 + 5];
final double r3 = x_vector[i7 + 6];
fx[i7 + 3] = r0 * q0 - (r1 * q1 + r2 * q2 + r3 * q3);
fx[i7 + 4] = r1 * q0 + r0 * q1 + (r2 * q3 - r3 * q2);
fx[i7 + 5] = r2 * q0 + r0 * q2 + (r3 * q1 - r1 * q3);
fx[i7 + 6] = r3 * q0 + r0 * q3 + (r1 * q2 - r2 * q1);
if (jt != null) {
jt[0][i7 + 3] = r0;
jt[1][i7 + 3] = -r1;
jt[2][i7 + 3] = -r2;
jt[3][i7 + 3] = -r3;
jt[0][i7 + 4] = r1;
jt[1][i7 + 4] = r0;
jt[2][i7 + 4] = -r3;
jt[3][i7 + 4] = r2;
jt[0][i7 + 5] = r2;
jt[1][i7 + 5] = r3;
jt[2][i7 + 5] = r0;
jt[3][i7 + 5] = -r1;
jt[0][i7 + 6] = r3;
jt[1][i7 + 6] = -r2;
jt[2][i7 + 6] = r1;
jt[3][i7 + 6] = r0;
}
}
return fx;
}
private double [] getFxDerivsVisualOld(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
final double q0 = vector[0];
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
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;
for (int i = 0; i < N; i++) { for (int i = 0; i < N; i++) {
int i4 = samples * i; if ((vect_x[i]== null) || (vect_y[i]== null)) {
int i7 = samples_x * i; for (int j = 0; j < samples; j++) {
// translations weights [samples * i + j] = 0.0;
final double x = x_vector[i7 + 0]; }
final double y = x_vector[i7 + 1]; for (int j = 0; j < samples_x; j++) {
final double z = x_vector[i7 + 2]; x_vector [samples_x * i + j] = 0.0;
final double s = q1 * x + q2 * y + q3 * z; y_inv_vector[samples_x * i + j] = 0.0;
final double fx_x = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; }
final double fx_y = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
final double fx_z = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z; } else {
// rotations // mode=4
final double r0 = x_vector[i7 + 3]; double w = ((vect_w != null)? vect_w[i] : 1.0);
final double r1 = x_vector[i7 + 4]; // quaternions
final double r2 = x_vector[i7 + 5]; Rotation rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
final double r3 = x_vector[i7 + 6]; vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
/* // unused Rotation rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
final double fx_q0 = r0 * q0 - (r1 * q1 + r2 * q2 + r3 * q3); vect_y[i][1][0], vect_y[i][1][1], vect_y[i][1][2]);
*/ x_vector[samples_x * i + 0] = vect_x[i][0][0]; // X
final double fx_q1 = r1 * q0 + r0 * q1 + (r2 * q3 - r3 * q2); x_vector[samples_x * i + 1] = vect_x[i][0][1]; // Y
final double fx_q2 = r2 * q0 + r0 * q2 + (r3 * q1 - r1 * q3); x_vector[samples_x * i + 2] = vect_x[i][0][2]; // Z
final double fx_q3 = r3 * q0 + r0 * q3 + (r1 * q2 - r2 * q1); x_vector[samples_x * i + 3] = rot_x.getQ0(); // Q0
// combined samples x_vector[samples_x * i + 4] = rot_x.getQ1(); // Q1
fx[i4 + 0] = fx_z / height; // Z x_vector[samples_x * i + 5] = rot_x.getQ2(); // Q2
fx[i4 + 1] = 2 * fx_q3; // 2 * Q3 x_vector[samples_x * i + 6] = rot_x.getQ3(); // Q3
fx[i4 + 2] = 2 * fx_q2 - fx_x / height; // 2 * Q2 - X / height double [] xyz_y = new double [] {
fx[i4 + 3] = 2 * fx_q1 + fx_y / height; // 2 * Q1 + Y / height vect_y[i][0][0],
vect_y[i][0][1],
if (jt != null) { vect_y[i][0][2]};
final double jt_0_x = 4*x*q0 - 2*z*q2 + 2*y*q3; double [] quat_y = new double [] {
final double jt_1_x = 2*s + 2*q1*x; rot_y.getQ0(),
final double jt_2_x = 2*z*q0 + 2*q1*y; rot_y.getQ1(),
final double jt_3_x = 2*y*q0 + 2*q1*z; rot_y.getQ2(),
rot_y.getQ3()};
final double jt_0_y = 4*y*q0 - 2*x*q3 + 2*z*q1; double [][] inv_y = invertTransRot(
final double jt_1_y = 2*z*q0 + 2*x*q2; xyz_y, // double [] xyz_src, // transformation to apply to (was reference_xyz)
final double jt_2_y = 2*s + 2*y*q2; quat_y); // double [] quat_src);
final double jt_3_y =-2*x*q0+ 2*z*q2; System.arraycopy(inv_y[0], 0, y_inv_vector, samples_x * i, 3);
System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3, 4);
final double jt_0_z = 4*z*q0 - 2*y*q1 + 2*x*q2; // y_vector remains all 0
final double jt_1_z =-2*y*q0 + 2*x*q3;
final double jt_2_z = 2*x*q0 + 2*y*q3; for (int j = 0; j < samples; j++) {
final double jt_3_z = 2*s + 2*z*q3; weights[samples * i + j] = w;
/* // unused sw += w;
final double jt_0_q0 = r0; }
final double jt_1_q0 = -r1;
final double jt_2_q0 = -r2;
final double jt_3_q0 = -r3;
*/
final double jt_0_q1 = r1;
final double jt_1_q1 = r0;
final double jt_2_q1 = -r3;
final double jt_3_q1 = r2;
final double jt_0_q2 = r2;
final double jt_1_q2 = r3;
final double jt_2_q2 = r0;
final double jt_3_q2 = -r1;
final double jt_0_q3 = r3;
final double jt_1_q3 = -r2;
final double jt_2_q3 = r1;
final double jt_3_q3 = r0;
// Z
jt[0][i4 + 0] = jt_0_z / height;
jt[1][i4 + 0] = jt_1_z / height;
jt[2][i4 + 0] = jt_2_z / height;
jt[3][i4 + 0] = jt_3_z / height;
// 2 * Q3
jt[0][i4 + 1] = 2 * jt_0_q3;
jt[1][i4 + 1] = 2 * jt_1_q3;
jt[2][i4 + 1] = 2 * jt_2_q3;
jt[3][i4 + 1] = 2 * jt_3_q3;
// 2 * Q2 - X / height
jt[0][i4 + 2] = 2 * jt_0_q2 - jt_0_x / height;
jt[1][i4 + 2] = 2 * jt_1_q2 - jt_1_x / height;
jt[2][i4 + 2] = 2 * jt_2_q2 - jt_2_x / height;
jt[3][i4 + 2] = 2 * jt_3_q2 - jt_3_x / height;
// 2 * Q1 + Y / height
jt[0][i4 + 3] = 2 * jt_0_q1 + jt_0_y / height;
jt[1][i4 + 3] = 2 * jt_1_q1 + jt_1_y / height;
jt[2][i4 + 3] = 2 * jt_2_q1 + jt_2_y / height;
jt[3][i4 + 3] = 2 * jt_3_q1 + jt_3_y / height;
}
}
return fx;
}
private double [] getFxDerivsVisualWrong(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
final double q0 = vector[0];
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
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; double k = (pure_weight)/sw;
double [] xyz_rot; for (int i = 0; i < weights.length; i++) weights[i] *= k;
double [] quat_rot; weights [samples * N] = 1.0 - pure_weight;
double [][] xyz_dq; y_vector[samples * N] = 1.0;
double [][] quat_dq; last_jt = new double [parameters_vector.length][];
if (debug_level > 0) {
for (int i = 0; i < N; i++) { debugYfX ( "Y-INV-", // String pfx,
int i4 = samples * i; y_inv_vector); // double [] data)
int i7 = samples_x * i; debugYfX ( "PIMU-", // String pfx,
// translations x_vector); // double [] data)
final double x = x_vector[i7 + 0];
final double y = x_vector[i7 + 1];
final double z = x_vector[i7 + 2];
final double s = q1 * x + q2 * y + q3 * z;
final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};
xyz_rot = applyTo(vector, xyz);
final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
quat_rot = compose(vector, quat_r);
// combined samples
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
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
quat_dq = composeDQ(quat_r);
// 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;
// 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;
// 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;
}
} }
return fx;
} }
// TODO: Consider adding differences between x and y for regularization (or it won't work) // TODO: Consider adding differences between x and y for regularization (or it won't work)
// goal - to minimize "unneeded" rotation along the common axis // goal - to minimize "unneeded" rotation along the common axis
...@@ -670,7 +464,12 @@ public class QuaternionLma { ...@@ -670,7 +464,12 @@ public class QuaternionLma {
vector, // double [] vector, vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][] jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level) debug_level); // final int debug_level)
case 4:return getFxDerivsVisualMode4( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
} }
// remains here for mode 0
double [] fx = new double [weights.length]; double [] fx = new double [weights.length];
final double q0 = vector[0]; final double q0 = vector[0];
final double q1 = vector[1]; final double q1 = vector[1];
...@@ -820,20 +619,23 @@ public class QuaternionLma { ...@@ -820,20 +619,23 @@ public class QuaternionLma {
for (int j = 0; j < 4; j++) { for (int j = 0; j < 4; j++) {
System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3); System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);
} }
quat_dq = composeQR_QdQ(vector_r,quat_r); quat_dq = composeQR_QdQ(vector_r,quat_r);
double [][] invy_mat = qMat(inv_y[1]); // 2 alternative ways with the same result
double [][] quat_dq_local = mulMat(invy_mat, quat_dq); if (debug_level < 1000) {
double [][] invy_mat = qMat(inv_y[1]);
double [][]dcomp_dsecond = composeDR(inv_y[1]); double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
double [][] quat_dq_local1 = new double [4][]; for (int j = 0; j < 4; j++) {
for (int j = 0; j < 4; j++) { System.arraycopy(quat_dq_local[j], 0, jt[j], i7+3, 4);
// quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]); }
quat_dq_local1[j] = mulMat(invy_mat, quat_dq[j]); } else {
} double [][] dcomp_dsecond = composeDR(inv_y[1]);
double [][] quat_dq_local1 = new double [4][];
for (int j = 0; j < 4; j++) { for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4); quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
}
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4);
}
} }
} }
} }
...@@ -910,7 +712,6 @@ public class QuaternionLma { ...@@ -910,7 +712,6 @@ public class QuaternionLma {
final double q1 = vector[1]; final double q1 = vector[1];
final double q2 = vector[2]; final double q2 = vector[2];
final double q3 = vector[3]; final double q3 = vector[3];
/// final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
final double [] vector_r = normSign(vector); final double [] vector_r = normSign(vector);
if (jt != null) { if (jt != null) {
for (int i = 0; i < vector.length; i++) { for (int i = 0; i < vector.length; i++) {
...@@ -977,6 +778,94 @@ public class QuaternionLma { ...@@ -977,6 +778,94 @@ public class QuaternionLma {
return fx; return fx;
} }
private double [] getFxDerivsVisualMode4(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
final double q0 = vector[0];
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
final double [] vector_r = normSign(vector);
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;
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
double [][] quat_dq;
double [][] inv_y = new double [][] {new double[3],new double[4]};
for (int i = 0; i < N; i++) {
int i4 = samples * i;
int i7 = samples_x * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i4+j] > 0) {
break has_data;
}
}
continue; // nothing to process for this scene
}
// translations
final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};
// rotations
final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
xyz_rot = applyTo(vector, xyz);
quat_rot = composeQR_Q(vector_r, quat_r);
System.arraycopy(y_inv_vector, i7, inv_y[0], 0, 3);
System.arraycopy(y_inv_vector, i7+3, inv_y[1], 0, 4);
double [][] comb_y = combineTransRot( //
inv_y[0], // double [] xyz_src, // transformation to apply to (was reference_xyz)
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 + 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
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
double [][] xyz_dq_local = new double [xyz_dq.length][];
for (int j = 0; j < xyz_dq.length; j++) {
xyz_dq_local[j] =combineTransRot(
null, // double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y[1], // double [] quat_src, // transformation to apply to (was reference_atr)
xyz_dq[j], // double [] xyz_target, // to which is applied (was scene_xyz)
null)[0]; // double [] quat_target // to which is applied (was scene_atr)
}
quat_dq = composeQR_QdQ(vector_r,quat_r);
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;
// 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;
// 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;
}
}
return fx;
}
private double [] getYminusFxWeighted( private double [] getYminusFxWeighted(
...@@ -1339,7 +1228,7 @@ public class QuaternionLma { ...@@ -1339,7 +1228,7 @@ public class QuaternionLma {
angles[0],angles[1],angles[2])); angles[0],angles[1],angles[2]));
} }
System.out.println(); System.out.println();
} else if (mode == 2) { } else { // if (mode == 2) {
System.out.println(String.format("%3s"+ System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s", // Z, 2*Q3, 2*Q2-X, 2*Q1+Y "\t%9s\t%9s\t%9s\t%9s", // Z, 2*Q3, 2*Q2-X, 2*Q1+Y
"N",pfx+"Z",pfx+"2*Q3",pfx+"2*Q2-X",pfx+"2*Q1+Y")); "N",pfx+"Z",pfx+"2*Q3",pfx+"2*Q2-X",pfx+"2*Q1+Y"));
...@@ -1550,7 +1439,9 @@ public class QuaternionLma { ...@@ -1550,7 +1439,9 @@ public class QuaternionLma {
/** /**
* Get derivatives of the composed quaternion (compose(q,r)) by the * Get derivatives of the composed quaternion (compose(q,r)) by the
* components of the second one (r). These derivatives do not depend * components of the second one (r). These derivatives do not depend
* on the second quaternion, so it is not in the input. * on the second quaternion, so it is not in the input.
* Also can be used to convert quaternion to a matrix for post-multiplying
* derivatives.
* @param q 4 components (scalar, vector) of the quaternion being * @param q 4 components (scalar, vector) of the quaternion being
* applied to the second quaternion. * applied to the second quaternion.
* @return 4x4 array, where columns correspond to composition components * @return 4x4 array, where columns correspond to composition components
...@@ -1559,19 +1450,18 @@ public class QuaternionLma { ...@@ -1559,19 +1450,18 @@ public class QuaternionLma {
*/ */
public static double [][] composeDR( // not used public static double [][] composeDR( // not used
double [] q) { double [] q) {
/*
return new double [][] { return new double [][] {
{ q[0], -q[1], -q[2], -q[3]}, { q[0], -q[1], -q[2], -q[3]},
{ q[1], q[0], q[3], -q[2]}, { q[1], q[0], q[3], -q[2]},
{ q[2], -q[3], q[0], q[1]}, { q[2], -q[3], q[0], q[1]},
{ q[3], q[2], -q[1], q[0]}}; { q[3], q[2], -q[1], q[0]}};
*/ /*
return new double [][] { return new double [][] {
{ q[0], q[1], q[2], q[3]}, { q[0], q[1], q[2], q[3]},
{-q[1], q[0],-q[3], q[2]}, {-q[1], q[0],-q[3], q[2]},
{-q[2], q[3], q[0],-q[1]}, {-q[2], q[3], q[0],-q[1]},
{-q[3],-q[2], q[1], q[0]}}; {-q[3],-q[2], q[1], q[0]}};
*/
} }
public static double [] addTo( public static double [] addTo(
...@@ -1662,11 +1552,29 @@ public class QuaternionLma { ...@@ -1662,11 +1552,29 @@ public class QuaternionLma {
public static double [][] qMat( public static double [][] qMat(
double [] q){ double [] q){
return new double [][] {
{ q[0], q[1], q[2], q[3]},
{-q[1], q[0],-q[3], q[2]},
{-q[2], q[3], q[0],-q[1]},
{-q[3],-q[2], q[1], q[0]}};
/*
return new double [][] { return new double [][] {
{q[0],-q[1],-q[2],-q[3]}, {q[0],-q[1],-q[2],-q[3]},
{q[1], q[0], q[3],-q[2]}, {q[1], q[0], q[3],-q[2]},
{q[2],-q[3], q[0], q[1]}, {q[2],-q[3], q[0], q[1]},
{q[3], q[2],-q[1], q[0]}}; {q[3], q[2],-q[1], q[0]}};
*/
}
public static double [][] transpose(
double [][] mat){
double [][] tmat = new double[mat[0].length][mat.length];
for (int i = 0; i < mat.length; i++) {
for (int j = 0; j < mat[0].length; j++) {
tmat[j][i] = mat[i][j];
}
}
return tmat;
} }
} }
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