diff --git a/doc/main.tex b/doc/main.tex index 7edb464..dbb6826 100644 --- a/doc/main.tex +++ b/doc/main.tex @@ -10,8 +10,8 @@ \newcommand{\matr}[1]{\mathbf{#1}} \title{On the Chaining of Helmert transformations} -\author{Kristian Evers} -\date{2022-04-22} +\author{Kristian Evers \& Lars Stenseng} +\date{2022-08-10} \begin{document} \maketitle @@ -132,9 +132,13 @@ \section{Chaing transformations} \ref{eq:t3}, \ref{eq:c3} and \ref{eq:r3} and then using them in eq. \ref{eq:simplehelmert}. -Shown here is only the 7-parameter version of the Helmert transformation -but of course this obviously also extends to the 14-parameter version and -combinations of the two. +\section{Including velocity} +Following the notation above we introduce the input, $V_A$ and output $V_B$ velocity vectors and the translation rate vector $\dot{T}$, rotation rate matrix $\dot{\matr{R}}$, and the scale rate factor $\dot{c}$. + + +\begin{equation} + V_B = V_A + \dot{T} + \dot{c} \dot{\matr{R}} P_A +\end{equation} \end{document} diff --git a/helmert.py b/helmert.py index 6c7b914..8854295 100644 --- a/helmert.py +++ b/helmert.py @@ -1,17 +1,232 @@ """ -Fun with Helmert transformations. +Helmert transformations primarily for geodetic applications. + +This small collection provides a set of tool to chain multiple 7 or 14 parameter +Helmert transformations together to one single Helmert transformation and apply that +transformation on a cartesian ECEF (Earth Centered Earth Fixed) station coordinate with +optional station velocity. """ + from enum import Enum +from numpy import array, rad2deg, deg2rad +from math import isclose + + +class Observation: + """ + Station observation class. + + Station observation class for cartesian ECEF coordinates and optional station + velocity. + """ + + def __init__( + self, + x: float = 0.0, + y: float = 0.0, + z: float = 0.0, + epoch: float = 0.0, + vx: float = 0.0, + vy: float = 0.0, + vz: float = 0.0, + stn_vel: bool = False, + ) -> None: + self.x = x + self.y = y + self.z = z + self.epoch = epoch + self.vx = vx + self.vy = vy + self.vz = vz + self.stn_vel = stn_vel + + def __repr__(self) -> str: + """ + Readable representation of the Observation object. + + Returns + ------- + str + Detailed info on the Observation object. + """ + return ( + f"Observation(x={self.x}, y={self.y}, z={self.z}, " + f"vx=(self.vx), vy=(self.vy), vz=(self.vz), " + f"epoch={self.epoch}, stn_vel={self.stn_vel})" + ) + + def __str__(self) -> str: + """ + Pretty print of the Observation object. + + Returns + ------- + str + Human readable representation of the Observation object. Elements are + formated and units included. + """ + string = f"X={self.x:13.5f} m Y={self.y:13.5f} m Z={self.z:13.5f} m " + string += f"Epoch: {self.epoch:8.3f} yr " + if self.stn_vel: + string += f"\nVX={self.vx:12.5f} m/yr VY={self.vy:12.5f} m/yr " + string += f"VZ={self.vz:12.5f} m/yr " + return string + + def __add__(self: "Observation", obs2: "Observation") -> "Observation": + """ + Add two Observation objects. + + obs = obs1 + obs2 + + Parameters + ---------- + obs2 : "Observation" + Observation object to be added to obs1. + + Returns + ------- + "Observation" + Observation object with the sum of each element. stn_vel is True if obs1 + and/or obs2 stn_vel is true. + + """ + obs1 = self + x = obs1.x + obs2.x + y = obs1.y + obs2.y + z = obs1.z + obs2.z + epoch = obs1.epoch + obs2.epoch + vx = obs1.vx + obs2.vx + vy = obs1.vy + obs2.vy + vz = obs1.vz + obs2.vz + stn_vel = obs1.stn_vel or obs2.stn_vel + return Observation(x, y, z, epoch, vx, vy, vz, stn_vel) + + def __sub__(self: "Observation", obs2: "Observation") -> "Observation": + """ + Subtract one Observation object from another. + + obs = obs1 - obs2 + + Parameters + ---------- + obs2 : "Observation" + Observation object to be added to obs1. + + Returns + ------- + "Observation" + Observation object with the difference of each element in obs1 and obs2. + stn_vel is True if obs1 and/or obs2 stn_vel is true. + + """ + obs1 = self + x = obs1.x - obs2.x + y = obs1.y - obs2.y + z = obs1.z - obs2.z + epoch = obs1.epoch - obs2.epoch + vx = obs1.vx - obs2.vx + vy = obs1.vy - obs2.vy + vz = obs1.vz - obs2.vz + stn_vel = obs1.stn_vel or obs2.stn_vel + return Observation(x, y, z, epoch, vx, vy, vz, stn_vel) + + def position(self, pos: array = None) -> array: + """ + Set and return position of the station. + + Updates the X, Y, Z coordinates and returns the new station coordinates. + If no coordinates is given the existing station coordinate is returned. + + Parameters + ---------- + pos : array, optional + 3 element array with the new cartesian X, Y, Z coordinate. + The default is None. + + Returns + ------- + array + 3 element array with the current cartesian X, Y, Z coordinate. + + """ + if pos is None: + return array([self.x, self.y, self.z]) + else: + self.x = pos[0] + self.y = pos[1] + self.z = pos[2] + return array([self.x, self.y, self.z]) + + def velocity(self, vel: array = None) -> array: + """ + Set and return velocity of the station. + + Updates the X, Y, Z velocity and returns the new station velocity. + If no velocity is given the existing station velocity is returned. -import numpy as np + Parameters + ---------- + pos : array, optional + 3 element array with the new cartesian X, Y, Z velocity. + The default is None. + + Returns + ------- + array + 3 element array with the current cartesian X, Y, Z velocity. + """ + if vel is None: + return array([self.vx, self.vy, self.vz]) + else: + self.vx = vel[0] + self.vy = vel[1] + self.vz = vel[2] + self.stn_vel = True + return array([self.vx, self.vy, self.vz]) + + def set_target_epoch(self, target_epoch: float) -> "Observation": + """ + Express the position at a new epoch using the velocity. + + Use the observation velocity to calculate the position difference between the + current observation epoch and the target epoch and add this difference to the + current position. + + Parameters + ---------- + target_epoch : float + Epoch to express the position at. + + Returns + ------- + "Observation" + Observation object at the new epoch. + + """ + dt = target_epoch - self.epoch + self.epoch = target_epoch + if self.stn_vel: + self.x += self.vx * dt + self.y += self.vy * dt + self.z += self.vz * dt + return self class Convention(Enum): + """Conventions to choose between representation of the Helmert transform.""" + POSITION_VECTOR = 1 COORDINATE_FRAME = 2 class Helmert: + """14 parameter Helmert transformation class. + + Class to express 14 parameter Helmert transformations and chain them together to + concatenate multiple Helmert transformations into a single 14 parameter Helmert + transformation. + """ + def __init__( self, x: float = 0.0, @@ -21,8 +236,66 @@ def __init__( rx: float = 0.0, ry: float = 0.0, rz: float = 0.0, + dx: float = 0.0, + dy: float = 0.0, + dz: float = 0.0, + ds: float = 0.0, + drx: float = 0.0, + dry: float = 0.0, + drz: float = 0.0, + ref_epoch: float = 0.0, + vel_model: bool = False, convention: Convention = Convention.POSITION_VECTOR, ) -> None: + """ + Create an instance of the Helmert class. + + The 14 parameter Helmert object provides the information nedded to transform an + observation from reference frame A to reference frame B. + + Parameters + ---------- + x : float, optional + Translation in meters along the X axis. The default is 0.0. + y : float, optional + Translation in meters along the Y axis. The default is 0.0. + z : float, optional + Translation in meters along the Z axis. The default is 0.0. + s : float, optional + Scaling of the system in parts per million. The default is 0.0. + rx : float, optional + Rotation around the X axis in arcseconds. The default is 0.0. + ry : float, optional + Rotation around the Y axis in arcseconds. The default is 0.0. + rz : float, optional + Rotation around the Z axis in arcseconds. The default is 0.0. + dx : float, optional + Translationrate in meters per year along the X axis. The default is 0.0. + dy : float, optional + Translationrate in meters per year along the Y axis. The default is 0.0. + dz : float, optional + Translationrate in meters per year along the Z axis. The default is 0.0. + ds : float, optional + Scalingrate of the system in parts per millionper year. The default is 0.0. + drx : float, optional + Rotationrate around the X axis in arcseconds per year. The default is 0.0. + dry : float, optional + Rotationrate around the Y axis in arcseconds per year. The default is 0.0. + drz : float, optional + Rotationrate around the Z axis in arcseconds per year. The default is 0.0. + ref_epoch : float, optional + Reference epoch for the transformation. The default is 0.0. + vel_model : bool, optional + Indicates that the transformation is a velocity model and i.e. a + transformation in time e.g. a plate motion model. The default is false. + convention : Convention, optional + The default is Convention.POSITION_VECTOR. + + Returns + ------- + None + + """ self.x = x self.y = y self.z = z @@ -30,17 +303,114 @@ def __init__( self.rx = rx self.ry = ry self.rz = rz + self.dx = dx + self.dy = dy + self.dz = dz + self.ds = ds + self.drx = drx + self.dry = dry + self.drz = drz + self.ref_epoch = ref_epoch + self.vel_model = vel_model self.convention = convention self.c = 1 + self.s * 1e-6 + self.dc = self.ds * 1e-6 - self.T = np.array([self.x, self.y, self.z]) + self.T = array([self.x, self.y, self.z]) + self.dT = array([self.dx, self.dy, self.dz]) self.R = self._build_rot_matrix(self.rx, self.ry, self.rz) + self.dR = self._build_drot_matrix(self.drx, self.dry, self.drz) + + def __repr__(self) -> str: + """ + Representation of the Helmert object. + + Returns + ------- + str + Human readeable version of the Helmert object. + + """ + return ( + f"Helmert(x={self.x}, y={self.y}, z={self.z}, s={self.s}, " + f"dx={self.dx}, dy={self.dy}, dz={self.dz}, ds={self.ds}, " + f"rx={self.rx}, ry={self.ry}, rz={self.rz}, " + f"drx={self.drx}, dry={self.dry}, drz={self.drz}, " + f"ref_epoch={self.ref_epoch}, vel_model={self.vel_model}, " + f"convention={self.convention})" + ) + + def __str__(self) -> str: + """ + Pretty printed representation with units of the Helmert object. + + Returns + ------- + str + Print the Helmert object elemets in a nicely formated way with fairly clear + indication of elements and their units. + + """ + return ( + f" Tx: {self.x:10.7f} m " + f" Ty: {self.y:10.7f} m " + f" Tz: {self.z:10.7f} m\n" + f"dTx: {self.dx:10.7f} m/yr " + f"dTy: {self.dy:10.7f} m/yr " + f"dTz: {self.dz:10.7f} m/yr\n" + f" Rx: {self.rx:10.7f} arcsec " + f" Ry: {self.ry:10.7f} arcsec " + f" Rz: {self.rz:10.7f} arcsec\n" + f"dRx: {self.drx:10.7f} arcsec/yr " + f"dRy: {self.dry:10.7f} arcsec/yr " + f"dRz: {self.drz:10.7f} arcsec/yr\n" + f" s: {self.s:11.9f} ppm " + f" ds: {self.ds:11.9f} ppm/yr\n" + f"Ref epoch: {self.ref_epoch:.3f} yr " + f"Velocity model: {self.vel_model} " + f"Convention: {self.convention}" + ) def __add__(self: "Helmert", H2: "Helmert") -> "Helmert": - # determine scaling, translations and rotations + """ + Chain two Helmert transformations together: H3 = H1 + H2. + + Parameters + ---------- + H2 : "Helmert" + Helmert transformation to chained together with the Helmert transformation + before the +. + + Raises + ------ + UserWarning + Warning to indicate that the two Helmert transformations to be chained + together does not have the same reference epoch. Use the set_target_epoch + function to express one of the Helmert transformations at the other Helmert + transformations reference epoch. + + Returns + ------- + "Helmert" + The combined Helmert transformation. + + """ H1 = self + vel_model = False + + if not isclose(H1.ref_epoch, H2.ref_epoch, abs_tol=2e-3): + raise UserWarning( + "Helmert transformations can only be chained if they have the same " + "reference epoch (i.e. within one day). Your epochs differ by: " + f"{H1.ref_epoch:.3f} - {H2.ref_epoch:.3f} = " + f"{H1.ref_epoch - H2.ref_epoch:.3f} years or " + f"~{(H1.ref_epoch - H2.ref_epoch) * 365.0:.1f} days.\n" + "Consider using set_target_epoch to shift one of the Helmert " + "transformations to the reference epoch of the other Helmert." + ) from None + # determine scaling, translations and rotations c = H1.c * H2.c T = H2.T + H2.c * H2.R.dot(H1.T) R = H2.R @ H1.R @@ -51,22 +421,78 @@ def __add__(self: "Helmert", H2: "Helmert") -> "Helmert": y = T[1] z = T[2] - rad2arcsec: float = lambda rad: np.rad2deg(rad * 3600.0) + rad2arcsec: float = lambda rad: rad2deg(rad * 3600.0) rx = rad2arcsec(R[2][1]) ry = rad2arcsec(R[0][2]) rz = rad2arcsec(R[1][0]) - return Helmert(x, y, z, s, rx, ry, rz) + if H1.vel_model or H2.vel_model: + vel_model = True + if not H1.vel_model: + H1.dx = 0.0 + H1.dy = 0.0 + H1.dz = 0.0 + H1.drx = 0.0 + H1.dry = 0.0 + H1.drz = 0.0 + H1.dc = 0.0 + H1.dT = array([H1.dx, H1.dy, H1.dz]) + H1.dR = self._build_drot_matrix(H1.drx, H1.dry, H1.drz) + if not H2.vel_model: + H2.dx = 0.0 + H2.dy = 0.0 + H2.dz = 0.0 + H2.drx = 0.0 + H2.dry = 0.0 + H2.drz = 0.0 + H2.dc = 0.0 + H2.dT = array([H2.dx, H2.dy, H2.dz]) + H2.dR = self._build_drot_matrix(H2.drx, H2.dry, H2.drz) + + # determine scaling rate, translation rates, and rotation rates + dc = H2.dc * H1.c + H2.c * H1.dc + dT = H2.dT + (H2.dc * H2.R + H2.c * H2.dR).dot(H1.T) + (H2.c * H2.R).dot(H1.dT) + dR = H2.dR @ H1.R + H2.R @ H1.dR + + # decompose the above + ds = dc * 1e6 + dx = dT[0] + dy = dT[1] + dz = dT[2] + + drx = rad2arcsec(dR[2][1]) + dry = rad2arcsec(dR[0][2]) + drz = rad2arcsec(dR[1][0]) + + return Helmert( + x, + y, + z, + s, + rx, + ry, + rz, + dx, + dy, + dz, + ds, + drx, + dry, + drz, + H1.ref_epoch, + vel_model, + self.convention, + ) def _build_rot_matrix(self, rx: float, ry: float, rz: float): """Construct rotation matrix with correctly scaled parameters.""" - arcsec2rad: float = lambda arcsec: np.deg2rad(arcsec) / 3600.0 + arcsec2rad: float = lambda arcsec: deg2rad(arcsec) / 3600.0 rx = arcsec2rad(rx) ry = arcsec2rad(ry) rz = arcsec2rad(rz) - R = np.array( + R = array( [ [1, -rz, ry], [rz, 1, -rx], @@ -81,11 +507,152 @@ def _build_rot_matrix(self, rx: float, ry: float, rz: float): # which we get by transposing the rotation matrix return R.T - def transform(self, P: np.array, inverse: bool = False) -> np.array: + def _build_drot_matrix(self, drx: float, dry: float, drz: float): + """Construct rotation matrix with correctly scaled parameters.""" + arcsec2rad: float = lambda arcsec: deg2rad(arcsec) / 3600.0 + + drx = arcsec2rad(drx) + dry = arcsec2rad(dry) + drz = arcsec2rad(drz) + + dR = array( + [ + [0, -drz, dry], + [drz, 0, -drx], + [-dry, drx, 0], + ] + ) + + if self.convention == Convention.POSITION_VECTOR: + return dR + + # If it's not position vector convention is must be coordinate frame + # which we get by transposing the rotation matrix + return dR.T + + def set_target_epoch(self, target_epoch: float) -> "Helmert": """ - Transform a cartesian coordinate. + Express the Helmert transformation at a new reference epoch. + + Parameters + ---------- + target_epoch : float + Epoch to express the Helmert transformation at. + + Returns + ------- + "Helmert" + Helmert transformation object expressed at target_epoch, i.e. + ref_epoch=target_epoch. + """ - if inverse: - return -self.T + 1 / self.c * self.R.T.dot(P) + dt = target_epoch - self.ref_epoch + self.x = self.x + self.dx * dt + self.y = self.y + self.dy * dt + self.z = self.z + self.dz * dt + self.s = self.s + self.ds * dt + self.rx = self.rx + self.drx * dt + self.ry = self.ry + self.dry * dt + self.rz = self.rz + self.drz * dt + self.ref_epoch = target_epoch + + self.c = 1 + self.s * 1e-6 + self.dc = self.ds * 1e-6 + + self.T = array([self.x, self.y, self.z]) + self.dT = array([self.dx, self.dy, self.dz]) + self.R = self._build_rot_matrix(self.rx, self.ry, self.rz) + self.dR = self._build_drot_matrix(self.drx, self.dry, self.drz) + return self + + def inverse(self) -> "Helmert": + """ + Change the sign on all elements in the Helmert object. + + Returns + ------- + "Helmert" + Invers Helmert object. + + """ + self.x = -self.x + self.y = -self.y + self.z = -self.z + self.s = -self.s + self.rx = -self.rx + self.ry = -self.ry + self.rz = -self.rz + self.dx = -self.dx + self.dy = -self.dy + self.dz = -self.dz + self.ds = -self.ds + self.drx = -self.drx + self.dry = -self.dry + self.drz = -self.drz + + self.c = 1 + self.s * 1e-6 + self.dc = self.ds * 1e-6 - return self.T + self.c * self.R.dot(P) + self.T = array([self.x, self.y, self.z]) + self.dT = array([self.dx, self.dy, self.dz]) + self.R = self._build_rot_matrix(self.rx, self.ry, self.rz) + self.dR = self._build_drot_matrix(self.drx, self.dry, self.drz) + return self + + def transform(self, posObs: "Observation", inverse: bool = False) -> "Observation": + """ + Apply the Helmert transformation on the observation. + + Apply the 14 parameter Helmert transformation on the observation to transform it + from reference frame A to reference frame B. + + Parameters + ---------- + posObs : "Observation" + coordinate observation, epoch and optional velocity to be transformed. + inverse : bool, optional + Reverse the transformation such that it transforms from B to A. + The default is False. + + Returns + ------- + "Observation" + The transformed observation, epoch and optional velocity. + If posObs.stn_vel is False, the transformed velocity will be zero. + """ + posObs_out = Observation() + dt = posObs.epoch - self.ref_epoch + if self.vel_model: + posObs_out.epoch = self.ref_epoch + else: + posObs_out.epoch = posObs.epoch + if inverse: + posObs_out.position( + -self.T + - self.dT * dt + + 1 / (self.c + self.dc * dt) * self.R.T.dot(posObs.position()) + ) + if posObs.stn_vel: + posObs_out.stn_vel = True + posObs_out.velocity( + posObs.velocity() + - self.dT + - self.dc * posObs.position() + - self.dR.dot(posObs.position()) + ) + else: + posObs_out.position( + self.T + + self.dT * dt + + (self.c + self.dc * dt) + * (self.R + self.dR * dt).dot(posObs.position()) + ) + if posObs.stn_vel: + posObs_out.stn_vel = True + posObs_out.velocity( + posObs.velocity() + + self.dT + + self.dc * posObs.position() + + self.dR.dot(posObs.position()) + ) + return posObs_out diff --git a/test.py b/test.py index a8b2041..27a4d90 100644 --- a/test.py +++ b/test.py @@ -4,7 +4,7 @@ import pyproj import pytest -from helmert import Helmert, Convention +from helmert import Helmert, Convention, Observation def is_vector_close(A: np.array, B: np.array): @@ -16,6 +16,14 @@ def is_vector_close(A: np.array, B: np.array): return True +def is_observation_close(A: "Observation", B: "Observation"): + if not is_vector_close(A.position(), B.position()): + return False + if not is_vector_close(A.velocity(), B.velocity()): + return False + return True + + # Parameters for transformation 1 @pytest.fixture() def H1(): @@ -27,6 +35,7 @@ def H1(): rx=0.000891, ry=0.00539, rz=-0.008772, + ref_epoch=2022.0, ) @@ -40,6 +49,7 @@ def H2(): rx=0.000521, ry=0.00923, rz=-0.004592, + ref_epoch=2022.0, ) @@ -50,11 +60,11 @@ def H3(H1, H2): @pytest.fixture() def coord(): - """Input coordinate (12.0E,55.0N)""" - return np.array([3586469.6568, 762327.6588, 5201383.5231]) + """Input coordinate (12.0E,55.0N,2022.0)""" + return Observation(3586469.6568, 762327.6588, 5201383.5231, 2022.0) -def test_two_consecutive_helmerts(H1, H2, coord): +def test_two_consecutive_7helmerts(H1, H2, coord): """Two consecutive helmerts""" c1 = H1.transform(coord) c2 = H2.transform(c1) @@ -64,7 +74,7 @@ def test_two_consecutive_helmerts(H1, H2, coord): c3 = H3.transform(coord) # Verify that the results are the same, if they are the math is correct - assert is_vector_close(c2, c3), "Math verification failed!" + assert is_vector_close(c2.position(), c3.position()), "Math verification failed!" def test_same_results_as_proj(H1, H2, H3, coord): @@ -80,15 +90,15 @@ def test_same_results_as_proj(H1, H2, H3, coord): +s={H2.s} +convention=position_vector """ helmert = pyproj.transformer.Transformer.from_pipeline(pipeline) - c = np.array(helmert.transform(coord[0], coord[1], coord[2])) + c = np.array(helmert.transform(coord.x, coord.y, coord.z)) c3 = H3.transform(coord) # Verify that the results are the same, if they are my math is the same as PROJ's - assert is_vector_close(c, c3), "PROJ verification failed!" + assert is_vector_close(c, c3.position()), "PROJ verification failed!" -def test_helmert_commutativeness(H1, H2, H3, coord): +def test_7helmert_commutativeness(H1, H2, H3, coord): """Are Helmert transformations commutative?""" H4 = H1 + H2 + H3 H5 = H3 + H2 + H1 @@ -96,7 +106,9 @@ def test_helmert_commutativeness(H1, H2, H3, coord): c1 = H4.transform(coord) c2 = H5.transform(coord) # Verify that Helmerts commute(?) - assert is_vector_close(c1, c2), "Commutative property verification failed!" + assert is_vector_close( + c1.position(), c2.position() + ), "Commutative property verification failed!" def test_inverse_transform(H1, coord): @@ -105,7 +117,9 @@ def test_inverse_transform(H1, coord): c2 = H1.transform(c1, inverse=True) # Verify that the inverse transform works - assert is_vector_close(coord, c2), "Inverse property verification failed!" + assert is_vector_close( + coord.position(), c2.position() + ), "Inverse property verification failed!" def test_convention(H1, coord): @@ -121,10 +135,10 @@ def test_convention(H1, coord): """ position_vector = pyproj.transformer.Transformer.from_pipeline(projstring) - a = np.array(position_vector.transform(coord[0], coord[1], coord[2])) + a = np.array(position_vector.transform(coord.x, coord.y, coord.z)) b = H1.transform(coord) - assert is_vector_close(a, b) + assert is_vector_close(a, b.position()) projstring = f""" +proj=helmert +x={H1.x} +y={H1.y} +z={H1.z} @@ -133,11 +147,11 @@ def test_convention(H1, coord): """ position_vector = pyproj.transformer.Transformer.from_pipeline(projstring) - a = np.array(position_vector.transform(coord[0], coord[1], coord[2])) + a = np.array(position_vector.transform(coord.x, coord.y, coord.z)) # Rebuild rotation matrix using coordinate frame convention H1.convention = Convention.COORDINATE_FRAME H1.R = H1._build_rot_matrix(H1.rx, H1.ry, H1.rz) b = H1.transform(coord) - assert is_vector_close(a, b) + assert is_vector_close(a, b.position())