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
Show 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 {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 3; // 2; // 1;
int quat_lma_mode = 4; // 3; // 2; // 1;
int debug_lev = 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
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 {
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
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_x = new double [quadCLTs.length][][]; // IMS XYZATR
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 {
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
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;
this.mode = mode; // 2;
samples = 4;
......@@ -350,307 +363,88 @@ public class QuaternionLma {
}
}
private double [] getFxDerivsOld(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
public void prepareLMAMode4(
int mode,
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) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
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;
jt[1][i3 + 1] = 2*z*q0 + 2*x*q2;
jt[2][i3 + 1] = 2*s + 2*y*q2;
jt[3][i3 + 1] =-2*x*q0+ 2*z*q2;
jt[0][i3 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;
jt[1][i3 + 2] =-2*y*q0 + 2*x*q3;
jt[2][i3 + 2] = 2*x*q0 + 2*y*q3;
jt[3][i3 + 2] = 2*s + 2*z*q3;
}
}
return fx;
}
private double [] getFxDerivs6DofOld(
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;
N = vect_x.length;
this.mode = mode; // 2;
samples = 4;
samples_x = 7;
height = avg_height;
pure_weight = 1.0 - reg_w;
x_vector = new double [samples_x * N];
y_vector = new double [samples * N + REGLEN];
y_inv_vector = new double [samples_x * N + REGLEN];
weights = new double [samples * N + REGLEN];
parameters_vector = quat0.clone();
double sw = 0;
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];
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
weights [samples * i + j] = 0.0;
}
for (int j = 0; j < samples_x; j++) {
x_vector [samples_x * i + j] = 0.0;
y_inv_vector[samples_x * i + j] = 0.0;
}
fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
for (int i = 0; i < N; i++) {
int i4 = samples * i;
int i7 = samples_x * 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;
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;
// 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];
/* // unused
final double fx_q0 = r0 * q0 - (r1 * q1 + r2 * q2 + r3 * q3);
*/
final double fx_q1 = r1 * q0 + r0 * q1 + (r2 * q3 - r3 * q2);
final double fx_q2 = r2 * q0 + r0 * q2 + (r3 * q1 - r1 * q3);
final double fx_q3 = r3 * q0 + r0 * q3 + (r1 * q2 - r2 * q1);
// combined samples
fx[i4 + 0] = fx_z / height; // Z
fx[i4 + 1] = 2 * fx_q3; // 2 * Q3
fx[i4 + 2] = 2 * fx_q2 - fx_x / height; // 2 * Q2 - X / height
fx[i4 + 3] = 2 * fx_q1 + fx_y / height; // 2 * Q1 + Y / height
if (jt != null) {
final double jt_0_x = 4*x*q0 - 2*z*q2 + 2*y*q3;
final double jt_1_x = 2*s + 2*q1*x;
final double jt_2_x = 2*z*q0 + 2*q1*y;
final double jt_3_x = 2*y*q0 + 2*q1*z;
final double jt_0_y = 4*y*q0 - 2*x*q3 + 2*z*q1;
final double jt_1_y = 2*z*q0 + 2*x*q2;
final double jt_2_y = 2*s + 2*y*q2;
final double jt_3_y =-2*x*q0+ 2*z*q2;
final double jt_0_z = 4*z*q0 - 2*y*q1 + 2*x*q2;
final double jt_1_z =-2*y*q0 + 2*x*q3;
final double jt_2_z = 2*x*q0 + 2*y*q3;
final double jt_3_z = 2*s + 2*z*q3;
/* // unused
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];
} else {
// mode=4
double w = ((vect_w != null)? vect_w[i] : 1.0);
// quaternions
Rotation rot_x = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
vect_x[i][1][0], vect_x[i][1][1], vect_x[i][1][2]);
Rotation rot_y = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
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
x_vector[samples_x * i + 1] = vect_x[i][0][1]; // Y
x_vector[samples_x * i + 2] = vect_x[i][0][2]; // Z
x_vector[samples_x * i + 3] = rot_x.getQ0(); // Q0
x_vector[samples_x * i + 4] = rot_x.getQ1(); // Q1
x_vector[samples_x * i + 5] = rot_x.getQ2(); // Q2
x_vector[samples_x * i + 6] = rot_x.getQ3(); // Q3
double [] xyz_y = new double [] {
vect_y[i][0][0],
vect_y[i][0][1],
vect_y[i][0][2]};
double [] quat_y = new double [] {
rot_y.getQ0(),
rot_y.getQ1(),
rot_y.getQ2(),
rot_y.getQ3()};
double [][] inv_y = invertTransRot(
xyz_y, // double [] xyz_src, // transformation to apply to (was reference_xyz)
quat_y); // double [] quat_src);
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);
// y_vector remains all 0
for (int j = 0; j < samples; j++) {
weights[samples * i + j] = w;
sw += w;
}
}
fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
double [][] quat_dq;
for (int i = 0; i < N; i++) {
int i4 = samples * i;
int i7 = samples_x * 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;
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;
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights [samples * N] = 1.0 - pure_weight;
y_vector[samples * N] = 1.0;
last_jt = new double [parameters_vector.length][];
if (debug_level > 0) {
debugYfX ( "Y-INV-", // String pfx,
y_inv_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
return fx;
}
// TODO: Consider adding differences between x and y for regularization (or it won't work)
// goal - to minimize "unneeded" rotation along the common axis
private double [] getFxDerivs(
......@@ -670,7 +464,12 @@ public class QuaternionLma {
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
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];
final double q0 = vector[0];
final double q1 = vector[1];
......@@ -820,23 +619,26 @@ public class QuaternionLma {
for (int j = 0; j < 4; j++) {
System.arraycopy(xyz_dq_local[j], 0, jt[j], i7, 3);
}
quat_dq = composeQR_QdQ(vector_r,quat_r);
// 2 alternative ways with the same result
if (debug_level < 1000) {
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(invy_mat, quat_dq);
double [][]dcomp_dsecond = composeDR(inv_y[1]);
double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq_local[j], 0, jt[j], i7+3, 4);
}
} else {
double [][] dcomp_dsecond = composeDR(inv_y[1]);
double [][] quat_dq_local1 = new double [4][];
for (int j = 0; j < 4; j++) {
// quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
quat_dq_local1[j] = mulMat(invy_mat, quat_dq[j]);
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);
}
}
}
}
return fx;
}
......@@ -910,7 +712,6 @@ 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}); // seems better with reversal
final double [] vector_r = normSign(vector);
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
......@@ -977,6 +778,94 @@ public class QuaternionLma {
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(
......@@ -1339,7 +1228,7 @@ public class QuaternionLma {
angles[0],angles[1],angles[2]));
}
System.out.println();
} else if (mode == 2) {
} else { // if (mode == 2) {
System.out.println(String.format("%3s"+
"\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"));
......@@ -1551,6 +1440,8 @@ public class QuaternionLma {
* Get derivatives of the composed quaternion (compose(q,r)) by the
* components of the second one (r). These derivatives do not depend
* 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
* applied to the second quaternion.
* @return 4x4 array, where columns correspond to composition components
......@@ -1559,19 +1450,18 @@ public class QuaternionLma {
*/
public static double [][] composeDR( // not used
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 [][] {
{ 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]}};
*/
}
public static double [] addTo(
......@@ -1662,11 +1552,29 @@ public class QuaternionLma {
public static double [][] qMat(
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 [][] {
{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]}};
*/
}
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