Skip to content

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

working snapshot (working on IMU-to-camera rotation calibration)

parent 77381e24
Hide whitespace changes
Inline Side-by-side
Showing with 642 additions and 196 deletions
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ 76f96ef8
......@@ -249,7 +249,7 @@ public class Imx5 {
* Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
* @param ims_atr original ATR of the IMS mount
* @param corr_q correction quaternion
* @return corrected ATR of teh IMU mount
* @return corrected ATR of the IMU mount
*/
public static double [] adjustMountAtrByQuat(
double [] ims_atr,
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 76f96ef8
......@@ -5181,7 +5181,7 @@ public class Interscene {
debugLevel); // int debugLevel
if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
......@@ -5476,20 +5476,20 @@ public class Interscene {
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][3];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][3];
double [][][] quat_lma_xyzatr = new double [quadCLTs.length][2][3];
double [][][] quat_lma_enu_xyzatr = new double [quadCLTs.length][2][3];
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
quat_lma_xyz[nscene] = new double[3];
quat_lma_xyzatr[nscene][0] = new double[3];
} else {
String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
quat_lma_xyzatr[nscene][0] = ers_reference.getSceneXYZ(ts);
}
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
quat_lma_enu_xyzatr[nscene][0] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
......@@ -5508,8 +5508,8 @@ public class Interscene {
int debug_level = 1;
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y,
quat_lma_enu_xyzatr, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyzatr, // 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,
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 76f96ef8
......@@ -5425,68 +5425,13 @@ public class OpticalFlow {
}
// later move to the right place
if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
last_index //(quadCLTs.length -1) // int last_index,
);
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 03; // 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);
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_index, // int last_index,
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
if (debugLevel > -3) {
System.out.println("Applying correction ot the IMS mount orientation:");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
}
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)
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
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;
System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println ("*** Need to save the main configuration file ***");
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
QuadCLT.adjustImuOrient(
clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
last_index, // int last_index,
debugLevel); // int debugLevel
}
if (run_ly) {
if (debugLevel > -3) {
......@@ -5602,7 +5547,7 @@ public class OpticalFlow {
debugLevel+1);
if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 76f96ef8
......@@ -245,6 +245,7 @@ public class QuadCLTCPU {
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[] quadCLTs,
......@@ -256,7 +257,7 @@ public class QuadCLTCPU {
double [] rms, // null or double[5];
int debugLevel
) {
final boolean use3 = (quat_lma_mode == 3); // true;
// 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;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
......@@ -267,53 +268,54 @@ public class QuadCLTCPU {
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
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++) {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
}
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++) {
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)) {
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
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;
quat0, // double [] quat0,
debugLevel); // int debug_level)
} else if ((quat_lma_mode == 1) || (quat_lma_mode == 3)) {
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++) {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
}
} 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,
vect_x, // double [][][] vect_x,
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;
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)
} 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, // 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)
vect_x, // quat_lma_xyz, // 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)
}
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
......@@ -360,9 +362,12 @@ public class QuadCLTCPU {
double [] quaternion, // null or double[4]
int debugLevel
) {
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,
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,
quadCLTs, // QuadCLT[] quadCLTs,
......@@ -390,10 +395,19 @@ public class QuadCLTCPU {
Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]);
// Rotation rotation_atr = rot.applyTo(scene_atr);
Rotation rotation_atr = rot.applyTo(ims_atr);
Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
// Rotation rotation_atr = rot.applyTo(ims_atr);
// Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
Rotation rotation_atr,rotation_atr2;
if (use_inv) {
rotation_atr = rot_invert.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
} else {
rotation_atr = rot.applyTo(ims_atr);
rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
}
rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
/// rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
if (debugLevel > -1) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
......@@ -427,6 +441,78 @@ public class QuadCLTCPU {
return rotated_xyzatr;
}
public static void adjustImuOrient(
CLTParameters clt_parameters, // CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
int ref_index,
int earliest_scene,
int last_index,
int debugLevel
) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
last_index //(quadCLTs.length -1) // int last_index,
);
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
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 debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_index, // int last_index,
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
if (debugLevel > -3) {
System.out.println("Applying correction to the IMS mount orientation:");
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;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("scale="+quat_scale);
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
}
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)
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
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;
System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println ("*** Need to save the main configuration file ***");
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
}
/**
* Refine scene poses (now only linear) from currently adjusted poses
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 76f96ef8
......@@ -40,6 +40,8 @@ public class QuaternionLma {
public static final int MODE_XYZQ_LOCAL = 3;
public static final int MODE_COMBO_LOCAL = 4;
public static final int MODE_COMPASS = 5;
public static final int MODE_XYZ4Q3 = 6; // Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
// public static final int MODE_XYZ3 = 6;
// public static final int MODE_XYZQ3 = 7;
// public static final int MODE_COMBO3 = 8;
......@@ -144,12 +146,12 @@ public class QuaternionLma {
}
public void prepareLMA(
double [][] vect_x,
double [][] vect_y,
double [][] vect_w,
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
double [][][] vect_x,
double [][][] vect_y,
double [][] vect_w,
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
N = vect_x.length;
pure_weight = 1.0 - reg_w;
mode = MODE_XYZ;
......@@ -169,8 +171,8 @@ public class QuaternionLma {
}
} else {
for (int j = 0; j < 3; j++) {
x_vector[samples * i + j] = vect_x[i][j];
y_vector[samples * i + j] = vect_y[i][j];
x_vector[samples * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
double w = (vect_w != null)? vect_w[i][j] : 1.0;
weights[samples*i + j] = w;
sw += w;
......@@ -195,6 +197,18 @@ 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 == MODE_XYZ4Q3) {
prepareLMA43(
mode, // int mode,
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;
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;
}
if (mode != MODE_XYZQ) {
prepareLMAMode3(
mode, // int mode,
......@@ -209,21 +223,26 @@ public class QuaternionLma {
}
N = vect_x.length;
this.mode = mode;
samples = 7;
samples = 3+ quat0.length;
samples_x = 7;
pure_weight = 1.0 - reg_w;
x_vector = new double [samples * N];
y_vector = new double [samples * N + REGLEN];
weights = new double [samples * N + REGLEN];
int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
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();
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
x_vector[samples * i + j] = 0.0;
y_vector[samples * i + j] = 0.0;
weights [samples * i + j] = 0.0;
}
for (int j = 0; j < samples_x; j++) {
x_vector[samples_x * i + j] = 0.0;
y_vector[samples * i + j] = 0.0;
weights [samples * i + j] = 0.0;
}
} else {
// xyz
double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
......@@ -233,22 +252,106 @@ public class QuaternionLma {
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]);
for (int j = 0; j < 3; j++) {
x_vector[samples * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
weights[samples * i + j] = w;
x_vector[samples_x * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
weights[samples * i + j] = w;
sw += w;
}
x_vector[samples * i + 3] = rot_x.getQ0();
x_vector[samples * i + 4] = rot_x.getQ1();
x_vector[samples * i + 5] = rot_x.getQ2();
x_vector[samples * i + 6] = rot_x.getQ3();
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();
w = 0.75*(1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 4; j++) {
x_vector[samples_x * i + 3] = rot_x.getQ0();
x_vector[samples_x * i + 4] = rot_x.getQ1();
x_vector[samples_x * i + 5] = rot_x.getQ2();
x_vector[samples_x * i + 6] = rot_x.getQ3();
w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
if (samples < samples_x) {
y_vector[samples * i + 3] = rot_y.getQ1();
y_vector[samples * i + 4] = rot_y.getQ2();
y_vector[samples * i + 5] = rot_y.getQ3();
for (int j = 0; j < 3; j++) {
weights[samples * i + 3 + j] = w;
sw += w;
}
} else {
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++) {
weights[samples * i + 3 + j] = w;
sw += w;
}
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
if (extra_samples>0) {
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,
y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
}
public void prepareLMA43(
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 reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
N = vect_x.length;
this.mode = mode;
samples = 6;
samples_x = 7;
pure_weight = 1.0 - reg_w;
int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
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();
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
y_vector[samples * i + j] = 0.0;
weights [samples * i + j] = 0.0;
}
for (int j = 0; j < samples_x; j++) {
x_vector[samples_x * i + j] = 0.0;
y_vector[samples * i + j] = 0.0;
weights [samples * i + j] = 0.0;
}
} else {
// xyz
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,
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]);
for (int j = 0; j < 3; j++) {
x_vector[samples_x * i + j] = vect_x[i][0][j];
y_vector[samples * i + j] = vect_y[i][0][j];
weights[samples * i + j] = w;
sw += w;
}
x_vector[samples_x * i + 3] = rot_x.getQ0();
x_vector[samples_x * i + 4] = rot_x.getQ1();
x_vector[samples_x * i + 5] = rot_x.getQ2();
x_vector[samples_x * i + 6] = rot_x.getQ3();
y_vector[samples * i + 3] = rot_y.getQ1();
y_vector[samples * i + 4] = rot_y.getQ2();
y_vector[samples * i + 5] = rot_y.getQ3();
w = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 3; j++) {
weights[samples * i + 3 + j] = w;
sw += w;
}
......@@ -256,8 +359,10 @@ 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) {
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,
......@@ -267,7 +372,6 @@ public class QuaternionLma {
}
}
public void prepareLMAMode3(
int mode,
......@@ -283,7 +387,7 @@ public class QuaternionLma {
samples = 7;
samples_x = 7;
pure_weight = 1.0 - reg_w;
int extra_samples = (quat0.length < 4)? 0:REGLEN;
int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples * N];
y_vector = new double [samples * N + extra_samples];
y_inv_vector = new double [samples_x * N + extra_samples];
......@@ -334,14 +438,16 @@ public class QuaternionLma {
quat_y); // double [] quat_target // to which is applied (was scene_atr)
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[samples_x * i + 3] = 1.0; // no rotation
double wt = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
double wr = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 3; j++) {
weights[samples * i + j] = wt;
sw += wt;
}
for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
// for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
for (int j = 0; j < 4; j++) {// 0 - q0, near 1.0
weights[samples * i + 3 + j] = wr;
sw += wr;
}
......@@ -349,7 +455,7 @@ public class QuaternionLma {
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
if (parameters_vector.length >=4) {
if (extra_samples > 0) {
weights [samples * N] = 1.0 - pure_weight;
y_inv_vector[samples * N] = 1.0;
}
......@@ -535,10 +641,22 @@ public class QuaternionLma {
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
switch (mode) {
case MODE_XYZQ:return getFxDerivs6Dof(
case MODE_XYZ4Q3:return getFxDerivs6Dof43(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case MODE_XYZQ:
if (vector.length < 4) {
return getFxDerivs6Dof33(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
} else {
return getFxDerivs6Dof(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
case MODE_COMBO:return getFxDerivsVisual( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
......@@ -634,18 +752,21 @@ public class QuaternionLma {
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
// TODO: Implement use3 (no Q0)
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(new double[] { q0,q1,q2,q3});
// 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];
/// 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;
......@@ -676,7 +797,8 @@ public class QuaternionLma {
quat_rot = composeQR_Q(vector_r, quat_r);
System.arraycopy(quat_rot, 0, fx, i7+3, 4);
if (jt != null) {
quat_dq = composeQR_QdQ(vector_r,quat_r);
quat_dq = composeQR_QdQ(vector_r,quat_r, true);
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq[j], 0, jt[j], i7+3, 4);
}
......@@ -698,10 +820,11 @@ public class QuaternionLma {
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;
......@@ -749,7 +872,7 @@ 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);
quat_dq = composeQR_QdQ(vector_r,quat_r,true);
// 2 alternative ways with the same result
if (debug_level < 1000) {
double [][] invy_mat = qMat(inv_y[1]);
......@@ -840,7 +963,7 @@ public class QuaternionLma {
for (int j = 0; j < xyz_dq_local3.length; j++) { // xyz_dq_local3.length==3
System.arraycopy(xyz_dq_local3[j], 0, jt[j], i7, 3);
}
quat_dq = composeQR_QdQ(vector_r,quat_r);
quat_dq = composeQR_QdQ(vector_r,quat_r,true);
// 2 alternative ways with the same result
if (debug_level < 1000) {
double [][] invy_mat = qMat(inv_y[1]);
......@@ -873,6 +996,144 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivs6Dof43( // vector[4], but only 3 for rotations
double [] vector, //
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
boolean use_inv = true;
double [] fx = new double [weights.length];
double [] qn = new double[4];
boolean [] invert = { false, use_inv, use_inv, use_inv};
final double [] vector_r = normSign(vector).clone(); // should be already q0>0
invertDeriv(vector_r, invert);
// final double [] vector_r = normSign(new double[] { use_inv? -vector[0]:vector[0],vector[1],vector[2],vector[3]});
final double l = qNorm (vector_r, qn); // calculates qn // normalized
final double [][] dQn_dQ = dQndQ(vector_r);
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
}
}
for (int i = 0; i < N; i++) {
int i6 = samples * i;
int i7 = samples_x * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i6+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]};
xyz_rot = applyTo(vector, xyz); // rotate + scale
System.arraycopy(xyz_rot, 0, fx, i6, 3);
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
for (int j = 0; j < 4; j++) {
System.arraycopy(xyz_dq[j], 0, jt[j], i6, 3);
}
}
// rotations
final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
quat_rot = composeQR_Q(qn, quat_r); // qn - normalized vector_r
System.arraycopy(quat_rot, 1, fx, i6+3, 3);
if (jt != null) {
double[][] quat_dqn = composeQR_QdQ(qn,quat_r,false); // use_inv); //
double[][] quat_dq = mulMat(dQn_dQ, quat_dqn);
invertDeriv(quat_dq, invert);
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq[j], 1, jt[j], i6+3, 3);
}
}
}
return fx;
}
private double [] getFxDerivs6Dof33( // vector[3], but only 3 for rotations
double [] vector3, //
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
boolean use_inv = false; // true;
double [] vector = new double[] {getQ0(vector3),vector3[0],vector3[1],vector3[2]};
double [] fx = new double [weights.length];
double [] qn = new double[4];
boolean [] invert = { false, use_inv, use_inv, use_inv};
final double [] vector_r = normSign(vector).clone(); // should be already q0>0
invertDeriv(vector_r, invert);
qNorm (vector_r, qn); // calculates qn // normalized
// final double [][] dQn_dQ = dQndQ(vector_r);
final double [][] dQn_dQ123 = dQ_dQ123(vector3);
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
jt[i] = new double [weights.length];
}
}
for (int i = 0; i < N; i++) {
int i6 = samples * i;
int i7 = samples_x * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i6+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]};
xyz_rot = applyTo(vector, xyz); // rotate + scale
System.arraycopy(xyz_rot, 0, fx, i6, 3);
if (jt != null) {
double [][] xyz_dqn = applyToDQ(vector, xyz);
/*
xyz_dq = mulMat(dQn_dQ, xyz_dqn);
for (int j = 1; j < 4; j++) {
System.arraycopy(xyz_dq[j], 0, jt[j-1], i6, 3);
}
*/
xyz_dq = mulMat(dQn_dQ123, xyz_dqn);
for (int j = 0; j < xyz_dq.length; j++) { // 3
System.arraycopy(xyz_dq[j], 0, jt[j], i6, 3);
}
}
// rotations
final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
quat_rot = composeQR_Q(qn, quat_r); // qn - normalized vector_r
System.arraycopy(quat_rot, 1, fx, i6+3, 3);
if (jt != null) {
double[][] quat_dqn = composeQR_QdQ(qn,quat_r,false); // use_inv); //
/*
double[][] quat_dq = mulMat(dQn_dQ, quat_dqn);
invertDeriv(quat_dq, invert);
for (int j = 1; j < 4; j++) {
System.arraycopy(quat_dq[j], 1, jt[j-1], i6+3, 3);
}
*/
double[][] quat_dq = mulMat(dQn_dQ123, quat_dqn); // [3][4]
invertDeriv(quat_dq, invert);
for (int j = 0; j < quat_dq.length; j++) { // 3
System.arraycopy(quat_dq[j], 1, jt[j], i6+3, 3);
}
}
}
return fx;
}
private double compareJT(
double [] vector,
double delta) {
......@@ -1008,7 +1269,7 @@ public class QuaternionLma {
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
// quat_dq = composeQR_QdQ(vector,quat_r);
quat_dq = composeQR_QdQ(vector_r,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;
......@@ -1095,7 +1356,7 @@ public class QuaternionLma {
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);
quat_dq = composeQR_QdQ(vector_r,quat_r,true);
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
// Z
......@@ -1479,17 +1740,18 @@ public class QuaternionLma {
data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
}
System.out.println();
} else if ((mode == MODE_XYZQ) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
// } else if (((mode == MODE_XYZQ) && ()) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
} else if (data.length >= x_vector.length) { // different data size data[3*nscene+...]
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
pfx+"A",pfx+"T",pfx+"R"));
for (int nscene = 0; nscene < N; nscene++) {
if ( (data[7*nscene + 3]*data[7*nscene + 3] +
data[7*nscene + 4]*data[7*nscene + 4] +
data[7*nscene + 5]*data[7*nscene + 5]+
data[7*nscene + 6]*data[7*nscene + 6]) < 0.001) {
if ( (data[samples_x*nscene + 3]*data[samples_x*nscene + 3] +
data[samples_x*nscene + 4]*data[samples_x*nscene + 4] +
data[samples_x*nscene + 5]*data[samples_x*nscene + 5]+
data[samples_x*nscene + 6]*data[samples_x*nscene + 6]) < 0.001) {
System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
continue;
}
......@@ -1504,6 +1766,35 @@ public class QuaternionLma {
angles[0],angles[1],angles[2]));
}
System.out.println();
} else if ((mode == MODE_XYZ4Q3) || (mode == MODE_XYZQ)) { // not when 7-long, it should be catched before
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
pfx+"A",pfx+"T",pfx+"R"));
for (int nscene = 0; nscene < N; nscene++) {
if ( (x_vector[samples_x*nscene + 3]*x_vector[samples_x*nscene + 3] +
x_vector[samples_x*nscene + 4]*x_vector[samples_x*nscene + 4] +
x_vector[samples_x*nscene + 5]*x_vector[samples_x*nscene + 5]+
x_vector[samples_x*nscene + 6]*x_vector[samples_x*nscene + 6]) < 0.001) {
System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
continue;
}
double q1 = data[samples*nscene + 3];
double q2 = data[samples*nscene + 4];
double q3 = data[samples*nscene + 5];
double q0 = Math.sqrt(1.0-q1*q1 - q2*q2 - q3*q3); // should be normalized!
Rotation rot = new Rotation(q0,q1,q2,q3, false);
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2],
q0,q1,q2,q3,
angles[0],angles[1],angles[2]));
}
System.out.println();
} 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
......@@ -1610,53 +1901,58 @@ public class QuaternionLma {
*/
public static double [][] composeQR_QdQ(
double [] q,
double [] r) {
return new double [][] {
// for inverted {-q0,q1,q2,q3}
// t=s*q' d/dQ0
/*
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
2*r[3]*q[0] + 2*r[1]*q[2] - 2*r[2]*q[1]},
// t=s*q' d/dQ1
{-2*r[0]*q[1],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-2*r[1]*q[2] - 2*r[3]*q[0] + 2*r[2]*q[1],
+2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3]},
// t=s*q'd/dQ2
{-2*r[0]*q[2],
-2*r[2]*q[1] + 2*r[3]*q[0] + 2*r[1]*q[2],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-2*r[2]*q[3] - 2*r[1]*q[0] + 2*r[3]*q[2]},
// t=s*q'd/dQ3
{-2*r[0]*q[3],
-2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
-2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
*/
// for non-inverted {q0,q1,q2,q3}
// t=s*q' d/dQ0
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
2*r[3]*q[0] + 2*r[1]*q[2] - 2*r[2]*q[1]},
// t=s*q' d/dQ1
{ 2*r[0]*q[1],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3],
2*r[1]*q[2] + 2*r[3]*q[0] - 2*r[2]*q[1],
-2*r[2]*q[0] - 2*r[3]*q[1] + 2*r[1]*q[3]},
// t=s*q'd/dQ2
{ 2*r[0]*q[2],
2*r[2]*q[1] - 2*r[3]*q[0] - 2*r[1]*q[2],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3],
2*r[2]*q[3] + 2*r[1]*q[0] - 2*r[3]*q[2]},
// t=s*q'd/dQ3
{ 2*r[0]*q[3],
2*r[3]*q[1] + 2*r[2]*q[0] - 2*r[1]*q[3],
2*r[3]*q[2] - 2*r[1]*q[0] - 2*r[2]*q[3],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3]}
};
double [] r,
boolean use_inv) {
// boolean use_inv = true;
if (use_inv) {
return new double [][] {
// for inverted {-q0,q1,q2,q3}
// t=s*q' d/dQ0
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
2*r[3]*q[0] + 2*r[1]*q[2] - 2*r[2]*q[1]},
// t=s*q' d/dQ1
{-2*r[0]*q[1],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-2*r[1]*q[2] - 2*r[3]*q[0] + 2*r[2]*q[1],
+2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3]},
// t=s*q'd/dQ2
{-2*r[0]*q[2],
-2*r[2]*q[1] + 2*r[3]*q[0] + 2*r[1]*q[2],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3],
-2*r[2]*q[3] - 2*r[1]*q[0] + 2*r[3]*q[2]},
// t=s*q'd/dQ3
{-2*r[0]*q[3],
-2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
-2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
};
} else {
return new double [][] {
// for non-inverted {q0,q1,q2,q3}
// t=s*q' d/dQ0
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
2*r[3]*q[0] + 2*r[1]*q[2] - 2*r[2]*q[1]},
// t=s*q' d/dQ1
{ 2*r[0]*q[1],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3],
2*r[1]*q[2] + 2*r[3]*q[0] - 2*r[2]*q[1],
-2*r[2]*q[0] - 2*r[3]*q[1] + 2*r[1]*q[3]},
// t=s*q'd/dQ2
{ 2*r[0]*q[2],
2*r[2]*q[1] - 2*r[3]*q[0] - 2*r[1]*q[2],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3],
2*r[2]*q[3] + 2*r[1]*q[0] - 2*r[3]*q[2]},
// t=s*q'd/dQ3
{ 2*r[0]*q[3],
2*r[3]*q[1] + 2*r[2]*q[0] - 2*r[1]*q[3],
2*r[3]*q[2] - 2*r[1]*q[0] - 2*r[2]*q[3],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3]}
};
}
}
public static double [][] composeQR_QdQ_(
double [] q,
......@@ -1856,7 +2152,7 @@ public class QuaternionLma {
return Math.sqrt(1.0-(q123[0]*q123[0]+q123[1]*q123[1]+q123[2]*q123[2]));
}
public static double [] dQuat123(
public static double [] dQuat123( // not used
double [] q123,
double [] dq0123, // [4]
double q0) { // null or double[3]
......@@ -1880,4 +2176,123 @@ public class QuaternionLma {
return dq123;
}
public static double [][] dQ_dQ123(
double [] q123){
double d = 1.0;
for (int i = 0; i < q123.length; i++) {
d-= q123[i] * q123[i];
}
double q0 = Math.sqrt(d);
double [][] mat = new double [q123.length][q123.length+1];
for (int i = 0; i < mat.length; i++) {
mat[i][0] = -q123[i]/q0;
mat[i][i+1] = 1;
}
return mat;
}
public static double qNorm(
final double [] q,
final double [] qn) {
double l = Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
if (qn != null) {
for (int i = 0; i < qn.length; i++) {
qn[i] = q[i]/l;
}
}
return l;
}
public static double [][] dQndQ(
final double [] q){
final double [][] dq = new double [q.length][q.length];
final double l = qNorm(q, null);
final double l3 = l*l*l;
for (int i = 0; i < q.length; i++) {
for (int j = 0; j < q.length; j++) {
dq[j][i] = -q[i]*q[j]/l3; // dq[i][j] = d;
}
dq[i][i] += 1/l;
}
return dq;
}
public static void invertDeriv(
double [][] deriv,
boolean [] invert){
for (int i = 0; i < deriv.length; i++) {
if (invert[i]) {
for (int j = 0; j < deriv[i].length; j++) {
deriv[i][j] *= -1;
}
}
}
}
public static void invertDeriv(
double [] deriv,
boolean [] invert){
for (int i = 0; i < deriv.length; i++) {
if (invert[i]) {
deriv[i] *= -1;
}
}
}
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 dia_x = getDiameter(vect_x,first_last);
double dia_y = getDiameter(vect_y,first_last);
if (!(dia_x > 0) || !(dia_y > 0)) return null;
double s = dia_x/dia_y;
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 static double getDiameter(
double [][][] xyzatr,
int [] first_last) {
if (first_last == null) {
first_last = new int [] {0,xyzatr.length-1};
}
int i0= first_last[0];
for (;i0 <= first_last[1]; i0++) if (xyzatr[i0] != null) break;
if (i0 > first_last[1]) return Double.NaN; // empty sequence
double l2 = 0;
int i1 = i0;
for (int i = i0; i <= first_last[1]; i++) if (xyzatr[i] != null){
double d = 0;
for (int j = 0; j < 3; j++) {
double dd = xyzatr[i][0][j]-xyzatr[i0][0][j];
d+= dd*dd;
}
if (d > l2) {
l2 = d;
i1 = i;
}
}
for (int i = i1; i <= first_last[1]; i++) if (xyzatr[i] != null){
double d = 0;
for (int j = 0; j < 3; j++) {
double dd = xyzatr[i][0][j]-xyzatr[i1][0][j];
d+= dd*dd;
}
if (d > l2) {
l2 = d;
i0 = i;
}
}
return Math.sqrt(l2);
}
}
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