Skip to content

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

working on quaterions <-> affines

parent 7ad0875e
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 808 additions and 50 deletions
src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java
View file @ 6a6d0c40
/**
** 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
View file @ 6a6d0c40
......@@ -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 0 → 100644
View file @ 6a6d0c40
/**
** 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 0 → 100644
View file @ 6a6d0c40
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;
}
}
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