Commit 203fad06 authored by Andrey Filippov's avatar Andrey Filippov

quaternions-affines

parent d3389b58
......@@ -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();
......
......@@ -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