From 6aaf1123737c11e7d7000c510eb2d82d74e6f6b5 Mon Sep 17 00:00:00 2001 From: Kristian Evers Date: Fri, 21 Mar 2025 14:51:59 +0100 Subject: [PATCH 1/2] Add 7 parameter Helmert Operator It inherits from HelmertTranslation as much as possible. It seems as the right thing to do, but it can turn out problematic in the future. Seems to work as intended. The Operator can not estimate parameters yet. That's the next step. --- src/transformo/operators/__init__.py | 4 +- src/transformo/operators/helmert.py | 193 ++++++++++++++++++++++++++- test/operators/test_helmert.py | 167 ++++++++++++++++++++++- 3 files changed, 358 insertions(+), 6 deletions(-) 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..5adfb2b 100644 --- a/src/transformo/operators/helmert.py +++ b/src/transformo/operators/helmert.py @@ -4,8 +4,10 @@ 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 @@ -60,13 +62,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 DataSource.can_estimate + + This exist as its own method so it can be overridden in a classes + enheriting 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): @@ -149,3 +161,176 @@ 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" + + +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. + """ + + # mypy will complain since HelmertTrainslation defines this as + # a Literal["helmert_translation"], 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: + # fmt: off + cos = np.cos + sin = np.sin + Rx = np.array( # pylint: disable=invalid-name + [ + [1, 0, 0], + [0, cos(rx), -sin(rx)], + [0, sin(rx), cos(rx)], + ] + ) + Ry = np.array( # pylint: disable=invalid-name + [ + [ cos(ry), 0, sin(ry)], + [ 0, 1, 0], + [-sin(ry), 0, cos(ry)], + ] + ) + Rz = np.array( # pylint: disable=invalid-name + [ + [cos(rz), -sin(rz), 0], + [sin(rz), cos(rz), 0], + [ 0, 0, 1], + ] + ) + # fmt: on + rotation_matrix = Rz @ Ry @ Rx + + if self.convention == RotationConvention.POSITION_VECTOR: + return rotation_matrix + + # If the convention is not position vector convention it must be + # coordinate frame which we get by transposing the rotation matrix + return rotation_matrix.T + + @property + def scale(self) -> float: + """Scale parameter, given in SI units.""" + 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 diff --git a/test/operators/test_helmert.py b/test/operators/test_helmert.py index e23c78a..ab7be2a 100644 --- a/test/operators/test_helmert.py +++ b/test/operators/test_helmert.py @@ -4,8 +4,9 @@ import numpy as np +from transformo.core import Transformer from transformo.datatypes import Parameter -from transformo.operators import HelmertTranslation +from transformo.operators import Helmert7Param, HelmertTranslation, RotationConvention def test_helmerttranslation_can_estimate(): @@ -110,3 +111,167 @@ 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) From ab89d9ca62660eed7b2a4f022a75a2a7454acfd4 Mon Sep 17 00:00:00 2001 From: Kristian Evers Date: Mon, 12 May 2025 15:28:22 +0200 Subject: [PATCH 2/2] Add rudimentary 7 parameter Helmert estimation `Operator.can_estimate` modified to not use matrices of zeros, since that would cause the helmert estimator to throw RuntimeWarnigs about division by zero. --- examples/bells_and_whistles.yaml | 4 +- src/transformo/core.py | 4 +- src/transformo/operators/helmert.py | 134 ++++++++++++++++++++-------- test/operators/test_helmert.py | 31 ++++++- 4 files changed, 133 insertions(+), 40 deletions(-) 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/helmert.py b/src/transformo/operators/helmert.py index 5adfb2b..c878afb 100644 --- a/src/transformo/operators/helmert.py +++ b/src/transformo/operators/helmert.py @@ -10,6 +10,7 @@ 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 @@ -70,10 +71,10 @@ def __init__(self, **kwargs) -> None: def _has_transformation_parameters_been_given(self): """ - Part of the __init__ process. Supports DataSource.can_estimate + Part of the __init__ process. Supports Operator.can_estimate This exist as its own method so it can be overridden in a classes - enheriting from HelmertTranslation. + 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): @@ -149,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) @@ -172,16 +171,58 @@ class RotationConvention(enum.Enum): 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 HelmertTrainslation defines this as - # a Literal["helmert_translation"], so we have it look the other + # 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" @@ -268,7 +309,6 @@ def arcsec2rad(arcsec) -> float: rz = arcsec2rad(self.rz) if self.small_angle_approximation: - rotation_matrix = np.array( [ [1, -rz, ry], @@ -277,43 +317,18 @@ def arcsec2rad(arcsec) -> float: ] ) else: - # fmt: off - cos = np.cos - sin = np.sin - Rx = np.array( # pylint: disable=invalid-name - [ - [1, 0, 0], - [0, cos(rx), -sin(rx)], - [0, sin(rx), cos(rx)], - ] - ) - Ry = np.array( # pylint: disable=invalid-name - [ - [ cos(ry), 0, sin(ry)], - [ 0, 1, 0], - [-sin(ry), 0, cos(ry)], - ] - ) - Rz = np.array( # pylint: disable=invalid-name - [ - [cos(rz), -sin(rz), 0], - [sin(rz), cos(rz), 0], - [ 0, 0, 1], - ] - ) - # fmt: on - rotation_matrix = Rz @ Ry @ Rx + rotation_matrix = R3(rz) @ R2(ry) @ R1(rx) if self.convention == RotationConvention.POSITION_VECTOR: return rotation_matrix - # If the convention is not position vector convention it must be - # coordinate frame which we get by transposing the 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, given in SI units.""" + """Scale parameter""" if self.s is None: return 1 return 1 + self.s * 1e-6 @@ -334,3 +349,50 @@ 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 ab7be2a..9b60e23 100644 --- a/test/operators/test_helmert.py +++ b/test/operators/test_helmert.py @@ -3,10 +3,11 @@ """ import numpy as np +import pytest -from transformo.core import Transformer from transformo.datatypes import Parameter from transformo.operators import Helmert7Param, HelmertTranslation, RotationConvention +from transformo.transformer import Transformer def test_helmerttranslation_can_estimate(): @@ -275,3 +276,31 @@ def test_helmert7param_small_angle_approximation(): 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])