Skip to content

I
imagej-elphel
Commit 1d564879 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

working snapshot

parent 7de7fe06
Show whitespace changes
Inline Side-by-side
Showing with 281 additions and 17 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 1d564879
......@@ -5437,7 +5437,7 @@ public class OpticalFlow {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 2; // 1;
int quat_lma_mode = 3; // 2; // 1;
int debug_lev = 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 1d564879
......@@ -274,6 +274,7 @@ public class QuadCLTCPU {
vect_y[nscene] = xyzatr[nscene];
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y, // double [][][] vect_y,
......@@ -281,7 +282,7 @@ public class QuadCLTCPU {
translation_weight, // double translation_weight, // 0.0 ... 1.0;
quat0, // double [] quat0,
debugLevel); // int debug_level)
} else if (quat_lma_mode == 1) {
} 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++) {
......@@ -289,6 +290,7 @@ public class QuadCLTCPU {
vect_y[nscene] = xyzatr[nscene];
}
quaternionLma.prepareLMA(
quat_lma_mode, // int mode,
vect_x, // double [][][] vect_x,
vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 1d564879
......@@ -47,6 +47,7 @@ public class QuaternionLma {
private double [] parameters_vector = null;
private double [] x_vector = null;
private double [] y_vector = null;
private double [] y_inv_vector = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private double [] last_ymfx = null;
......@@ -106,6 +107,7 @@ public class QuaternionLma {
public void prepareLMA(
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
......@@ -113,8 +115,20 @@ 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 != 1) {
prepareLMAMode3(
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;
}
N = vect_x.length;
mode = 1;
this.mode = mode;
samples = 7;
samples_x = 7;
pure_weight = 1.0 - reg_w;
......@@ -122,8 +136,6 @@ public class QuaternionLma {
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)) {
......@@ -135,17 +147,18 @@ public class QuaternionLma {
} else {
// xyz
double w = translation_weight*((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]);
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();
......@@ -175,7 +188,100 @@ public class QuaternionLma {
}
public void prepareLMAMode3(
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 = 7;
samples_x = 7;
pure_weight = 1.0 - reg_w;
x_vector = new double [samples * N];
y_vector = new double [samples * N + REGLEN];
y_inv_vector = new double [samples_x * N + REGLEN];
weights = new double [samples * N + REGLEN];
parameters_vector = quat0.clone();
double [] 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_inv_vector[samples * i + j] = 0.0;
weights [samples * i + j] = 0.0;
}
} else {
// xyz
for (int j = 0; j < 3; j++) {
x_vector[samples_x * i + j] = vect_x[i][0][j];
}
// 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]);
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();
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]);
// just testing - y should be all 0
double [] xyz_y = new double [] {
vect_y[i][0][0],
vect_y[i][0][1],
vect_y[i][0][2]};
double [] quat_y = new double [] {
rot_y.getQ0(),
rot_y.getQ1(),
rot_y.getQ2(),
rot_y.getQ3()};
double [][] inv_y = invertTransRot(
xyz_y, // double [] xyz_src, // transformation to apply to (was reference_xyz)
quat_y); // double [] quat_src);
double [][] comb_y = combineTransRot( // just to verify it is 0;
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_y, // double [] xyz_target, // to which is applied (was scene_xyz)
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);
double wt = 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 = 0; j < 4; j++) {
weights[samples * i + 3 + j] = wr;
sw += wr;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights [samples * N] = 1.0 - pure_weight;
y_inv_vector[samples * N] = 1.0;
last_jt = new double [parameters_vector.length][];
if (debug_level > 0) {
debugYfX ( "Y-INV-", // String pfx,
y_inv_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
}
public void prepareLMA(
int mode,
double avg_height, //
double [][][] vect_x, // []{{x,y,z},{a,t,r}}
double [][][] vect_y, // []{{x,y,z},{a,t,r}}
......@@ -184,7 +290,7 @@ public class QuaternionLma {
double [] quat0,
final int debug_level) {
N = vect_x.length;
mode = 2;
this.mode = mode; // 2;
samples = 4;
samples_x = 7;
height = avg_height;
......@@ -560,6 +666,10 @@ public class QuaternionLma {
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 3:return getFxDerivs6DofMode3(
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];
......@@ -599,7 +709,6 @@ public class QuaternionLma {
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
// final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
......@@ -635,11 +744,9 @@ public class QuaternionLma {
}
// 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);
......@@ -649,6 +756,92 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivs6DofMode3(
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});
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
jt[i][samples * N] = 2 * vector[i];
}
}
fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
double [][] quat_dq;
double [][] inv_y = new double [][] {new double[3],new double[4]};
for (int i = 0; i < N; i++) {
int 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]};
// 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)
System.arraycopy(comb_y[0], 0, fx, i7, 3);
System.arraycopy(comb_y[1], 0, fx, i7+3, 4);
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)
}
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);
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(invy_mat, quat_dq);
double [][]dcomp_dsecond = composeDR(inv_y[1]);
double [][] quat_dq_local1 = new double [4][];
for (int j = 0; j < 4; j++) {
// quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
quat_dq_local1[j] = mulMat(invy_mat, quat_dq[j]);
}
for (int j = 0; j < 4; j++) {
System.arraycopy(quat_dq_local1[j], 0, jt[j], i7+3, 4);
}
}
}
return fx;
}
private double compareJT(
double [] vector,
double delta) {
......@@ -665,7 +858,7 @@ public class QuaternionLma {
-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",
"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
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]));
......@@ -674,7 +867,7 @@ public class QuaternionLma {
}
}
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",
"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
errors[0], errors[1], errors[2], errors[3]));
double err=0;
for (int i = 0; i < vector.length; i++) {
......@@ -1127,7 +1320,8 @@ public class QuaternionLma {
public void debugYfX (
String pfx,
double [] data) {
if ((mode == 1) || ((mode == 2) && (data.length == x_vector.length))) { // different data size data[3*nscene+...]
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if ((mode == 1) || (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",
......@@ -1166,6 +1360,27 @@ public class QuaternionLma {
return new String[] {};
}
public static double [][] combineTransRot(
double [] xyz_src, // transformation to apply to (was reference_xyz)
double [] quat_src, // transformation to apply to (was reference_atr)
double [] xyz_target, // to which is applied (was scene_xyz)
double [] quat_target // to which is applied (was scene_atr)
) {
double [] offs = applyTo(quat_src, xyz_target);
double [] xyz = (xyz_src==null)? offs : addTo(xyz_src,offs);
double [] quat = (quat_target==null)? null: compose(quat_src,quat_target); // includes normSign()
return new double [][] {xyz,quat};
}
public static double [][] invertTransRot(
double [] xyz_src, // transformation to apply to (was reference_xyz)
double [] quat_src){ // transformation to apply to (was reference_atr)
double [] quat = normSign(new double[] {-quat_src[0],quat_src[1],quat_src[2],quat_src[3]});
double [] xyz = applyTo(quat,new double [] {-xyz_src[0],-xyz_src[1],-xyz_src[2]});
return new double [][] {xyz,quat};
}
/**
* Apply quaternion q to quaternion r
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
......@@ -1359,6 +1574,16 @@ public class QuaternionLma {
{-q[3],-q[2], q[1], q[0]}};
}
public static double [] addTo(
double [] offs,
double [] xyz) {
return new double [] {
offs[0]+xyz[0],
offs[1]+xyz[1],
offs[2]+xyz[2]};
}
/**
* Apply quaternion to a 3D vector
* @param q 4 components (scalar, vector) of the quaternion being applied
......@@ -1407,4 +1632,41 @@ public class QuaternionLma {
{-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]}};
}
public static double [] mulMat(
double [][] mat,
double [] vect) {
double [] rslt = new double[mat.length];
for (int i = 0; i < mat.length; i++) {
for (int j = 0; j < vect.length; j++) {
rslt[i]+= mat[i][j]* vect[j];
}
}
return rslt;
}
public static double [][] mulMat(
double [][] mat,
double [][] mat1) {
double [][] rslt = new double[mat.length][mat1[0].length];
for (int i = 0; i < rslt.length; i++) {
for (int j = 0; j < rslt[0].length; j++) {
for (int k = 0; k < mat[0].length; k++) {
rslt[i][j] += mat[i][k]* mat1[k][j];
}
}
}
return rslt;
}
public static double [][] qMat(
double [] q){
return new double [][] {
{q[0],-q[1],-q[2],-q[3]},
{q[1], q[0], q[3],-q[2]},
{q[2],-q[3], q[0], q[1]},
{q[3], q[2],-q[1], q[0]}};
}
}
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