Skip to content

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

new quatToAffine() with any direction

parent ace24186
Expand all Show whitespace changes
Inline Side-by-side
Showing with 359 additions and 68 deletions
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java
View file @ d9f79276
......@@ -36,6 +36,18 @@ public final class QuatUtils { // static class
* t3 = (r0s3 - r1s2 + r2s1 + r3s0)
*/
public static String toString(
double [] quat_i,
boolean degrees) {
double [] quat=quat_i.clone();
double scale =normalizeInPlace(quat);
String fmt_rad = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.9f, dir=%12.9f, scale = %12.10f";
String fmt_deg = "[%12.9f, %12.9f,%12.9f, %12.9f], tilt=%12.7f\u00B0, dir=%12.7f\u00B0, scale=%12.10f";
double s = degrees ? (180/Math.PI):1;
String fmt=degrees ? fmt_deg : fmt_rad;
return String.format(fmt, quat[0], quat[1],quat[2],quat[3], s*2*Math.acos(quat[0]), s*Math.atan2(quat[2],quat[1]), scale);
}
/**
* Multiply to quaternions
* @param r first quaternion
......@@ -44,8 +56,7 @@ public final class QuatUtils { // static class
*/
public static double [] multiply(
double [] r,
double [] s
) {
double [] s ) {
double [] t = {
r[0]*s[0] - r[1]*s[1] - r[2]*s[2] - r[3]*s[3],
r[0]*s[1] + r[1]*s[0] - r[2]*s[3] + r[3]*s[2],
......@@ -54,6 +65,59 @@ public final class QuatUtils { // static class
return t;
}
public static double [] multiplyScaled(
double [] r,
double [] s ) {
return scale(multiply(normalize(r),normalize(s)),norm(r)*norm(s));
}
public static double [] invertScaled(double [] 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
double [] r,
double [] s ) {
return multiply(r, invert(s));
}
public static double [] divideScaled(
double [] r,
double [] s ) {
return multiplyScaled(r, invertScaled(s));
}
public static double [] divideScaled1(
double [] r,
double [] s ) {
return multiply(r, invertScaled(s));
}
public static double norm(
double [] quat) {
return Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
......@@ -199,6 +263,86 @@ public final class QuatUtils { // static class
return affine;
}
public static double [][] quatToAffine(
double [] quat,
boolean invert,
boolean y_down_ccw){
/*
The product of two quaternions:
t = rs
(t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3)
t0 = (r0s0 - r1s1 - r2s2 - r3s3)
t1 = (r0s1 + r1s0 - r2s3 + r3s2)
t2 = (r0s2 + r1s3 + r2s0 - r3s1)
t3 = (r0s3 - r1s2 + r2s1 + r3s0)
p = (0, x, y, z)
p'= inv(q) * p *q for active rotation (we'll need an inverse to get source pixel coordinates x,y
point (z==0), starting with map coordinates
p = (0, x, y, 0)
pq0 = ( - r1s1 - r2s2 ) = (-x * q[1] - y * q[2])
pq1 = ( + r1s0 - r2s3 ) = ( x * q[0] - y * q[3])
pq2 = ( + r1s3 + r2s0 ) = ( x * q[3] + y * q[0])
pq3 = ( - r1s2 + r2s1 ) = (-x * q[2] + y * q[1])
~q=[q0,-q1,-q2,-q3]
~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + -q3pq3) = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 + -q1pq0 - -q2pq3 + -q3pq2) = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 + -q1pq3 + -q2pq0 - -q3pq1) = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 - -q1pq2 + -q2pq1 + -q3pq0) = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
~q*p*q0 = (q0pq0 + q1pq1 + q2pq2 + q3pq3)
~q*p*q1 = (q0pq1 - q1pq0 + q2pq3 - q3pq2)
~q*p*q2 = (q0pq2 - q1pq3 - q2pq0 + q3pq1)
~q*p*q3 = (q0pq3 + q1pq2 - q2pq1 - q3pq0)
~q*p*q0 = q[0]*(-x * q[1] - y * q[2]) + q[1]*( x * q[0] - y * q[3]) + q[2]*( x * q[3] + y * q[0]) + q[3]*(-x * q[2] + y * q[1])
~q*p*q1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
~q*p*q2 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
~q*p*q3 = q[0]*(-x * q[2] + y * q[1]) + q[1]*( x * q[3] + y * q[0]) - q[2]*( x * q[0] - y * q[3]) - q[3]*(-x * q[1] - y * q[2])
x1 = q[0]*( x * q[0] - y * q[3]) - q[1]*(-x * q[1] - y * q[2]) + q[2]*(-x * q[2] + y * q[1]) - q[3]*( x * q[3] + y * q[0])
y1 = q[0]*( x * q[3] + y * q[0]) - q[1]*(-x * q[2] + y * q[1]) - q[2]*(-x * q[1] - y * q[2]) + q[3]*( x * q[0] - y * q[3])
x1 = x* (q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) + y* (-q[0]*q[3] + q[1]*q[2] + q[2]*q[1] - q[3]*q[0])
y1 = x* (q[0]*q[3] + q[1]*q[2] + q[2]*q[1] + q[3]*q[0]) + y* ( q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3])
x1 = x* (q00 + q11 - q22 - q33) + y* (-q03 + q12 + q12 - q03)
y1 = x* (q03 + q12 + q12 + q03) + y* ( q00 - q11 + q22 - q33)
x1 = x* (q00 + q11 - q22 - q33) + y*2*(q12 - q03)
y1 = x*2*(q03 + q12) + y* ( q00 - q11 + q22 - q33)
where:
q00 = q[0]*q[0]
q11 = q[1]*q[1]
q22 = q[1]*q[1]
q33 = q[1]*q[1]
q03 = q[0]*q[3]
q12 = q[1]*q[2]
*/
double scale = normQuat(quat);
double scale_y = scale * (y_down_ccw?-1:1); // invert y from quaternions to affines
double q00 = quat[0]*quat[0];
double q11 = quat[1]*quat[1];
double q22 = quat[1]*quat[1];
double q33 = quat[1]*quat[1];
double q03 = quat[0]*quat[3];
double q12 = quat[1]*quat[2];
double [][] affine = {
{scale*(q00 + q11 - q22 - q33), scale*2*(q12 - q03)},
{scale_y*2*(q03 + q12), scale_y*(q00 - q11 + q22 - q33)}};
if (invert) {
affine = matInverse2x2(affine);
}
return affine;
}
/**
* Restore quaternion (rotation+scale) from affine traqnsform (only [2][2] is used, ok to have [2][3] input
* As there are 2 solutions, both are provided in the output. As the
......@@ -231,7 +375,8 @@ public final class QuatUtils { // static class
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(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;
}
......@@ -253,7 +398,7 @@ public final class QuatUtils { // static class
return m;
}
public static double [] matMul (
public static double [] matMult (
double [][] m1,
double [] v){
if (m1[0].length != v.length) {
......@@ -268,7 +413,14 @@ public final class QuatUtils { // static class
}
return v2;
}
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]}};
}
......
src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java
View file @ d9f79276
......@@ -43,6 +43,15 @@ public class SingularValueDecomposition {
public double getMinScale() {return Math.min(w1,w2);}
public double getMaxScale() {return Math.max(w1,w2);}
public String toString(boolean degrees) {
//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 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";
double s = degrees ? (180/Math.PI):1;
String fmt=degrees ? fmt_deg : fmt_rad;
return String.format(fmt, scale, s*getTiltAngle(), s*gamma, s*beta, s*rot, w1, w2, ratio);
}
public double [][] getW(){
return new double [][] {{w1,0},{0,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