Commit d9f79276 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

new quatToAffine() with any direction

parent ace24186

src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java

+193 −63

File changed.

Preview size limit exceeded, changes collapsed.

src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java

+157 −5
Original line number Diff line number Diff line
@@ -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

+9 −0
Original line number Diff line number Diff line
@@ -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}};