working on quaterions <-> affines

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

+/**
+ ** OrthoMap - Dealing with orthographic maps 
+ **
+ ** Copyright (C) 2024 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **
+ **  OrthoMap.java is free software: you can redistribute it and/or modify
+ **  it under the terms of the GNU General Public License as published by
+ **  the Free Software Foundation, either version 3 of the License, or
+ **  (at your option) any later version.
+ **
+ **  This program is distributed in the hope that it will be useful,
+ **  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ **  GNU General Public License for more details.
+ **
+ **  You should have received a copy of the GNU General Public License
+ **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ ** -----------------------------------------------------------------------------**
+ **
+ */
+
 package com.elphel.imagej.orthomosaic;
 
 import java.awt.Color;
@@ -9,23 +32,15 @@ import java.io.IOException;
 import java.io.ObjectInputStream;
 import java.io.ObjectOutputStream;
 import java.io.Serializable;
-import java.nio.charset.StandardCharsets;
-import java.nio.file.Files;
-import java.nio.file.Path;
-import java.nio.file.Paths;
-import java.text.SimpleDateFormat;
 import java.time.LocalDateTime;
 import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.Calendar;
 import java.util.Collections;
 import java.util.Comparator;
 import java.util.HashMap;
-import java.util.List;
 import java.util.Properties;
 import java.util.concurrent.atomic.AtomicInteger;
 
-import com.elphel.imagej.cameras.CLTParameters;
 import com.elphel.imagej.common.DoubleFHT;
 import com.elphel.imagej.common.DoubleGaussianBlur;
 import com.elphel.imagej.common.GenericJTabbedDialog;
@@ -98,9 +113,9 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	public LocalDateTime                        dt;
 	// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
 	// coordinates relative to vert_meters
-	public double [][]                          affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[]
+	public double [][]                          affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[], positive Y is down (as in images)
 	public double                               orig_pix_meters;
-	public double []                            vert_meters;   // offset of the image vertical in meters (scale-invariant)
+	public double []                            vert_meters;   // offset of the image vertical in meters (scale-invariant),  right (X) and down (Y)
 	public int                                  orig_width;
 	public int                                  orig_height;
 	public transient FloatImageData             orig_image;
@@ -704,7 +719,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	}
 	
 	/**
-	 * Get vertical point offset from the top-left corner of the original orthoimage in meters
+	 * Get vertical point offset from the top-left corner of the original orthoimage in meters (right and down)
 	 * @return vertical point X,Y offset in meters from the top-left image corner
 	 */
 	public double [] getVertMeters() {
@@ -3700,10 +3715,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		ImageDtt.startAndJoin(threads);
 		return weights;
 	}
-	
-	
-	
-	
+
 	
 	/**
 	 * Fit a plane to the input data, relative to the specified point in Rectangle wnd 
@@ -5508,7 +5520,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	 * R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} ,
 	 * non-uniform scale transform W={{w1,0}{0,w2}}, and rotation
 	 * R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}}
-	 * A=R1*W*R2 using singular value decomposition
+	 * A=R1*W*R2 using singular value decomposition -- tested
 	 * https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation   
 	 * @param A - input 2x2 matrix
 	 * @return {s,beta,w,gamma,beta+gamma}
@@ -5546,12 +5558,57 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 	 * @param A - linear transformation matrix from rectified ground coordinates
 	 *        to source image coordinates. OK to use 2x3 affine matrix,extra
 	 *        components will be ignored. 
-	 * @return {beta, s, t, beta+gamma}, beta+gamma - total rotation
+	 * @param y_down_ccw - positive Y is down, positive angles are CCW
+	 * @return {gamma, s, t, beta+gamma}, beta+gamma - total rotation
 	 */
 	public static double [] singularValueDecomposeScaleTilt(
-			double [][] A) {
+			double [][] A,
+			boolean y_down_ccw) {
+		double [] svd=singularValueDecompose(A);
+		double w1 = svd[1], w2 = svd[2], g_p_b=svd[4]; // pure rotation // , beta=svd[0]
+		double gamma = svd[3];
+		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
+		// Affines are Y-up?
+		
+		// considering tilt in y direction (unless long_axis), it should have higher scale
+		// (and source image coordinates), while X should correspond to
+		// the axis of rotation, and scale is just scale caused by error in
+		// altitude.
+		double s = Math.min(w1, w2);
+		double t = w1/w2; // <=1.0, ==cos(tilt)
+		if (w1 > w2) { // rotate tilt by PI/2
+			t = w2/w1;
+			gamma += Math.PI/2; // start with rotation (last in matrices)
+			if (gamma > Math.PI) {
+				gamma -= 2* Math.PI;
+			}
+		}
+		
+		if (gamma > Math.PI/2) {
+			gamma -= Math.PI;
+		} else if (gamma < -Math.PI) {
+			gamma += Math.PI;
+		}
+		
+		if (y_down_ccw) {
+			gamma =-gamma; 
+			g_p_b = -g_p_b; // pure rotation
+		}
+		
+		return new double [] {gamma, s,t, g_p_b};
+	}
+	
+	public static double [] singularValueDecomposeScaleTiltOld(
+			double [][] A,
+			boolean y_down_ccw) {
 		double [] svd=singularValueDecompose(A);
 		double w1 = svd[1], w2 = svd[2], beta=svd[0], g_p_b=svd[4];
+		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
+		// Affines are Y-up?
+		if (y_down_ccw) {
+			beta =-beta; // find where is the error that beta is negated, check rotation
+			g_p_b = -g_p_b;
+		}
 		
 		// considering tilt in y direction, it should have higher scale
 		// (and source image coordinates), while X should correspond to
@@ -5566,6 +5623,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 				beta -= 2* Math.PI;
 			}
 		}
