cars 1.0.0rc3__cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl

cars/core/projection.py

@@ -0,0 +1,843 @@
+#!/usr/bin/env python
+# coding: utf8
+#
+# Copyright (c) 2020 Centre National d'Etudes Spatiales (CNES).
+#
+# This file is part of CARS
+# (see https://github.com/CNES/cars).
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+"""
+Projection module:
+contains some general purpose functions using polygons and data projections
+"""
+# pylint: disable=C0302(too-many-lines)
+
+# Standard imports
+import logging
+import os
+from typing import List, Tuple
+
+# Third party imports
+import numpy as np
+import pandas
+import pyproj
+import rasterio as rio
+import xarray as xr
+from pyproj import CRS
+from rasterio.features import shapes
+from shapely.geometry import Polygon, shape
+from shapely.ops import transform
+
+from cars.core import constants as cst
+from cars.core import inputs, outputs, utils
+from cars.orchestrator.cluster.log_wrapper import cars_profile
+
+
+def compute_dem_intersection_with_poly(  # noqa: C901
+    srtm_file: str, ref_poly: Polygon, ref_epsg: int
+) -> Polygon:
+    """
+    Compute the intersection polygon between the defined dem regions
+    and the reference polygon in input
+
+    :raise Exception: when the input dem doesn't intersect the reference polygon
+
+    :param srtm_file: srtm file
+    :param ref_poly: reference polygon
+    :param ref_epsg: reference epsg code
+    :return: The intersection polygon between the defined dem regions
+        and the reference polygon in input
+    """
+    dem_poly = None
+
+    if os.path.isdir(os.path.abspath(srtm_file)):
+        raise RuntimeError("srtm input should be a file and not a directory")
+
+    if inputs.rasterio_can_open(srtm_file):
+        with rio.open(srtm_file) as data:
+            xmin = min(data.bounds.left, data.bounds.right)
+            ymin = min(data.bounds.bottom, data.bounds.top)
+            xmax = max(data.bounds.left, data.bounds.right)
+            ymax = max(data.bounds.bottom, data.bounds.top)
+
+            try:
+                file_epsg = data.crs.to_epsg()
+                file_bb = Polygon(
+                    [
+                        (xmin, ymin),
+                        (xmin, ymax),
+                        (xmax, ymax),
+                        (xmax, ymin),
+                        (xmin, ymin),
+                    ]
+                )
+
+                # transform polygon if needed
+                if ref_epsg != file_epsg:
+                    file_bb = polygon_projection(file_bb, file_epsg, ref_epsg)
+
+                left, bottom, right, top = ref_poly.bounds
+
+                # To ensure that the window used for extracting the SRTM
+                # polygons completely contains the ref_poly,
+                # a margin must be applied. Using the exact bounds of
+                # ref_poly could potentially lead to issues.
+                spatial_ref = CRS.from_epsg(ref_epsg)
+                if spatial_ref.is_geographic:
+                    margin = 0.001
+                else:
+                    margin = 50
+
+                left = left - margin
+                right = right + margin
+                bottom = bottom - margin
+                top = top + margin
+
+                min_row, min_col = data.index(left, top)
+                max_row, max_col = data.index(right, bottom)
+
+                window = rio.windows.Window(
+                    col_off=int(min_col),
+                    row_off=int(min_row),
+                    width=int(max_col - min_col),
+                    height=int(max_row - min_row),
+                )
+
+                # if the srtm tile intersects the reference polygon
+                if file_bb.intersects(ref_poly):
+                    local_dem_poly = None
+
+                    # retrieve valid polygons
+                    for poly, val in shapes(
+                        data.read(1, window=window),
+                        data.dataset_mask(window=window),
+                        transform=data.window_transform(window),
+                    ):
+                        if val != 0:
+                            poly = shape(poly)
+                            poly = poly.buffer(0)
+                            if ref_epsg != file_epsg:
+                                poly = polygon_projection(
+                                    poly, file_epsg, ref_epsg
+                                )
+
+                            # combine valid polygons
+                            if local_dem_poly is None:
+                                local_dem_poly = poly
+                            else:
+                                local_dem_poly = poly.union(local_dem_poly)
+
+                    # combine the tile valid polygon to the other
+                    # tiles' ones
+                    if dem_poly is None:
+                        dem_poly = local_dem_poly
+                    else:
+                        dem_poly = dem_poly.union(local_dem_poly)
+
+            except AttributeError as attribute_error:
+                logging.warning(
+                    "Impossible to read the SRTM"
+                    "tile epsg code: {}".format(attribute_error)
+                )
+
+    # compute dem coverage polygon over the reference polygon
+    if dem_poly is None or not dem_poly.intersects(ref_poly):
+        raise RuntimeError("The input DEM does not intersect the useful zone")
+
+    dem_cover = dem_poly.intersection(ref_poly)
+
+    area_cover = dem_cover.area
+    area_inter = ref_poly.area
+
+    return dem_cover, area_cover / area_inter * 100.0
+
+
+def polygon_projection(poly: Polygon, epsg_in: int, epsg_out: int) -> Polygon:
+    """
+    Projects a polygon from an initial epsg code to another
+
+    :param poly: poly to project
+    :param epsg_in: initial epsg code
+    :param epsg_out: final epsg code
+    :return: The polygon in the final projection
+    """
+    # Get CRS from input EPSG codes
+    crs_in = pyproj.CRS.from_epsg(epsg_in)
+    crs_out = pyproj.CRS.from_epsg(epsg_out)
+    # Project polygon between CRS (keep always_xy for compatibility)
+    project = pyproj.Transformer.from_crs(crs_in, crs_out, always_xy=True)
+    poly = transform(project.transform, poly)
+
+    return poly
+
+
+def polygon_projection_crs(poly: Polygon, crs_in: CRS, crs_out: CRS) -> Polygon:
+    """
+    Projects a polygon from an initial crs to another
+
+    :param poly: poly to project
+    :param crs_in: initial crs
+    :param crs_out: final crs
+    :return: The polygon in the final projection
+    """
+    # Project polygon between CRS (keep always_xy for compatibility)
+    project = pyproj.Transformer.from_crs(crs_in, crs_out, always_xy=True)
+    poly = transform(project.transform, poly)
+
+    return poly
+
+
+def geo_to_ecef(
+    lat: np.ndarray, lon: np.ndarray, alt: np.ndarray
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Point transformation from Geodetic of ellipsoid WGS-84) to ECEF
+    ECEF: Earth-centered, Earth-fixed
+
+    :param lat: input geodetic latitude (angle in degree)
+    :param lon:  input geodetic longitude (angle in degree)
+    :param alt: input altitude above geodetic ellipsoid (meters)
+    :return:  ECEF (Earth centered, Earth fixed) x, y, z coordinates tuple
+                                                    (in meters)
+    """
+    epsg_in = 4979  # EPSG code for Geocentric WGS84 in lat, lon, alt (degree)
+    epsg_out = 4978  # EPSG code for ECEF WGS84 in x, y, z (meters)
+
+    return point_cloud_conversion(
+        np.array([[lon, lat, alt]]), epsg_in, epsg_out
+    )[0]
+
+
+def ecef_to_enu(  # pylint: disable=too-many-positional-arguments
+    x_ecef: np.ndarray,
+    y_ecef: np.ndarray,
+    z_ecef: np.ndarray,
+    lat0: np.ndarray,
+    lon0: np.ndarray,
+    alt0: np.ndarray,
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Coordinates conversion from ECEF Earth Centered to
+    East North Up Coordinate from a reference point (lat0, lon0, alt0)
+
+    See Wikipedia page for details:
+    https://en.wikipedia.org/wiki/Geographic_coordinate_conversion
+
+    :param x_ecef: target x ECEF coordinate (meters)
+    :param y_ecef: target y ECEF coordinate (meters)
+    :param z_ecef: target z ECEF coordinate (meters)
+    :param lat0: Reference geodetic latitude
+    :param lon0: Reference geodetic longitude
+    :param alt0: Reference altitude above geodetic ellipsoid (meters)
+    :return: ENU (xEast, yNorth zUp) target coordinates tuple (meters)
+    """
+    # Intermediate computing for ENU conversion
+    cos_lat0 = np.cos(np.radians(lat0))
+    sin_lat0 = np.sin(np.radians(lat0))
+
+    cos_long0 = np.cos(np.radians(lon0))
+    sin_long0 = np.sin(np.radians(lon0))
+
+    # Determine ECEF coordinates from reference geodetic
+    x0_ecef, y0_ecef, z0_ecef = geo_to_ecef(lat0, lon0, alt0)
+
+    x_east = (-(x_ecef - x0_ecef) * sin_long0) + (
+        (y_ecef - y0_ecef) * cos_long0
+    )
+    y_north = (
+        (-cos_long0 * sin_lat0 * (x_ecef - x0_ecef))
+        - (sin_lat0 * sin_long0 * (y_ecef - y0_ecef))
+        + (cos_lat0 * (z_ecef - z0_ecef))
+    )
+    z_up = (
+        (cos_lat0 * cos_long0 * (x_ecef - x0_ecef))
+        + (cos_lat0 * sin_long0 * (y_ecef - y0_ecef))
+        + (sin_lat0 * (z_ecef - z0_ecef))
+    )
+
+    return x_east, y_north, z_up
+
+
+def geo_to_enu(  # pylint: disable=too-many-positional-arguments
+    lat: np.ndarray,
+    lon: np.ndarray,
+    alt: np.ndarray,
+    lat0: np.ndarray,
+    lon0: np.ndarray,
+    alt0: np.ndarray,
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Point transformation from WGS-84 Geodetic coordinates to to ENU.
+    Use geo_to_ecef and ecef_to_enu functions.
+
+    :param lat: input geodetic latitude (angle in degree)
+    :param lon:  input geodetic longitude (angle in degree)
+    :param alt: input altitude above geodetic ellipsoid (meters)
+    :param lat0: Reference geodetic latitude
+    :param lon0: Reference geodetic longitude
+    :param alt0: Reference altitude above geodetic ellipsoid (meters)
+    :return: ENU (xEast, yNorth zUp) target coordinates tuple (meters)
+    """
+    x_ecef, y_ecef, z_ecef = geo_to_ecef(lat, lon, alt)
+    return ecef_to_enu(x_ecef, y_ecef, z_ecef, lat0, lon0, alt0)
+
+
+def enu_to_aer(
+    x_east: np.ndarray, y_north: np.ndarray, z_up: np.ndarray
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    ENU coordinates to Azimuth, Elevation angle, Range from ENU origin
+    Beware: Elevation angle is not the altitude.
+
+    :param x_east: ENU East coordinate (meters)
+    :param y_north: ENU North coordinate (meters)
+    :param z_up: ENU Up coordinate (meters)
+    :return: Azimuth, Elevation Angle, Slant Range (degrees, degrees, meters)
+    """
+
+    xy_range = np.hypot(x_east, y_north)  # Distance of e, n vector
+    xyz_range = np.hypot(xy_range, z_up)  # Distance of e, n, u vector
+    elevation = np.arctan2(z_up, xy_range)
+    azimuth = np.arctan2(x_east, y_north) % (2 * np.pi)
+    # From [-pi,+pi] to [0,2pi]
+
+    azimuth = np.degrees(azimuth)
+    elevation = np.degrees(elevation)
+
+    return azimuth, elevation, xyz_range
+
+
+def geo_to_aer(  # pylint: disable=too-many-positional-arguments
+    lat: np.ndarray,
+    lon: np.ndarray,
+    alt: np.ndarray,
+    lat0: np.ndarray,
+    lon0: np.ndarray,
+    alt0: np.ndarray,
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Gives Azimuth, Elevation angle and Slant Range
+    from a Reference to a Point with geodetic coordinates.
+
+    :param lat: input geodetic latitude (angle in degree)
+    :param lon: input geodetic longitude (angle in degree)
+    :param alt: input altitude above geodetic ellipsoid (meters)
+    :param lat0: Reference geodetic latitude
+    :param lon0: Reference geodetic longitude
+    :param alt0: Reference altitude above geodetic ellipsoid (meters)
+    :return: Azimuth, Elevation Angle, Slant Range (degrees, degrees, meters)
+    """
+    x_east, y_north, z_up = geo_to_enu(lat, lon, alt, lat0, lon0, alt0)
+    return enu_to_aer(x_east, y_north, z_up)
+
+
+def point_cloud_conversion(
+    cloud_in: np.ndarray, epsg_in: int, epsg_out: int
+) -> np.ndarray:
+    """
+    Convert a point cloud from a SRS to another one.
+
+    :param cloud_in: cloud to project
+    :param epsg_in: EPSG code of the input SRS
+    :param epsg_out: EPSG code of the output SRS
+    :return: Projected point cloud
+    """
+    # Get CRS from input EPSG codes
+    crs_in = pyproj.CRS.from_epsg(epsg_in)
+    crs_out = pyproj.CRS.from_epsg(epsg_out)
+
+    # Project point cloud between CRS (keep always_xy for compatibility)
+    cloud_in = np.array(cloud_in).T
+    transformer = pyproj.Transformer.from_crs(crs_in, crs_out, always_xy=True)
+
+    cloud_in = transformer.transform(*cloud_in)
+    cloud_in = np.array(cloud_in).T
+
+    return cloud_in
+
+
+def point_cloud_conversion_crs(
+    cloud_in: np.ndarray, crs_in: int, crs_out: int
+) -> np.ndarray:
+    """
+    Convert a point cloud from a SRS to another one.
+
+    :param cloud_in: cloud to project
+    :param crs_in: crs of the input SRS
+    :param crs_out: crs of the output SRS
+    :return: Projected point cloud
+    """
+    # Project point cloud between CRS (keep always_xy for compatibility)
+    cloud_in = np.array(cloud_in).T
+    transformer = pyproj.Transformer.from_crs(crs_in, crs_out, always_xy=True)
+
+    cloud_in = transformer.transform(*cloud_in)
+    cloud_in = np.array(cloud_in).T
+
+    return cloud_in
+
+
+def get_xyz_np_array_from_dataset(
+    cloud_in: xr.Dataset,
+) -> Tuple[np.array, List[int]]:
+    """
+    Get a numpy array of size (nb_points, 3) with the columns
+    being the x, y and z coordinates from a dataset as given
+    in output of the triangulation.
+
+    The original epipolar geometry shape is also given in output
+    in order to reshape the output numpy array in its
+    original geometry if necessary.
+
+    :param cloud_in: input xarray dataset
+    :return: a tuple composed of the xyz numàpy array and its original shape
+    """
+    xyz = np.stack(
+        (cloud_in[cst.X].values, cloud_in[cst.Y].values, cloud_in[cst.Z]),
+        axis=-1,
+    )
+    xyz_shape = xyz.shape
+    xyz = np.reshape(xyz, (-1, 3))
+
+    return xyz, xyz_shape
+
+
+def get_converted_xy_np_arrays_from_dataset(
+    cloud_in: xr.Dataset, epsg_out: int
+) -> Tuple[np.array, np.array]:
+    """
+    Get the x and y coordinates as numpy array
+    in the new referential indicated by epsg_out.
+    TODO: add test
+
+    :param cloud_in: input xarray dataset
+    :param epsg_out: target epsg code
+    :return: a tuple composed of the x and y numpy arrays
+    """
+    xyz, xyz_shape = get_xyz_np_array_from_dataset(cloud_in)
+    epsg = int(cloud_in.attrs[cst.EPSG])
+    xyz = point_cloud_conversion(xyz, epsg, epsg_out)
+    xyz = xyz.reshape(xyz_shape)
+    if isinstance(cloud_in, xr.Dataset):
+        proj_x = xyz[:, :, 0]
+        proj_y = xyz[:, :, 1]
+    else:
+        proj_x = xyz[:, 0]
+        proj_y = xyz[:, 1]
+    return proj_x, proj_y
+
+
+def point_cloud_conversion_dataset(cloud: xr.Dataset, epsg_out: int):
+    """
+    Convert a point cloud as an xarray.Dataset to another epsg (inplace)
+    TODO: add test
+
+    :param cloud: cloud to project
+    :param epsg_out: EPSG code of the output SRS
+    """
+
+    if cloud.attrs[cst.EPSG] != epsg_out:
+        xyz, xyz_shape = get_xyz_np_array_from_dataset(cloud)
+
+        xyz = point_cloud_conversion(xyz, cloud.attrs[cst.EPSG], epsg_out)
+        xyz = xyz.reshape(xyz_shape)
+        xyz[xyz == np.inf] = np.nan
+        if isinstance(cloud, xr.Dataset):
+            # # Update cloud_in x, y and z values
+            cloud[cst.X].values = xyz[:, :, 0]
+            cloud[cst.Y].values = xyz[:, :, 1]
+            cloud[cst.Z].values = xyz[:, :, 2]
+
+            # # Update EPSG code
+            cloud.attrs[cst.EPSG] = epsg_out
+        elif isinstance(cloud, pandas.DataFrame):
+            cloud[cst.X] = xyz[:, 0]
+            cloud[cst.Y] = xyz[:, 1]
+            cloud[cst.Z] = xyz[:, 2]
+            cloud.attrs[cst.EPSG] = epsg_out
+        else:
+            logging.error(
+                "point_cloud_conversion_dataset error: point cloud is unknown"
+            )
+
+
+def point_cloud_conversion_dataframe(
+    cloud: pandas.DataFrame, epsg_in: int, epsg_out: int
+):
+    """
+    Convert a point cloud as a panda.DataFrame to another epsg (inplace)
+
+    :param cloud: cloud to project
+    :param epsg_in: EPSG code of the input SRS
+    :param epsg_out: EPSG code of the output SRS
+    """
+    xyz_in = cloud.loc[:, [cst.X, cst.Y, cst.Z]].values
+
+    if xyz_in.shape[0] != 0:
+        xyz_in = point_cloud_conversion(xyz_in, epsg_in, epsg_out)
+        cloud[cst.X] = xyz_in[:, 0]
+        cloud[cst.Y] = xyz_in[:, 1]
+        cloud[cst.Z] = xyz_in[:, 2]
+
+
+def ground_polygon_from_envelopes(
+    poly_envelope1: Polygon,
+    poly_envelope2: Polygon,
+    epsg1: int,
+    epsg2: int,
+    tgt_epsg: int = 4326,
+) -> Tuple[Polygon, Tuple[int, int, int, int]]:
+    """
+    compute the ground polygon of the intersection of two envelopes
+
+    :raise: Exception when the envelopes don't intersect one to each other
+
+    :param poly_envelope1: path to the first envelope
+    :param poly_envelope2: path to the second envelope
+    :param epsg1: EPSG code of poly_envelope1
+    :param epsg2: EPSG code of poly_envelope2
+    :param tgt_epsg: EPSG code of the new projection
+           (default value is set to 4326)
+    :return: a tuple with the shapely polygon of the intersection
+             and the intersection's bounding box
+             (described by a tuple (minx, miny, maxx, maxy))
+    """
+    # project to the correct epsg if necessary
+    if epsg1 != tgt_epsg:
+        poly_envelope1 = polygon_projection(poly_envelope1, epsg1, tgt_epsg)
+
+    if epsg2 != tgt_epsg:
+        poly_envelope2 = polygon_projection(poly_envelope2, epsg2, tgt_epsg)
+
+    # intersect both envelopes
+    if poly_envelope1.intersects(poly_envelope2):
+        inter = poly_envelope1.intersection(poly_envelope2)
+    else:
+        raise RuntimeError("The two envelopes do not intersect one another")
+
+    return inter, inter.bounds
+
+
+# pylint: disable=too-many-positional-arguments
+@cars_profile(name="Ground intersection envelopes", interval=0.5)
+def ground_intersection_envelopes(
+    sensor1,
+    sensor2,
+    geomodel1,
+    geomodel2,
+    geometry_plugin: str,
+    envelope_1_path: str,
+    envelope_2_path: str,
+    out_intersect_path: str,
+    envelope_file_driver: str = "GeoJSON",
+    intersect_file_driver: str = "GPKG",
+) -> Tuple[Polygon, Tuple[int, int, int, int]]:
+    """
+    Compute ground intersection of two images with envelopes:
+    1/ Create envelopes for left, right images
+    2/ Read vectors and polygons with adequate EPSG codes.
+    3/ compute the ground polygon of the intersection of two envelopes
+    4/ Write the GPKG vector
+
+    Returns a shapely polygon and intersection bounding box
+
+    :raise: Exception when the envelopes don't intersect one to each other
+
+    :param sensor1: path to left sensor image
+    :param sensor2: path to right sensor image
+    :param geomodel1: path and attributes for left geomodel
+    :param geomodel2: path and attributes for right geomodel
+    :param geometry_plugin: name of geometry plugin to use
+    :param envelope_1_path: Path to the output shapefile left
+    :param envelope_2_path: Path to the output shapefile right
+    :param dem_dir: Directory containing DEM tiles
+    :param default_alt: Default altitude above ellipsoid
+    :param out_intersect_path: out vector file path to create
+    :param envelope_file_driver: driver used to write envelope files
+    :param intersect_file_driver: driver used to write the intersection file
+    :return: a tuple with the shapely polygon of the intersection
+             and the intersection's bounding box
+             (described by a tuple (minx, miny, maxx, maxy))
+    """
+    geometry_plugin.image_envelope(
+        sensor1, geomodel1, envelope_1_path, envelope_file_driver
+    )
+    geometry_plugin.image_envelope(
+        sensor2, geomodel2, envelope_2_path, envelope_file_driver
+    )
+
+    # Read vectors shapefiles
+    poly1, epsg1 = inputs.read_vector(envelope_1_path)
+    poly2, epsg2 = inputs.read_vector(envelope_2_path)
+
+    # Find polygon intersection from left, right polygons
+    inter_poly, (
+        inter_xmin,
+        inter_ymin,
+        inter_xmax,
+        inter_ymax,
+    ) = ground_polygon_from_envelopes(poly1, poly2, epsg1, epsg2, epsg1)
+
+    # Write intersection file vector from inter_poly
+    outputs.write_vector(
+        [inter_poly], out_intersect_path, epsg1, intersect_file_driver
+    )
+
+    return inter_poly, (inter_xmin, inter_ymin, inter_xmax, inter_ymax)
+
+
+def get_ground_direction(  # pylint: disable=too-many-positional-arguments
+    sensor,
+    geomodel,
+    geometry_plugin: str,
+    x_coord: float = None,
+    y_coord: float = None,
+    z0_coord: float = None,
+    z_coord: float = None,
+) -> np.ndarray:
+    """
+    For a given image (x,y) point, compute the direction vector to ground
+    The function uses the direct localization operation and makes a z
+    variation to get a ground direction vector.
+    By default, (x,y) is put at image center and z0, z at RPC geometric
+    model limits.
+
+    :param TODO
+    :param geometry_plugin: name of geometry plugin to use
+    :param x_coord: X Coordinate in input image sensor
+    :param y_coord: Y Coordinate in input image sensor
+    :param z0_coord: Z altitude reference coordinate
+    :param z_coord: Z Altitude coordinate to take the image
+    :param dem: path to the dem directory
+    :param geoid: path to the geoid file
+    :return: (lat0,lon0,alt0, lat,lon,alt) origin and end vector coordinates
+    """
+    # Define x, y in image center if not defined
+    img_size_x, img_size_y = inputs.rasterio_get_size(sensor)
+
+    if x_coord is None:
+        x_coord = img_size_x / 2
+    if y_coord is None:
+        y_coord = img_size_y / 2
+
+    # Check x, y to be in image
+    assert x_coord >= 0
+    assert x_coord <= img_size_x
+    assert y_coord >= 0
+    assert y_coord <= img_size_y
+
+    # Define z and z0 from img RPC constraints if not defined
+    (min_alt, max_alt) = utils.get_elevation_range_from_metadata(sensor)
+    if z0_coord is None:
+        z0_coord = min_alt
+    if z_coord is None:
+        z_coord = max_alt
+
+    # Check z0 and z to be in RPC constraints
+    assert z0_coord >= min_alt
+    assert z0_coord <= max_alt
+    assert z_coord >= min_alt
+    assert z_coord <= max_alt
+
+    # Get origin vector coordinate with z0 altitude
+    lat0, lon0, alt0 = geometry_plugin.direct_loc(
+        sensor,
+        geomodel,
+        np.array([x_coord]),
+        np.array([y_coord]),
+        np.array([z0_coord]),
+    )
+    # Get end vector coordinate with z altitude
+    lat, lon, alt = geometry_plugin.direct_loc(
+        sensor,
+        geomodel,
+        np.array([x_coord]),
+        np.array([y_coord]),
+        z_coord=np.array([z_coord]),
+    )
+
+    return np.array([lat0, lon0, alt0, lat, lon, alt])
+
+
+def get_ground_angles(  # pylint: disable=too-many-positional-arguments
+    sensor1,
+    sensor2,
+    geomodel1,
+    geomodel2,
+    geometry_plugin,
+    x1_coord: float = None,
+    y1_coord: float = None,
+    z1_0_coord: float = None,
+    z1_coord: float = None,
+    x2_coord: float = None,
+    y2_coord: float = None,
+    z2_0_coord: float = None,
+    z2_coord: float = None,
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
+    """
+    For a given image (x,y) point, compute the Azimuth angle,
+    Elevation angle (not the altitude !) and Range from Ground z0 perspective
+    for both stereo image (img1: left and img2: right)
+
+    Calculate also the convergence angle between the two satellites from ground.
+
+    The function use get_ground_direction function to have coordinates of
+    ground direction vector and compute angles and range.
+
+    Ref: Jeong, Jaehoon. (2017).
+    IMAGING GEOMETRY AND POSITIONING ACCURACY OF DUAL SATELLITE STEREO IMAGES:
+    A REVIEW. ISPRS Annals of Photogrammetry, Remote Sensing and Spatial
+    Information Sciences.
+    IV-2/W4. 235-242. 10.5194/isprs-annals-IV-2-W4-235-2017.
+
+    Perspectives: get bisector  elevation (BIE), and asymmetry angle
+
+    :param TODO
+    :param geometry_plugin: name of geometry plugin to use
+    :param x1_coord: X Coordinate in input left image1  sensor
+    :param y1_coord: Y Coordinate in input left image1 sensor
+    :param z1_0_coord: Left image1 Z altitude origin coordinate for ground
+    direction vector
+    :param z1_coord:  Left image1 Z altitude end coordinate for ground
+    direction vector
+    :param x2_coord: X Coordinate in input right image2 sensor
+    :param y2_coord: Y Coordinate in input right image2 sensor
+    :param z2_0_coord: Right image2 Z altitude origin coordinate for ground
+    direction vector
+    :param z2_coord: Right image2 Z altitude end coordinate for ground
+    direction vector
+    :return: Left Azimuth, Left Elevation Angle,
+    Right Azimuth, Right Elevation Angle, Convergence Angle
+    """
+    # TODO remove ? unused
+
+    # Get image1 <-> satellite vector from image1 metadata geometric model
+    lat1_0, lon1_0, alt1_0, lat1, lon1, alt1 = get_ground_direction(
+        sensor1,
+        geomodel1,
+        geometry_plugin,
+        x1_coord,
+        y1_coord,
+        z1_0_coord,
+        z1_coord,
+    )
+    # Get East North Up vector for left image1
+    x1_e, y1_n, y1_u = enu1 = geo_to_enu(
+        lat1, lon1, alt1, lat1_0, lon1_0, alt1_0
+    )
+    # Convert vector to Azimuth, Elevation, Range (unused)
+    az1, elev_angle1, __ = enu_to_aer(x1_e, y1_n, y1_u)
+
+    # Get image2 <-> satellite vector from image2 metadata geometric model
+    lat2_0, lon2_0, alt2_0, lat2, lon2, alt2 = get_ground_direction(
+        sensor2,
+        geomodel2,
+        geometry_plugin,
+        x2_coord,
+        y2_coord,
+        z2_0_coord,
+        z2_coord,
+    )
+    # Get East North Up vector for right image2
+    x2_e, y2_n, y2_u = enu2 = geo_to_enu(
+        lat2, lon2, alt2, lat2_0, lon2_0, alt2_0
+    )
+    # Convert ENU to Azimuth, Elevation, Range (unused)
+    az2, elev_angle2, __ = enu_to_aer(x2_e, y2_n, y2_u)
+
+    # Get convergence angle from two enu vectors.
+    convergence_angle = np.degrees(utils.angle_vectors(enu1, enu2))
+
+    return az1, elev_angle1, az2, elev_angle2, convergence_angle
+
+
+def get_output_crs(epsg, out_conf):
+    """
+    Détermine le CRS de sortie en fonction de la config.
+    """
+    geoid = out_conf.get("geoid")
+    crs_epsg = CRS(f"EPSG:{epsg}")
+
+    if len(crs_epsg.axis_info) != 2:
+        return crs_epsg  # the user himself set a 3D CRS
+
+    geoid_is_path = isinstance(geoid, str)
+
+    if geoid_is_path:  # user given geoid
+        vepsg = guess_vcrs_from_file_name(geoid)
+        if vepsg is None:
+            custom_wkt = (
+                'VERTCRS["Custom geoid height",'
+                + f'    VDATUM["Custom geoid model (file: {geoid})"],'
+                + "    CS[vertical,1],"
+                + '    AXIS["gravity-related height (h)", up],'
+                + '    LENGTHUNIT["metre", 1, ID["EPSG", 9001]]'
+                "]"
+            )
+            logging.warning(
+                "Could not create a known VCRS from the geoid file."
+            )
+            return CRS.from_wkt(
+                f'COMPOUNDCRS["EPSG:{epsg} + Custom geoid height",'
+                f"    {crs_epsg.to_wkt()},"
+                f"    {custom_wkt}]"
+            )
+        # a vepsg was found using the geoid file
+        return CRS(f"EPSG:{epsg}+{vepsg}")
+
+    if geoid:  # geoid == True
+        return CRS(f"EPSG:{epsg}+5773")
+
+    # geoid == False
+    wgs84_wkt = (
+        'VERTCRS["WGS 84 ellipsoidal height",'
+        + '    VDATUM["WGS 84"],'
+        + "    CS[vertical,1],"
+        + '    AXIS["ellipsoidal height (h)", up],'
+        + '    LENGTHUNIT["metre", 1, ID["EPSG", 9001]]'
+        "]"
+    )
+    logging.warning("The output VCRS is WGS84.")
+
+    return CRS.from_wkt(
+        f'COMPOUNDCRS["EPSG:{epsg} + WGS84 ellipsoidal height",'
+        f"    {crs_epsg.to_wkt()},"
+        f"    {wgs84_wkt}]"
+    )
+
+
+def guess_vcrs_from_file_name(filepath):
+    """
+    Tries to detect the geoid's EPSG from the file name
+    """
+    filename = os.path.basename(filepath).lower()
+
+    known_models = {
+        "egm96": 5773,  # EGM96 height
+        "egm_96": 5773,  # alias
+        "egm 96": 5773,  # alias
+        "egm1996": 5773,  # alias
+        "egm08": 3855,  # EGM2008 height
+        "egm_08": 3855,  # alias
+        "egm 08": 3855,  # alias
+        "egm2008": 3855,  # alias
+    }
+
+    for key, vepsg in known_models.items():
+        if key in filename:
+            return vepsg
+
+    # aucun match connu
+    return None