Commit ace24186 authored by Andrey Filippov's avatar Andrey Filippov

fixing, but so far scale > 1 for stretch, while affines are image coord

from map coord
parent 6a6d0c40
...@@ -314,6 +314,11 @@ public class OrthoAltitudeMatch { ...@@ -314,6 +314,11 @@ public class OrthoAltitudeMatch {
true, // boolean stretch, true, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt) make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw); y_down_ccw); // boolean y_down_ccw);
double [][] aff1_shrink = QuatUtils.quatToAffine(
quats01[0], // double [] quat,
false, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] aff2_shrink = QuatUtils.quatToAffine( double [][] aff2_shrink = QuatUtils.quatToAffine(
quats01[2], // double [] quat, quats01[2], // double [] quat,
false, // boolean stretch, false, // boolean stretch,
...@@ -325,27 +330,37 @@ public class OrthoAltitudeMatch { ...@@ -325,27 +330,37 @@ public class OrthoAltitudeMatch {
make__pure_tilt, // boolean make__pure_tilt) make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw); y_down_ccw); // boolean y_down_ccw);
double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair, y_down_ccw); double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair,true, y_down_ccw);
double [] sqaffd_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaffd_pm[1])}; // normalizes double [] sqaffd_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaffd_pm[1])}; // normalizes
System.out.println("qaffd_pm[0]= ["+qaffd_pm[0][0]+ ","+qaffd_pm[0][1]+ ","+qaffd_pm[0][2]+ ","+qaffd_pm[0][3]+ "] scale="+sqaffd_pm[0]); System.out.println("qaffd_pm[0]= ["+qaffd_pm[0][0]+ ","+qaffd_pm[0][1]+ ","+qaffd_pm[0][2]+ ","+qaffd_pm[0][3]+ "] scale="+sqaffd_pm[0]);
System.out.println("qaffd_pm[1]= ["+qaffd_pm[1][0]+ ","+qaffd_pm[1][1]+ ","+qaffd_pm[1][2]+ ","+qaffd_pm[1][3]+ "] scale="+sqaffd_pm[1]); System.out.println("qaffd_pm[1]= ["+qaffd_pm[1][0]+ ","+qaffd_pm[1][1]+ ","+qaffd_pm[1][2]+ ","+qaffd_pm[1][3]+ "] scale="+sqaffd_pm[1]);
double [][] qaff1_pm = QuatUtils.affineToQuatScaled(aff1_stretch, y_down_ccw);
double [] sqaff1_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaff1_pm[1])}; // normalizes double [][] qaff1a_pm = QuatUtils.affineToQuatScaled(aff1_shrink, false,y_down_ccw);
double [] sqaff1a_pm = {QuatUtils.normalizeInPlace(qaff1a_pm[0]),QuatUtils.normalizeInPlace(qaff1a_pm[1])}; // normalizes
System.out.println("qaff1a_pm[0]= ["+qaff1a_pm[0][0]+ ","+qaff1a_pm[0][1]+ ","+qaff1a_pm[0][2]+ ","+qaff1a_pm[0][3]+ "] scale="+sqaff1a_pm[0]);
System.out.println("qaff1a_pm[1]= ["+qaff1a_pm[1][0]+ ","+qaff1a_pm[1][1]+ ","+qaff1a_pm[1][2]+ ","+qaff1a_pm[1][3]+ "] scale="+sqaff1a_pm[1]);
double [][] qaff1_pm = QuatUtils.affineToQuatScaled(aff1_stretch, true, y_down_ccw);
double [] sqaff1_pm = {QuatUtils.normalizeInPlace(qaff1_pm[0]),QuatUtils.normalizeInPlace(qaff1_pm[1])}; // normalizes
System.out.println("qaff1_pm[0]= ["+qaff1_pm[0][0]+ ","+qaff1_pm[0][1]+ ","+qaff1_pm[0][2]+ ","+qaff1_pm[0][3]+ "] scale="+sqaff1_pm[0]); System.out.println("qaff1_pm[0]= ["+qaff1_pm[0][0]+ ","+qaff1_pm[0][1]+ ","+qaff1_pm[0][2]+ ","+qaff1_pm[0][3]+ "] scale="+sqaff1_pm[0]);
System.out.println("qaff1_pm[1]= ["+qaff1_pm[1][0]+ ","+qaff1_pm[1][1]+ ","+qaff1_pm[1][2]+ ","+qaff1_pm[1][3]+ "] scale="+sqaff1_pm[1]); System.out.println("qaff1_pm[1]= ["+qaff1_pm[1][0]+ ","+qaff1_pm[1][1]+ ","+qaff1_pm[1][2]+ ","+qaff1_pm[1][3]+ "] scale="+sqaff1_pm[1]);
double [][] qaff2a_pm = QuatUtils.affineToQuatScaled(aff2_shrink, y_down_ccw);
double [][] qaff2a_pm = QuatUtils.affineToQuatScaled(aff2_shrink, false, y_down_ccw);
double [] sqaff2a_pm = {QuatUtils.normalizeInPlace(qaff2a_pm[0]),QuatUtils.normalizeInPlace(qaff2a_pm[1])}; // normalizes double [] sqaff2a_pm = {QuatUtils.normalizeInPlace(qaff2a_pm[0]),QuatUtils.normalizeInPlace(qaff2a_pm[1])}; // normalizes
System.out.println("qaff2a_pm[0]= ["+qaff2a_pm[0][0]+","+qaff2a_pm[0][1]+","+qaff2a_pm[0][2]+","+qaff2a_pm[0][3]+"] scale="+sqaff2a_pm[0]); System.out.println("qaff2a_pm[0]= ["+qaff2a_pm[0][0]+","+qaff2a_pm[0][1]+","+qaff2a_pm[0][2]+","+qaff2a_pm[0][3]+"] scale="+sqaff2a_pm[0]);
System.out.println("qaff2a_pm[1]= ["+qaff2a_pm[1][0]+","+qaff2a_pm[1][1]+","+qaff2a_pm[1][2]+","+qaff2a_pm[1][3]+"] scale="+sqaff2a_pm[1]); System.out.println("qaff2a_pm[1]= ["+qaff2a_pm[1][0]+","+qaff2a_pm[1][1]+","+qaff2a_pm[1][2]+","+qaff2a_pm[1][3]+"] scale="+sqaff2a_pm[1]);
double [][] qaff2_pm = QuatUtils.affineToQuatScaled(aff2_stretch, y_down_ccw);
double [][] qaff2_pm = QuatUtils.affineToQuatScaled(aff2_stretch, true, y_down_ccw);
double [] sqaff2_pm = {QuatUtils.normalizeInPlace(qaff2_pm[0]),QuatUtils.normalizeInPlace(qaff2_pm[1])}; // normalizes double [] sqaff2_pm = {QuatUtils.normalizeInPlace(qaff2_pm[0]),QuatUtils.normalizeInPlace(qaff2_pm[1])}; // normalizes
System.out.println("qaff2_pm[0]= ["+qaff2_pm[0][0]+ ","+qaff2_pm[0][1]+ ","+qaff2_pm[0][2]+ ","+qaff2_pm[0][3]+ "] scale="+sqaff2_pm[0]); System.out.println("qaff2_pm[0]= ["+qaff2_pm[0][0]+ ","+qaff2_pm[0][1]+ ","+qaff2_pm[0][2]+ ","+qaff2_pm[0][3]+ "] scale="+sqaff2_pm[0]);
System.out.println("qaff2_pm[1]= ["+qaff2_pm[1][0]+ ","+qaff2_pm[1][1]+ ","+qaff2_pm[1][2]+ ","+qaff2_pm[1][3]+ "] scale="+sqaff2_pm[1]); System.out.println("qaff2_pm[1]= ["+qaff2_pm[1][0]+ ","+qaff2_pm[1][1]+ ","+qaff2_pm[1][2]+ ","+qaff2_pm[1][3]+ "] scale="+sqaff2_pm[1]);
double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], y_down_ccw);
double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], false, y_down_ccw);
double [] sqaffine0_pm ={QuatUtils.normalizeInPlace(qaffine0_pm[0]),QuatUtils.normalizeInPlace(qaffine0_pm[1])}; // normalizes double [] sqaffine0_pm ={QuatUtils.normalizeInPlace(qaffine0_pm[0]),QuatUtils.normalizeInPlace(qaffine0_pm[1])}; // normalizes
System.out.println("qaffine0_pm[0]=["+qaffine0_pm[0][0]+ ","+qaffine0_pm[0][1]+ ","+qaffine0_pm[0][2]+ ","+qaffine0_pm[0][3]+ "] scale="+sqaffine0_pm[0]); System.out.println("qaffine0_pm[0]=["+qaffine0_pm[0][0]+ ","+qaffine0_pm[0][1]+ ","+qaffine0_pm[0][2]+ ","+qaffine0_pm[0][3]+ "] scale="+sqaffine0_pm[0]);
System.out.println("qaffine0_pm[1]=["+qaffine0_pm[1][0]+ ","+qaffine0_pm[1][1]+ ","+qaffine0_pm[1][2]+ ","+qaffine0_pm[1][3]+ "] scale="+sqaffine0_pm[1]); System.out.println("qaffine0_pm[1]=["+qaffine0_pm[1][0]+ ","+qaffine0_pm[1][1]+ ","+qaffine0_pm[1][2]+ ","+qaffine0_pm[1][3]+ "] scale="+sqaffine0_pm[1]);
double [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1], y_down_ccw);
double [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1], false, y_down_ccw);
double [] sqaffine1_pm ={QuatUtils.normalizeInPlace(qaffine1_pm[0]),QuatUtils.normalizeInPlace(qaffine1_pm[1])}; // normalizes double [] sqaffine1_pm ={QuatUtils.normalizeInPlace(qaffine1_pm[0]),QuatUtils.normalizeInPlace(qaffine1_pm[1])}; // normalizes
System.out.println("qaffine1_pm[0]=["+qaffine1_pm[0][0]+ ","+qaffine1_pm[0][1]+ ","+qaffine1_pm[0][2]+ ","+qaffine1_pm[0][3]+ "] scale="+sqaffine1_pm[0]); System.out.println("qaffine1_pm[0]=["+qaffine1_pm[0][0]+ ","+qaffine1_pm[0][1]+ ","+qaffine1_pm[0][2]+ ","+qaffine1_pm[0][3]+ "] scale="+sqaffine1_pm[0]);
System.out.println("qaffine1_pm[1]=["+qaffine1_pm[1][0]+ ","+qaffine1_pm[1][1]+ ","+qaffine1_pm[1][2]+ ","+qaffine1_pm[1][3]+ "] scale="+sqaffine1_pm[1]); System.out.println("qaffine1_pm[1]=["+qaffine1_pm[1][0]+ ","+qaffine1_pm[1][1]+ ","+qaffine1_pm[1][2]+ ","+qaffine1_pm[1][3]+ "] scale="+sqaffine1_pm[1]);
......
...@@ -211,23 +211,26 @@ public final class QuatUtils { // static class ...@@ -211,23 +211,26 @@ public final class QuatUtils { // static class
*/ */
public static double [][] affineToQuatScaled ( public static double [][] affineToQuatScaled (
double [][] affine, double [][] affine,
boolean stretch,
boolean y_down_ccw) { boolean y_down_ccw) {
SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta( SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
affine, affine,
y_down_ccw); y_down_ccw);
double scale = svd.getMinScale(); // double scale = stretch? svd.getMinScale(): (1/svd.getMinScale());
double scale = stretch? svd.getMinScale(): svd.getMaxScale();
double rot = svd.getRotAngle(); // TODO: check sign ! double rot = svd.getRotAngle(); // TODO: check sign !
// Now beta,rot correspond to Y - up // Now beta,rot correspond to Y - up
double chalfrot = Math.cos(rot/2); double chalfrot = Math.cos(rot/2);
double shalfrot = Math.sin(rot/2); double shalfrot = Math.sin(rot/2);
double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
double cbeta = Math.cos(svd.beta); // rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
double sbeta = Math.sin(svd.beta); // TODO: check sign ! double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta));
double smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
double tiltAngle = svd.getTiltAngle(); // >0 double tiltAngle = svd.getTiltAngle(); // >0
double ctilt = Math.cos(tiltAngle/2); double ctilt = Math.cos(tiltAngle/2);
double stilt = Math.sin(tiltAngle/2); double stilt = Math.sin(tiltAngle/2);
double [] q_plus = {ctilt, stilt*cbeta, stilt*sbeta, 0}; double [] q_plus = {ctilt, stilt*cmbeta, stilt*smbeta, 0};
double [] q_minus = {ctilt, -stilt*cbeta, -stilt*sbeta, 0}; double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)}; double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
return quats_pm; return quats_pm;
} }
......
...@@ -186,7 +186,7 @@ public class SingularValueDecomposition { ...@@ -186,7 +186,7 @@ public class SingularValueDecomposition {
while (gamma >= Math.PI/2) { while (gamma >= Math.PI/2) {
gamma -= Math.PI; gamma -= Math.PI;
} }
while (gamma < Math.PI/2) { while (gamma < -Math.PI/2) {
gamma += Math.PI; gamma += Math.PI;
} }
......
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