Skip to content

I
imagej-elphel
Commit 0835c213 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Next snapshot

parent 93c00b54
Show whitespace changes
Inline Side-by-side
Showing with 1070 additions and 57 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 0835c213
......@@ -39,6 +39,9 @@ import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAccumulator;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import java.util.concurrent.ThreadLocalRandom;
import com.elphel.imagej.calibration.CalibrationFileManagement;
......@@ -5434,8 +5437,16 @@ public class OpticalFlow {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 1; // 2;
int debug_lev = 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,
......@@ -5444,10 +5455,9 @@ public class OpticalFlow {
last_index, // int last_index,
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
0); // int debugLevel
}
debug_lev); // int debugLevel
}
if (run_ly) {
if (debugLevel > -3) {
System.out.println("**** Running LY adjustments *****");
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 0835c213
......@@ -242,8 +242,11 @@ public class QuadCLTCPU {
* @return quaternion componets
*/
public static double [] getRotationFromXYZCameraIms(
public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] ims_xyzatr,
......@@ -253,12 +256,6 @@ public class QuadCLTCPU {
double [] rms, // null or double[5];
int debugLevel
) {
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];
}
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;
......@@ -269,6 +266,44 @@ 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) {
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];
}
quaternionLma.prepareLMA(
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
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) {
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];
}
quaternionLma.prepareLMA(
vect_x, // double [][][] vect_x,
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,
......@@ -276,6 +311,8 @@ public class QuadCLTCPU {
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)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
......@@ -307,6 +344,9 @@ public class QuadCLTCPU {
public static double [][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] ims_xyzatr,
......@@ -317,8 +357,11 @@ public class QuadCLTCPU {
double [] quaternion, // null or double[4]
int debugLevel
) {
double [] quat = getRotationFromXYZCameraIms(
double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_height, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
......@@ -336,17 +379,27 @@ public class QuadCLTCPU {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS
double [] rotated_xyz = new double[3];
// double [] rotated_atr = new double[3];
Rotation rot_invert = rot.revert();
for (int nscene = early_index; nscene <= last_index; nscene++) {
rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);
Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
Rotation rotation_atr = rot.applyTo(scene_atr);
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?
rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
if (debugLevel > -1) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 0835c213
......@@ -26,12 +26,21 @@
package com.elphel.imagej.tileprocessor;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import Jama.Matrix;
public class QuaternionLma {
private final static int REGLEN = 1; // number of extra (regularization) samples
private int N = 0;
// Mode2 - compensating camera uncertainty:dpx/dx ~= -dpx/daz/height(m) dpy/dy ~= dpy/dtl/height(m)
private int mode = 0; // 0 xyz, 1 - xyz,quat, 2: Z/x, 2Q3, 2Q2-X/h, 2Q1+Y/h
// private boolean use_6dof = false;
private int samples = 3;
private int samples_x = 3;
private double height = 1;
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
......@@ -62,6 +71,9 @@ public class QuaternionLma {
final int debug_level) {
N = vect_x.length;
pure_weight = 1.0 - reg_w;
mode = 0;
samples = 3;
samples_x = 3;
x_vector = new double [3* N];
y_vector = new double [3* N + REGLEN];
weights = new double [3* N + REGLEN];
......@@ -69,34 +81,183 @@ public class QuaternionLma {
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < 3; j++) {
x_vector[3 * i + j] = 0.0;
y_vector[3 * i + j] = 0.0;
weights[3*i + j] = 0.0;
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;
}
} else {
for (int j = 0; j < 3; j++) {
x_vector[3 * i + j] = vect_x[i][j];
y_vector[3 * i + j] = vect_y[i][j];
x_vector[samples * i + j] = vect_x[i][j];
y_vector[samples * i + j] = vect_y[i][j];
double w = (vect_w != null)? vect_w[i][j] : 1.0;
weights[3*i + j] = w;
weights[samples*i + j] = w;
sw += w;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights[3 * N] = 1.0 - pure_weight;
y_vector[3 * N] = 1.0;
weights[samples * N] = 1.0 - pure_weight;
y_vector[samples * N] = 1.0;
last_jt = new double [parameters_vector.length][];
}
// 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(
public void prepareLMA(
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;
mode = 1;
samples = 7;
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];
parameters_vector = quat0.clone();
double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
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;
}
} else {
// xyz
double w = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
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;
sw += w;
}
// 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 * 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++) {
weights[samples * i + 3 + j] = w;
sw += w;
}
}
}
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 ( "", // String pfx,
y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
}
public void prepareLMA(
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) {
N = vect_x.length;
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];
weights = new double [samples * N + REGLEN];
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;
}
} else {
// mode=2
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
y_vector[samples * i + 0] = vect_y[i][0][2]/height; // Z
y_vector[samples * i + 1] = 2*rot_y.getQ3(); // 2 * Q3
y_vector[samples * i + 2] = 2*rot_y.getQ2() - vect_y[i][0][0] / height; // 2 * Q2 - X / height
y_vector[samples * i + 3] = 2*rot_y.getQ1() + vect_y[i][0][1] / height; // 2 * Q1 + Y / height
for (int j = 0; j < samples; j++) {
weights[samples * i + j] = w;
sw += w;
}
}
}
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 ( "", // String pfx,
y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
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];
......@@ -137,6 +298,492 @@ public class QuaternionLma {
}
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;
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++) {
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];
}
}
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;
}
}
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(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
switch (mode) {
case 1:return getFxDerivs6Dof(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 2:return getFxDerivsVisual( // 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][samples * N] = 2 * vector[i];
}
}
fx[samples * N] = q0*q0 + q1*q1 + q2 * q2 + q3*q3;
double [] xyz_rot;
double [][] xyz_dq;
for (int i = 0; i < N; i++) {
int i3 = 3 * i;
final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
xyz_rot = applyTo(vector, xyz);
System.arraycopy(xyz_rot, 0, fx, i3, 3);
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
for (int j = 0; j < 4; j++) {
System.arraycopy(xyz_dq[j], 0, jt[j], i3, 3);
}
}
}
return fx;
}
private double [] getFxDerivs6Dof(
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(new double[] {-q0,q1,q2,q3}); // seems better with reversal
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;
for (int i = 0; i < N; i++) {
int i7 = samples * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i7+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);
System.arraycopy(xyz_rot, 0, fx, i7, 3);
if (jt != null) {
xyz_dq = applyToDQ(vector, xyz);
for (int j = 0; j < 4; j++) {
System.arraycopy(xyz_dq[j], 0, jt[j], i7, 3);
}
}
// rotations
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);
quat_rot = composeQR_Q(vector_r, quat_r);
System.arraycopy(quat_rot, 0, fx, i7+3, 4);
if (jt != null) {
// quat_dq = composeDQ(quat_r);
quat_dq = composeQR_QdQ(vector_r,quat_r);
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq[j], 0, jt[j], i7+3, 4);
}
}
}
return fx;
}
private double compareJT(
double [] vector,
double delta) {
double [] errors=new double [vector.length];
double [][] jt = new double [vector.length][];
System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
double [] fx_center = getFxDerivs(
vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
1); // debug_level);
double [][] jt_delta = getFxDerivsDelta(
vector, // double [] vector,
delta, // final double delta,
-1); // final int debug_level)
for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {
System.out.println(String.format(
"%3d\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f",
n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],
jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],
jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
for (int i = 0; i < vector.length; i++) {
errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
}
}
System.out.println(String.format(
"-\t-\t-\t-\t-\t-\t-\t-\t-\t%10.7f\t%10.7f\t%10.7f\t%10.7f",
errors[0], errors[1], errors[2], errors[3]));
double err=0;
for (int i = 0; i < vector.length; i++) {
err = Math.max(errors[i], err);
}
return err;
}
private double [][] getFxDerivsDelta(
double [] vector,
final double delta,
final int debug_level) {
double [][] jt = new double [vector.length][weights.length];
for (int nv = 0; nv < vector.length; nv++) {
double [] vpm = vector.clone();
vpm[nv]+= 0.5*delta;
double [] fx_p = getFxDerivs(
vpm,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level);
vpm[nv]-= delta;
double [] fx_m = getFxDerivs(
vpm,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level);
for (int i = 0; i < weights.length; i++) if (weights[i] > 0) {
jt[nv][i] = (fx_p[i]-fx_m[i])/delta;
}
}
return jt;
}
private double [] getFxDerivsVisual(
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(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++) {
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;
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]};
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 = composeQR_Q(vector_r, quat_r);
quat_rot = composeQR_Q(vector_r, quat_r);
double [] quat_rot1 = compose(vector_r, 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 = composeQR_QdQ(vector,quat_r);
quat_dq = composeQR_QdQ(vector_r,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;
}
private double [] getYminusFxWeighted(
final double [] fx,
......@@ -145,6 +792,7 @@ public class QuaternionLma {
final double [] wymfw = new double [fx.length];
double s_rms=0;
double rms_pure=0;
// int num_comp = use_6dof? 7 : 3;
for (int i = 0; i < fx.length; i++) {
double d = y_vector[i] - fx[i];
double wd = d * weights[i];
......@@ -154,7 +802,7 @@ public class QuaternionLma {
wd = 0.0;
d = 0.0;
}
if (i == (3 * N)) {
if (i == (samples * N)) {
rms_pure = Math.sqrt(s_rms/pure_weight);;
}
wymfw[i] = wd;
......@@ -166,7 +814,6 @@ public class QuaternionLma {
rms_fp[0] = rms;
rms_fp[1] = rms_pure;
}
return wymfw;
}
......@@ -294,7 +941,23 @@ public class QuaternionLma {
}
System.out.println();
}
if (debug_level > 0) {
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
debugYfX ( "fx-", // String pfx,
fx); // double [] data)
if (debug_level > 1) {
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
parameters_vector, // double [] vector,
delta); // double delta);
System.out.println("Maximal error = "+err);
}
}
return rslt[0]? iter : -1;
}
......@@ -313,6 +976,19 @@ public class QuaternionLma {
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
if (debug_level > 0) {
debugYfX ( "fx0-", // String pfx,
fx); // double [] data)
}
if (debug_level > 1) {
double delta = 1E-5;
System.out.println("\n\n");
double err = compareJT(
parameters_vector, // double [] vector,
delta); // double delta);
System.out.println("Maximal error = "+err);
}
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
......@@ -447,7 +1123,41 @@ public class QuaternionLma {
return rslt;
}
public void debugYfX (
String pfx,
double [] data) {
if ((mode == 1) || ((mode == 2) && (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++) {
Rotation rot = new Rotation(data[7*nscene + 3],data[7*nscene + 4],data[7*nscene + 5],data[7*nscene + 6], 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_x*nscene + 0],data[samples_x*nscene + 1],data[samples_x*nscene + 2],
data[samples_x*nscene + 3],data[samples_x*nscene + 4],data[samples_x*nscene + 5],data[samples_x*nscene + 6],
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
"N",pfx+"Z",pfx+"2*Q3",pfx+"2*Q2-X",pfx+"2*Q1+Y"));
for (int nscene = 0; nscene < N; nscene++) {
System.out.println(String.format("%3d"+
"\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], data[samples*nscene + 3]));
}
System.out.println();
}
}
//TODO: implement
......@@ -455,4 +1165,244 @@ public class QuaternionLma {
return new String[] {};
}
/**
* Apply quaternion q to quaternion r
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
* @param r - 4 components (scalar, vector) of the target quaternion to which to apply the first one
* @return composed quaternion
*/
public static double [] compose(
double [] q,
double [] r) {
return normSign(new double [] {
r[0] * q[0] - (r[1] * q[1] + r[2] * q[2] + r[3] * q[3]),
r[1] * q[0] + r[0] * q[1] + (r[2] * q[3] - r[3] * q[2]),
r[2] * q[0] + r[0] * q[2] + (r[3] * q[1] - r[1] * q[3]),
r[3] * q[0] + r[0] * q[3] + (r[1] * q[2] - r[2] * q[1])});
}
public static double [] normSign(double [] q) {
if (q[0] >= 0) return q;
return new double [] {-q[0], -q[1], -q[2], -q[3]};
}
/**
* Apply quaternion q to quaternion r
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
* @param r - 4 components (scalar, vector) of the target quaternion to which to apply the first one
* @return composed quaternion
*/
public static double [] composeQR_Q(
double [] q,
double [] r) {
return normSign(new double [] {
-q[0]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
-q[1]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
-q[2]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
-q[3]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]),// s[3];
q[1]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
-q[0]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
+q[2]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]) // s[3]
-q[3]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]),// s[2]);
q[2]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
-q[0]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
+q[3]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]
-q[1]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]),// s[3]);
q[3]*(r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]) // s[0]
-q[0]*(r[3]*q[0] + r[0]*q[3] + r[1]*q[2] - r[2]*q[1]) // s[3]
+q[1]*(r[2]*q[0] + r[0]*q[2] + r[3]*q[1] - r[1]*q[3]) // s[2]
-q[2]*(r[1]*q[0] + r[0]*q[1] + r[2]*q[3] - r[3]*q[2]) // s[1]);
});
}
/**
* Get derivatives of the composed quaternion (composeQR_Q(q,r)) by the
* components of the rotation (q). Rotation is Q*R*Q~
* @param q 4 components (scalar, vector) of the quaternion Q to apply to
* scene rotations r.
* @param r 4 components (scalar, vector) of the quaternion to which
* the first one is applied.
* @return 4x4 array, where columns correspond to composition components
* (samples in LMA) and rows - to the target quaternion.
*/
public static double [][] composeQR_QdQ(
double [] q,
double [] r) {
return new double [][] {
// 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]}
/*
// 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,
double [] r) {
return new double [][] {
// 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]}
};
}
/**
* Get derivatives of the composed quaternion (compose(q,r)) by the
* components of the first one (q). These derivatives do not depend
* on the first quaternion, so it is not in the input.
* @param r 4 components (scalar, vector) of the quaternion to which
* the first one is applied.
* @return 4x4 array, where columns correspond to composition components
* (samples in LMA) and rows - to the second quaternion
* (missing from the input) components.
*/
public static double [][] composeDQ(
double [] r) {
/*
return new double [][] {
{ r[0], -r[1], -r[2], -r[3]},
{ r[1], r[0], -r[3], r[2]},
{ r[2], r[3], r[0], -r[1]},
{ r[3], -r[2], r[1], r[0]}};
*/
return new double [][] {
{ r[0], r[1], r[2], r[3]},
{-r[1], r[0], r[3],-r[2]},
{-r[2],-r[3], r[0], r[1]},
{-r[3], r[2],-r[1], r[0]}};
}
/**
* 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.
* @param q 4 components (scalar, vector) of the quaternion being
* applied to the second quaternion.
* @return 4x4 array, where columns correspond to composition components
* (samples in LMA) and rows - to the source quaternion
* (missing from the input) components.
*/
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]}};
}
/**
* Apply quaternion to a 3D vector
* @param q 4 components (scalar, vector) of the quaternion being applied
* to as vector.
* @param xyz 1-d array representing a 3D vector {X, Y, Z}
* @return rotated 3D vector as 1 1D array {X, Y, Z}
*/
public static double [] applyTo(
double [] q,
double [] xyz) {
final double s = q[1] * xyz[0] + q[2] * xyz[1] + q[3] * xyz[2];
return new double [] {
2 * (q[0] * (xyz[0] * q[0] - (q[2] * xyz[2] - q[3] * xyz[1])) + s * q[1]) - xyz[0],
2 * (q[0] * (xyz[1] * q[0] - (q[3] * xyz[0] - q[1] * xyz[2])) + s * q[2]) - xyz[1],
2 * (q[0] * (xyz[2] * q[0] - (q[1] * xyz[1] - q[2] * xyz[0])) + s * q[3]) - xyz[2]};
}
/**
* Get derivatives of the rotated vector (see applyTo(q,xyz)) by the components of the quaterion q
* @param q 4 components (scalar, vector) of the quaternion being applied
* to as vector.
* @param xyz 1-d array representing a 3D vector {X, Y, Z}
* @return 4x3 array, where columns correspond to xyz components (samples in LMA)
* and rows - to the quaternion q components.
*/
public static double [][] applyToDQ(
double [] q,
double [] xyz) {
final double s = q[1] * xyz[0] + q[2] * xyz[1] + q[3] * xyz[2];
/*
return new double [][] {
{4*xyz[0]*q[0]-2*xyz[2]*q[2]+2*xyz[1]*q[3], 2*s + 2*xyz[0]*q[1], 2*xyz[2]*q[0]+2*xyz[1]*q[1], 2*xyz[1]*q[0]+2*xyz[2]*q[1]},
{4*xyz[1]*q[0]-2*xyz[0]*q[3]+2*xyz[2]*q[1], 2*xyz[2]*q[0]+2*xyz[0]*q[2], 2*s + 2*xyz[1]*q[2], 2*xyz[0]*q[0]+2*xyz[2]*q[2]},
{4*xyz[2]*q[0]-2*xyz[1]*q[1]+2*xyz[0]*q[2], 2*xyz[1]*q[0]+2*xyz[0]*q[3], 2*xyz[0]*q[0]+2*xyz[1]*q[3], 2*s +2*xyz[2]*q[3]}};
*/
/*
return new double[][] {
{4*xyz[0]*q[0]-2*xyz[2]*q[2]+2*xyz[1]*q[3], 4*xyz[1]*q[0]-2*xyz[0]*q[3]+2*xyz[2]*q[1],4*xyz[2]*q[0]-2*xyz[1]*q[1]+2*xyz[0]*q[2]},
{2*s + 2*xyz[0]*q[1], 2*xyz[2]*q[0]+2*xyz[0]*q[2], 2*xyz[1]*q[0]+2*xyz[0]*q[3]},
{2*xyz[2]*q[0]+2*xyz[1]*q[1], 2*s + 2*xyz[1]*q[2], 2*xyz[0]*q[0]+2*xyz[1]*q[3]},
{2*xyz[1]*q[0]+2*xyz[2]*q[1], 2*xyz[0]*q[0]+2*xyz[2]*q[2], 2*s +2*xyz[2]*q[3]}};
*/
return new double[][] {
{ 4*xyz[0]*q[0]-2*xyz[2]*q[2]+2*xyz[1]*q[3], 4*xyz[1]*q[0]-2*xyz[0]*q[3]+2*xyz[2]*q[1],4*xyz[2]*q[0]-2*xyz[1]*q[1]+2*xyz[0]*q[2]},
{ 2*s + 2*xyz[0]*q[1], 2*xyz[2]*q[0]+2*xyz[0]*q[2], -2*xyz[1]*q[0]+2*xyz[0]*q[3]},
{-2*xyz[2]*q[0]+2*xyz[1]*q[1], 2*s + 2*xyz[1]*q[2], 2*xyz[0]*q[0]+2*xyz[1]*q[3]},
{ 2*xyz[1]*q[0]+2*xyz[2]*q[1], -2*xyz[0]*q[0]+2*xyz[2]*q[2], 2*s +2*xyz[2]*q[3]}};
}
}
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