+		// beta should be in the range of +/-pi/2
+		if (beta > Math.PI/2) {
+			beta -= Math.PI;
+		} else if (beta < -Math.PI) {
+			beta += Math.PI;
+		}
 		return new double [] {beta, s,t, g_p_b};
 	}
 	
@@ -5648,7 +5711,36 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		}		      
 		ImageDtt.startAndJoin(threads);
 		return data_out;
+	}
 
+ 	public static double [] extractWoi(
+			final double [] data0,
+			final int       width,
+			Rectangle woi_in) {
+		final int height = data0.length/width;
+		if (woi_in == null) {
+			woi_in =new Rectangle(0,0,width,height);
+		}
+		final Rectangle woi = new Rectangle(woi_in);
+		final int woi_len = woi.width*woi.height;
+		final double [] data_out =new double [woi_len];
+		final Thread[] threads = ImageDtt.newThreadArray();
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int iPix = ai.getAndIncrement(); iPix < woi_len; iPix = ai.getAndIncrement()) {
+						int x = woi.x + (iPix % woi.width);
+						int y = woi.y + (iPix / woi.width);
+						int nPix = y * width + x;
+						data_out[iPix] = data0[nPix];
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return data_out;
 	}
-	
+ 	
+ 	
 }

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

@@ -1040,7 +1040,7 @@ public class OrthoMapsCollection  implements Serializable{
 							int opX = nPix % ownd_width + obounds[0][0]; // absolute output pX, pY
 							int opY = nPix / ownd_width + obounds[1][0];
 							double dX = (opX - scaled_out_center[0]) /scale; // in meters
-							double dY = (opY - scaled_out_center[1]) /scale;
+							double dY = (opY - scaled_out_center[1]) /scale; // in meters, Y-down
 							double [] xy_src = { // pixels of the source image
 									src_scale * (affine[0][0]*dX + affine[0][1]*dY + affine[0][2] + src_center[0]),
 									src_scale * (affine[1][0]*dX + affine[1][1]*dY + affine[1][2] + src_center[1])};
@@ -7192,6 +7192,7 @@ public class OrthoMapsCollection  implements Serializable{
 				indices[i] = i;
 			}
 		}
+		boolean y_down_ccw = true;
 		String [] stats = new String[2]; // header, body
 		StringBuffer sb = new StringBuffer();
 //		String head_fmt = "%3s\t%17s\t%26s\t%13s\t%13s\t%7s\t%6s\t%7s\t%6s\t%6s\t%6s\t%6s\n";
