Skip to content

I
imagej-elphel
Commit 203fad06 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

quaternions-affines

parent d3389b58
Show whitespace changes
Inline Side-by-side
Showing with 319 additions and 18 deletions
src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java
View file @ 203fad06
......@@ -57,11 +57,18 @@ public class OrthoAltitudeMatch {
int debugLevel) {
boolean dbg_planes = false;
boolean y_down_ccw = true;
boolean invert_q2a = true;
boolean invert_q2a = false;
boolean test_quat = false;
boolean test_quat0 = false;
boolean test_quat2= true;
boolean use_degrees = true;
boolean debug_tilts = true;
boolean invert_order= false; // print them
boolean invert_y = true; // only for tilts->affines
boolean show_details =false;
boolean do_not_save = true;
if (test_quat0) {
QuatUtils.testQuatAff();
......@@ -212,12 +219,33 @@ public class OrthoAltitudeMatch {
}
double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
if (test_quat2) {
System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
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) {
affine_pair= SingularValueDecomposition.removeTiltRotScale(
affine_pair, // double [][] A,
false, // boolean removeTilt,
true, // boolean removeRot,
true, // boolean removeScale,
false, // boolean removeOffset,
false); // boolean max_is_scale);
} else {
affine_pair= new double [][] {affine_pair[0].clone(),affine_pair[1].clone()};
}
double [][] affine_pair_nors = SingularValueDecomposition.removeTiltRotScale(
affine_pair, // double [][] A,
false, // boolean removeTilt,
true, // boolean removeRot,
true, // boolean removeScale,
false, // boolean removeOffset,
false); // boolean max_is_scale);
// calculate second from first and pair
double [][] affine0 = ortho_maps[ipair[0]].getAffine();
......@@ -268,10 +296,27 @@ public class OrthoAltitudeMatch {
}
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(
alt_datas[0], // double [] tilts0,
alt_data, // double [] tilts_diff,
affine_pair_nors, // double [][] affine_pair_nors,
invert_q2a, // boolean invert_q2a) // invert result affines (to match "usual")
// debug_tilts,
invert_order, // boolean invert_order,
invert_y); // boolean invert_y)
if (!show_details) {
continue;
}
alt_datas[0]=tilts0_mod.clone();
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]
}
System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair "));
System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
System.out.println();
......@@ -303,9 +348,9 @@ public class OrthoAltitudeMatch {
for (int ns = 0; ns < quat_scenes.length; ns++) {
quat_scenes[ns] = QuatUtils.sceneRelLocalGround(
alt_datas[ns], // double [] txy,
affines[ns], // double [][] affine,
remove_rs? null:affines[ns], // double [][] affine,
y_down_ccw); // boolean y_down_ccw)
System.out.println("quat_scenes["+ns+"]="+QuatUtils.toString(quat_scenes[ns],use_degrees));
System.out.println("quat_scenes["+ns+"]= "+QuatUtils.toString(quat_scenes[ns],use_degrees));
// 0, 1 and simulated 2?
}
double [] quat1a = QuatUtils.multiplyScaled(quat_scenes[0], quat_diff_scenes);
......@@ -322,9 +367,35 @@ public class OrthoAltitudeMatch {
System.out.println();
}
System.out.println("================== compare diff affines w/o rot and scale");
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("svd_affine_pair_nors= " + svd_affine_pair_nors.toString(use_degrees));
System.out.println();
double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(
alt_datas[0], // double [] tilts0,
alt_data, // double [] tilts_diff,
invert_q2a, // boolean invert_q2a) // false
debug_tilts,
invert_order, // boolean invert_order,
invert_y); // boolean invert_y)
SingularValueDecomposition svd_affine_tilt_diff =
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_tilt_diff, y_down_ccw);
System.out.println(QuatUtils.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));
SingularValueDecomposition svd_adq_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq_err, y_down_ccw);
System.out.println(QuatUtils.affinesToString(adq_err, "adq_err"));
System.out.println("svd_adq_err= " + svd_adq_err.toString(use_degrees));
System.out.println();
System.out.println("+++++++++++++ Relative affine from quat2 to quat0 (from tilts, should match affine_pair)");
System.out.println(QuatUtils.affinesToString(affine_pair, "affine_pair "));
System.out.println("svd_affine_pair= " + svd_affine_pair.toString(use_degrees));
System.out.println("+++++++++++++ Relative affine from quat2 to quat0 (from tilts, should match affine_pair)");
double [][] adq20 = QuatUtils.matMult2x2(aff_qscenes[2], QuatUtils.matInverse2x2(aff_qscenes[0]));
SingularValueDecomposition svd_adq20=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20, y_down_ccw);
......@@ -332,6 +403,12 @@ public class OrthoAltitudeMatch {
System.out.println("svd_adq20= " + svd_adq20.toString(use_degrees));
System.out.println();
double [][] adq20_err = QuatUtils.matMult2x2(adq20, QuatUtils.matInverse2x2(affine_pair));
SingularValueDecomposition svd_adq20_err=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20_err, y_down_ccw);
System.out.println(QuatUtils.affinesToString(adq20_err, "adq20_err"));
System.out.println("svd_adq20_err= " + svd_adq20_err.toString(use_degrees));
System.out.println();
// double [][] adq20i = QuatUtils.matMult2x2(QuatUtils.matInverse2x2(aff_qscenes[0]),aff_qscenes[2]);
double [][] adq20i = QuatUtils.matInverse2x2(adq20);
SingularValueDecomposition svd_adq20i=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20i, y_down_ccw);
......@@ -339,11 +416,6 @@ public class OrthoAltitudeMatch {
System.out.println("svd_adq20i= " + svd_adq20i.toString(use_degrees));
System.out.println();
double [][] adq20_diff = QuatUtils.matMult2x2(adq20, QuatUtils.matInverse2x2(affine_pair));
SingularValueDecomposition svd_adq20_diff=SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(adq20_diff, y_down_ccw);
System.out.println(QuatUtils.affinesToString(adq20_diff, "adq20_diff"));
System.out.println("svd_adq20_diff= " + svd_adq20_diff.toString(use_degrees));
System.out.println();
/*
double [][] iadq20 = QuatUtils.matInverse2x2(adq20);
......@@ -734,7 +806,9 @@ public class OrthoAltitudeMatch {
// double [] alt_data = new double[3];
// System.arraycopy(alt_data5, 0, alt_data, 0, alt_data.length);
if (!do_not_save) {
pairwiseOrthoMatch.setAltData(alt_data);
}
if (log_append && (log_path != null)) { // assuming directory exists
StringBuffer sb = new StringBuffer();
......
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java
View file @ 203fad06
......@@ -23,6 +23,8 @@
package com.elphel.imagej.orthomosaic;
import com.elphel.imagej.common.GenericJTabbedDialog;
public final class QuatUtils { // static class
public static final double [] UNIT_QUAT = {1,0,0,0};
public static final double [][] UNIT_AFFINE = {{1,0},{0,1}};
......@@ -209,6 +211,8 @@ public final class QuatUtils { // static class
public static String affinesToString(
double [][] a,
String name) {
boolean max_is_scale = false;
boolean show_diff_unity=true;
int flen = name.length();
String fmt1 = String.format("%%%ds = [[",flen);
String fmt2 = String.format("%%%ds [",flen);
......@@ -229,9 +233,40 @@ public final class QuatUtils { // static class
}
}
}
if (show_diff_unity) {
double [][] a_noS = SingularValueDecomposition.removeTiltRotScale(
a, // double [][] A,
false, // boolean removeTilt,
false, // boolean removeRot,
true, // boolean removeScale,
false, // boolean removeOffset,
max_is_scale); // boolean max_is_scale);
double [][] a_noRS = SingularValueDecomposition.removeTiltRotScale(
a, // double [][] A,
false, // boolean removeTilt,
true, // boolean removeRot,
true, // boolean removeScale,
false, // boolean removeOffset,
max_is_scale); // boolean max_is_scale);
double mag = diffUnity(a);
double mag_noS = diffUnity(a_noS);
double mag_noRS = diffUnity(a_noRS);
s+=String.format(", magnitude=%12.9f, mag_no_scale=%12.9f mag_tilts=%12.9f", mag, mag_noS, mag_noRS);
}
return s;
}
public static double diffUnity(
double [][] a) {
return Math.sqrt((a[0][0]-1)*(a[0][0]-1) + (a[1][1]-1)*(a[1][1]-1) + a[0][1]*a[0][1] + a[1][0]*a[1][0]);
}
public static double [] quatRotDirTiltScale(
double rot,
double dir,
......@@ -369,11 +404,202 @@ public final class QuatUtils { // static class
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);
double [] qrt= multiply(qrot,qinv_tilts); // just for debugging
// double [] quat= scale(multiply(qrot,qinv_tilts),scale);
double [] quat= scale(multiply(qinv_tilts,qrot),scale);
return quat;
}
/**
* Calculate relative affine transform from scene0 to scene1
* from tilts of the first scene and differential tilt from
* scene0 to scene1 (tilt1 - tilt0). Individual scene tilt
* measurement depends on finding ground planes that may be
* inacurate due to non-flat surfaces, so differential tilt
* is much better. Tilts are calculated after scene rotations
* are corrected, so the result affine does not include rotation
* and scale (they should be removed from the differential
* affine when comparing, This method is intended for fitting
* tilts0 (first manually, then with LMA) to get the best fit
* between the differential tilt and differential affine
* transform and so resolving ambiguity of the sign of affine
* tilt.
* @param tilts0 first scene tilts (tiltX, tiltY, optional offset)
* @param tilts_diff differential tilt (scene1 - scene0)
* @param invert_q2a invert affines from quternions to match "usual" ones
* @return differential affine transform (no scale and rotation)
*/
public static double[][] diffAffineFromTilts(
double [] tilts0,
double [] tilts_diff,
boolean invert_q2a, // invert result affines (to match "usual")
boolean debug){
return diffAffineFromTilts(
tilts0,
tilts_diff,
invert_q2a, // invert result affines (to match "usual")
debug,
false, // boolean invert_order,
false); // boolean invert_y)
}
public static double[][] diffAffineFromTilts(
double [] tilts0,
double [] tilts_diff,
boolean invert_q2a, // invert result affines (to match "usual")
boolean debug,
boolean invert_order,
boolean invert_y) {
boolean y_down_ccw = true;
double [] tilts1 = tilts0.clone();
for (int i = 0; i < tilts_diff.length;i++) {
tilts1[i] += tilts_diff[i];
}
double [][] tilts = {tilts0, tilts1};
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);
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
}
}
// double [][] iaffines0 = matInverse2x2(affines[0]);
double [][] affine_diff = matMult2x2(affines[1], matInverse2x2(affines[0]));
if (invert_order) {
affine_diff = matMult2x2(matInverse2x2(affines[0]), affines[1]);
}
if (debug) {
double [] w_min_max_diff = SingularValueDecomposition.getMinMaxEigenValues(affine_diff);
double aff_tilt_diff = w_min_max_diff[1]/w_min_max_diff[0] - 1.0; // >=0.0
System.out.println(String.format(" tilt0=%12.9f, tilt1=%12.9f, tilt_diff=%12.9f",aff_tilts[0], aff_tilts[1], aff_tilt_diff));
}
/*
SingularValueDecomposition[] svds = new SingularValueDecomposition[2];
for (int nscene = 0; nscene < 2; nscene++) {
svds[nscene] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[nscene], y_down_ccw ^ invert_y);
}
SingularValueDecomposition svds_diff =SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_diff, y_down_ccw);
*/
return affine_diff;
}
public static double [] manualFitTilt(
double [] tilts0,
double [] tilts_diff,
double [][] affine_pair_nors,
boolean invert_q2a, // invert result affines (to match "usual")
boolean invert_order,
boolean invert_y) {
boolean y_down_ccw = true;
boolean use_degrees = true;
boolean debug_tilts = true;
double [] tilt0_delta = new double[3]; // [2] - not used
double [] tilts0_mod = tilts0.clone();
double [] prev_tilts=new double [3];
double [] prev2_tilts=new double [3];
double last_err = Double.NaN;
double prev_err = Double.NaN;
double prev2_err = Double.NaN;
double err_scale = 1e6;
while(true) {
double last_tilt=Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);
double prevt_tilt=Math.sqrt(prev_tilts[0]*prev_tilts[0]+prev_tilts[1]*prev_tilts[1]);
GenericJTabbedDialog gd = new GenericJTabbedDialog("Select tilt correction, TILT="+
String.format("TILT=%.3f%% (%.3f%%)",100*last_tilt, 100*prevt_tilt),450,150);
gd.addNumericField(String.format("tiltX (%8.5f)",tilts0[0]), tilt0_delta[0], 6,10, String.format("was %f, %f", prev_tilts[0],prev2_tilts[0]),
"Set correction to tilts0[0]");
gd.addNumericField(String.format("tiltY (%8.5f)",tilts0[1]), tilt0_delta[1], 6,10, String.format("was %f, %f", prev_tilts[1],prev2_tilts[1]),
"Set correction to tilts0[1]");
gd.addMessage(String.format("Last error\u00D7%.0e = %.4f (previous was %.4f, %4f)", err_scale, err_scale*last_err, err_scale*prev_err, err_scale*prev2_err));
gd.showDialog();
if (gd.wasCanceled()) break;
prev2_tilts = prev_tilts.clone();
prev_tilts = tilt0_delta.clone();
tilt0_delta[0] = gd.getNextNumber();
tilt0_delta[1] = gd.getNextNumber();
double [] tilts1_mod = tilts0_mod.clone();
for (int i = 0; i < 2; i++) {
tilts0_mod[i] = tilts0[i]+tilt0_delta[i];
tilts1_mod[i] += tilts_diff[i];
// System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",
// i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));
}
double tilt0_delta_len = Math.sqrt(tilt0_delta[0]*tilt0_delta[0]+tilt0_delta[1]*tilt0_delta[1]);
double tilt0_len = Math.sqrt(tilts0_mod[0]*tilts0_mod[0]+tilts0_mod[1]*tilts0_mod[1]);
double tilt1_len = Math.sqrt(tilts1_mod[0]*tilts1_mod[0]+tilts1_mod[1]*tilts1_mod[1]);
double tilts_diff_len = Math.sqrt(tilts_diff[0]*tilts_diff[0]+tilts_diff[1]*tilts_diff[1]);
double tilt01_dot = tilts0_mod[0]*tilts1_mod[0]+tilts0_mod[1]*tilts1_mod[1];
double tilt01_acos = Math.acos(tilt01_dot/tilt0_len/tilt1_len)*180/Math.PI;
// double
System.out.println("---------------------------------------: "+
String.format("TILT_CORR=%.3f%% (TILT0=%.3f%%, TILT1=%.3f%%, TILTS_COS=%.1f\u00B0, TILT_DIFF=%.3f%%)",
100*tilt0_delta_len,
100*tilt0_len,
100*tilt1_len,
tilt01_acos,
100*tilts_diff_len));
for (int i = 0; i < 2; i++) {
System.out.println(String.format("tilt0%d diff:%9.6f was:%9.6f delta:%9.6f final:%9.6f",
i, tilts_diff[i], tilts0[i], tilt0_delta[i], tilts0_mod[i]));
}
double [][] affine_tilt_diff = QuatUtils.diffAffineFromTilts(
tilts0_mod, // double [] tilts0,
tilts_diff, // double [] tilts_diff,
invert_q2a, // boolean invert_q2a) // false
debug_tilts,
invert_order, // boolean invert_order,
invert_y); // boolean invert_y)
SingularValueDecomposition svd_affine_tilt_diff =
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_tilt_diff, y_down_ccw);
//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("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("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 [] 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("svd_adq_err= " + svd_adq_err.toString(use_degrees));
/*
double err = Math.sqrt(
(adq_err[0][0]-1.0)*(adq_err[0][0]-1.0)+
(adq_err[1][1]-1.0)*(adq_err[1][1]-1.0) +
adq_err[0][1]*adq_err[0][1]+adq_err[1][0]*adq_err[1][0]);
System.out.println(String.format("err=%12.10f",err));
*/
System.out.println(String.format("AFF_TILT_ERR\u00D7%.0e=%.4f (previous were %.4f, %.4f)",
err_scale, err_scale*aff_tilt_err, err_scale*last_err, err_scale*prev_err));
System.out.println();
prev2_err=prev_err;
prev_err=last_err;
last_err=aff_tilt_err;
}
return tilts0_mod; // tilt0_delta;
}
/**
* Remove rotation around Z-axis, keep only tilt around axis in XY plane
......
src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java
View file @ 203fad06
......@@ -4,7 +4,7 @@ public class SingularValueDecomposition {
double beta;
double gamma;
double w1;
double w2;
double w2; // < w1 after singularValueDecompose
double rot;
double scale; // min (w1,w2) or sqrt(w1*w2) for raw singularValueDecompose()
double ratio; // <=1.0, ==cos(tilt) or w2/w1 for raw singularValueDecompose()
......@@ -90,7 +90,7 @@ public class SingularValueDecomposition {
double beta = (g_p_b - g_m_b)/2;
SingularValueDecomposition svd= new SingularValueDecomposition (beta,gamma,w1,w2,g_p_b);
svd.scale = Math.sqrt(svd.w1*svd.w2);
svd.ratio = svd.w2/svd.w1;
svd.ratio = svd.w2/svd.w1; // <= 1.0;
return svd;
}
......@@ -112,7 +112,7 @@ public class SingularValueDecomposition {
if (A[0].length < 3) {
return AR;
}
A=A.clone();
A = new double [][] {A[0].clone(),A[1].clone()};
if (removeOffset) {
for (int i = 0; i < 2; i++) {
A[i][2] = 0;
......@@ -162,7 +162,8 @@ public class SingularValueDecomposition {
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;
return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});
return new double[] {w2,w1}; // because w1_m_w2_2 >=0 so w1 >= w2
// return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});
}
......@@ -209,7 +210,7 @@ public class SingularValueDecomposition {
// (and source image coordinates), while X should correspond to
// the axis of rotation, and scale is just scale caused by error in
// altitude.
scale = Math.min(w1, w2);
scale = Math.min(w1, w2); // w1 >= w2 after singularValueDecompose() !
ratio = w1/w2; // <=1.0, ==cos(tilt)
if (w1 > w2) { // rotate tilt by PI/2
ratio = w2/w1;
......
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