Commit bc9e1a60 authored by Andrey Filippov's avatar Andrey Filippov

just a working snapshot

parent 767ae031
...@@ -111,7 +111,6 @@ public class OrientationSceneLMA { ...@@ -111,7 +111,6 @@ public class OrientationSceneLMA {
return weights.length; return weights.length;
} }
private double [] getFxDerivs( private double [] getFxDerivs(
final double [] vector, final double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][] final double [][] jt, // should be null or initialized with [vector.length][]
...@@ -124,6 +123,117 @@ public class OrientationSceneLMA { ...@@ -124,6 +123,117 @@ public class OrientationSceneLMA {
} }
} }
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
// first cycle - minimize per-pair errors (differences between singular values)
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
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 [] q2iq1q12;
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,
fX,
fx_frag.length*npair,
fx_frag.length);
if (jt!=null) {
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]); // /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];
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
// second cycle - regularization (average scale diffs, q1,q2,q3}
double [][] pull_threads = new double [threads.length][4];
final int pull_index = num_pairs*4; // start of pull sums in fx
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int nthread = ati.getAndIncrement();
double [] qscene = new double[4];
for (int nscene = ai.getAndIncrement(); nscene < num_scenes; nscene = ai.getAndIncrement()) {
System.arraycopy(vector, nscene*4, qscene, 0, 4);
double [] qscene_norm=QuatUtils.normalize(qscene); // use q1,q2,q3, maybe q1 and q2 (from DEM) lower weight than q3 (from images)
double scale_diff = QuatUtils.norm(qscene)-1.0;
qscene_norm[0] = scale_diff; // {scale_diff, qn1, qn2, qn3};
for (int i = 0; i < 4; i++) {
pull_threads[nthread][i] += qscene_norm[i];
}
if (jt != null) {
double [][] dnq_dq = QuatUtils.dnormalize_dq(qscene);
double [] dscale = QuatUtils.dscale_dq(qscene);
dnq_dq[0] = dscale;
for (int npar = 0; npar <4; npar++) {
System.arraycopy(
dnq_dq[npar],
0 ,
jt[nscene*4+npar],
pull_index,
4);
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// sum partial sums from threads
for (int nthread = 0; nthread < pull_threads.length; nthread++ ) {
for (int i = 0; i < 4; i++) {
fX[pull_index+i] += pull_threads[nthread][i];
}
}
return fX;
}
public double [] getFxDerivs_debug(
final double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level)
{
double [] fX = new double [weights.length]; // num_pairs + vector.length];
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
jt[i] = new double [weights.length]; // weights.length];
}
}
final Thread[] threads = ImageDtt.newThreadArray(); final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0); final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0); final AtomicInteger ati = new AtomicInteger(0);
...@@ -265,6 +375,9 @@ public class OrientationSceneLMA { ...@@ -265,6 +375,9 @@ public class OrientationSceneLMA {
return fX; return fX;
} }
private double [][] getFxDerivsDelta( private double [][] getFxDerivsDelta(
double [] vector, double [] vector,
final double delta, final double delta,
......
...@@ -79,7 +79,7 @@ public class OrthoAltitudeMatch { ...@@ -79,7 +79,7 @@ public class OrthoAltitudeMatch {
boolean do_not_save = true; boolean do_not_save = true;
// OrientationSceneLMA.testGetPairErrQuaternion (); // OrientationSceneLMA.testGetPairErrQuaternion ();
OrientationSceneLMA.testGetPairPairScaleDirError(); // OrientationSceneLMA.testGetPairPairScaleDirError();
if (test_quat0) { if (test_quat0) {
QuatUtils.testQuatAff(); QuatUtils.testQuatAff();
......
...@@ -133,6 +133,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -133,6 +133,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
public transient int num_scenes = -1;; // number of scenes that made up this image public transient int num_scenes = -1;; // number of scenes that made up this image
public transient double sfm_gain = Double.NaN; // maximal SfM gain of this map public transient double sfm_gain = Double.NaN; // maximal SfM gain of this map
public transient double [] equalize = {1,0}; // rectified value = equalize[0]*source_value+equalize[1] public transient double [] equalize = {1,0}; // rectified value = equalize[0]*source_value+equalize[1]
public transient double [] qorient = null; // quternion representing orientation+scale of the scene
private void writeObject(ObjectOutputStream oos) throws IOException { private void writeObject(ObjectOutputStream oos) throws IOException {
// temporary fix: // temporary fix:
// double [][] affine_clone = {affine[0].clone(), affine[1].clone()}; // double [][] affine_clone = {affine[0].clone(), affine[1].clone()};
...@@ -191,6 +192,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -191,6 +192,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
// pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>(); // pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
} }
public double [] getQOrinet() {
return qorient;
}
public void setQOrient(double [] q) {
qorient = q;
}
public double [][] getERSAffine(){ public double [][] getERSAffine(){
return ers_affine; return ers_affine;
} }
......
...@@ -41,6 +41,7 @@ public class PairwiseOrthoMatch implements Serializable { ...@@ -41,6 +41,7 @@ public class PairwiseOrthoMatch implements Serializable {
public transient double [] alt_data = null; public transient double [] alt_data = null;
public transient double [] equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1] public transient double [] equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]
public transient double [] quat = null; // relative orientation of the second scene (ERS affine removed). Will be eventually saved public transient double [] quat = null; // relative orientation of the second scene (ERS affine removed). Will be eventually saved
public transient double [][] qaffine = null; // affine relative to rotated scenes
// below - not saved/restored // below - not saved/restored
public transient boolean ok = false; // not saved/restored public transient boolean ok = false; // not saved/restored
public transient int [] nxy = null; // not saved, just to communicate for logging public transient int [] nxy = null; // not saved, just to communicate for logging
...@@ -52,6 +53,13 @@ public class PairwiseOrthoMatch implements Serializable { ...@@ -52,6 +53,13 @@ public class PairwiseOrthoMatch implements Serializable {
this.quat = quat; this.quat = quat;
} }
public double [][] getQAffine() {
return qaffine;
}
public void setQAffine(double [][] qaffine) {
this.qaffine = qaffine;
}
// public PairwiseOrthoMatch() {} // public PairwiseOrthoMatch() {}
public double getOverlap() { public double getOverlap() {
return overlap; return overlap;
...@@ -120,6 +128,8 @@ public class PairwiseOrthoMatch implements Serializable { ...@@ -120,6 +128,8 @@ public class PairwiseOrthoMatch implements Serializable {
pom.equalize1to0 = this.equalize1to0.clone(); pom.equalize1to0 = this.equalize1to0.clone();
pom.ok = this.ok; pom.ok = this.ok;
pom.alt_data = this.alt_data.clone(); pom.alt_data = this.alt_data.clone();
pom.quat = (quat!=null)? quat.clone() : null;
pom.qaffine = (qaffine!=null) ? (new double [][] {qaffine[0].clone(),qaffine[1].clone()}):null;
return pom; return pom;
} }
/** /**
......
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