docs: Update docstrings, minor coding style changes and removing dead code.

Author: hmakelin

docs/pages/developer_guide/integrate/pose_estimators.rst

@@ -53,7 +53,7 @@ You can use the below snippets to get started with your own :class:`.PoseEstimat
 
             import numpy as np
             from typing import Optional, Tuple
-            from gisnav.pose_estimators.pose_estimator import PoseEstimator
+            from gisnav.pose_estimators import PoseEstimator
 
             class MyPoseEstimator(PoseEstimator):
 
@@ -86,7 +86,7 @@ You can use the below snippets to get started with your own :class:`.PoseEstimat
 
             import numpy as np
             from typing import Optional, Tuple
-            from gisnav.pose_estimators.keypoint_pose_estimator import KeypointPoseEstimator
+            from gisnav.pose_estimators import KeypointPoseEstimator
 
             class MyKeypointPoseEstimator(KeypointPoseEstimator):
 

docs/pages/developer_guide/launch/run_individual_nodes.rst

@@ -1,6 +1,6 @@
 Run individual node
 ____________________________________________________
-It is recommended that you :ref:`Launch GISNav using ROS launch files`. However, you can also launch individual nodes
+It is recommended that you :ref:`Launch from ROS launch file`. However, you can also launch individual nodes
 and optionally provide launch values for ROS parameters from the YAML files in the ``launch/params/`` folder:
 
 .. code-block:: bash

gisnav/__init__.py

@@ -0,0 +1 @@
+"""ROS 2 package for estimating airborne drone global position by matching video to map retrieved from onboard GIS server."""

gisnav/assertions.py

@@ -1,5 +1,5 @@
 """Common assertions for convenience"""
-from typing import Optional, Any, Union, Sequence, Collection, Tuple, get_args
+from typing import Any, Union, Sequence, Collection, Tuple
 
 import numpy as np
 
@@ -9,7 +9,6 @@ def assert_type(value: object, type_: Any) -> None:
 
     :param value: Object to check
     :param type_: Type to be asserted
-    :return:
     """
     assert isinstance(value, type_), f'Type {type(value)} provided when {type_} was expected.'
 
@@ -19,43 +18,39 @@ def assert_ndim(value: np.ndarray, ndim: int) -> None:
 
     :param value: Numpy array to check
     :param ndim: Required number of dimensions
-    :return:
     """
     assert value.ndim == ndim, f'Unexpected number of dimensions: {value.ndim} ({ndim} expected).'
 
 
-def assert_len(value: Union[Sequence, Collection], len_: int):
+def assert_len(value: Union[Sequence, Collection], len_: int) -> None:
     """Asserts a specific length for a sequence or a collection (e.g. a list).
 
     :param value: Sequence or collection to check
     :param len_: Required length
-    :return:
     """
     assert len(value) == len_, f'Unexpected length: {len(value)} ({len_} expected).'
 
 
-def assert_shape(value: np.ndarray, shape: tuple):
+def assert_shape(value: np.ndarray, shape: tuple) -> None:
     """Asserts a specific shape for np.ndarray.
 
     :param value: Numpy array to check
     :param shape: Required shape
-    :return:
     """
     assert value.shape == shape, f'Unexpected shape: {value.shape} ({shape} expected).'
 
 
-def assert_pose(pose: Tuple[np.ndarray, np.ndarray]):
+def assert_pose(pose: Tuple[np.ndarray, np.ndarray]) -> None:
     """Asserts that provided tuple is a valid pose (r, t)
 
     :param pose: Tuple consisting of a rotation (3, 3) and translation (3, 1) numpy arrays with valid values
-    :return: True if pose tuple is correctly formatted and valid
     """
     r, t = pose
     assert_rotation_matrix(r)
     assert_shape(t, (3, 1))
 
 
-def assert_rotation_matrix(r: np.ndarray):
+def assert_rotation_matrix(r: np.ndarray) -> None:
     """Asserts that matrix is a valid rotation matrix
 
     Provided matrix of shape (3, 3) with valid values

gisnav/data.py

@@ -24,19 +24,16 @@
 warnings.filterwarnings(action='ignore', category=UserWarning, message='Gimbal lock detected.')
 
 from xml.etree import ElementTree
-from typing import Optional
+from typing import Optional, Tuple
 from dataclasses import dataclass, field
 from collections import namedtuple
-from multiprocessing.pool import AsyncResult
 from scipy.spatial.transform import Rotation
-from geopandas import GeoSeries
 from shapely.geometry import box
+from geographic_msgs.msg import GeoPoint
 
 from gisnav.assertions import assert_type, assert_ndim, assert_shape, assert_len
 from gisnav.geo import GeoPt, GeoTrapezoid, GeoValueError
 
-from geographic_msgs.msg import GeoPoint as ROSGeoPoint
-
 Dim = namedtuple('Dim', 'height width')
 TimePair = namedtuple('TimePair', 'local foreign')
 BBox = namedtuple('BBox', 'left bottom right top')
@@ -50,7 +47,7 @@ class Position:
     .. note::
         (x, y, z) coordinates are in ENU frame
     """
