Commit 0abd52b8 authored by Andrey Filippov's avatar Andrey Filippov

fixing quaternions-affines

parent d9f79276
...@@ -56,6 +56,13 @@ public class OrthoAltitudeMatch { ...@@ -56,6 +56,13 @@ public class OrthoAltitudeMatch {
String orthoMapsCollection_path, String orthoMapsCollection_path,
int debugLevel) { int debugLevel) {
boolean y_down_ccw = true; boolean y_down_ccw = true;
boolean invert_q2a = true;
boolean test_quat = true;
if (test_quat) {
QuatUtils.testQuatAff();
}
int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs
available_pairs, // pairs_defined_abs,// int [][] pairs, available_pairs, // pairs_defined_abs,// int [][] pairs,
null); // int [] indices_in) // preselected indices or null null); // int [] indices_in) // preselected indices or null
...@@ -197,7 +204,7 @@ public class OrthoAltitudeMatch { ...@@ -197,7 +204,7 @@ public class OrthoAltitudeMatch {
} }
double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]}; double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
boolean test_quat = true;
if (test_quat) { if (test_quat) {
System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]); System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
boolean use_degrees = true; boolean use_degrees = true;
...@@ -331,6 +338,7 @@ public class OrthoAltitudeMatch { ...@@ -331,6 +338,7 @@ public class OrthoAltitudeMatch {
{A1a.get(1,0),A1a.get(1,1)}}; {A1a.get(1,0),A1a.get(1,1)}};
double [][][] affines = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine(), affine1a}; double [][][] affines = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine(), affine1a};
boolean make__pure_tilt = false; boolean make__pure_tilt = false;
/*
double [][] aff1_stretch = QuatUtils.quatToAffine( double [][] aff1_stretch = QuatUtils.quatToAffine(
quats01[0], // double [] quat, quats01[0], // double [] quat,
true, // boolean stretch, true, // boolean stretch,
...@@ -351,6 +359,25 @@ public class OrthoAltitudeMatch { ...@@ -351,6 +359,25 @@ 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_stretch = QuatUtils.quatToAffine( // use old for stretch
quats01[0], // double [] quat,
true, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] aff1_shrink = QuatUtils.quatToAffine(
quats01[0], // double [] quat,
invert_q2a, // boolean stretch,
y_down_ccw); // boolean y_down_ccw);
double [][] aff2_shrink = QuatUtils.quatToAffine(
quats01[2], // double [] quat,
invert_q2a, // boolean stretch,
y_down_ccw); // boolean y_down_ccw);
double [][] aff2_stretch = QuatUtils.quatToAffine( // use old for stretch
quats01[2], // double [] quat,
true, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair,true, y_down_ccw); double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair,true, y_down_ccw);
System.out.println("qaffd_pm[0]= "+QuatUtils.toString(qaffd_pm[0],use_degrees)); System.out.println("qaffd_pm[0]= "+QuatUtils.toString(qaffd_pm[0],use_degrees));
...@@ -390,8 +417,10 @@ public class OrthoAltitudeMatch { ...@@ -390,8 +417,10 @@ public class OrthoAltitudeMatch {
double [][][][] raffines_pm = new double [affines.length][2][][]; double [][][][] raffines_pm = new double [affines.length][2][][];
SingularValueDecomposition[][] rsvd_pm = new SingularValueDecomposition [affines.length][2]; SingularValueDecomposition[][] rsvd_pm = new SingularValueDecomposition [affines.length][2];
for (int i = 0; i < raffines_pm.length;i++) { for (int i = 0; i < raffines_pm.length;i++) {
raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],false,true,true); // raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],false,true,true);
raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],false,false,true); // raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],false,false,true);
raffines_pm[i][0] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[0],invert_q2a,true);
raffines_pm[i][1] = QuatUtils.quatToAffine(QuatUtils.affineToQuatScaled(affines[i], false, y_down_ccw)[1],invert_q2a,true);
rsvd_pm[i][0] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][0], y_down_ccw); rsvd_pm[i][0] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][0], y_down_ccw);
rsvd_pm[i][1] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][1], y_down_ccw); rsvd_pm[i][1] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(raffines_pm[i][1], y_down_ccw);
System.out.println("raffines["+i+"][0]= [["+raffines_pm[i][0][0][0]+ ","+raffines_pm[i][0][0][1]+"],"); System.out.println("raffines["+i+"][0]= [["+raffines_pm[i][0][0][0]+ ","+raffines_pm[i][0][0][1]+"],");
...@@ -444,17 +473,18 @@ public class OrthoAltitudeMatch { ...@@ -444,17 +473,18 @@ public class OrthoAltitudeMatch {
double [] svariants= new double [qvariants.length]; double [] svariants= new double [qvariants.length];
double [][][] taffines = new double [qvariants.length][][]; double [][][] taffines = new double [qvariants.length][][];
SingularValueDecomposition[] svd_avars = new SingularValueDecomposition[qvariants.length]; SingularValueDecomposition[] svd_avars = new SingularValueDecomposition[qvariants.length];
/// double [] svariants1= new double [qvariants1.length];
for (int i = 0; i < svariants.length; i++) svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]); for (int i = 0; i < svariants.length; i++) svariants[i] = QuatUtils.normalizeInPlace(qvariants[i]);
/// for (int i = 0; i < svariants1.length; i++) svariants1[i] = QuatUtils.normalizeInPlace(qvariants1[i]);
for (int i = 0; i < svariants.length; i++) { for (int i = 0; i < svariants.length; i++) {
System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]); System.out.println(" qvariant["+i+"] = ["+qvariants[i][0]+ ","+qvariants[i][1]+ ","+qvariants[i][2]+ ","+qvariants[i][3]+ "] scale="+svariants[i]);
/// System.out.println("qvariant1["+i+"] = ["+qvariants1[i][0]+ ","+qvariants1[i][1]+ ","+qvariants1[i][2]+ ","+qvariants1[i][3]+ "] scale="+svariants1[i]); /// taffines[i] = QuatUtils.quatToAffine(
/// qvariants[i], // double [] quat,
/// false, // boolean stretch,
/// false, // make__pure_tilt, // boolean make__pure_tilt)
/// y_down_ccw); // boolean y_down_ccw);
taffines[i] = QuatUtils.quatToAffine( taffines[i] = QuatUtils.quatToAffine(
qvariants[i], // double [] quat, qvariants[i], // double [] quat,
false, // boolean stretch, invert_q2a, // boolean invert,
false, // make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw); y_down_ccw); // boolean y_down_ccw);
svd_avars[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(taffines[i], y_down_ccw); svd_avars[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(taffines[i], y_down_ccw);
} }
...@@ -473,8 +503,8 @@ public class OrthoAltitudeMatch { ...@@ -473,8 +503,8 @@ public class OrthoAltitudeMatch {
} }
double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invdert order? double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch); // invert order?
double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invdert order? double [][] aff_combo1= QuatUtils.matMult(aff1_stretch,aff2_shrink); // invert order?
SingularValueDecomposition svd_affine_pair = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw) SingularValueDecomposition svd_affine_pair = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
SingularValueDecomposition[] svd_affines = { SingularValueDecomposition[] svd_affines = {
SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw), SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw),
......
...@@ -75,26 +75,6 @@ public final class QuatUtils { // static class ...@@ -75,26 +75,6 @@ public final class QuatUtils { // static class
return scale(invert(normalize(quat)),1.0/norm(quat)); return scale(invert(normalize(quat)),1.0/norm(quat));
} }
/*
public static double [] divide( // pure rotation
double [] r,
double [] s ) {
return multiply(invert(r), s);
}
public static double [] divideScaled(
double [] r,
double [] s ) {
return multiplyScaled(invertScaled(r), s);
}
public static double [] divideScaled1(
double [] r,
double [] s ) {
return multiply(invertScaled(r), s);
}
*/
public static double [] divide( // pure rotation public static double [] divide( // pure rotation
double [] r, double [] r,
...@@ -114,6 +94,164 @@ public final class QuatUtils { // static class ...@@ -114,6 +94,164 @@ public final class QuatUtils { // static class
return multiply(r, invertScaled(s)); return multiply(r, invertScaled(s));
} }
public static void testQuatAff() {
double rot1_deg = 5.0;
double dir1_deg = 80.0;
double tilt1_deg = 10.0;
double scale1 = 1.0;
double rot2_deg = 15.0;
double dir2_deg = 20.0;
double tilt2_deg = 12.0;
double scale2 = 1.1;
boolean run = true;
boolean invert_q2a= true;
boolean y_down_ccw = true;
System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
System.out.println("invert_q2a="+invert_q2a+", y_down_ccw="+y_down_ccw);
while (run) {
double rot1 = rot1_deg*Math.PI/180;
double dir1 = dir1_deg*Math.PI/180;
double tilt1 = tilt1_deg*Math.PI/180;
double rot2 = rot2_deg*Math.PI/180;
double dir2 = dir2_deg*Math.PI/180;
double tilt2 = tilt2_deg*Math.PI/180;
System.out.println("-----------------------------------");
System.out.println("rot1_deg="+rot1_deg+", dir1_deg="+dir1_deg+", tilt1_deg="+tilt1_deg+", scale1="+scale1);
System.out.println("rot2_deg="+rot2_deg+", dir2_deg="+dir2_deg+", tilt2_deg="+tilt2_deg+", scale2="+scale2);
System.out.println("rot1="+rot1+", dir1="+dir1+", tilt1="+tilt1+", scale1="+scale1);
System.out.println("rot2="+rot2+", dir2="+dir2+", tilt2="+tilt2+", scale2="+scale2);
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));
testQuatAff(quat1, quat2, invert_q2a, y_down_ccw);
System.out.println("run="+run);
}
}
public static void testQuatAff(
double [] quat1,
double [] quat2,
boolean invert_q2a,
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));
double [][] quats= {quat1,quat2,quat_diff};
double [][][] affines = new double [quats.length][][];
SingularValueDecomposition [] svds= new SingularValueDecomposition [quats.length];
/*
for (int i = 0; i < quats.length; i++){
affines[i] = quatToAffine(quats[i], invert_q2a,y_down_ccw);
String name = (i==2)? "_diff" : "["+i+"] ";
System.out.println(affinesToString(affines[i], "affines"+name));
svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
System.out.println("svd_affines"+name+" = "+svds[i].toString(use_degrees));
}
*/
for (int i = 0; i < 2; i++){
affines[i] = quatToAffine(quats[i], invert_q2a,y_down_ccw);
System.out.println(affinesToString(affines[i], "affines["+i+"] "));
svds[i] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[i], y_down_ccw);
System.out.println("svd_affines["+i+"] = "+svds[i].toString(use_degrees));
}
// get differential affines
affines[2] = matMult2x2 (affines[1], matInverse2x2(affines[0]));
System.out.println(affinesToString(affines[2], "affines_diff"));
svds[2] = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw);
System.out.println("svd_affines_diff = "+svds[2].toString(use_degrees));
double [][][] quats_pm = new double [2][][];
double [][][] quats_diff_pm = new double [2][2][];
for (int i = 0; i <2; i++) {
quats_pm[i] = affineToQuatScaled(affines[i], false, y_down_ccw);
quats_diff_pm[i][0]= divideScaled(quats_pm[i][0], quats[i]);
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();
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();
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));
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);
}
for (int i = 0; i < qvariants.length; i++) {
System.out.println("quat_var["+i+"]= "+QuatUtils.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));
}
return;
}
public static String affinesToString(
double [][] a,
String name) {
int flen = name.length();
String fmt1 = String.format("%%%ds = [[",flen);
String fmt2 = String.format("%%%ds [",flen);
String s = "";
for (int i = 0; i < a.length; i++) {
if (i==0) s+= String.format(fmt1, name);
else s+= String.format(fmt2, "");
for (int j=0; j < a[i].length; j++) {
s += String.format("%12.9f", a[i][j]);
if (j < (a[i].length-1)) {
s+= ", ";
} else {
if (i < (a.length-1)) {
s += "],\n";
} else {
s += "]]";
}
}
}
}
return s;
}
public static double [] quatRotDirTiltScale(
double rot,
double dir,
double tilt,
double scale) {
double chalfrot = Math.cos(rot/2);
double shalfrot = Math.sin(rot/2);
double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
double beta = dir;;
double cmbeta = Math.cos(-beta);
double smbeta = Math.sin(-beta);
double tiltAngle = tilt; // >0
double ctilt = Math.cos(tiltAngle/2);
double stilt = Math.sin(tiltAngle/2);
double [] q_plus = {ctilt, stilt*cmbeta, stilt*smbeta, 0};
// double [] q_minus = {ctilt, -stilt*cmbeta, -stilt*smbeta, 0};
double [] qs = scale(multiply(qrot,q_plus), scale);
return qs;
}
...@@ -263,6 +401,52 @@ public final class QuatUtils { // static class ...@@ -263,6 +401,52 @@ public final class QuatUtils { // static class
return affine; return affine;
} }
// external invert
public static double [][] affineToQuatScaled (
double [][] affine,
boolean y_down_ccw) {
boolean invert = true;
if (invert) {
affine = matInverse2x2(affine);
} else {
affine = affine.clone(); // isolate from input
}
// double [] eigen_vals= SingularValueDecomposition.getMinMaxEigenValues(affine);
// double scale = eigen_vals[1];
SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
affine,
y_down_ccw);
double y_sign = y_down_ccw? -1 : 1;
double scale = svd.getMaxScale();
matScale(affine, 1/scale);
double rot = svd.getRotAngle(); // rot sign should correspond Y-up (quaterions)
double chalfrot = Math.cos(rot/2);
double shalfrot = Math.sin(rot/2);
double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
double [][] invRotMatrix = SingularValueDecomposition.rotMatrix(-y_sign*rot); // undo rotation from affine
affine = matMult2x2(affine, invRotMatrix); // check symmetrical diagonal
// undo rotation
// Now beta,rot correspond to Y - up
// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
boolean stretch = false;
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 ctilt = Math.cos(tiltAngle/2);
double stilt = Math.sin(tiltAngle/2);
double [] q_plus = {ctilt, stilt*cmbeta, stilt*smbeta, 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(qrot,q_plus), scale),scale(multiply(qrot, q_minus), scale)};
return quats_pm;
}
public static double [][] quatToAffine( public static double [][] quatToAffine(
double [] quat, double [] quat,
boolean invert, boolean invert,
...@@ -325,26 +509,50 @@ q22 = q[1]*q[1] ...@@ -325,26 +509,50 @@ q22 = q[1]*q[1]
q33 = q[1]*q[1] q33 = q[1]*q[1]
q03 = q[0]*q[3] q03 = q[0]*q[3]
q12 = q[1]*q[2] q12 = q[1]*q[2]
q00 + q11 - q22 - q33 = a00
q12 - q03 = a01/2
q03 + q12 = a10/2
q00 - q11 + q22 - q33 = a11
q00 - q33 = (a00+a11)/2
q11 - q22 = (a00-a11)/2
q12 - q03 = a01/2
q03 + q12 = a10/2
q12 = (a10+a01)/2
q03 = (a10-a01)/2
q00 - q33 = (a00+a11)/2
q11 - q22 = (a00-a11)/2
q12 = c0
q03 = c1
q00 - q33 = c2
q11 - q22 = c3
*/ */
quat = quat.clone(); // not to modify original
double scale = normQuat(quat); double scale = normQuat(quat);
double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
// double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
double q00 = quat[0]*quat[0]; double q00 = quat[0]*quat[0];
double q11 = quat[1]*quat[1]; double q11 = quat[1]*quat[1];
double q22 = quat[1]*quat[1]; double q22 = quat[2]*quat[2];
double q33 = quat[1]*quat[1]; double q33 = quat[3]*quat[3];
double q03 = quat[0]*quat[3]; double q03 = quat[0]*quat[3];
double q12 = quat[1]*quat[2]; double q12 = quat[1]*quat[2]*y_sign;
double [][] affine = { double [][] affine = {
{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)}, {scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
{scale_y*2*(q03 + q12), scale_y*(q00 - q11 + q22 - q33)}}; {scale*2*(q03 + q12), scale*(q00 - q11 + q22 - q33)}};
if (invert) { if (invert) {
affine = matInverse2x2(affine); affine = matInverse2x2(affine);
} }
return affine; return affine;
} }
/** /**
* Restore quaternion (rotation+scale) from affine traqnsform (only [2][2] is used, ok to have [2][3] input * Restore quaternion (rotation+scale) from affine transform (only [2][2] is used, ok to have [2][3] input
* As there are 2 solutions, both are provided in the output. As the * As there are 2 solutions, both are provided in the output. As the
* input linear transformation matrix converts ground coordinates to source * input linear transformation matrix converts ground coordinates to source
* image coordinates, the scale in the tilt direction is > than scale in the * image coordinates, the scale in the tilt direction is > than scale in the
...@@ -357,9 +565,15 @@ q12 = q[1]*q[2] ...@@ -357,9 +565,15 @@ q12 = q[1]*q[2]
double [][] affine, double [][] affine,
boolean stretch, boolean stretch,
boolean y_down_ccw) { boolean y_down_ccw) {
affine = matInverse2x2(affine);
// TODO: why have to use gamma for direction?
SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta( SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
affine, affine,
y_down_ccw); y_down_ccw);
// SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(
// affine,
// y_down_ccw);
// double scale = stretch? svd.getMinScale(): (1/svd.getMinScale()); // double scale = stretch? svd.getMinScale(): (1/svd.getMinScale());
double scale = stretch? svd.getMinScale(): svd.getMaxScale(); double scale = stretch? svd.getMinScale(): svd.getMaxScale();
double rot = svd.getRotAngle(); // TODO: check sign ! double rot = svd.getRotAngle(); // TODO: check sign !
...@@ -368,8 +582,11 @@ q12 = q[1]*q[2] ...@@ -368,8 +582,11 @@ q12 = q[1]*q[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
// rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot) // rotation after rot is -beta (rot(beta)*W*rot(-beta)*rot(rot)
double cmbeta = stretch ? Math.cos(-svd.beta) : (Math.sin(-svd.beta)); // 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 smbeta = stretch ? Math.sin(-svd.beta) : (-Math.cos(-svd.beta)); // TODO: check sign !
double mbeta= svd.gamma; // -svd.beta
double cmbeta = stretch ? Math.cos(mbeta) : (Math.sin(mbeta));
double smbeta = stretch ? Math.sin(mbeta) : (-Math.cos(mbeta)); // 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);
...@@ -397,6 +614,19 @@ q12 = q[1]*q[2] ...@@ -397,6 +614,19 @@ q12 = q[1]*q[2]
} }
return m; return m;
} }
public static double [][] matMult2x2 (
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++) {
for (int k = 0; k < 2; k++) {
m[i][j] += m1[i][k]*m2[k][j];
}
}
}
return m;
}
public static double [] matMult ( public static double [] matMult (
double [][] m1, double [][] m1,
...@@ -413,6 +643,17 @@ q12 = q[1]*q[2] ...@@ -413,6 +643,17 @@ q12 = q[1]*q[2]
} }
return v2; return v2;
} }
public static void matScale (
double [][] a,
double scale){
for (int i = 0; i < 2; i++) {
for (int j=0; j < 2; j++) {
a[i][j] *= scale;
}
}
}
public static double [][] matInverse2x2( public static double [][] matInverse2x2(
double [][] a) { double [][] a) {
// throw new IllegalArgumentException("Only [2][2] arrays"); // throw new IllegalArgumentException("Only [2][2] arrays");
......
...@@ -90,6 +90,21 @@ public class SingularValueDecomposition { ...@@ -90,6 +90,21 @@ public class SingularValueDecomposition {
svd.ratio = svd.w2/svd.w1; svd.ratio = svd.w2/svd.w1;
return svd; return svd;
} }
public static double [] getMinMaxEigenValues(double [][] A) {
double a00=A[0][0],a01=A[0][1],a10=A[1][0],a11=A[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;
return (w1 < w2) ? (new double[] {w1,w2}) : (new double[] {w2,w1});
}
/** /**
* Use singular value decomposition and then split scaling {{w1,0},{0,w1}} * Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
* into overall scaling caused by zoom != 1.0 because of altitude error * into overall scaling caused by zoom != 1.0 because of altitude error
...@@ -153,7 +168,7 @@ public class SingularValueDecomposition { ...@@ -153,7 +168,7 @@ public class SingularValueDecomposition {
while (beta >= Math.PI/2) { while (beta >= Math.PI/2) {
beta -= Math.PI; beta -= Math.PI;
} }
while (beta < Math.PI/2) { while (beta < -Math.PI/2) {
beta += Math.PI; beta += 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