diff --git a/examples/bells_and_whistles.yaml b/examples/bells_and_whistles.yaml index eb40d84..6ce19d9 100644 --- a/examples/bells_and_whistles.yaml +++ b/examples/bells_and_whistles.yaml @@ -12,7 +12,9 @@ target_data: operators: - name: null - type: dummy_operator + type: helmert_7param + convention: coordinate_frame + small_angle_approximation: true presenters: - type: datasource_presenter diff --git a/src/transformo/core.py b/src/transformo/core.py index ca21bd0..c8f5c43 100644 --- a/src/transformo/core.py +++ b/src/transformo/core.py @@ -461,10 +461,10 @@ def can_estimate(self) -> bool: # given as input. If `estimate()` is implemented *some* parameters # will be estimated and stored in the `Operator` but they will be # discarded when `estimate()` is executed again with proper input. - zeros = np.zeros(shape=(4, 3)) ones = np.ones(shape=(4, 3)) + twos = ones * 2 try: - self.estimate(zeros, zeros, ones, ones) + self.estimate(ones, ones, twos, twos) except NotImplementedError: return False diff --git a/src/transformo/operators/__init__.py b/src/transformo/operators/__init__.py index addc226..5501907 100644 --- a/src/transformo/operators/__init__.py +++ b/src/transformo/operators/__init__.py @@ -10,13 +10,15 @@ from transformo.core import Operator from transformo.datatypes import Parameter -from .helmert import HelmertTranslation +from .helmert import Helmert7Param, HelmertTranslation, RotationConvention from .proj import ProjOperator __all__ = [ "Operator", "DummyOperator", "HelmertTranslation", + "Helmert7Param", + "RotationConvention", "ProjOperator", ] diff --git a/src/transformo/operators/helmert.py b/src/transformo/operators/helmert.py index a9ba5e5..c878afb 100644 --- a/src/transformo/operators/helmert.py +++ b/src/transformo/operators/helmert.py @@ -4,10 +4,13 @@ from __future__ import annotations +import enum import math -from typing import Literal, Optional +from functools import cached_property +from typing import TYPE_CHECKING, Any, Literal, Optional import numpy as np +from numpy import cos, sin from transformo._typing import CoordinateMatrix, Vector from transformo.core import Operator @@ -60,13 +63,23 @@ class HelmertTranslation(Operator): def __init__(self, **kwargs) -> None: super().__init__(**kwargs) - # if one or more parameter is given at instantiation time - if not _isnan(self.x) or not _isnan(self.y) or not _isnan(self.z): - self._transformation_parameters_given = True + # determine if the operation can estimate parameters or not + self._has_transformation_parameters_been_given() self._sanitize_parameters() # ... from now on we can rely on parameters being useful + def _has_transformation_parameters_been_given(self): + """ + Part of the __init__ process. Supports Operator.can_estimate + + This exist as its own method so it can be overridden in a classes + inheriting from HelmertTranslation. + """ + # if one or more parameter is given at instantiation time + if not _isnan(self.x) or not _isnan(self.y) or not _isnan(self.z): + self._transformation_parameters_given = True + def _sanitize_parameters(self) -> None: """Make sure that translation parameters are not None.""" if _isnan(self.x): @@ -137,8 +150,6 @@ def estimate( Parameters `x`, `y` and `z` of this operator *will* be overwritten once this method is called. - - Weights for source and target coordinates are ignored. """ avg_source = np.average(source_coordinates, axis=0, weights=source_weights) @@ -149,3 +160,239 @@ def estimate( self.x = mean_translation[0] self.y = mean_translation[1] self.z = mean_translation[2] + + +class RotationConvention(enum.Enum): + """ + Rotation convention of Helmert transformations. + """ + + POSITION_VECTOR = "position_vector" + COORDINATE_FRAME = "coordinate_frame" + + +# fmt: off +def R1(rx: float) -> CoordinateMatrix: + """Rotation matrix for rotating about the x-axis.""" + return np.array( + [ + [1, 0, 0], + [0, cos(rx), -sin(rx)], + [0, sin(rx), cos(rx)], + ] + ) + +def R2(ry: float) -> CoordinateMatrix: + """Rotation matrix for rotating about the y-axis.""" + return np.array( + [ + [ cos(ry), 0, sin(ry)], + [ 0, 1, 0], + [-sin(ry), 0, cos(ry)], + ] + ) + +def R3(rz: float) -> CoordinateMatrix: + """Rotation matrix for rotating about the z-axis.""" + return np.array( + [ + [cos(rz), -sin(rz), 0], + [sin(rz), cos(rz), 0], + [ 0, 0, 1], + ] + ) +# fmt: on + + +class Helmert7Param(HelmertTranslation): + """ + The 7 paramter Helmert transformation performs a translation in the three + principal directions of a earth-centered, earth-fixed coordinate system (or + a similarly shaped celestial body), as well as rotations around those axes + and a scaling of the coordinates. + + A coordinate in vector Va is transformed into vector Vb using the expression + below: + + Vb = T + (1+s*10^-6) * R * Va + + Where T consist of the three translation parameters, s is the scaling factor + and R is a rotation matrix. The principal components of T, s and R can be + estimated, resulting in three translations, three rotations and a scale factor. + """ + + # mypy will complain since Helmert7Param defines this as + # a Literal["helmert_7param"], so we have it look the other + # way for a brief moment when checking the types. + if TYPE_CHECKING: + type: Any = "helmert_7param" + else: + type: Literal["helmert_7param"] = "helmert_7param" + + convention: RotationConvention + small_angle_approximation: bool = True + + # Rotation parameters - given in arc seconds + rx: Optional[float] = float("nan") + ry: Optional[float] = float("nan") + rz: Optional[float] = float("nan") + + # Scale parameter - given in ppm + s: Optional[float] = float("nan") + + def __init__(self, convention: RotationConvention, **kwargs) -> None: + super().__init__(convention=convention, **kwargs) + + def _has_transformation_parameters_been_given(self): + # if one or more parameter is given at instantiation time + parameters_instantiated = [ + not _isnan(self.x), + not _isnan(self.y), + not _isnan(self.z), + not _isnan(self.rx), + not _isnan(self.ry), + not _isnan(self.rz), + not _isnan(self.s), + ] + + if any(parameters_instantiated): + self._transformation_parameters_given = True + + def _sanitize_parameters(self) -> None: + """Make sure that translation parameters are not None.""" + super()._sanitize_parameters() + + if _isnan(self.rx): + self.rx = 0.0 + + if _isnan(self.ry): + self.ry = 0.0 + + if _isnan(self.rz): + self.rz = 0.0 + + if _isnan(self.s): + self.s = 0.0 + + def _parameter_list(self) -> list[Parameter]: + params: list[Parameter] = super()._parameter_list() + + if not _isnan(self.rx) and self.rx != 0.0: + params.append(Parameter("rx", _float(self.rx))) + + if not _isnan(self.ry) and self.ry != 0.0: + params.append(Parameter("ry", _float(self.ry))) + + if not _isnan(self.rz) and self.rz != 0.0: + params.append(Parameter("rz", _float(self.rz))) + + if not _isnan(self.s) and self.s != 0.0: + params.append(Parameter("s", _float(self.s))) + + params.append(Parameter("convention", self.convention.value)) + if self.small_angle_approximation: + params.append(Parameter("approx")) + + return params + + @cached_property + def R(self) -> CoordinateMatrix: # pylint: disable=invalid-name + """ + Rotation matrix. + """ + + def arcsec2rad(arcsec) -> float: + return np.deg2rad(arcsec) / 3600.0 + + rx = arcsec2rad(self.rx) + ry = arcsec2rad(self.ry) + rz = arcsec2rad(self.rz) + + if self.small_angle_approximation: + rotation_matrix = np.array( + [ + [1, -rz, ry], + [rz, 1, -rx], + [-ry, rx, 1], + ] + ) + else: + rotation_matrix = R3(rz) @ R2(ry) @ R1(rx) + + if self.convention == RotationConvention.POSITION_VECTOR: + return rotation_matrix + + # If the convention is not "position vector" it must be + # "coordinate frame" which we get by transposing the rotation matrix + return rotation_matrix.T + + @property + def scale(self) -> float: + """Scale parameter""" + if self.s is None: + return 1 + return 1 + self.s * 1e-6 + + def forward(self, coordinates: CoordinateMatrix) -> CoordinateMatrix: + """ + Forward method of the 7 parameter Helmert. + """ + + # Since the coordinates are contained in a Nx3 matrix, we deviate + # from the single-point Helmert formulation of B = T + s * R*A. + # By transposing the rotation matrix we get the same results + # when instead doing B = T + s * A*R^T. + return self.T + self.scale * coordinates @ self.R.T + + def inverse(self, coordinates: CoordinateMatrix) -> CoordinateMatrix: + """ + Inverse method of the 7 parameter Helmert. + """ + return -self.T + 1 / self.scale * coordinates @ self.R + + def estimate( + self, + source_coordinates: CoordinateMatrix, + target_coordinates: CoordinateMatrix, + source_weights: CoordinateMatrix, + target_weights: CoordinateMatrix, + ) -> None: + """ + Estimate parameters using small angle approximation. + """ + + def rad2arcsec(rad) -> float: + return np.rad2deg(rad) * 3600 + + # Build design matrix + N = len(source_coordinates) + A = np.zeros((N * 3, 7)) + + for i, c in enumerate(source_coordinates): + R = np.array( + [ + [0, c[2], -c[1]], + [-c[2], 0, c[0]], + [c[1], -c[0], 0], + ] + ) + + if self.convention == RotationConvention.COORDINATE_FRAME: + R = R.T + + A[i * 3 : i * 3 + 3, :] = np.column_stack([np.eye(3), c[0:3], R]) + + b = target_coordinates.flatten() + + coeffs, _, _, _ = np.linalg.lstsq(A, b, rcond=None) + + self.x = coeffs[0] + self.y = coeffs[1] + self.z = coeffs[2] + + sd = coeffs[3] + self.s = (sd - 1) * 1e6 # [ppm] + + self.rx = rad2arcsec(coeffs[4] / sd) + self.ry = rad2arcsec(coeffs[5] / sd) + self.rz = rad2arcsec(coeffs[6] / sd) diff --git a/test/operators/test_helmert.py b/test/operators/test_helmert.py index e23c78a..9b60e23 100644 --- a/test/operators/test_helmert.py +++ b/test/operators/test_helmert.py @@ -3,9 +3,11 @@ """ import numpy as np +import pytest from transformo.datatypes import Parameter -from transformo.operators import HelmertTranslation +from transformo.operators import Helmert7Param, HelmertTranslation, RotationConvention +from transformo.transformer import Transformer def test_helmerttranslation_can_estimate(): @@ -110,3 +112,195 @@ def test_helmerttranslation_as_operator(): assert op.parameters[0] == Parameter("x", 3) assert op.parameters[1] == Parameter("y", 5) assert op.parameters[2] == Parameter("z", 10) + + +def test_helmert7param_instantiation(): + """ + Basic tests of instantiating the Helmert7Param class + """ + h7 = Helmert7Param( + convention=RotationConvention.COORDINATE_FRAME, + x=1.0, + y=1.0, + z=1.0, + rx=0.001, + ry=0.002, + rz=0.003, + s=0.005, + ) + + assert isinstance(h7, Helmert7Param) + assert h7.x == 1.0 + assert h7.y == 1.0 + assert h7.z == 1.0 + assert h7.rx == 0.001 + assert h7.ry == 0.002 + assert h7.rz == 0.003 + assert h7.s == 0.005 + + assert h7._transformation_parameters_given is True + assert h7.can_estimate is False + + h7 = Helmert7Param(convention=RotationConvention.COORDINATE_FRAME) + + assert h7._transformation_parameters_given is False + assert h7.can_estimate is True + + +def test_helmert7param_parameters(): + """Test that Helmert7Param.parameters returns the correct values.""" + + h7 = Helmert7Param( + convention=RotationConvention.COORDINATE_FRAME, + test_helmert7param_small_angle_approximation=True, + x=1.0, + y=1.0, + z=1.0, + rx=0.001, + ry=0.002, + rz=0.003, + s=0.005, + ) + + for param in h7.parameters: + print(param.name, param.value) + + assert h7.parameters[0].name == "x" and h7.parameters[0].value == 1.0 + assert h7.parameters[1].name == "y" and h7.parameters[1].value == 1.0 + assert h7.parameters[2].name == "z" and h7.parameters[2].value == 1.0 + assert h7.parameters[3].name == "rx" and h7.parameters[3].value == 0.001 + assert h7.parameters[4].name == "ry" and h7.parameters[4].value == 0.002 + assert h7.parameters[5].name == "rz" and h7.parameters[5].value == 0.003 + assert h7.parameters[6].name == "s" and h7.parameters[6].value == 0.005 + assert ( + h7.parameters[7].name == "convention" + and h7.parameters[7].value == "coordinate_frame" + ) + assert h7.parameters[8].name == "approx" + assert len(h7.parameters) == 9 + + h7_2 = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + x=1.0, + rz=0.003, + ) + + print() + for param in h7_2.parameters: + print(param.name, param.value) + + assert h7_2.parameters[0].name == "x" and h7_2.parameters[0].value == 1.0 + assert h7_2.parameters[1].name == "rz" and h7_2.parameters[1].value == 0.003 + assert ( + h7_2.parameters[2].name == "convention" + and h7_2.parameters[2].value == "position_vector" + ) + + +def test_helmert7param_transformation(source_coordinates): + """Test the forward transformation of Helmert7Param.""" + + h7 = Helmert7Param( + convention=RotationConvention.COORDINATE_FRAME, + x=1234.0, + y=923.0, + z=523.0, + rx=0.001, + ry=0.002, + rz=0.003, + s=0.005, + ) + + projstring = "+proj=helmert " + for param in h7.parameters: + projstring += f"{param.as_proj_param} " + + print(projstring) + T = Transformer.from_projstring(projstring) + + transformo_coords = h7.forward(source_coordinates) + proj_coords = T.transform_many(source_coordinates) + + # We are not going to get an exact match but it's very close and good enough + assert np.allclose(proj_coords, transformo_coords) + + # Check the inverse transformation + roundtrip = h7.inverse(transformo_coords) + assert np.allclose(source_coordinates, roundtrip) + + +def test_helmert7param_small_angle_approximation(): + """ + Test the rotation matrix with and without the small angles approximation. + """ + + rx = 23.1 + ry = 22.252 + rz = 39.42 + + # Check that the rotation matrices are different when produced with + # and without the small angle approximation. Of course we use sufficiently + # large rotations for this to be the case. + helmert_small_angle = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + small_angle_approximation=True, + rx=rx, + ry=ry, + rz=rz, + ) + helmert_full = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + small_angle_approximation=False, + rx=rx, + ry=ry, + rz=rz, + ) + + assert not np.allclose(helmert_small_angle.R, helmert_full.R) + + # Test specific values of rotation matrices, again with and + # without the small angles approximation. + arcsec2rad: float = lambda arcsec: np.deg2rad(arcsec) / 3600.0 + + x_rotation_small_angle = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + small_angle_approximation=True, + rx=rx, + ) + + x_rotation = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + small_angle_approximation=False, + rx=rx, + ) + + assert x_rotation_small_angle.R[2][1] == arcsec2rad(rx) + assert x_rotation.R[2][1] != arcsec2rad(rx) + + +def test_helmert7parameter_estimation(source_coordinates, target_coordinates): + """ + Verify that estimation of a 7 parameter Helmert works. + + The test is slightly dumb as it simply checks that transformation of the + source coordinates somewhat matches the target coordinates. + """ + h = Helmert7Param( + convention=RotationConvention.POSITION_VECTOR, + small_angle_approximation=True, + ) + + weights = np.ones_like(source_coordinates) + h.estimate(source_coordinates, target_coordinates, weights, weights) + + estimated_coordinates = h.forward(source_coordinates) + + assert estimated_coordinates[0, 0] == pytest.approx(target_coordinates[0, 0]) + assert estimated_coordinates[0, 1] == pytest.approx(target_coordinates[0, 1]) + assert estimated_coordinates[0, 2] == pytest.approx(target_coordinates[0, 2]) + assert estimated_coordinates[1, 0] == pytest.approx(target_coordinates[1, 0]) + assert estimated_coordinates[1, 1] == pytest.approx(target_coordinates[1, 1]) + assert estimated_coordinates[1, 2] == pytest.approx(target_coordinates[1, 2]) + assert estimated_coordinates[-1, 0] == pytest.approx(target_coordinates[-1, 0]) + assert estimated_coordinates[-1, 1] == pytest.approx(target_coordinates[-1, 1]) + assert estimated_coordinates[-1, 2] == pytest.approx(target_coordinates[-1, 2])