-    xy: GeoPt                    # XY coordinates (e.g. longitude & latitude in WGS84)
+    xy: GeoPt                       # XY coordinates (e.g. longitude & latitude in WGS84)
     altitude: Altitude
     attitude: Optional[Attitude]    # attitude in NED frame
     timestamp: Optional[int]        # Reference timestamp of position
@@ -95,7 +92,7 @@ def to_esd(self) -> Attitude:
 
         :return: Attitude in SED frame
         """
-        nadir_pitch = np.array([0, np.sin(np.pi/4), 0, np.sin(np.pi/4)])  # Adjust origin to nadir facing camera
+        nadir_pitch = np.array([0, np.sin(np.pi / 4), 0, np.sin(np.pi / 4)])  # Adjust origin to nadir facing camera
         r = Rotation.from_quat(self.q) * Rotation.from_quat(nadir_pitch)
         q = r.as_quat()
         q = np.array([q[1], -q[0], q[2], -q[3]])  # NED to ESD
@@ -163,40 +160,40 @@ def __post_init__(self):
 @dataclass(frozen=True)
 class MapData(_ImageHolder):
     """Keeps map frame related data in one place and protects it from corruption."""
-    #bbox: GeoSquare
     bbox: BBox
     elevation: Optional[Img] = None  # Optional elevation raster
 
     def __post_init__(self):
         """Post-initialization validity check."""
         if self.elevation is not None:
             assert self.elevation.arr.shape[0:2], self.image.arr.shape[0:2]
-            assert_ndim(self.elevation.arr, 2) # Grayscale image expected
+            assert_ndim(self.elevation.arr, 2)  # Grayscale image expected
 
 
 # noinspection PyClassHasNoInit
 @dataclass(frozen=True)
 class ContextualMapData(_ImageHolder):
-    """Contains the rotated and cropped map image for _match estimation"""
-    image: Img = field(init=False)  # This is the cropped and rotated map which is same size as the camera frames
+    """Contains the rotated and cropped map image pose estimation"""
+    image: Img = field(init=False)              # Cropped and rotated map which is the same size as the camera frames
     elevation: Optional[Img] = field(init=None) # Rotated elevation raster (optional) in meters
-    rotation: float                 # radians
-    crop: Dim                       # Same value will also be found at image.dim (but not at initialization)
-    map_data: MapData               # This is the original (square) map with padding
+    rotation: float                             # radians
+    crop: Dim                                   # Same value will also be found at image.dim (but not at initialization)
+    map_data: MapData                           # This is the original (square) map with padding
     pix_to_wgs84: np.ndarray = field(init=False)
-    mock_data: bool = False         # Indicates that this was used for field of view guess (mock map data)
-    altitude_scaling: Optional[float] = None # altitude scaling (elevation raster meters -> camera pixels)
+    mock_data: bool = False                     # Indicates that this was used for field of view guess (mock map data)
+    altitude_scaling: Optional[float] = None    # altitude scaling (elevation raster meters -> camera pixels)
 
+    # TODO: update docs - only one transformation is returned
     def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
-        """Returns tuple of affine 2D transformation matrix for converting matched pixel coordinates to WGS84 coordinates
-        along with intermediate transformations
+        """Returns tuple of affine 2D transformation matrix for converting matched pixel coordinates to WGS84
+        coordinates along with intermediate transformations
 
-        These transformations can be used to reverse the rotation and cropping that :func:`~rotate_and_crop_map` did to
+        These transformations can be used to reverse the rotation and cropping that :func:`._rotate_and_crop_map` did to
         the original map.
 
-        :return: Tuple containing 2D affinre transformations from 1. pixel coordinates to WGS84, 2. from original unrotated
-        and uncropped map pixel coordinates to WGS84, 3. from rotated map coordinates to unrotated map coordinates, and 4.
-        from cropped map coordinates to uncropped (but still rotated) map pixel coordinates.
+        :return: Tuple containing 2D affine transformations from 1. pixel coordinates to WGS84, 2. from original
+            unrotated and uncropped map pixel coordinates to WGS84, 3. from rotated map coordinates to unrotated map
+            coordinates, and 4. from cropped map coordinates to uncropped (but still rotated) map pixel coordinates.
         """
         map_dim_arr = np.array(self.map_data.image.dim)
         img_dim_arr = np.array(self.image.dim)
