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