Skip to content

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

Debugged QuaternionLma.MODE_COMBO_LOCAL

parent 3a3ced75
Hide whitespace changes
Inline Side-by-side
Showing with 217 additions and 40 deletions
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 3f84a242
......@@ -257,6 +257,7 @@ public class QuadCLTCPU {
double [] rms, // null or double[5];
int debugLevel
) {
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;
......@@ -276,20 +277,44 @@ public class QuadCLTCPU {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
}
double [][][] 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){
if ((quat_lma_mode == quaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
// Is it needed now (below)? It scalesvect_y to have the same length as vect_x
double [][][] vect_y_scaled;
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}}
new int [] {early_index, last_index}); // int [] first_last){
} 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,
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)
quat0, // double [] quat0,
debugLevel); // int debug_level)
double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
if (mn_mx_diag[0]*quat_max_ratio*quat_max_ratio < mn_mx_diag[1]) {
double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
if (debugLevel > -1) {
System.out.println("==== Calculated reg_w = "+reg_w);
}
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)) {
......@@ -320,14 +345,6 @@ public class QuadCLTCPU {
debugLevel); // int debug_level)
}
} else {
/*
double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene][0];
vect_y[nscene] = xyzatr[nscene][0];
}
*/
quaternionLma.prepareLMA(
vect_x, // quat_lma_xyz, // double [][] vect_x,
vect_y_scaled, // vect_y, // double [][][] vect_y,
......@@ -493,7 +510,8 @@ public class QuadCLTCPU {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
// int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 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_weight = 1.0 / (avg_z + 1.0);
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 3f84a242
......@@ -479,7 +479,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 reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double reg_w, // regularization weight [0..1)
double [] quat0,
final int debug_level) {
if (mode != MODE_COMBO) {
......@@ -501,9 +501,10 @@ public class QuaternionLma {
samples_x = 7;
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];
y_vector = new double [samples * N + REGLEN];
weights = new double [samples * N + REGLEN];
y_vector = new double [samples * N + extra_samples];
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
double sw = 0;
for (int i = 0; i < N; i++) {
......@@ -545,8 +546,15 @@ public class QuaternionLma {
}
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;
if (extra_samples > 0) {
double w = (1.0 - pure_weight)/parameters_vector.length;
for (int i = 0; i < parameters_vector.length; i++) {
weights [samples * N + i] = w;
y_vector[samples * N] = 0.0; // or target value
}
}
// 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 ( "", // String pfx,
......@@ -556,7 +564,7 @@ public class QuaternionLma {
}
}
public void prepareLMAMode4(
public void prepareLMAMode4( // MODE_COMBO_LOCAL
int mode,
double avg_height, //
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
......@@ -572,10 +580,11 @@ public class QuaternionLma {
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];
int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples_x * N];
y_vector = new double [samples * N + extra_samples];
y_inv_vector = new double [samples_x * N + extra_samples];
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
double sw = 0;
for (int i = 0; i < N; i++) {
......@@ -627,8 +636,15 @@ public class QuaternionLma {
}
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;
if (extra_samples > 0) {
double w = (1.0 - pure_weight)/parameters_vector.length;
for (int i = 0; i < parameters_vector.length; i++) {
weights [samples * N + i] = w;
y_vector[samples * N] = 0.0; // or target value
}
}
// 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,
......@@ -640,6 +656,7 @@ public class QuaternionLma {
// TODO: Consider adding differences between x and y for regularization (or it won't work)
// goal - to minimize "unneeded" rotation along the common axis
// updated for MODE_XYZQ/3, MODE_COMBO_LOCAL/3 ( /3 - only 3 quaternion components
private double [] getFxDerivs(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
......@@ -677,11 +694,18 @@ public class QuaternionLma {
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
case MODE_COMBO_LOCAL:
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)
case MODE_COMBO_LOCAL:
if (parameters_vector.length < 4) { // updated for [3]
return getFxDerivsVisualMode43( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
} else {
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)
}
case MODE_COMPASS:return getFxDerivsCompass( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
......@@ -1307,23 +1331,23 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivsVisualMode4(
private double [] getFxDerivsVisualMode4( // seems rotation is opposite sign
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 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];
// jt[i][samples * N] = 2 * vector[i];
}
}
fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
// fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
......@@ -1393,9 +1417,144 @@ public class QuaternionLma {
jt[3][i4 + 3] = 2 * quat_dq_local[3][1] + xyz_dq_local[3][1] / height;
}
}
if (weights.length > N*samples) {
for (int i = 0; i < vector.length; i++) {
fx[samples*N + i] = vector[i];
if (jt != null) {
jt[i][samples*N + i] = 1.0;
}
}
}
return fx;
}
private double [] getFxDerivsVisualMode43(
double [] vector3,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
double [] fx = new double [weights.length];
double [] qn = new double[4];
final double [] vector_r = normSign(vector).clone(); // should be already q0>0
qNorm (vector_r, qn); // calculates qn // normalized
final double [][] dQn_dQ123 = dQ_dQ123(vector3);
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
jt[i] = new double [weights.length];
}
}
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
/*
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, false); // true);
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
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;
*/
// 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;
//dQn_dQ123
double [][] d_dq = mulMat(dQn_dQ123, d_dQn);
for (int j = 0; j < d_dq.length; j++) { // 3
System.arraycopy(d_dq[j], 0, jt[j], i4, samples);
}
}
}
if (weights.length > N*samples) {
for (int i = 0; i < vector3.length; i++) {
fx[samples*N + i] = vector3[i];
if (jt != null) {
jt[i][samples*N + i] = 1.0;
}
}
}
return fx;
}
private double [] getYminusFxWeighted(
final double [] fx,
......
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