Commit 767ae031 authored by Andrey Filippov's avatar Andrey Filippov

Debugging OrientationSceneLMA - working, will clean up

parent f9ea6e08
......@@ -78,6 +78,9 @@ public class OrthoAltitudeMatch {
boolean do_not_save = true;
// OrientationSceneLMA.testGetPairErrQuaternion ();
OrientationSceneLMA.testGetPairPairScaleDirError();
if (test_quat0) {
QuatUtils.testQuatAff();
}
......
......@@ -6286,6 +6286,11 @@ public class OrthoMapsCollection implements Serializable{
public boolean altutudeMatchPairs(
CLTParameters clt_parameters,
String orthoMapsCollection_path) {
//OrientationSceneLMA.testGetPairErrQuaternion ();
// Create list of all pairs (after recreating all overlaps with updated affines)
ArrayList<Point> pairs_list = new ArrayList<Point>();
boolean dbg00 = false;
......
......@@ -70,6 +70,7 @@ public final class QuatUtils { // static class
/**
* Differential d_(p*q)/dp
* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
* Seems that (11) for []x is transposed!
* @param p quaternion
* @param q quaternion
* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of p,
......@@ -79,16 +80,57 @@ public final class QuatUtils { // static class
double [] p,
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]}
/*
{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]}
*/
};
}
public static double [][] d_pq_dp(
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]}
};
}
public static double [][] d_pq_dp(
double [] p,
double [] q,
double delta) {
double [][] d_pq_dp_delta=new double[4][4];
for (int npar = 0; npar < 4; npar++) {
double [] vpm = p.clone();
vpm[npar] += 0.5*delta;
double [] qd_p = multiply(vpm, q);
vpm[npar] -= delta;
double [] qd_m = multiply(vpm, q);
for (int i = 0; i < 4; i++) {
d_pq_dp_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
}
}
return d_pq_dp_delta;
}
/**
* Differential d_(p*q)/dq
* https://faculty.sites.iastate.edu/jia/files/inline-files/quaternion.pdf
* Seems that (11) for []x is transposed!
* @param p quaternion
* @param q quaternion
* @return matrix[4][4], where rows correspond to elements of quaternion (p*q), and columns - indices of q,
......@@ -98,13 +140,51 @@ public final class QuatUtils { // static class
double [] p,
double [] q) {
return new double[][] {
{p[0],-p[1],-p[2], -p[3]},
{p[1], p[0], p[3], -p[2]},
{p[2],-p[3], p[0], p[1]},
{p[3], p[2],-p[1], p[0]}
/*
{p[0],-p[1],-p[2], -p[3]},
{p[1], p[0],-p[3], p[2]},
{p[2], p[3], p[0], -p[1]},
{p[3],-p[2], p[1], p[0]}
*/
};
}
public static double [][] d_pq_dq(
double [] p) {
return new double[][] {
{p[0],-p[1],-p[2], -p[3]},
{p[1], p[0], p[3], -p[2]},
{p[2],-p[3], p[0], p[1]},
{p[3], p[2],-p[1], p[0]} };
}
public static double [][] d_pq_dq(
double [] p,
double [] q,
double delta) {
double [][] d_pq_dq_delta=new double[4][4];
for (int npar = 0; npar < 4; npar++) {
double [] vpm = q.clone();
vpm[npar] += 0.5*delta;
double [] qd_p = multiply(p, vpm);
vpm[npar] -= delta;
double [] qd_m = multiply(p, vpm);
for (int i = 0; i < 4; i++) {
d_pq_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
}
}
return d_pq_dq_delta;
}
/**
* Derivative of inverse quanternion
* @param q quternion
......@@ -123,6 +203,25 @@ public final class QuatUtils { // static class
};
}
public static double [][] d_invert_dq(
double [] q,
double delta) {
double [][] d_invert_dq_delta=new double[4][4];
for (int npar = 0; npar < 4; npar++) {
double [] vpm = q.clone();
vpm[npar] += 0.5*delta;
double [] qd_p = invert(vpm);
vpm[npar] -= delta;
double [] qd_m = invert(vpm);
for (int i = 0; i < 4; i++) {
d_invert_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
}
}
return d_invert_dq_delta;
}
public static double [][] dnormalize_dq(
double [] q){
double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
......@@ -135,6 +234,26 @@ public final class QuatUtils { // static class
{ -q3*q0 /l3, -q3*q1 /l3, -q3*q2 /l3, (l2 - q3*q3)/l3}
};
}
public static double [][] dnormalize_dq(
double [] q,
double delta) {
double [][] dnormalize_dq_delta=new double[4][4];
for (int npar = 0; npar < 4; npar++) {
double [] vpm = q.clone();
vpm[npar] += 0.5*delta;
double [] qd_p = normalize(vpm);
vpm[npar] -= delta;
double [] qd_m = normalize(vpm);
for (int i = 0; i < 4; i++) {
dnormalize_dq_delta[i][npar] = (qd_p[i]-qd_m[i])/delta;
}
}
return dnormalize_dq_delta;
}
public static double [] dscale_dq(
double [] q){
double q0= q[0],q1=q[1],q2=q[2],q3=q[3];
......@@ -142,6 +261,21 @@ public final class QuatUtils { // static class
return new double [] {q0/l, q1/l, q2/l,q3/l};
}
public static double [] dscale_dq(
double [] q,
double delta) {
double [] dscale_dq_delta=new double[4];
for (int npar = 0; npar < 4; npar++) {
double [] vpm = q.clone();
vpm[npar] += 0.5*delta;
double qd_p = norm(vpm);
vpm[npar] -= delta;
double qd_m = norm(vpm);
dscale_dq_delta[npar] = (qd_p-qd_m)/delta;
}
return dscale_dq_delta;
}
/**
......
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