@@ -212,20 +209,16 @@ def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]
         uncropped_to_unrotated = np.vstack((rotation, rotation_padding))
 
         src_corners = create_src_corners(*self.map_data.image.dim)
-        #dst_corners = self.map_data.bbox.to_crs('epsg:4326').coords  # .reshape(-1, 1, 2)
         coords = np.array(box(*self.map_data.bbox).exterior.coords)
         gt = GeoTrapezoid(coords)  # .reshape(-1, 1, 2)
-        #dst_corners = np.flip(coords[:-1], axis=1)  # from ENU frame to WGS 84 axis order
         dst_corners = gt.square_coords
         dst_corners = np.flip(dst_corners, axis=1)  # from ENU frame to WGS 84 axis order
         unrotated_to_wgs84 = cv2.getPerspectiveTransform(np.float32(src_corners).squeeze(),
                                                          np.float32(dst_corners).squeeze())
 
         # Ratio of boundaries in pixels and meters -> Altitude (z) scaling factor
-        #vertical_scaling = abs(self.map_data.bbox.meter_length / \
-        #                       (2*self.map_data.image.dim.width + 2*self.map_data.image.dim.height))
         vertical_scaling = abs(gt.meter_length / \
-                               (2*self.map_data.image.dim.width + 2*self.map_data.image.dim.height))
+                               (2 * self.map_data.image.dim.width + 2 * self.map_data.image.dim.height))
 
         pix_to_wgs84_ = unrotated_to_wgs84 @ uncropped_to_unrotated @ pix_to_uncropped
 
@@ -234,9 +227,7 @@ def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]
         return pix_to_wgs84_  # , unrotated_to_wgs84, uncropped_to_unrotated, pix_to_uncropped
 
     def _rotate_and_crop_map(self, elevation: bool = False) -> np.ndarray:
-        """Rotates map counter-clockwise and then crops a dimensions-sized part from the middle.
-
-        Map needs padding so that a circle with diameter of the diagonal of the img_size rectangle is enclosed in map.
+        """Rotates map counter-clockwise and then crops a dimensions-sized part from the middle
 
         :param elevation: Set True to do rotation on elevation raster instead
         :return: Rotated and cropped map raster
@@ -260,7 +251,7 @@ def _rotate_and_crop_map(self, elevation: bool = False) -> np.ndarray:
 
     @staticmethod
     def _crop_center(img: np.ndarray, dimensions: Dim) -> np.ndarray:
-        """Crops dimensions sized part from center.
+        """Crops dimensions sized part from center
 
         :param img: Image to crop
         :param dimensions: Dimensions of area to crop (not of image itself)
@@ -269,9 +260,8 @@ def _crop_center(img: np.ndarray, dimensions: Dim) -> np.ndarray:
         cx, cy = tuple(np.array(img.shape[0:2]) / 2)
         img_cropped = img[math.floor(cy - dimensions.height / 2):math.floor(cy + dimensions.height / 2),
                       math.floor(cx - dimensions.width / 2):math.floor(cx + dimensions.width / 2)]
-        assert (
-        img_cropped.shape[0:2] == dimensions.height, dimensions.width), 'Something went wrong when cropping the ' \
-                                                                        'map raster. '
+        assert (img_cropped.shape[0:2] == dimensions.height, dimensions.width), \
+            'Something went wrong when cropping the map raster.'
         return img_cropped
 
     def __post_init__(self):
@@ -319,21 +309,6 @@ def __iter__(self):
             yield item
 
 
-# noinspection PyClassHasNoInit
-@dataclass(frozen=True)
-class Snapshot:
-    """Snapshot of vehicle and environment state"""
-    synchronized_time: Optional[int]
-    attitude: Optional[Attitude]
-    gimbal_attitude: Optional[Attitude]
-    gimbal_set_attitude: Optional[Attitude]
-    global_position: Optional[GeoPt]
-    altitude: Optional[Altitude]
-    home_altitude: Optional[Altitude]
-    terrain_altitude: Optional[Altitude]
-    local_frame_origin: Optional[Position]
-
-
 # noinspection PyClassHasNoInit
 @dataclass(frozen=True)
 class Altitude:
@@ -345,7 +320,7 @@ class Altitude:
     home: Above home or starting location
 
     .. note::
-        Altitude AGL should always be known (cannot be None)
+        TODO: Altitude AGL should always be known (cannot be None)
 
     .. seealso::
         `Altitude definitions <https://ardupilot.org/copter/docs/common-understanding-altitude.html>`_
@@ -355,24 +330,12 @@ class Altitude:
     ellipsoid: Optional[float]
     home: Optional[float]
 
+    # TODO
     #def __post_init__(self):
     #    """Post-initialization validity checks"""
     #    assert self.agl is not None
 
 
