Commit 3f84a242 authored by Andrey Filippov's avatar Andrey Filippov

Debugged QuaternionLma.MODE_COMBO_LOCAL

parent 3a3ced75
......@@ -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(
// 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){
if ((quat_lma_mode == quaternionLma.MODE_COMBO) || (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
} 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
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);
......
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