Skip to content

I
imagej-elphel
Commit 767ae031 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Debugging OrientationSceneLMA - working, will clean up

parent f9ea6e08
Hide whitespace changes
Inline Side-by-side
Showing with 520 additions and 16 deletions
src/main/java/com/elphel/imagej/orthomosaic/OrientationSceneLMA.java
View file @ 767ae031
......@@ -47,7 +47,10 @@ public class OrientationSceneLMA {
private double [][] last_jt = null;
public boolean last3only = false; // true; // debug feature
public double delta= 1e-7;
public double delta= 1e-5; // 7;
public boolean debug_deriv = true;
public int debug_width = 12;
public int debug_decimals = 9;
public int prepareLMA(
......@@ -131,15 +134,38 @@ public class OrientationSceneLMA {
double [] qscene1 = new double[4];
double [] qscene2 = new double[4];
double [][][] derivs = (jt!=null) ? (new double [2][][]): null;
double [][][] qderivs = (jt != null) ? (new double [2][][]) : null;
for (int npair = ai.getAndIncrement(); npair < num_pairs; npair = ai.getAndIncrement()) {
int [] scene_ind = {4 * cpairs[npair][0], 4 * cpairs[npair][1]};
System.arraycopy(vector, scene_ind[0], qscene1, 0, 4);
System.arraycopy(vector, scene_ind[1], qscene2, 0, 4);
double [] fx_frag =getPairScaleDirError( //{scale-1, qn1,qn2,qn3}
qscene1, // double [] qscene1,
qscene2, // double [] qscene2,
qpairs[npair], // double [] qpair12,
derivs); // double [][][] derivs)
double [] q2iq1q12;
if (debug_deriv && (debug_level>10)) {
q2iq1q12 = getPairErrQuaternion(
qscene1, // double [] qscene1,
qscene2, // double [] qscene2,
qpairs[npair], // double [] qpair12,
qderivs, // double [][][] qderivs)
delta, // double delta,
debug_width, // int fmt_width,
debug_decimals, // int fmt_decimals,
npair); // int npair);
} else {
q2iq1q12 = getPairErrQuaternion( // qderivs here tested
qscene1, // double [] qscene1,
qscene2, // double [] qscene2,
qpairs[npair], // double [] qpair12,
qderivs); // double [][][] qderivs)
}
double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
double [] fx_frag = {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};
System.arraycopy(
fx_frag,
0,
......@@ -147,14 +173,46 @@ public class OrientationSceneLMA {
fx_frag.length*npair,
fx_frag.length);
if (jt!=null) {
double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
qscene1, // double [] q1,
qscene2, // double [] q2,
qpairs[npair],
delta); // double delta)
// printDerivDelta("npair="+npair+" qderivs[0], delta="+delta, qderivs[0], qderivs_delta[0],debug_width,debug_decimals);
// printDerivDelta("npair="+npair+" qderivs[1], delta="+delta, qderivs[1], qderivs_delta[1],debug_width,debug_decimals);
double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
if (debug_deriv) {
double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
printDerivDelta("npair="+npair+" dq2iq1q12_norm, delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
printDerivDelta("npair="+npair+" dscale_dq, delta="+delta, dscale, dscale_delta,debug_width,debug_decimals);
}
dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
if (debug_deriv) {
double [][] dq2iq1q12_norm_delta=QuatUtils.dnormalize_dq(q2iq1q12_norm,delta);
double [] dscale_delta = QuatUtils.dscale_dq(q2iq1q12_norm, delta);
dq2iq1q12_norm_delta[0] = dscale_delta; // replace first row (q_norm/dq) with dsca/e/dq
printDerivDelta("npair="+npair+" dq2iq1q12_normD,delta="+delta, dq2iq1q12_norm, dq2iq1q12_norm_delta,debug_width,debug_decimals);
}
derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq2 (scene 2)
for (int iscene = 0; iscene <2; iscene++) {
for (int npar = 0; npar <4; npar++) {
for (int i = 0; i < 4; i++) {
jt[scene_ind[iscene]+npar][fx_frag.length*npair+i] = derivs[iscene][i][npar];
}
/*
System.arraycopy(
derivs[iscene][npar], // null
0,
jt[scene_ind[iscene]+npar],
fx_frag.length*npair,
fx_frag.length);
*/
}
}
}
......@@ -268,8 +326,9 @@ public class OrientationSceneLMA {
errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
}
}
for (int i = 0; i < vector.length; i++) {
System.out.print("\t\t");
System.out.print(String.format("%3s","err"));
for (int i = 0; i < 2* vector.length; i++) {
System.out.print(String.format("\t%12s",""));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",errors[i]));
......@@ -647,7 +706,7 @@ public class OrientationSceneLMA {
* @return
*/
public static double [] getPairErrQuaternion(
public static double [] getPairErrQuaternion0(
double [] qscene1,
double [] qscene2,
double [] qpair12,
......@@ -668,11 +727,240 @@ public class OrientationSceneLMA {
return q2iq1q12;
}
public static double [] getPairErrQuaternion(
double [] q1,
double [] q2,
double [] q12,
double [][][] qderivs) {
double [] iq2 = QuatUtils.invert(q2);
double [] iq2q1 = QuatUtils.multiply(iq2, q1);
double [] iq2q1q12 = QuatUtils.multiply(iq2q1, q12);
if (qderivs != null) {
double [][] diq2_dq2 = QuatUtils.d_invert_dq(q2);
double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
double [][] diq2q1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq2);
double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iq2);
double [][] diq2q1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(q12),diq2q1_dq1);
qderivs[0] = diq2q1q12_dq1;
qderivs[1] = diq2q1q12_dq2;
}
return iq2q1q12;
}
public static double [] getPairErrQuaternion( // test only
double [] q1,
double [] q2,
double [][][] qderivs) {
double [] iq2 = QuatUtils.invert(q2);
double [] iq2q1 = QuatUtils.multiply(iq2, q1);
if (qderivs != null) {
double [][] diq2_dq2 = QuatUtils.d_invert_dq(q2);
double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(q1), diq2_dq2);
double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iq2);
qderivs[0] = diq2q1_dq1;
qderivs[1] = diq2q1_dq2;
}
return iq2q1;
}
public static double [][][] getPairErrQuaternionDelta( // test only
double [] q1,
double [] q2,
double delta) {
double [][][] qderivs = new double [2][4][4];
for (int nscene = 0; nscene <2; nscene++) {
for (int npar = 0; npar < 4; npar++) {
double [][] vpm = new double [][] {q1.clone(),q2.clone()};
vpm[nscene][npar] += 0.5*delta;
double [] qd_p = getPairErrQuaternion(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
null); // double [][][] qderivs)
vpm[nscene][npar]-= delta;
double [] qd_m = getPairErrQuaternion(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
null); // double [][][] qderivs)
for (int i = 0; i < 4; i++) {
qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
}
// System.out.println("nscene="+nscene+", npar="+npar);
}
}
return qderivs;
}
public static void testGetPairErrQuaternion () {
double delta = 1e-5;
int debug_width = 12,debug_decimals=9;
double [] q1 = {0.9995, 0.01, 0.015, 0.02};
double [] q2 = {0.999, 0.02, 0.01, 0.015};
double [] q12 = {1.1, 0.1, 0.15, -0.2};
double [][][] qderivs = new double [2][4][4];
for (int n = 0; n < 10; n++) {
getPairErrQuaternion( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
qderivs, // double [][][] qderivs)
delta, // double delta,
debug_width, // int fmt_width,
debug_decimals, // int fmt_decimals,
0); // int npair) { //
double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
delta); // double delta)
printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
System.out.println();
}
}
public static void testGetPairPairScaleDirError () {
double delta = 1e-5;
int debug_width = 12,debug_decimals=9;
double [] q1 = {0.9995, 0.01, 0.015, 0.02};
double [] q2 = {0.999, 0.02, 0.01, 0.015};
double [] q12 = {1.1, 0.1, 0.15, -0.2};
double [][][] qderivs = new double [2][4][4];
for (int n = 0; n < 10; n++) {
/*
getPairErrQuaternion( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
qderivs); // double [][][] qderivs)
*/
/*
getPairErrQuaternion( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
qderivs, // double [][][] qderivs)
delta, // double delta,
debug_width, // int fmt_width,
debug_decimals, // int fmt_decimals,
0); // int npair) { //
*/
getPairScaleDirError(
q1, // double [] q1,
q2, // double [] q2,
q12,
qderivs); // double [][][] qderivs)
/*
double [][][] qderivs_delta = getPairErrQuaternionDelta( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
delta); // double delta)
*/
double [][][] qderivs_delta = getPairScaleDirError( // test only
q1, // double [] q1,
q2, // double [] q2,
q12,
delta); // double delta)
printDerivDelta(" scene1, delta="+delta, qderivs[0], qderivs_delta[0], debug_width,debug_decimals);
printDerivDelta(" scene2, delta="+delta, qderivs[1], qderivs_delta[1], debug_width,debug_decimals);
System.out.println();
}
}
public static double [] getPairErrQuaternion(
double [] qscene1,
double [] qscene2,
double [] qpair12,
double [][][] qderivs,
double delta,
int fmt_width,
int fmt_decimals,
int npair) { //
double [] iqscene2 = QuatUtils.invert(qscene2);
double [] iq2q1 = QuatUtils.multiply(iqscene2, qscene1);
double [] q2iq1q12 = QuatUtils.multiply(iq2q1, qpair12);
if (qderivs != null) {
double [][] diqscene2_dq2 = QuatUtils.d_invert_dq(qscene2);
{
double [][] diqscene2_dq2_delta=QuatUtils.d_invert_dq(qscene2,delta);
printDerivDelta("npair="+npair+" diqscene2_dq2, delta="+delta, diqscene2_dq2, diqscene2_dq2_delta,fmt_width,fmt_decimals);
}
double [][] diq2q1_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1), diqscene2_dq2);
{
double [][] diq2q1_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iqscene2,qscene1, delta), diqscene2_dq2);
printDerivDelta("npair="+npair+" diq2q1_dq2, delta="+delta, diq2q1_dq2, diq2q1_dq2_delta,fmt_width,fmt_decimals);
}
double [][] dq2iq1q12_dq2 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq2);
{
double [][] dq2iq1q12_dq2_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq2);
printDerivDelta("npair="+npair+" dq2iq1q12_dq2, delta="+delta, dq2iq1q12_dq2, dq2iq1q12_dq2_delta,fmt_width,fmt_decimals);
}
double [][] diq2q1_dq1 = QuatUtils.d_pq_dq(iqscene2,qscene1);
{
double [][] diq2q1_dq1_delta= QuatUtils.d_pq_dq(iqscene2,qscene1,delta);
printDerivDelta("npair="+npair+" diq2q1_dq1, delta="+delta, diq2q1_dq1, diq2q1_dq1_delta,fmt_width,fmt_decimals);
}
double [][] dq2iq1q12_dq1 = QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12),diq2q1_dq1);
{
double [][] dq2iq1q12_dq1_dq1_delta= QuatUtils.matMult(QuatUtils.d_pq_dp(iq2q1,qpair12,delta),diq2q1_dq1);
printDerivDelta("npair="+npair+" dq2iq1q12_dq1, delta="+delta, dq2iq1q12_dq1, dq2iq1q12_dq1_dq1_delta,fmt_width,fmt_decimals);
}
qderivs[0] = dq2iq1q12_dq1;
qderivs[1] = dq2iq1q12_dq2;
}
return q2iq1q12;
}
public static double [][][] getPairErrQuaternionDelta(
double [] qscene1,
double [] qscene2,
double [] qpair12,
double delta) {
double [][][] qderivs = new double [2][4][4];
for (int nscene = 0; nscene <2; nscene++) {
for (int npar = 0; npar < 4; npar++) {
double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
vpm[nscene][npar] += 0.5*delta;
double [] qd_p = getPairErrQuaternion(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
qpair12,// double [][][] qderivs),
null); // double [][][] qderivs)
vpm[nscene][npar]-= delta;
double [] qd_m = getPairErrQuaternion(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
qpair12,// double [][][] qderivs),
null); // double [][][] qderivs)
for (int i = 0; i < 4; i++) {
qderivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
}
}
}
return qderivs;
}
public static double [] getPairScaleDirError(
double [] qscene1,
double [] qscene2,
double [] qpair12,
double [][][] derivs) {
boolean truncate = false; // true;
double [][][] qderivs = (derivs != null) ? (new double [2][][]) : null;
double [] q2iq1q12 = getPairErrQuaternion(
qscene1, // double [] qscene1,
......@@ -682,15 +970,89 @@ public class OrientationSceneLMA {
double [] q2iq1q12_norm=QuatUtils.normalize(q2iq1q12); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
double scale_diff = QuatUtils.norm(q2iq1q12)-1.0;
if (derivs != null) {
double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12_norm);
double [] dscale = QuatUtils.dscale_dq(q2iq1q12_norm);
dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq1 (scene 2)
if (truncate) {
derivs[0] = qderivs[0];
derivs[1] = qderivs[1];
} else {
double [][] dq2iq1q12_norm = QuatUtils.dnormalize_dq(q2iq1q12);
double [] dscale = QuatUtils.dscale_dq(q2iq1q12);
dq2iq1q12_norm[0] = dscale; // replace first row (q_norm/dq) with dsca/e/dq
derivs[0] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[0]); // /dq1 (scene 1)
derivs[1] = QuatUtils.matMult(dq2iq1q12_norm,qderivs[1]); // /dq1 (scene 2)
}
}
if (truncate) return q2iq1q12;
return new double [] {scale_diff, q2iq1q12_norm[1], q2iq1q12_norm[2], q2iq1q12_norm[3]};
}
public static double [][][] getPairScaleDirError(
double [] qscene1,
double [] qscene2,
double [] qpair12,
double delta) {
double [][][] sd_derivs = new double [2][4][4];
for (int nscene = 0; nscene <2; nscene++) {
for (int npar = 0; npar < 4; npar++) {
double [][] vpm = new double [][] {qscene1.clone(),qscene2.clone()};
vpm[nscene][npar] += 0.5*delta;
double [] qd_p = getPairScaleDirError(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
qpair12,// double [][][] qderivs),
null); // double [][][] qderivs)
vpm[nscene][npar]-= delta;
double [] qd_m = getPairScaleDirError(
vpm[0], // double [] qscene1,
vpm[1], // double [] qscene2,
qpair12,// double [][][] qderivs),
null); // double [][][] qderivs)
for (int i = 0; i < 4; i++) {
sd_derivs[nscene][i][npar] = (qd_p[i] - qd_m[i])/delta;
}
}
}
return sd_derivs;
}
public static void printDerivDelta(String name, double [] deriv, double [] delta, int width, int decimals) {
double max_err = 0;
double [] diff= new double [deriv.length];
String fmt ="\t%"+width+"."+decimals+"f";
for (int i = 0; i < diff.length; i++) {
diff[i] = deriv[i]-delta[i];
max_err = Math.max(max_err, Math.abs(diff[i]));
}
System.out.println (name+", max_err = "+max_err);
System.out.print ("deriv ");
for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, deriv[i]));
System.out.println();
System.out.print ("delta ");
for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, delta[i]));
System.out.println();
System.out.print (" diff ");
for (int i = 0; i < diff.length; i++) System.out.print(String.format(fmt, diff[i]));
System.out.println();
}
public static void printDerivDelta(String name, double [][] deriv2, double [][] delta2, int width, int decimals) {
int l = deriv2[0].length;
double [] deriv = new double [deriv2.length*l];
double [] delta = new double [deriv.length];
for (int i = 0; i < deriv2.length; i++) {
for (int j = 0; j <l; j++) {
deriv[i*l+j] = deriv2[i][j];
delta[i*l+j] = delta2[i][j];
}
}
printDerivDelta(name, deriv, delta, width, decimals);
}
}
src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java
View file @ 767ae031
......@@ -78,6 +78,9 @@ public class OrthoAltitudeMatch {
boolean do_not_save = true;
// OrientationSceneLMA.testGetPairErrQuaternion ();
OrientationSceneLMA.testGetPairPairScaleDirError();
if (test_quat0) {
QuatUtils.testQuatAff();
}
......
src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java
View file @ 767ae031
......@@ -6286,7 +6286,12 @@ public class OrthoMapsCollection implements Serializable{
public boolean altutudeMatchPairs(
CLTParameters clt_parameters,
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>();
boolean dbg00 = false;
for (OrthoMap map : ortho_maps) {
......
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java
View file @ 767ae031
......@@ -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,12 +140,50 @@ 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
......@@ -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