Skip to content

I
imagej-elphel
Commit 83ce74fe authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

LMA to adjust scene's presumably LMA-related affines (not tilt-related)

parent 203fad06
Hide whitespace changes
Inline Side-by-side
Showing with 1555 additions and 106 deletions
src/main/java/com/elphel/imagej/orthomosaic/ERSTiltLMA.java 0 → 100644
View file @ 83ce74fe
/**
** ERSTiltLMA - Extract ERS-related errors (affine distortions that do not match
** known ground plane tilts) by comparing multiple overlapping pairs. If there were
** no such errors, then combined affine transforms calculated from the local ground
** plane tilts should match affine transforms for image pairs. Additional condition -
** minimization of the per-scene ERS correction transforms (weighted by the number
** of participating pairs and their overlap, and ???)
**
** The ERS errors possibly come for faulty absolute ERS correction that was not
** tested - only relative to a single reference scene. Maybe it can be improved in
** the future.
**
** Copyright (C) 2024 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** PairwiseOrthoMatch.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.orthomosaic;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.tileprocessor.ImageDtt;
import Jama.Matrix;
public class ERSTiltLMA {
public int [] indices;
public int [][] cpairs = null;
public int num_scenes = 0;
public int num_pairs = 0;
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
private double [] parameters_vector = null;
// private double [] x_vector = null; // not used. Save total weight
// private double [] y_vector = null;
// private double weight = 0; // total weight
private double weight_pure = 0;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double [] last_ymfx = null;
private double [][] last_jt = null;
public double [][][] aff_pairs_nosr = null;
public double [][][][] aff_tilts = null;
public boolean invert_q2a = false;
// debug
public boolean last3only = false; // true; // debug feature
public double scale_pairs = 1e3; // 1e4; // increase fX and derivatives for pairs
public double delta= 1e-7;
public double fx_scale = 1;
public double [][] getPseudoXY(){
double [][] pseudo_xy = new double [num_scenes][];
for (int nscene = 0; nscene < num_scenes; nscene++) {
pseudo_xy[nscene] = new double [] {parameters_vector[2*nscene], parameters_vector[2*nscene+1]};
}
return pseudo_xy;
}
public double [][] getPseudoXY(int npair){
return getPseudoXY(parameters_vector, npair);
}
public double [][] getPseudoXY(double [] vector, int npair){
double [][] pseudo_xy= new double[2][];
for (int i = 0; i < 2; i++) {
pseudo_xy[i] = new double [] {vector[2*cpairs[npair][i]],vector[2*cpairs[npair][i]+1]};
}
return pseudo_xy;
}
public double [][][] getERSAffines(){
double [][][] ers_affines = new double [num_scenes][][];
for (int nscene = 0; nscene < num_scenes; nscene++) {
ers_affines[nscene] = QuatUtils.pseudoTiltToAffine(new double [] {parameters_vector[2*nscene], parameters_vector[2*nscene+1]});
}
return ers_affines;
}
public SingularValueDecomposition [] getSVD() {
SingularValueDecomposition [] svd = new SingularValueDecomposition[num_scenes];
double [][][] ers_affines = getERSAffines();
for (int nscene = 0; nscene < num_scenes; nscene++) {
svd[nscene] = SingularValueDecomposition.singularValueDecompose(ers_affines[nscene]);
}
return svd;
}
public void printResults(boolean degrees) {
double [][] pseudo_xy = getPseudoXY();
double [][][] ers_affines = getERSAffines();
SingularValueDecomposition [] svd = getSVD();
String svd_title = SingularValueDecomposition.titleString(degrees);
System.out.println(String.format("%4s\t%4s\t%11s\t%11s\t%11s\t%s\t%11s\t%11s\t%11s\t%11s",
"#", "scn", "px","py","r",svd_title,"aff[0][0]","aff[0][1]","aff[1][0]","aff[1][1]"));
for (int nscene=0; nscene < num_scenes; nscene++) {
double px = pseudo_xy[nscene][0], py = pseudo_xy[nscene][1];
double r = Math.sqrt(px*px+py*py);
double [][] aff = ers_affines[nscene];
String ssvd = svd[nscene].toString(degrees, 1);
System.out.println(String.format("%4d\t%4d\t%11.8f\t%11.8f\t%11.8f\t%s\t%11.8f\t%11.8f\t%11.8f\t%11.8f",
nscene, indices[nscene], px,py,r,ssvd,aff[0][0],aff[0][1],aff[1][0],aff[1][1]));
}
return;
}
/*
* Starting with tilts0 and tilt1 = tilt0+tilt_diff (differential
* tilt between the two scenes has better accuracy than individual
* scenes ground planes tilts as it less depends on the scene
* flatness - maybe compare difference tilt to the difference of the
* individual ones to determine applicability of this method.
* Then affine transforms for each of the scenes relative to the local
* ground surface plane is:
* AFF0= AFF_ERS0 * AFF_TILT0 // (AFF_TILT0 calculated from the TILT0 only)
* and
* AFF1= AFF_ERS1 * AFF_TILT1
* AFF_DIFF = AFF_ERS1 * AFF_ERS0.inverse() - affine transform from scene0
* to scene 1 that should match independently measured (from image comparison)
* AFF_PAIR_NOROT (with rotation and scale removed)
* The AFF_ERR = AFF_DIFF * AFF_PAIR_NOROT.inverse() should have tilt minimized,
* and scale ignored.
*
* AFF_ERS<0,1> are defined by direction angle (beta=-gamma as no rotation) and
* k=w2/w1 = w2 assuming w1==1 >= w2
*
* So there will be 4 parameters per pair and (1-AFF_ERR.w2/AFF_ERR.w1) as output
*
*/
public int prepareLMA(
int [] indices, // should all be used
int [][] cpairs,
double [] weights_scenes, // sfm, number used?
double [] weights_pairs, // from matching tilts(flatness) (and worst sfm, per-pair rmse)?
double weight_pairs_k,
double [][][] tilts, // [pair][scene(2)][tilt(2)]
double [][][] affine_pairs,
int debug_level) {
this.indices = indices; // maybe not needed
this.cpairs = cpairs;
num_scenes = indices.length;
num_pairs = cpairs.length;
aff_pairs_nosr = new double [num_pairs][][];
aff_tilts = new double [num_pairs][2][][];
for (int npair = 0; npair < num_pairs; npair++) {
double [][] affine_pair_nors = SingularValueDecomposition.removeTiltRotScale(
affine_pairs[npair], // double [][] A,
false, // boolean removeTilt,
true, // boolean removeRot,
true, // boolean removeScale,
false, // boolean removeOffset,
false); // boolean max_is_scale);
aff_pairs_nosr[npair] = QuatUtils.matInverse2x2(affine_pair_nors); // maybe reverse order and do not use inversion?
for (int i = 0; i < 2; i++) {
aff_tilts[npair][i] = QuatUtils.tiltToAffine( // convert tilts to affines as they will not be modified later
tilts[npair][i]); // double [] tilt,
}
}
parameters_vector = new double [2 * num_scenes];
weights = new double [num_pairs + 2 * num_scenes];
double sum_weights = 0;
for (int i = 0; i < num_pairs; i++) {
weights[i] = weights_pairs[i] * weight_pairs_k;
sum_weights += weights[i];
}
weight_pure= sum_weights;
for (int i= 0; i < num_scenes; i++) {
weights[num_pairs + 2*i + 0] = weights_scenes[i];
weights[num_pairs + 2*i + 1] = weights_scenes[i];
sum_weights += 2 * weights_scenes[i];
}
double k = 1.0/sum_weights;
for (int i = 0; i < weights.length; i++) {
weights[i] *= k;
}
weight_pure *= k;
last_jt = new double [parameters_vector.length][];
return weights.length;
}
private double [] getFxDerivs(
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 AtomicInteger ai = 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() {
for (int npair = ai.getAndIncrement(); npair < num_pairs; npair = ai.getAndIncrement()) {
/*
double [][] pseudo_xy= new double[2][];
for (int i = 0; i < 2; i++) {
pseudo_xy[i] = new double [] {vector[2*cpairs[npair][i]],vector[2*cpairs[npair][i]+1]};
}
*/
double [][] pseudo_xy = getPseudoXY(vector,npair);
double [][][] aff_err = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
pseudo_xy, // double [][] txy, // [scene][direction]
aff_tilts[npair], // double [][][] a_tilts,
aff_pairs_nosr[npair], // double [][] iaff_pair, // affine pair inversed
invert_q2a); // boolean invert_q2a) // invert result affines (to match "usual")
if (jt == null) {
aff_err = new double [][][] {aff_err[0]}; //
}
double [][] WdW = SingularValueDecomposition.getMinMaxEigenValues(
aff_err); // double [][][] AdA)
fX[npair] = scale_pairs * (WdW[0][1] - WdW[0][0]);
if (jt != null) {
for (int nscene = 0; nscene < 2; nscene++) {
for (int j = 0; j < 2; j++) { // 2 parameters per scene
jt[2*cpairs[npair][nscene]+j][npair] = scale_pairs* (WdW[2*nscene+j+1][1] - WdW[2*nscene+j+1][0]);
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
// second cycle - minimize ERS corrections (tilts)
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nscene = ai.getAndIncrement(); nscene < num_pairs; nscene = ai.getAndIncrement()) {
int pindx = num_pairs + 2*nscene;
for (int j = 0; j < 2; j++) {
fX[pindx+j] = vector[2*nscene + j];
if (jt != null) {
// jt[2*nscene + 1][pindx] = 1.0; // only for w, does not depend on beta
jt[2*nscene + j][pindx + j] = 1.0; // for both pseudo_x and pseudo-Y
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return fX;
}
private double [][] getFxDerivsDelta(
double [] vector,
final double delta,
final int debug_level) {
double [][] jt = new double [vector.length][weights.length];
for (int nv = 0; nv < vector.length; nv++) {
double [] vpm = vector.clone();
vpm[nv]+= 0.5*delta;
double [] fx_p = getFxDerivs(
vpm,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level);
vpm[nv]-= delta;
double [] fx_m = getFxDerivs(
vpm,
null, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level);
for (int i = 0; i < weights.length; i++) if (weights[i] > 0) {
jt[nv][i] = (fx_p[i]-fx_m[i])/delta;
}
}
return jt;
}
private double compareJT(
double [] vector,
double delta,
boolean last3only) { // do not process samples - they are tested before
double [] errors=new double [vector.length];
double [][] jt = new double [vector.length][];
System.out.print("Parameters vector = [");
for (int i = 0; i < vector.length; i++) {
System.out.print(vector[i]);
if (i < (vector.length -1)) System.out.print(", ");
}
System.out.println("]");
getFxDerivs(
vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
1); // debug_level);
double [][] jt_delta = getFxDerivsDelta(
vector, // double [] vector,
delta, // final double delta,
-1); // final int debug_level)
int start_index = last3only? (weights.length-3) : 0;
for (int n = start_index; n < weights.length; n++) if (weights[n] > 0) {
System.out.print(String.format("%3d",n));
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt[i][n]));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt_delta[i][n]));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt[i][n]-jt_delta[i][n]));
}
System.out.println();
/*
System.out.println(String.format(
"%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]));
*/
for (int i = 0; i < vector.length; i++) {
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");
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",errors[i]));
}
/*
System.out.println(String.format(
"-\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++) {
err = Math.max(errors[i], err);
}
return err;
}
private double [] getYminusFxWeighted(
final double [] fx,
final double [] rms_fp // null or [2]
) {
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final AtomicInteger ati = new AtomicInteger(0);
final double [] wymfw = new double [fx.length];
double [] swd2 = new double[threads.length];
for (int ithread = 0; ithread < threads.length; ithread++) { // first sum for pairs
threads[ithread] = new Thread() {
public void run() {
int nthread = ati.getAndIncrement();
for (int n = ai.getAndIncrement(); n < num_pairs; n = ai.getAndIncrement()) {
double d = fx_scale *(-fx[n]); // - fx[n]; // +y_vector[i]
double wd = d * weights[n];
wymfw[n] = wd;
swd2[nthread] += d * wd;
}
}
};
}
ImageDtt.startAndJoin(threads);
double s_rms_pure = 0;
for (int n = 0; n < swd2.length; n++) {
s_rms_pure += swd2[n];
}
// System.out.println("ai.get()="+ai.get());
// important to set - after first cycle ai is left 16(number of threads) larger than number of cycles!
// It is so, because it first increments, then tests if (n < num_pairs)
ai.set(num_pairs);
ati.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int nthread = ati.getAndIncrement();
for (int n = ai.getAndIncrement(); n < fx.length; n = ai.getAndIncrement()) {
double d = fx_scale *(-fx[n]); // - fx[n]; // +y_vector[i]
double wd = d * weights[n];
wymfw[n] = wd;
swd2[nthread] += d * wd;
}
}
};
}
ImageDtt.startAndJoin(threads);
double s_rms = 0; // start from scratch
for (int n = 0; n < swd2.length; n++) {
s_rms += swd2[n];
}
if (rms_fp != null) {
rms_fp[0] = Math.sqrt(s_rms);
rms_fp[1] = Math.sqrt(s_rms_pure/weight_pure);
}
return wymfw;
}
public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
double lambda, // 0.1
double lambda_scale_good,// 0.5
double lambda_scale_bad, // 8.0
double lambda_max, // 100
double rms_diff, // 0.001
int num_iter, // 20
boolean last_run,
String dbg_prefix,
int debug_level)
{
boolean [] rslt = {false,false};
this.last_rms = null; // remove?
int iter = 0;
if (dbg_prefix != null) {
// debugStateImage(dbg_prefix+"-initial");
}
for (iter = 0; iter < num_iter; iter++) {
rslt = lmaStep(
lambda,
rms_diff,
debug_level);
if (dbg_prefix != null) {
// debugStateImage(dbg_prefix+"-step_"+iter);
}
if (rslt == null) {
return -1; // false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step"+String.format("%3d",iter)+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (rslt[1]) {
break;
}
if (rslt[0]) { // good
lambda *= lambda_scale_good;
} else {
lambda *= lambda_scale_bad;
if (lambda > lambda_max) {
break; // not used in lwir
}
}
}
if (rslt[0]) { // better
if (iter >= num_iter) { // better, but num tries exceeded
if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
}
} else { // improved over initial ?
if (last_rms[0] < initial_rms[0]) { // NaN
rslt[0] = true;
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
}
}
boolean show_intermediate = true;
if (show_intermediate && (debug_level > 0)) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (debug_level > 2){
System.out.println("iteration="+iter);
}
if (debug_level > 0) {
if ((debug_level > 1) || last_run) { // (iter == 1) || last_run) {
if (!show_intermediate) {
System.out.println("LMA: iter="+iter+", full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
}
}
if ((debug_level > -2) && !rslt[0]) { // failed
if ((debug_level > 1) || (iter == 1) || last_run) {
System.out.println("LMA failed on iteration = "+iter);
}
System.out.println();
}
return rslt[0]? iter : -1;
}
private boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
boolean [] rslt = {false,false};
// maybe the following if() branch is not needed - already done in prepareLMA !
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
last_rms = new double[2];
if (debug_level > 1) {
System.out.println("lmaStep(): first step");
}
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
if (debug_level > -1) { // temporary
/*
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
// TODO: Restore/implement
if (debug_level > 3) {
double delta = this.delta;
double delta_err=compareJT(
parameters_vector, // double [] vector,
delta, // double delta,
last3only); // boolean last3only); // do not process samples - they are tested before
System.out.println("\nMaximal error = "+delta_err);
}
}
if (debug_level > 3) { // 0) {
double delta = this.delta; // 1E-3;
double delta_err=compareJT(
parameters_vector, // double [] vector,
delta, // double delta,
last3only); // boolean last3only); // do not process samples - they are tested before
System.out.println("\nMaximal error = "+delta_err);
}
Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary
this.last_jt)); // double [][] jt) // null
if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv = null;
try {
jtjl_inv = wjtjlambda.inverse(); // check for errors
} catch (RuntimeException e) {
rslt[1] = true;
if (debug_level > 0) {
System.out.println("Singular Matrix!");
}
return rslt;
}
if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
//last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) {
System.out.println("mdelta");
mdelta.print(18, 6);
}
double scale = 1.0;
double [] delta = mdelta.getColumnPackedCopy();
double [] new_vector = parameters_vector.clone();
for (int i = 0; i < parameters_vector.length; i++) {
new_vector[i] += scale * delta[i];
}
double [] fx = getFxDerivs(
new_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
double [] rms = new double[2];
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
rms); // final double [] rms_fp // null or [2]
if (debug_level > 2) {
/*
dbgYminusFx(this.last_ymfx, "next y-fX");
dbgXY(new_vector, "XY-correction");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
this.parameters_vector = new_vector.clone();
if (debug_level > 2) {
// print vectors in some format
/*
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
System.out.println();
*/
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
this.last_rms); // final double [] rms_fp // null or [2]
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
if (debug_level > 2) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
return rslt;
}
private double [][] getWJtJlambda(
final double lambda,
final double [][] jt)
{
final int num_pars = jt.length;
final int num_pars2 = num_pars * num_pars;
final int nup_points = jt[0].length;
final double [][] wjtjl = new double [num_pars][num_pars];
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
int i = indx / num_pars;
int j = indx % num_pars;
if (j >= i) {
double d = 0.0;
for (int k = 0; k < nup_points; k++) {
if (jt[i][k] != 0) {
d+=0;
}
d += weights[k]*jt[i][k]*jt[j][k];
}
wjtjl[i][j] = d;
if (i == j) {
wjtjl[i][j] += d * lambda;
} else {
wjtjl[j][i] = d;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return wjtjl;
}
public double [] getRms() {
return last_rms;
}
public double [] getInitialRms() {
return initial_rms;
}
}
src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java
View file @ 83ce74fe
......@@ -23,11 +23,10 @@
package com.elphel.imagej.orthomosaic;
import java.awt.Point;
import java.awt.Rectangle;
import java.io.IOException;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Calendar;
import com.elphel.imagej.calibration.CalibrationFileManagement;
......@@ -60,7 +59,7 @@ public class OrthoAltitudeMatch {
boolean invert_q2a = false;
boolean test_quat = false;
boolean test_quat0 = false;
boolean test_quat2= true;
boolean test_quat2= false; // true;
boolean use_degrees = true;
boolean debug_tilts = true;
......@@ -140,9 +139,22 @@ public class OrthoAltitudeMatch {
centers); // double [][] centers)
final int width = wh[0];
// final int height = wh[1];
final int num_scenes = indices.length;
final int num_pairs = condensed_pairs.length;
// int num_pairs = 0; // available_pairs.length
// ArrayList<Point> failed_pairs = new ArrayList<Point>();
double [] weights_scenes = new double [num_scenes];
double [] weights_pairs = new double [num_pairs];
Arrays.fill(weights_scenes, 1.0);
Arrays.fill(weights_pairs, 1.0);
double [][][] scene_tilts_pairs = new double [num_pairs][2][];
double [][][] affine_pairs = new double [num_pairs][][];
double [] flat_err = new double[num_pairs];
double [] overlaps = new double [num_pairs];
double tilt_err_threshold = 0.01; // reduce weight if larger
double overlop_pow = 2.0; // squared
double weight_pairs_k = 100.0;
for (int npair = 0; npair < condensed_pairs.length; npair++) {
int [] cpair = condensed_pairs[npair]; // index alt_multi
int [] ipair = {indices[cpair[0]], indices[cpair[1]]};
......@@ -218,13 +230,63 @@ public class OrthoAltitudeMatch {
}
}
double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
double [][] alt_datas = new double[3][];
boolean [][] masks = new boolean[2][];
double [][] alt_data5s = new double[2][];
double [][] data_overlap = new double[2][];
// calculate individual tilts
for (int ns = 0; ns < cpair.length; ns++) {
data_overlap[ns] = OrthoMap.extractWoi(
alt_multi[cpair[ns]], // final double [] data0,
width, // final int width,
woi_overlap); // Rectangle woi_in);
for (int ntry = 0; ntry <= alt_refine; ntry++) {
String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
alt_data5s[ns] = OrthoMap.getPlane(
data_overlap[ns], // final double [] data,
masks[ns], // final boolean [] mask,
weight, // final double [] weight,
woi_overlap.width, // final int width,
xy0, // final double [] xy0) {
dbg_name);
if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
masks[ns] = OrthoMap.removeRelativeLowHigh (
data_overlap[ns], // final double [] data,
null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
alt_abs_outliers, // final double abs_diff,
alt_outliers, // final double rel_frac,
alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
woi_overlap.width,// final int width, // only used with ground_plane != null;
num_bins); // final int num_bins)
} else {
break;
}
}
alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
}
double tilt_err2 = 0;
alt_datas[2] = new double [3];
for (int i = 0; i < alt_datas[2].length; i++) {
alt_datas[2][i] = alt_datas[0][i]+alt_data[i]; // simulated alt_datas[1]
if (i < 2) {
double dt = alt_datas[2][i]-alt_datas[1][i];
tilt_err2+=dt*dt;
}
}
scene_tilts_pairs[npair] = new double [][] {alt_datas[0], alt_datas[2]}; // {tilt0, tilt1 (synthetic)
affine_pairs[npair] = pairwiseOrthoMatch.getAffine(); // contains scale + rot
flat_err[npair] = Math.sqrt(tilt_err2);
overlaps[npair] = pairwiseOrthoMatch.getOverlap();
if (!test_quat2) {
continue;
}
if (test_quat2) {
System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]+
", invert_q2a="+invert_q2a+", invert_order="+invert_order+", invert_y="+invert_y);
boolean [][] masks = new boolean[2][];
double [][] alt_data5s = new double[2][];
double [][] data_overlap = new double[2][];
double [][] alt_datas = new double[3][];
double [][] affine_pair = pairwiseOrthoMatch.getAffine();
boolean remove_rs = false;
if (remove_rs) {
......@@ -265,37 +327,6 @@ public class OrthoAltitudeMatch {
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};
// calculate individual tilts
for (int ns = 0; ns < cpair.length; ns++) {
data_overlap[ns] = OrthoMap.extractWoi(
alt_multi[cpair[ns]], // final double [] data0,
width, // final int width,
woi_overlap); // Rectangle woi_in);
for (int ntry = 0; ntry <= alt_refine; ntry++) {
String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
alt_data5s[ns] = OrthoMap.getPlane(
data_overlap[ns], // final double [] data,
masks[ns], // final boolean [] mask,
weight, // final double [] weight,
woi_overlap.width, // final int width,
xy0, // final double [] xy0) {
dbg_name);
if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
masks[ns] = OrthoMap.removeRelativeLowHigh (
data_overlap[ns], // final double [] data,
null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
alt_abs_outliers, // final double abs_diff,
alt_outliers, // final double rel_frac,
alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
woi_overlap.width,// final int width, // only used with ground_plane != null;
num_bins); // final int num_bins)
} else {
break;
}
}
alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
}
// alt_datas[0] = alt_datas[0].clone();
double [] tilts0_mod = QuatUtils.manualFitTilt(
......@@ -494,44 +525,10 @@ public class OrthoAltitudeMatch {
if (test_quat) {
System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
boolean [][] masks = new boolean[2][];
double [][] alt_data5s = new double[2][];
double [][] data_overlap = new double[2][];
double [][] alt_datas = new double[3][];
double [] quat_diff = QuatUtils.tiltToQuaternion(
alt_data,
y_down_ccw); // boolean y_down_ccw)
double [][] quats01 = new double [alt_datas.length][];
for (int ns = 0; ns < cpair.length; ns++) {
data_overlap[ns] = OrthoMap.extractWoi(
alt_multi[cpair[ns]], // final double [] data0,
width, // final int width,
woi_overlap); // Rectangle woi_in);
for (int ntry = 0; ntry <= alt_refine; ntry++) {
String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
alt_data5s[ns] = OrthoMap.getPlane(
data_overlap[ns], // final double [] data,
masks[ns], // final boolean [] mask,
weight, // final double [] weight,
woi_overlap.width, // final int width,
xy0, // final double [] xy0) {
dbg_name);
if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
masks[ns] = OrthoMap.removeRelativeLowHigh (
data_overlap[ns], // final double [] data,
null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
alt_abs_outliers, // final double abs_diff,
alt_outliers, // final double rel_frac,
alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
woi_overlap.width,// final int width, // only used with ground_plane != null;
num_bins); // final int num_bins)
} else {
break;
}
}
alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
}
alt_datas[2] = new double [3];
for (int i = 0; i < alt_datas[2].length; i++) {
alt_datas[2][i] = alt_datas[0][i]+alt_data[i];
......@@ -875,6 +872,66 @@ public class OrthoAltitudeMatch {
debugLevel-4); // final int debugLevel)
*/
}
// double tilt_err_threshold = 0.01; // reduce weight if larger
//overlop_pow = 2.0
double max_flat_err = 0;//tilt_err_threshold
for (int npair = 0; npair < num_pairs; npair++) {
max_flat_err = Math.max(max_flat_err, flat_err[npair]);
//weights_pairs
weights_pairs[npair] = tilt_err_threshold/Math.max(tilt_err_threshold, flat_err[npair])*Math.pow(overlaps[npair], overlop_pow);
}
ERSTiltLMA ersTiltLMA = new ERSTiltLMA();
ersTiltLMA.prepareLMA(
indices, // int [] indices, // should all be used
condensed_pairs, // int [][] cpairs,
weights_scenes, // double [] weights_scenes, // sfm, number used?
weights_pairs, // double [] weights_pairs, // from matching tilts(flatness) (and worst sfm, per-pair rmse)?
weight_pairs_k, // double weight_pairs_k,
scene_tilts_pairs, // double [][][] tilts, // [pair][scene(2)][tilt(2)]
affine_pairs, // double [][][] affine_pairs,
debugLevel); // int debug_level)
double lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
double lambda_max = 1000;
boolean last_run = false;
double rms_diff = 0.0001;
int num_iter = 20;
int lma_rslt=ersTiltLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, //int num_iter, // 20
last_run, // boolean last_run,
null, // String dbg_prefix,
debugLevel); // int debug_level)
System.out.println("LMA -> "+lma_rslt);
if (lma_rslt >= 0) {
if (debugLevel > -3) {
ersTiltLMA.printResults(use_degrees);
}
}
System.out.println();
if (debugLevel > 1) {
for (int npair = 0; npair < ersTiltLMA.num_pairs; npair++) {
double [][] pseudo_xy= ersTiltLMA.getPseudoXY(npair);
QuatUtils.testPseudoAffineDiffAndDerivatives(
pseudo_xy, // double [][] xy, // [scene][direction]
ersTiltLMA.aff_tilts[npair], // double [][][] a_tilts,
ersTiltLMA.aff_pairs_nosr[npair], // double [][] iaff_pair, // affine pair inversed
invert_q2a); // boolean invert_q2a){ // invert result affines (to match "usual")
System.out.println();
}
}
//printResults(boolean degrees)
if (orthoMapsCollection_path != null) {
try {
orthoMapsCollection.writeOrthoMapsCollection(orthoMapsCollection_path);
......
src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java
View file @ 83ce74fe
......@@ -114,6 +114,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
// coordinates relative to vert_meters
public double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[], positive Y is down (as in images)
public transient double [][] ers_affine = new double[][] {{1,0},{0,1}}; // orientation only for remaining ERS, positive Y is down (as in images)
public double orig_pix_meters;
public double [] vert_meters; // offset of the image vertical in meters (scale-invariant), right (X) and down (Y)
public int orig_width;
......@@ -190,6 +191,14 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
// pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
}
public double [][] getERSAffine(){
return ers_affine;
}
public void setERSAffine(double [][] ers_affine) { // cloned
// this.ers_affine = ers_affine;
this.ers_affine = new double [][] {ers_affine[0].clone(),ers_affine[1].clone()};
}
double getEqualized(double d) {
return d * equalize[0] + equalize[1];
}
......
src/main/java/com/elphel/imagej/orthomosaic/PairwiseOrthoMatch.java
View file @ 83ce74fe
......@@ -40,10 +40,18 @@ public class PairwiseOrthoMatch implements Serializable {
public transient double overlap = 0.0;
public transient double [] alt_data = null;
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
// below - not saved/restored
public transient boolean ok = false; // not saved/restored
public transient int [] nxy = null; // not saved, just to communicate for logging
public double [] getQuaternion() {
return quat;
}
public void setQuaternion(double [] quat) { // clone() by caller
this.quat = quat;
}
// public PairwiseOrthoMatch() {}
public double getOverlap() {
return overlap;
......
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java
View file @ 83ce74fe
......@@ -128,7 +128,7 @@ public final class QuatUtils { // static class
System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
double [] quat1 = quatRotDirTiltScale(rot1, dir1,tilt1,scale1);
double [] quat2 = quatRotDirTiltScale(rot2, dir2,tilt2,scale2);
// System.out.println("quats01[2]= "+QuatUtils.toString(quats01[2],use_degrees));
// System.out.println("quats01[2]= "+toString(quats01[2],use_degrees));
testQuatAff(quat1, quat2, invert_q2a, y_down_ccw);
System.out.println("run="+run);
}
......@@ -140,9 +140,9 @@ public final class QuatUtils { // static class
boolean y_down_ccw) {
boolean use_degrees= true;
double [] quat_diff = divideScaled(quat2, quat1);
System.out.println("quat1= "+QuatUtils.toString(quat1,use_degrees));
System.out.println("quat2= "+QuatUtils.toString(quat2,use_degrees));
System.out.println("quat_diff= "+QuatUtils.toString(quat_diff,use_degrees));
System.out.println("quat1= "+toString(quat1,use_degrees));
System.out.println("quat2= "+toString(quat2,use_degrees));
System.out.println("quat_diff= "+toString(quat_diff,use_degrees));
double [][] quats= {quat1,quat2,quat_diff};
double [][][] affines = new double [quats.length][][];
SingularValueDecomposition [] svds= new SingularValueDecomposition [quats.length];
......@@ -175,35 +175,35 @@ public final class QuatUtils { // static class
quats_diff_pm[i][1]= divideScaled(quats_pm[i][1], quats[i]);
}
System.out.println("quat1= "+QuatUtils.toString(quat1,use_degrees));
System.out.println("quats_pm[0][0]="+QuatUtils.toString(quats_pm[0][0],use_degrees));
System.out.println("quats_pm[0][1]="+QuatUtils.toString(quats_pm[0][1],use_degrees));
System.out.println("qdiff_pm[0][0]="+QuatUtils.toString(quats_diff_pm[0][0],use_degrees));
System.out.println("qdiff_pm[0][1]="+QuatUtils.toString(quats_diff_pm[0][1],use_degrees));
System.out.println("quat1= "+toString(quat1,use_degrees));
System.out.println("quats_pm[0][0]="+toString(quats_pm[0][0],use_degrees));
System.out.println("quats_pm[0][1]="+toString(quats_pm[0][1],use_degrees));
System.out.println("qdiff_pm[0][0]="+toString(quats_diff_pm[0][0],use_degrees));
System.out.println("qdiff_pm[0][1]="+toString(quats_diff_pm[0][1],use_degrees));
System.out.println();
System.out.println("quat2= "+QuatUtils.toString(quat2,use_degrees));
System.out.println("quats_pm[1][0]="+QuatUtils.toString(quats_pm[1][0],use_degrees));
System.out.println("quats_pm[1][1]="+QuatUtils.toString(quats_pm[1][1],use_degrees));
System.out.println("qdiff_pm[1][0]="+QuatUtils.toString(quats_diff_pm[1][0],use_degrees));
System.out.println("qdiff_pm[1][1]="+QuatUtils.toString(quats_diff_pm[1][1],use_degrees));
System.out.println("quat2= "+toString(quat2,use_degrees));
System.out.println("quats_pm[1][0]="+toString(quats_pm[1][0],use_degrees));
System.out.println("quats_pm[1][1]="+toString(quats_pm[1][1],use_degrees));
System.out.println("qdiff_pm[1][0]="+toString(quats_diff_pm[1][0],use_degrees));
System.out.println("qdiff_pm[1][1]="+toString(quats_diff_pm[1][1],use_degrees));
System.out.println();
double [][] qvariants = {
QuatUtils.divideScaled(quats_pm[1][0], quats_pm[0][0]),
QuatUtils.divideScaled(quats_pm[1][1], quats_pm[0][0]),
QuatUtils.divideScaled(quats_pm[1][0], quats_pm[0][1]),
QuatUtils.divideScaled(quats_pm[1][1], quats_pm[0][1])};
System.out.println("quat_diff= "+QuatUtils.toString(quat_diff,use_degrees));
divideScaled(quats_pm[1][0], quats_pm[0][0]),
divideScaled(quats_pm[1][1], quats_pm[0][0]),
divideScaled(quats_pm[1][0], quats_pm[0][1]),
divideScaled(quats_pm[1][1], quats_pm[0][1])};
System.out.println("quat_diff= "+toString(quat_diff,use_degrees));
double [][] qvariants_diff = new double [qvariants.length][2];
for (int i = 0; i < qvariants_diff.length; i++) {
qvariants_diff[i] = QuatUtils.divideScaled(qvariants[i], quat_diff);
qvariants_diff[i] = divideScaled(qvariants[i], quat_diff);
}
for (int i = 0; i < qvariants.length; i++) {
System.out.println("quat_var["+i+"]= "+QuatUtils.toString(qvariants[i],use_degrees));
System.out.println("quat_var["+i+"]= "+toString(qvariants[i],use_degrees));
}
System.out.println();
for (int i = 0; i < qvariants.length; i++) {
System.out.println("qvar_diffs["+i+"]= "+QuatUtils.toString(qvariants_diff[i],use_degrees));
System.out.println("qvar_diffs["+i+"]= "+toString(qvariants_diff[i],use_degrees));
}
return;
}
......@@ -403,12 +403,30 @@ public final class QuatUtils { // static class
double chalfrot = Math.cos(rot/2);
double shalfrot = Math.sin(rot/2);
double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
double [] qinv_tilts = invert(qtilts); //scene to ground
double [] qrt= multiply(qrot,qinv_tilts); // just for debugging
// double [] quat= scale(multiply(qrot,qinv_tilts),scale);
// double [] qrt= multiply(qrot,qinv_tilts); // just for debugging
double [] quat= scale(multiply(qinv_tilts,qrot),scale);
return quat;
}
}
/**
* No scale/rotation
* @param txy
* @param y_down_ccw
* @return
*/
public static double [] sceneRelLocalGround(
double [] txy,
boolean y_down_ccw) { // boolean y_down_ccw) for tilts and affine
double [] qtilts = tiltToQuaternion(txy,y_down_ccw);
double [] qinv_tilts = invert(qtilts); //scene to ground
return qinv_tilts;
}
/**
* Calculate relative affine transform from scene0 to scene1
* from tilts of the first scene and differential tilt from
......@@ -458,11 +476,17 @@ public final class QuatUtils { // static class
double [][][] affines= new double [2][][];
double [] aff_tilts = new double[2];
for (int nscene = 0; nscene < 2; nscene++) {
/*
double [] quat = sceneRelLocalGround(
tilts[nscene], // double [] txy,
null, // double [][] affine,
y_down_ccw); // boolean y_down_ccw)
affines[nscene] = QuatUtils.quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);
affines[nscene] = quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);
*/
affines[nscene] = tiltToAffine(
tilts[nscene], // double [] tilt,
invert_q2a, // boolean invert_q2a, // invert result affines (to match "usual")
invert_y); // boolean invert_y)
if (debug) {
double [] w_min_max = SingularValueDecomposition.getMinMaxEigenValues(affines[nscene]);
aff_tilts[nscene] = w_min_max[1]/w_min_max[0]-1.0; // >=0.0
......@@ -488,6 +512,20 @@ public final class QuatUtils { // static class
return affine_diff;
}
public static double [][] tiltToAffine(
double [] tilt,
boolean invert_q2a, // invert result affines (to match "usual")
boolean invert_y){
boolean y_down_ccw = true;
double [] quat = sceneRelLocalGround(
tilt, // double [] txy,
null, // double [][] affine,
y_down_ccw); // boolean y_down_ccw)
double [][] affine = quatToAffine(quat ,invert_q2a, y_down_ccw ^ invert_y);
return affine;
}
public static double [] manualFitTilt(
double [] tilts0,
......@@ -553,7 +591,7 @@ public final class QuatUtils { // static class
i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));
}
double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(
double [][] affine_tilt_diff = diffAffineFromTilts(
tilts0_mod, // double [] tilts0,
tilts_diff, // double [] tilts_diff,
invert_q2a, // boolean invert_q2a) // false
......@@ -565,21 +603,21 @@ public final class QuatUtils { // static class
//affine_pair_nors
SingularValueDecomposition svd_affine_pair_nors =
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair_nors, y_down_ccw);
System.out.println(QuatUtils.affinesToString(affine_pair_nors, "affine_pair_nors"));
System.out.println(affinesToString(affine_pair_nors, "affine_pair_nors"));
System.out.println("svd_affine_pair_nors= " + svd_affine_pair_nors.toString(use_degrees));
System.out.println();
System.out.println(QuatUtils.affinesToString(affine_tilt_diff, "affine_tilt_diff"));
System.out.println(affinesToString(affine_tilt_diff, "affine_tilt_diff"));
System.out.println("svd_affine_tilt_diff= " + svd_affine_tilt_diff.toString(use_degrees));
System.out.println();
double [][] adq_err = QuatUtils.matMult2x2(affine_tilt_diff, QuatUtils.matInverse2x2(affine_pair_nors));
double [][] adq_err = matMult2x2(affine_tilt_diff, matInverse2x2(affine_pair_nors));
double [] w_min_max_err = SingularValueDecomposition.getMinMaxEigenValues(adq_err);
double aff_tilt_err = w_min_max_err[1]/w_min_max_err[0]-1.0; // >=0.0
// System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0]-1.0, aff_tilts[1]-1.0, aff_tilt_diff-1.0));
SingularValueDecomposition svd_adq_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq_err, y_down_ccw);
System.out.println(QuatUtils.affinesToString(adq_err, "adq_err"));
System.out.println(affinesToString(adq_err, "adq_err"));
System.out.println("svd_adq_err= " + svd_adq_err.toString(use_degrees));
/*
double err = Math.sqrt(
......@@ -902,6 +940,20 @@ q11 - q22 = c3
}
return m;
}
public static double [][] matAdd2x2 (
double [][] m1,
double [][] m2){
double [][] m = new double[m1.length][m2[0].length];
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
m[i][j] = m1[i][j] + m2[i][j];
}
}
return m;
}
public static double [][] matMult2x2 (
double [][] m1,
double [][] m2){
......@@ -916,6 +968,24 @@ q11 - q22 = c3
return m;
}
public static double [][] dmatMult2x2 (
double [][] dm1,
double [][] m1,
double [][] dm2,
double [][] m2){
double [][] dm = new double[m1.length][m2[0].length];
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
for (int k = 0; k < 2; k++) {
dm[i][j] += dm1[i][k]*m2[k][j] + m1[i][k]*dm2[k][j];
}
}
}
return dm;
}
public static double [] matMult (
double [][] m1,
double [] v){
......@@ -944,13 +1014,546 @@ q11 - q22 = c3
public static double [][] matInverse2x2(
double [][] a) {
// throw new IllegalArgumentException("Only [2][2] arrays");
double idet = 1.0/(a[0][0] * a[1][1] - a[0][1] * a[1][0]);
return new double [][] {
{ idet*a[1][1], -idet*a[0][1]},
{-idet*a[1][0], idet*a[0][0]}};
}
public static double [][] dmatInverse2x2(
double [][] da,
double [][] a) {
double det = (a[0][0] * a[1][1] - a[0][1] * a[1][0]);
double idet = 1.0/det;
double ddet = a[0][0] * da[1][1]+da[0][0] * a[1][1] - a[0][1] * da[1][0]- da[0][1] * a[1][0];
double didet = -1/det/det*ddet;
return new double [][] {
{ didet*a[1][1] + idet*da[1][1], -didet*a[0][1] - idet*da[0][1]},
{-didet*a[1][0] - idet*da[1][0], didet*a[0][0] + idet*da[0][0]}};
}
/**
* c = cos (phi), s = sin (phi), w1 = 1, w2 = k, k1=1-k,
* tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
* c = tx/t, s=ty/t, t2=t*t
* | c, -s | | w1, 0 | | c, s | | 1-(1-k)*s^2, s*c*(1-k) | | t2 - k1*ty*ty, k1*tx*ty |
* | | * | | * | | = | | = 1/t2 * | |
* | s, c | | 0, w2| | -s, c | | s*c*(1-k), 1-(1-k)*c^2 | | k1*tx*ty, t2 -k1*tx*tx |
*
*/
public static double [][][] tiltToAffineAndDerivatives (
double [] txy){
double sigma = 2e-4;
double sigma2 = sigma*sigma;
double tx = txy[0],ty=txy[1];
if (tx*tx < sigma2) tx = sigma;
if (ty*ty < sigma2) ty = sigma;
double t2 = tx*tx + ty*ty;
double t = Math.sqrt(t2);
double k = 1/Math.sqrt(1+t2);
double c = tx/t, s=ty/t, c2 = c*c, s2=s*s, sc=s*c, k1=1-k;
double d = k/(1+t2); //1/sqrt(1+tx^2+ty^2)^3
double dk1_dtx = tx*d;
double dk1_dty = ty*d;
double dc2_dtxy=2*txy[0]*txy[1]/(t2*t2);
double dc2_dtx=dc2_dtxy * txy[1];
double ds2_dty=dc2_dtxy * txy[0];
double ds2_dtx= -dc2_dtx;
double dc2_dty= -ds2_dty;
double dsc_dtxy = (txy[0]*txy[0] - txy[1]*txy[1])/(t2*t2);
double dsc_dtx = dsc_dtxy * txy[1];
double dsc_dty = -dsc_dtxy * txy[0];
double [][][] a = {
{ // a
{1-k1*s2, k1*sc},
{k1*sc, 1-k1*c2}
},
{ // da/dtx
{-dk1_dtx * s2 - k1 * ds2_dtx, dk1_dtx * sc + k1 * dsc_dtx},
{ dk1_dtx * sc + k1 * dsc_dtx, -dk1_dtx * c2 - k1 * dc2_dtx}
},
{ // da/dty
{-dk1_dty * s2 - k1 * ds2_dty, dk1_dty * sc + k1 * dsc_dty},
{ dk1_dty * sc + k1 * dsc_dty, -dk1_dty * c2 - k1 * dc2_dty}
}
};
return a;
}
public static double [][] tiltToAffine (
double [] txy){
double t2 = txy[0]*txy[0] + txy[1]*txy[1];
double t = Math.sqrt(t2);
double k = 1/Math.sqrt(1+t2);
double c = (t==0)?1:(txy[0]/t), s=(t==0)?0:(txy[1]/t), c2 = c*c, s2=s*s, sc=s*c, k1=1-k;
return new double [][] { // a
{1-k1*s2, k1*sc},
{k1*sc, 1-k1*c2}};
}
/**
* Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY)
* @param txy tilt defining ERS parameters
* @param tilt actual orientation tilt (no rotation)
* @return
*/
public static double [][][] getERSAffineAndDerivatives (
double [] txy,
double [][] a_tilt){
double [][][] ers_with_derivs = tiltToAffineAndDerivatives (
txy); // double [] txy);
double [][][] a = new double [ers_with_derivs.length][][];
for (int i = 0; i < ers_with_derivs.length; i++) {
a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
}
return a;
}
/**
* Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
* @param txy a pair of ERS tilts (each as tiltx, tilty)
* @param tilts a pair of the scene tilts
* @param aff_pair a differential affine from the image comparison, should have rotation and scale removed
* @param invert_q2a invert scene affines (tmp debug feature)
* @param invert_y invert direction of the y axis (tmp debug feature)
* @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
* transform and the one from image comparison
*/
public static double [][][] affineDiffAndDerivatives(
double [][] txy, // [scene][direction]
double [][][] a_tilts,
double [][] iaff_pair, // affine pair inversed
boolean invert_q2a){ // invert result affines (to match "usual")
double [][][][] affs = new double [2][][][];
double [][][] aff = new double [3][][];
for (int nscene = 0; nscene < affs.length; nscene++) {
affs[nscene] = getERSAffineAndDerivatives (
txy[nscene], // double [] txy,
a_tilts[nscene]); // double [] tilt,
if (invert_q2a) {
affs[nscene][0] = matInverse2x2(affs[nscene][0]);
for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
affs[nscene][i] = dmatInverse2x2(
affs[nscene][i], // double [][] da,
affs[nscene][0]); // double [][] a)
}
}
}
double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
iaff0[0] = matInverse2x2(affs[0][0]); // inverse affine matrix aff0
for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
iaff0[i] = dmatInverse2x2(
affs[0][i], // double [][] da,
affs[0][0]); // double [][] a)
}
// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
/*
for (int i = 1; i < aff.length; i++) {
aff_diff[i] = dmatMult2x2 (
affs[1][i], // double [][] dm1,
affs[1][0], // double [][] m1,
iaff0[i], // double [][] dm2,
iaff0[0]); // double [][] m2)
}
*/
double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
// double [][] iaff_pair = matInverse2x2(aff_pair);
for (int i = 0; i < aff_err.length; i++) {
aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
}
return aff_err;
}
/**
* c = cos (phi), s = sin (phi), w, k=1-w,
* tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
* c = tx/t, s=ty/t, t2=t*t
* | c, -s | | 1, 0 | | c, s | | 1-(1-w)*s*s, s*c*(1-w) |
* | | * | | * | | = | |
* | s, c | | 0, w | | -s, c | | s*c*(1-w), 1-(1-w)*c*c |
*
*/
public static double [][][] angleWToAffineAndDerivatives (
double beta,
double w){ // w<1
double c = Math.cos(beta), s = Math.sin(beta), c2 = c * c, s2 = s*s, sc= s*c,k = 1-w;
double [][][] a = {
{ // a
{1-k*s2, k*sc},
{k*sc, 1-k*c2}
},
{ // da/dbeta
{-k*2*sc, k*(c2-s2)},
{k*(c2-s2), k*2*sc}
},
{ // da/dw
{-s2, sc},
{sc, -c2}
}
};
return a;
}
/**
* Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY)
* @param beta
* @param a_tilt actual orientation affine (no rotation)
* @return
*/
public static double [][][] getERSAffineAndDerivatives (
double beta,
double w, // w<1
double [][] a_tilt){
double [][][] ers_with_derivs = angleWToAffineAndDerivatives (
beta, // double beta,
w); // double w){ // w<1
double [][][] a = new double [ers_with_derivs.length][][];
for (int i = 0; i < ers_with_derivs.length; i++) {
a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
}
return a;
}
/**
* Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
* @param txy a pair of ERS tilts (each as tiltx, tilty)
* @param tilts a pair of the scene tilts
* @param aff_pair a differential affine from the image comparison, should have rotation and scale removed
* @param invert_q2a invert scene affines (tmp debug feature)
* @param invert_y invert direction of the y axis (tmp debug feature)
* @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
* transform and the one from image comparison
*/
public static double [][][] affineDiffAndDerivatives(
double [] betas,
double [] ws, // w<1
double [][][] a_tilts,
double [][] iaff_pair, // affine pair inversed
boolean invert_q2a){ // invert result affines (to match "usual")
double [][][][] affs = new double [2][][][];
double [][][] aff = new double [3][][];
for (int nscene = 0; nscene < affs.length; nscene++) {
affs[nscene] = getERSAffineAndDerivatives (
betas[nscene], // double beta,
ws[nscene], // double w, // w<1
a_tilts[nscene]); // double [] tilt,
if (invert_q2a) {
affs[nscene][0] = matInverse2x2(affs[nscene][0]);
for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
affs[nscene][i] = dmatInverse2x2(
affs[nscene][i], // double [][] da,
affs[nscene][0]); // double [][] a)
}
}
}
double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
iaff0[0] = matInverse2x2(affs[0][0]); // inverse affine matrix aff0
for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
iaff0[i] = dmatInverse2x2(
affs[0][i], // double [][] da,
affs[0][0]); // double [][] a)
}
// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
for (int i = 0; i < aff_err.length; i++) {
aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
}
return aff_err;
}
/**
* Generate affine transform corresponding to arbitrary stretch from 2 parameters
* centered around zero that have lower power than actual tilts and so
* "better" derivatives for LMA near (0,0). They are still all 0, but
* minimizing each of them will add non-zero derivatives
* @param xy pair of X/y parameters
* @return affine [2][2] matrix and 2 derivatives by X and Y
*/
public static double [][][] pseudoTiltToAffineAndDerivatives0 (
double [] xy){
double x = xy[0],y=xy[1];
double [][][] a = {
{ // a
{1-y*y, x*y},
{x*y, 1-x*x}
},
{ // da/dx
{0, y},
{y, -2*x}
},
{ // da/dy
{-2*y, x},
{x, 0}
}
};
return a;
}
/**
* Different input to improve derivatives, based on dual angle
* Starting with SVD angle and a (1,w) pair of singular values:
* c = cos (phi), s = sin (phi), w, k=1-w,
* tx, ty(down), t= sqrt(tx*tx+ty*ty), w2= 1/sqrt(1+tx*tx+ty*ty)
* c = tx/t, s=ty/t, t2=t*t
* | c, -s | | 1, 0 | | c, s | | 1-k*s*s, s*c*k |
* | | * | | * | | = | |
* | s, c | | 0, w | | -s, c | | s*c*k, 1-k*c*c |
* converted to dual angle
* x = k * cos(2*phi)/2, y = k*sin(2*phi)/2, k/2 = sqrt(x*x + y*y)
* r = k/2
*
* | 1-k*s*s, s*c*k | | (1-r) + x, y |
* | | = | |
* | s*c*k, 1-k*c*c | | y, (1-r) -x |
*
* Simplification (by the expense of changing w1, w2 to 1 +/- sqrt (x*x+y*y),
* gamma = -beta = atan2(y,x)/2:
* | 1 + x, y | | 1, 0 | | 0, 1 |
* | |, d/dx= | |, d/dy = | |
* | y, 1-x | | 0, -1 | | 1, 0 |
*
* @param xy - a pair of pseudo-coordinates definining scaled tilt affine transformation.
* @return an affine transformation as 2x2 array and two of its derivatives by x and y,
* combined into [3][2][2] array.
*/
public static double [][][] pseudoTiltToAffineAndDerivatives (
double [] xy){
double x = xy[0],y=xy[1];
double [][][] a = {
{ // a
{1+x, y},
{y, 1-x}
},
{ // da/dx
{1, 0},
{0, - 1}
},
{ // da/dy
{0, 1},
{1, 0}
}
};
return a;
}
public static double [][] pseudoTiltToAffine(
double [] xy){
double x = xy[0],y=xy[1];
double [][] a =
{ // a
{1+x, y},
{y, 1-x}
};
return a;
}
/**
* Get scene affine and derivatives from ERS parameters (txy) and tilt (tiltX, tiltY)
* @param xy pseudo-tilt defining ERS parameters
* @param tilt actual orientation tilt (no rotation)
* @return
*/
public static double [][][] pseudoERSAffineAndDerivatives (
double [] xy,
double [][] a_tilt){
double [][][] ers_with_derivs = pseudoTiltToAffineAndDerivatives (
xy); // double [] xy);
double [][][] a = new double [ers_with_derivs.length][][];
for (int i = 0; i < ers_with_derivs.length; i++) {
a[i] = matMult2x2(ers_with_derivs[i], a_tilt);
}
return a;
}
/**
* Get affine diference and its derivatives (by phi-s and k-s of the ERS affine transforms) for a pair of scenes
* @param xy a pair of ERS pseudo-tilts (each as tiltx, tilty)
* @param tilts a pair of the scene tilts
* @param aff_pair a differential affine from the image comparison, should have rotation and scale removed
* @param invert_q2a invert scene affines (tmp debug feature)
* @param invert_y invert direction of the y axis (tmp debug feature)
* @return [5] error affine transform (and derivatives by 4 parameters) - difference between tilt/ers calculated
* transform and the one from image comparison
*/
public static double [][][] pseudoAffineDiffAndDerivatives(
double [][] xy, // [scene][direction]
double [][][] a_tilts,
double [][] iaff_pair, // affine pair inversed
boolean invert_q2a){ // invert result affines (to match "usual")
double [][][][] affs = new double [2][][][];
double [][][] aff = new double [3][][];
for (int nscene = 0; nscene < affs.length; nscene++) {
affs[nscene] = pseudoERSAffineAndDerivatives (
xy[nscene], // double [] xy,
a_tilts[nscene]); // double [] tilt,
if (invert_q2a) {
affs[nscene][0] = matInverse2x2(affs[nscene][0]);
for (int i = 1; i < affs[nscene].length; i++) { // derivatives of inverse affine matrix
affs[nscene][i] = dmatInverse2x2(
affs[nscene][i], // double [][] da,
affs[nscene][0]); // double [][] a)
}
}
}
double [][][] iaff0 = new double [aff.length][][]; // d_phi, d_k
iaff0[0] = matInverse2x2(affs[0][0]); // inverse affine matrix aff0
for (int i = 1; i < iaff0.length; i++) { // derivatives of inverse affine matrix
iaff0[i] = dmatInverse2x2(
affs[0][i], // double [][] da,
affs[0][0]); // double [][] a)
}
// Now iaffs0 is inverse of affs[0] and 2 of its derivatives (by tx and ty)
double [][][] aff_diff = new double [5][][]; //aff_err, d_aff_err/d_tx0, d_aff_err/d_y0, d_aff_err/d_tx1, d_aff_err/d_y1
aff_diff[0] = matMult2x2(affs[1][0], iaff0[0]); // aff_err
aff_diff[1] = matMult2x2(affs[1][0], iaff0[1]); // d_aff_err/d_tx0
aff_diff[2] = matMult2x2(affs[1][0], iaff0[2]); // d_aff_err/d_ty0
aff_diff[3] = matMult2x2(affs[1][1], iaff0[0]); // d_aff_err/d_tx1
aff_diff[4] = matMult2x2(affs[1][2], iaff0[0]); // d_aff_err/d_ty1
double [][][] aff_err = new double [aff_diff.length][][]; //aff_err, d_aff_err/d_phi, d_aff_err/d_k
for (int i = 0; i < aff_err.length; i++) {
aff_err[i] = matMult2x2(aff_diff[i],iaff_pair);
}
return aff_err;
}
public static void testPseudoAffineDiffAndDerivatives(
double [][] xy, // [scene][direction]
double [][][] a_tilts,
double [][] iaff_pair, // affine pair inversed
boolean invert_q2a){ // invert result affines (to match "usual")
double delta= 1e-6;
xy = new double [][] {
{xy[0][0],xy[0][1]},
{xy[1][0],xy[1][1]}
};
a_tilts = new double[][][] {
{
{a_tilts[0][0][0],a_tilts[0][0][1]},
{a_tilts[0][1][0],a_tilts[0][1][1]}
},
{
{a_tilts[1][0][0],a_tilts[1][0][1]},
{a_tilts[1][1][0],a_tilts[1][1][1]}
}
};
iaff_pair = new double [][] {
{iaff_pair[0][0],iaff_pair[0][1]},
{iaff_pair[1][0],iaff_pair[1][1]}
};
testPseudoAffineDiffAndDerivatives(
xy, // double [][] xy, // [scene][direction]
a_tilts, // double [][][] a_tilts,
iaff_pair, // double [][] iaff_pair, // affine pair inversed
invert_q2a, //boolean invert_q2a, // invert result affines (to match "usual")
delta); // double delta);
}
public static void testPseudoAffineDiffAndDerivatives(
double [][] xy, // [scene][direction]
double [][][] a_tilts,
double [][] iaff_pair, // affine pair inversed
boolean invert_q2a, // invert result affines (to match "usual")
double delta) {
while (xy[0][0] < 1) {
double [][][] aff_err = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
xy, // double [][] txy, // [scene][direction]
a_tilts, // double [][][] a_tilts,
iaff_pair, // double [][] iaff_pair, // affine pair inversed
invert_q2a); // boolean invert_q2a) // invert result affines (to match "usual")
double [][] WdW = SingularValueDecomposition.getMinMaxEigenValues(
aff_err); // double [][][] AdA)
double [] dWdW = new double [WdW.length];
for (int i = 0; i < WdW.length; i++) {
dWdW[i] = WdW[i][1]-WdW[i][0];
}
double [][] WdW_delta = new double [WdW.length][2];
double [] dWdW_delta = new double [WdW.length];
for (int i = 1; i < dWdW_delta.length; i++) {
int nscene= (i-1) / 2;
int ynx = (i-1) % 2;
double [][] xy_p = new double [][] {xy[0].clone(), xy[1].clone()};
double [][] xy_m = new double [][] {xy[0].clone(), xy[1].clone()};
xy_p[nscene][ynx] += delta/2;
xy_m[nscene][ynx] -= delta/2;
double [][][] aff_err_p = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
xy_p, // double [][] txy, // [scene][direction]
a_tilts, // double [][][] a_tilts,
iaff_pair, // double [][] iaff_pair, // affine pair inversed
invert_q2a); // boolean invert_q2a) // invert result affines (to match "usual")
double [][][] aff_err_m = QuatUtils.pseudoAffineDiffAndDerivatives( //affineDiffAndDerivatives(
xy_m, // double [][] txy, // [scene][direction]
a_tilts, // double [][][] a_tilts,
iaff_pair, // double [][] iaff_pair, // affine pair inversed
invert_q2a); // boolean invert_q2a) // invert result affines (to match "usual")
double [][] WdW_p = SingularValueDecomposition.getMinMaxEigenValues(
aff_err_p); // double [][][] AdA)
double [][] WdW_m = SingularValueDecomposition.getMinMaxEigenValues(
aff_err_m); // double [][][] AdA)
WdW_delta[i][0] = (WdW_p[0][0]-WdW_m[0][0])/delta;
WdW_delta[i][1] = (WdW_p[0][1]-WdW_m[0][1])/delta;
dWdW_delta[i] = WdW_delta[i][1]-WdW_delta[i][0];
}
double s = 1; //
System.out.println(String.format("%5s\t%20s\t%20s\t%20s\t%20s",
"mode#", "dx0", "dy0", "dx1", "dy1"));
System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
"deriv", s*dWdW[0], s*dWdW[1], s*dWdW[2], s*dWdW[3]));
System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
"delta", s*dWdW_delta[0], s*dWdW_delta[1],s*dWdW_delta[2],s*dWdW_delta[3]));
System.out.println(String.format("%5s\t%20.12f\t%20.12f\t%20.12f\t%20.12f",
"diff", s*(dWdW_delta[0]-dWdW[0]), s*(dWdW_delta[1]-dWdW[1]), s*(dWdW_delta[2]-dWdW[2]), s*(dWdW_delta[3]-dWdW[3])));
System.out.println ("Change xy[0][0] >= 1.0 to exit");
System.out.println ();
}
return;
}
}
src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java
View file @ 83ce74fe
......@@ -49,13 +49,37 @@ public class SingularValueDecomposition {
public double getMinScale() {return Math.min(w1,w2);}
public double getMaxScale() {return Math.max(w1,w2);}
public static String titleString(boolean degrees) {
SingularValueDecomposition svd= new SingularValueDecomposition(0,0,0,0,0);
return svd.toString(degrees, 2);
}
public String toString(boolean degrees) {
return toString(degrees,0);
}
/**
* generate String representation of SingularValueDecomposition instance
* @param degrees true - degrees, false - radians
* @param tab_mode 0 - commas, in-line names; 1 - tabs, data only; 2 tab-sepatated title string (does not start with a tab)
* @return formatted String
*/
public String toString(boolean degrees, int tab_mode) {
//System.out.println("svd_affine_pair= ["+svd_affine_pair.scale+ ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+
// "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
String tab_title_rad = String.format("%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s",
"scale","tilt","gamma","beta","rot", "w1", "w2", "ratio");
String tab_title_deg = String.format("%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s\t%10s",
"scale","tilt\u00B0","gamma\u00B0","beta\u00B0","rot\u00B0", "w1", "w2", "ratio");
String tab_fmt_rad = "%10.8f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.8f\t%10.8f\t%10.8f";
String tab_fmt_deg = "%10.8f\t%10.5f\t%10.5f\t%10.5f\t%10.5f\t%10.8f\t%10.8f\t%10.8f";
String fmt_rad = " scale=%10.8f,tilt= %10.7f, gamma=%10.7f, beta=%10.7f, rot=%10.7f, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
String fmt_deg = " scale=%10.8f,tilt= %10.5f\u00B0, gamma=%10.5f\u00B0, beta=%10.5f\u00B0, rot=%10.5f\u00B0, w1=%10.8f, w2=%10.8f, ratio=%10.8f";
if (tab_mode==2) {
return degrees?tab_title_deg:tab_title_rad;
}
String fmt = (tab_mode==1)?(degrees ? tab_fmt_deg : tab_fmt_rad):(degrees ? fmt_deg : fmt_rad);
double s = degrees ? (180/Math.PI):1;
String fmt=degrees ? fmt_deg : fmt_rad;
// String fmt=degrees ? fmt_deg : fmt_rad;
return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, ratio);
}
......@@ -167,6 +191,41 @@ public class SingularValueDecomposition {
}
/**
* Get a pair of singular values (starting with smaller) and their derivatives
* @param AdA affine transform, followed by an array of affine transform derivatives
* (top index - number of derivative)
* @return {{w2,w1}, {dw2/dx1,dw1/dx1}, {dw2/dx2,dw1/dx2}, ...}
*/
public static double [][] getMinMaxEigenValues(
double [][][] AdA) {
double a00=AdA[0][0][0],a01=AdA[0][0][1],a10=AdA[0][1][0],a11=AdA[0][1][1];
double b00=(a00+a11)/2; //, b11 = b00;
double c00=(a00-a11)/2; //, c11 =-c00;
double b01=(a01-a10)/2; //, b10 =-b01;
double c01=(a01+a10)/2; //, c10 = c01;
double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
double w1 = w1_p_w2_2 + w1_m_w2_2;
double w2 = w1_p_w2_2 - w1_m_w2_2;
double [][] rslt = new double [AdA.length][];
rslt[0] = new double [] {w2,w1}; // w1=1, w2 <=1;
for (int i = 1; i < AdA.length; i++) {
double d_a00=AdA[i][0][0],d_a01=AdA[i][0][1],d_a10=AdA[i][1][0],d_a11=AdA[i][1][1];
double d_b00=(d_a00+d_a11)/2; //, b11 = b00;
double d_c00=(d_a00-d_a11)/2; //, c11 =-c00;
double d_b01=(d_a01-d_a10)/2; //, b10 =-b01;
double d_c01=(d_a01+d_a10)/2; //, c10 = c01;
double d_w1_p_w2_2= (b00*d_b00 + b01*d_b01)/w1_p_w2_2;
double d_w1_m_w2_2= (c00*d_c00 + c01*d_c01)/w1_m_w2_2;
double d_w1 = d_w1_p_w2_2 + d_w1_m_w2_2;
double d_w2 = d_w1_p_w2_2 - d_w1_m_w2_2;
rslt[i] = new double [] {d_w2,d_w1};
}
return rslt;
}
/**
* Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
* into overall scaling caused by zoom != 1.0 because of altitude error
......
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