@@ -7224,7 +7225,13 @@ public class OrthoMapsCollection  implements Serializable{
 			double sfm_gain = map.getSfmGain();
 			int scenes = map.getNumberScenes();
 			double [][] affine = map.getAffine();
-			double [] svd_st = OrthoMap.singularValueDecomposeScaleTilt(affine);
+			SingularValueDecomposition svd_st =SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(
+					affine,
+					y_down_ccw); // boolean y_down_ccw)
+//			double [] svd_st = OrthoMap.singularValueDecomposeScaleTilt(
+//					affine,
+//					y_down_ccw); // boolean y_down_ccw)
+			// svd_st[0] - now gamma - short axis relative to map 
 			sb.append(String.format(
 					"%3d\t%17s\t%26s\t%11.6f\t%11.6f\t%7.2f\t"+
 							"%6.2f\t%7.4f\t%6d\t%6d\t%6d\t%6d\t"+
@@ -7234,7 +7241,7 @@ public class OrthoMapsCollection  implements Serializable{
 							indx, name, sdt, lla[0], lla[1], lla[2] - agl,//  ground level
 							agl, pix_size_cm, width, height, vert_pix[0], vert_pix[1],
 							scenes, sfm_gain,
-							svd_st[1],svd_st[2],svd_st[0],svd_st[3], affine[0][2], affine[1][2],
+							svd_st.scale,svd_st.ratio,svd_st.beta,svd_st.rot, affine[0][2], affine[1][2],
 							affine[0][0], affine[0][1], affine[1][0], affine[1][1]));
 		}
 		stats[1] = sb.toString();

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

+/**
+ ** OrthoMap - Dealing with orthographic maps 
+ **
+ ** Copyright (C) 2024 Elphel, Inc.
+ **
+ ** -----------------------------------------------------------------------------**
+ **
+ **  OrthoMap.java is free software: you can redistribute it and/or modify
+ **  it under the terms of the GNU General Public License as published by
+ **  the Free Software Foundation, either version 3 of the License, or
+ **  (at your option) any later version.
+ **
+ **  This program is distributed in the hope that it will be useful,
+ **  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ **  GNU General Public License for more details.
+ **
+ **  You should have received a copy of the GNU General Public License
+ **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ ** -----------------------------------------------------------------------------**
+ **
+ */
+
+package com.elphel.imagej.orthomosaic;
+
+public final class QuatUtils { // static class
+	public static final double []   UNIT_QUAT =   {1,0,0,0};
+	public static final double [][] UNIT_AFFINE = {{1,0},{0,1}};
+	private QuatUtils() {}
+	/*
+	 * t= r*s
+	 * (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)
+	 */
+
+	/**
+	 * Multiply to quaternions
+	 * @param r first quaternion
+	 * @param s second quaternion
+	 * @return product of two quaternions: r*s
+	 */
+	public static double [] multiply(
+			double [] r,
+			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],
+			     r[0]*s[2] + r[1]*s[3] + r[2]*s[0] - r[3]*s[1],
+			     r[0]*s[3] - r[1]*s[2] + r[2]*s[1] + r[3]*s[0]};
+		return t;
+	}
+	
+	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]);
+	}
+	public static double [] invert(
+			double [] quat) {
+		return new double [] {quat[0], -quat[1], -quat[2], -quat[3]};
+	}
+
+	public static double [] normalize(
+			double [] quat) {
+//		double k = 1/norm(quat);
+//		return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
+		return scale (quat, 1/norm(quat));
+	}
+	public static double normalizeInPlace(
+			double [] quat) {
+		double scale = norm(quat);
+		for (int i = 0; i < quat.length; i++) {
+			quat[i] /= scale;
+		}
+		return scale;
+	}
+	
+	public static double [] scale(
+			double [] quat,
+			double k) {
+		return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
+	}	
+	/**
+	 * Convert two tilts {tx,ty} Z=x*tx+y*ty to quaternion. Source y is down, quaternion - x - same, y - opposite (positive - up), z - up from the XY plane. 
+	 * @param txy {tiltX, tiltY,...} - may contain other values like offset and center
+	 * @param y_down_ccw Invert Y for tilts
+	 * @return plane rotation quaternion rotation righth-hand thread
+	 */
+	public static double [] tiltToQuaternion(
+			double [] txy,
+			boolean	y_down_ccw) { // boolean y_down_ccw)
+		double tx = txy[0], ty = y_down_ccw?-txy[1]:txy[1]; // invert y
+		double t2 = tx*tx + ty*ty;
+		double t = Math.sqrt(t2);
+		if (t == 0) {
+			return UNIT_QUAT;
+		}
+		double [] axis = {ty/t,-tx/t,0}; // pi/2 CW
+		
+		/* sin (A/2) = +-sqrt((1 - cos(A))/2)
+		 * cos (A/2) = +-sqrt((1 + cos(A))/2)
+		 * cos(A) = sqrt(t*t+1)
+		 */
+		double cos_theta = 1/Math.sqrt(t2+1);
+		double cos_theta2 = Math.sqrt((1 + cos_theta)/2);
+		double sin_theta2 = Math.sqrt((1 - cos_theta)/2);
+		double [] quat = new double[] {cos_theta2, sin_theta2*axis[0], sin_theta2*axis[1], 0};
+		return quat;
+	}
+	
+	/**
+	 * Remove rotation around Z-axis, keep only tilt around axis in XY plane
+	 * @param quat_in input rotation that may include rotation around Z (vertical) axis
+	 * @return nearest rotation around axis in XY plane
+	 */
+	public static double [] pureTilt(
+			double [] quat_in) {
+		double r0 = quat_in[0], r1 = quat_in[1], r2 = quat_in[2], r3 = quat_in[3];
+		if (r0 == 0) {
+			return UNIT_QUAT;
+		}
+		double r3_r0 = r3/r0;
+		// find {s0,0,0,s3} - rotation around vertical axis so r*s will not have Z component
+		double s0 = Math.sqrt(1/(1 + r3_r0*r3_r0));
+		double s3 = -r3_r0*s0;
+		
+		double [] quat = {
+				r0 * s0 - r3 * s3,
+				r1 * s0 - r2 * s3,
+				r1 * s3 + r2 * s0,
+				r0 * s3 + r3 * s0
+		};
+		return quat;
+	}
+
+	/**
+	 * Normalize quaterion (in-place) and return its original length to be used as scale
+	 * @param quat quaternion with encoded scale, will be normalized in place
+	 * @return length of the input quaternion before normalization;
+	 */
+	public static double normQuat(double [] quat) {
+		double s2 = 0;
+		for (double q:quat) {
+			s2+= q*q;
+		}
+		double l = Math.sqrt(s2);
+		for (int i = 0; i < quat.length;i++) {
+			quat[i] /= l;
+		}
+		return l;
+	}
+	
+	/**
+	 * Convert rotation around some axis in XY plane to affine transformation
+	 * @param quat rotation, quat[3]~=0 (y-up)
+	 * @param stretch project to tilted plane (stretch perpendicular to rotation axis)
+	 * @param make__pure_tilt rotate around vertical axis (Z) to make quat[3]=0
+	 * @return [2][2] affine transform ( y-down)
+	 */
+	public static double [][] quatToAffine(
+			double [] quat,
+			boolean   stretch,
+			boolean   make__pure_tilt,
+			boolean   y_down_ccw){
+		// TODO: use scale
+		double scale = normQuat(quat);
+		double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
+		if (make__pure_tilt) {
+			quat = pureTilt(quat);
+		}
+		double ax = quat[1], ay = quat[2] * y_sign;
+		double l = Math.sqrt(ax*ax+ay*ay);
+		if (l == 0) {
+			return UNIT_AFFINE;
+		}
+		ax /= l;
+		ay /= l;
+		double cos_theta = 2 * quat[0]*quat[0] - 1; // cos(2 * A) = 2 * cos(A)^2 -1
+		double k = stretch ? (1/cos_theta) : cos_theta;
+		double s = k - 1;
+		double ax2 = ax*ax, ay2=ay*ay, axy = ax*ay;
+		double [][] affine = {
+//				{1 + s * ax * ax,       s * ax * ay}, 
+//				{  - s * ax * ay ,  1 + s * ay * ay}};
+///		{k*ax2 + ay2,-s*axy}, 
+///		{-s*axy ,      ax2 + k*ay2}};
+				{1 + s*ay2,     -s*axy}, 
+				{   -s*axy  ,1 + s*ax2}};
+//		{1 + s*ay2,     s*axy}, 
+//		{   s*axy  ,1 + s*ax2}};
+		for (int i = 0; i < 2; i++) {
+			for (int j=0; j < 2; j++) {
+				affine[i][j] *= scale;
+			}
+		}
+		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
+	 * input linear transformation matrix converts ground coordinates to source
+	 * image coordinates, the scale in the tilt direction is > than scale in the
+	 * perpendicular direction (tilt axis).
+	 * @param affine affine transform, only [2][2] top-left subarray is used
+	 * @param y_down_ccw if true, affine corresponds to to y-down (image) coordinate system
+	 * @return a pair of quaternions including scale (sqrt(q0^2+q1^2+q2^2+q3^2)
+	 */
+	public static double [][] affineToQuatScaled (
+			double [][] affine,
+			boolean     y_down_ccw) {
+		SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
+				affine,
+				y_down_ccw);
+		double scale = svd.getMinScale();
+		double rot = svd.getRotAngle(); // TODO: check sign !
+		// Now beta,rot correspond to Y - up
+		double chalfrot = Math.cos(rot/2); 
+		double shalfrot = Math.sin(rot/2); 
+		double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
+		double cbeta = Math.cos(svd.beta);
+		double sbeta = Math.sin(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*cbeta,  stilt*sbeta, 0};
+		double [] q_minus = {ctilt, -stilt*cbeta, -stilt*sbeta, 0};
+		double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
+		return quats_pm;
+	}
+	
+	public static double [][] matMult (
+			double [][] m1,
+			double [][] m2){
+		if (m1[0].length != m2.length) {
+			System.out.println("matMul(): m1[0].length != m2.length ("+m1[0].length+" !="+m2.length+")");
+			return null;
+		}
+		double [][] m = new double[m1.length][m2[0].length];
+		for (int i = 0; i < m.length; i++) {
+			for (int j = 0; j < m[i].length; j++) {
+				for (int k = 0; k < m1.length; k++) {
+					m[i][j] += m1[i][k]*m2[k][j];
+				}
+			}
+		}
+		return m;
+	}
+	
+	public static double [] matMul (
+			double [][] m1,
+			double []   v){
+		if (m1[0].length != v.length) {
+			System.out.println("matMul(): m1[0].length != v.length ("+m1[0].length+" !="+v.length+")");
+			return null;
+		}
+		double [] v2 = new double[v.length];
+		for (int i = 0; i < m1.length; i++) {
+				for (int k = 0; k < m1.length; k++) {
+					v2[i] += m1[i][k]*v[k];
+				}
+		}
+		return v2;
+	}
+
+	
+	
+	
+}

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