-
-
-# noinspection PyClassHasNoInit
-@dataclass(frozen=True)
-class InputData:
-    """InputData of vehicle state and other variables needed for postprocessing pose estimates"""
-    r_guess: Optional[np.ndarray]
-    """Camera expected attitude"""
-
-    snapshot: Snapshot
-    """Snapshot of vehicle and environment state"""
-
-
 # noinspection PyClassHasNoInit
 @dataclass
 class FOV:
@@ -418,16 +381,16 @@ def __post_init__(self):
 class FixedCamera:
     """WGS84-fixed camera attributes
 
-    Collects field of view and map_match under a single structure that is intended to be stored in input data context as
-    visual odometry fix reference. Includes the needed map_match and pix_to_wgs84 transformation for the vo fix.
+    # TODO: refactor this class out - it was used earlier for visual odometry but is now redundant
+
+    Collects field of view and map_match under a single structure.
     """
     image_pair: ImagePair
     pose: Pose
     timestamp: int
-    #snapshot: Snapshot
     terrain_altitude_amsl: Optional[float]
     terrain_altitude_ellipsoid: Optional[float]
-    home_position: Optional[ROSGeoPoint]
+    home_position: Optional[GeoPoint]
     fov: FOV = field(init=False)
     position: Position = field(init=False)
     h: np.ndarray = field(init=False)
@@ -459,7 +422,7 @@ def _estimate_fov(self) -> Optional[FOV]:
             # Could not create a valid FOV
             return None
 
-    def _estimate_attitude(self) -> np.ndarray:
+    def _estimate_attitude(self) -> Attitude:
         """Estimates gimbal (not vehicle) attitude in NED frame
 
         .. note::
@@ -473,7 +436,6 @@ def _estimate_attitude(self) -> np.ndarray:
         assert not np.isnan(rT).any()
         gimbal_estimated_attitude = Rotation.from_matrix(rT)  # rotated map pixel frame
 
-        #gimbal_estimated_attitude *= Rotation.from_rotvec(-(np.pi/2) * np.array([0, 1, 0]))  # camera body
         gimbal_estimated_attitude *= Rotation.from_rotvec(
             self.image_pair.ref.rotation * np.array([0, 0, 1]))  # unrotated map pixel frame
 
@@ -489,7 +451,7 @@ def _get_fov_and_c(img_arr_shape: Tuple[int, int], h_mat: np.ndarray) -> Tuple[n
 
         :param img_arr_shape: Image array shape tuple (height, width)
         :param h_mat: Homography matrix
-        :return: Tuple of FOV corner coordinates and prinicpal point np.ndarrays
+        :return: Tuple of FOV corner coordinates and principal point np.ndarrays
         """
         assert_type(img_arr_shape, tuple)
         assert_len(img_arr_shape, 2)
@@ -519,13 +481,12 @@ def _estimate_position(self, terrain_altitude_amsl: Optional[float], terrain_alt
         :param terrain_altitude_amsl: Optional ground elevation above AMSL in meters
         :param terrain_altitude_ellipsoid: Optional ground elevation above WGS 84 ellipsoid in meters
         :param crs: CRS to use for the Position
-        :return: Camera position
+        :return: Camera position or None if not available
         """
         assert self.fov is not None  # Call _estimate_fov before _estimate_position!
         # Translation in WGS84 (and altitude or z-axis translation in meters above ground)
         assert_type(self.image_pair.ref, ContextualMapData)  # need pix_to_wgs84
         t_wgs84 = self.image_pair.ref.pix_to_wgs84 @ np.append(self.camera_position[0:2], 1)
-        #t_wgs84[2] = -self.image_pair.ref.pix_to_wgs84[2][2] * self.camera_position[2]  # In NED frame z-coordinate is negative above ground, make altitude >0
         t_wgs84[2] = -self.fov.scaling * self.camera_position[2]  # In NED frame z-coordinate is negative above ground, make altitude >0
 
         # Check that we have all the values needed for a global position
@@ -560,9 +521,6 @@ def __post_init__(self):
             raise DataValueError('Please provide valid image pair.')
 
         img = self.image_pair.qry
-        #if self.snapshot.terrain_altitude.amsl is not None and self.snapshot.terrain_altitude.ellipsoid is None or \
-        #    self.snapshot.terrain_altitude.amsl is not None and self.snapshot.terrain_altitude.ellipsoid is None:
-        #        raise DataValueError('Please provide terrain altitude in both AMSL and above WGS 84 ellipsoid.')
         if self.terrain_altitude_amsl is not None and self.terrain_altitude_ellipsoid is None or \
             self.terrain_altitude_amsl is not None and self.terrain_altitude_ellipsoid is None:
                 raise DataValueError('Please provide terrain altitude in both AMSL and above WGS 84 ellipsoid.')