+package com.elphel.imagej.orthomosaic;
+
+public class SingularValueDecomposition {
+	double beta;
+	double gamma;
+	double w1;
+	double w2;
+	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()
+	boolean st= false;
+	boolean y_down_ccw;	
+	public SingularValueDecomposition(
+			double beta,
+			double gamma,
+			double w1,
+			double w2,
+			double rot) {
+		this.beta = beta;
+		this.gamma = gamma;
+		this.w1 = w1;
+		this.w2 = w2;
+		this.rot = rot;
+	}
+	public static double [][] rotMatrix(double a){
+		double c = Math.cos(a);
+		double s = Math.sin(a);
+		return new double [][] {{c,s},{-s,c}};
+		
+	}
+	// A = getR1() * getW() getR2() = getR1() * getW() * getIR1() * getRot()
+	public double [][] getR1(){	return rotMatrix(beta);}
+	public double [][] getR2(){	return rotMatrix(gamma);}
+	public double [][] getIR1(){return rotMatrix(-beta);}
+	public double [][] getRot(){return rotMatrix(rot);}
+	public double getTiltAngle() {
+		if (ratio <= 1.0) return Math.acos(ratio);
+		else return Math.acos(1/ratio);
+	}
+	public double getRotAngle() {return rot;}
+	public double getScale() {return scale;}
+	public double getAvgScale() {return Math.sqrt(w1*w2);}
+	public double getMinScale() {return Math.min(w1,w2);}
+	public double getMaxScale() {return Math.max(w1,w2);}
+	
+	
+	public double [][] getW(){
+		return new double [][] {{w1,0},{0,w1}};
+	}
+	
+	/**
+	 * Decomposing linear transform into rotation
+	 * R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} ,
+	 * non-uniform scale transform W={{w1,0}{0,w2}}, and rotation
+	 * R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}}
+	 * A=R1*W*R2 using singular value decomposition -- tested
+	 * https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation   
+	 * @param A - input 2x2 matrix
+	 * @return {s,beta,w,gamma,beta+gamma}
+	 * A = B + C
+	 * b00= b11; c00=-c01
+	 * b10=-b01; c10= c01
+	 */
+	public static SingularValueDecomposition singularValueDecompose(
+			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;
+		double g_p_b = Math.atan2(b01, b00);
+		double g_m_b = Math.atan2(c01, c00);
+		double gamma = (g_p_b + g_m_b)/2; 
+		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;
+		return svd;
+	}
+	/**
+	 * Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
+	 * into overall scaling caused by zoom != 1.0 because of altitude error
+	 * and unidirectional scaling caused by tilted projection plane. As the
+	 * input linear transformation matrix converts ground coordinates to source
+	 * image coordinates, the scale in the tilt direction is > than scale in the
+	 * perpendicular direction (tilt axis).
+	 * Matrix R1 is additionally rotated by PI/2 if needed so W={{w1,0},{0,w2}}
+	 * has w2>=w1 and W={{s,0},{0,s/t}}, where t <= 1.0 and equals to cos(tilt) 
+	 * 
+	 * @param A - linear transformation matrix from rectified ground coordinates
+	 *        to source image coordinates. OK to use 2x3 affine matrix,extra
+	 *        components will be ignored. 
+	 * @param y_down_ccw - positive Y is down, positive angles are CCW
+	 * @return SingularValueDecomposition instance with scale and ratio (<=1) defined, gamma and beta updated
+	 */
+
+	public static SingularValueDecomposition singularValueDecomposeScaleTiltGamma(
+			double [][] A,
+			boolean y_down_ccw) {
+		SingularValueDecomposition svd = singularValueDecompose(A);
+		svd.setScaleTiltGamma(y_down_ccw);
+		return svd;
+	}
+
+	public static SingularValueDecomposition singularValueDecomposeScaleTiltBeta(
+			double [][] A,
+			boolean y_down_ccw) {
+		SingularValueDecomposition svd = singularValueDecompose(A);
+		svd.setScaleTiltBeta(y_down_ccw);
+		return svd;
+	}
+	
+	
+	public void setScaleTiltGamma(
+			boolean y_down_ccw) {
+		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
+		// Affines are Y-up?
+		
+		// considering tilt in y direction (unless long_axis), it should have higher scale
+		// (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);
+		ratio = w1/w2; // <=1.0, ==cos(tilt)
+		if (w1 > w2) { // rotate tilt by PI/2
+			ratio = w2/w1;
+			gamma += Math.PI/2; // start with rotation (last in matrices)
+			if (gamma > Math.PI) {
+				gamma -= 2* Math.PI;
+			}
+		}
+		
+		if (gamma > Math.PI/2) {
+			gamma -= Math.PI;
+		} else if (gamma < -Math.PI) {
+			gamma += Math.PI;
+		}
+		
+		beta = rot-gamma;
+		while (beta >= Math.PI/2) {
+			beta -= Math.PI;
+		}
+		while (beta < Math.PI/2) {
+			beta += Math.PI;
+		}
+		
+		if (y_down_ccw) {
+			gamma *= -1; 
+			beta *= -1; 
+			rot *= -1; // pure rotation
+		}
+		this.y_down_ccw = y_down_ccw;
+		st = true;
+	}
+	
+	public void setScaleTiltBeta(
+			boolean y_down_ccw) {
+		// For Y-down, angles are CW positive, for Y-up angles are CCW positive
+		// Affines are Y-up?
+		
+		// considering tilt in y direction (unless long_axis), it should have higher scale
+		// (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);
+		ratio = w1/w2; // <=1.0, ==cos(tilt)
+		if (w1 > w2) { // rotate tilt by PI/2
+			ratio = w2/w1;
+			beta += Math.PI/2; // start with rotation (last in matrices)
+			if (beta > Math.PI) {
+				beta -= 2* Math.PI;
+			}
+		}
+		
+		if (beta >= Math.PI/2) {
+			beta -= Math.PI;
+		} else if (beta < -Math.PI) {
+			beta += Math.PI;
+		}
+		
+		gamma = rot-beta;
+		while (gamma >= Math.PI/2) {
+			gamma -= Math.PI;
+		}
+		while (gamma < Math.PI/2) {
+			gamma += Math.PI;
+		}
+		
+		if (y_down_ccw) {
+			beta *= -1; 
+			gamma *= -1; 
+			rot *= -1; // pure rotation
+		}
+		this.y_down_ccw = y_down_ccw;
+		st = true;
+	}
+	
+	
+}