Merge pull request #17 from hmakelin/hmakelin-height

Enable use of elevation data for pose estimation

Author: hmakelin

README.md

@@ -27,7 +27,7 @@ You will need to have [NVIDIA Container Toolkit][2] for Docker installed.
 ```bash
 git clone https://github.com/hmakelin/gisnav-docker.git
 cd gisnav-docker
-docker-compose build --build-arg WITH_GISNAV px4-sitl
+docker-compose build px4-sitl
 ```
 
 > **Note** The build for the `px4-sitl` image takes a long time, especially if you are building it for the first time.

config/typhoon_h480__ksql_airport.yaml

@@ -4,14 +4,15 @@ mock_gps_node:
       url: 'http://localhost:80/wms'
       version: '1.3.0'
       layers: ['imagery']
+      elevation_layers: ['osm-buildings']
       styles: ['']
       srs: 'EPSG:4326'
       image_format: 'image/jpeg'
       request_timeout: 10
     misc:
       attitude_deviation_threshold: 10 # degrees
       max_pitch: 30  # 30  # Used by _should_match
-      min_match_altitude: 80
+      min_match_altitude: 50
     map_update:
       update_delay: 1  # Keep low (e.g. 1 second) if vehicle flying fast or gimbal_projection: True
       gimbal_projection: True

docs/pages/developer_guide.rst

@@ -247,7 +247,7 @@ You can use the below snippets to get started with your own :class:`.PoseEstimat
             # TODO: implement initializer
             raise NotImplementedError
 
-        def estimate_pose(query, reference, k, guess):
+        def estimate_pose(query, reference, k, guess = None, elevation_ref = None):
             """Returns pose between query and reference images"""
             # Do your pose estimation magic here
             #r = ...  # np.ndarray of shape (3, 3)
@@ -271,7 +271,7 @@ If you want to create a :class:`.KeypointPoseEstimator`, you can also start with
 
     class MyPoseEstimator(KeypointPoseEstimator):
 
-        def __init__(self, ):
+        def __init__(self):
             # TODO: implement initializer
             raise NotImplementedError
 

gisnav/data.py

@@ -160,19 +160,28 @@ def __post_init__(self):
 @dataclass(frozen=True)
 class MapData(_ImageHolder):
     """Keeps map frame related data in one place and protects it from corruption."""
-    bbox: GeoBBox
+    bbox: GeoSquare
+    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
 
 
 # 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
+    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
     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)
 
     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
@@ -214,17 +223,19 @@ 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) -> np.ndarray:
+    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.
 
+        :param elevation: Set True to do rotation on elevation raster instead
         :return: Rotated and cropped map raster
         """
-        cx, cy = tuple(np.array(self.map_data.image.arr.shape[0:2]) / 2)
+        image = self.map_data.image if not elevation else self.map_data.elevation
+        cx, cy = tuple(np.array(image.arr.shape[0:2]) / 2)
         degrees = math.degrees(self.rotation)
         r = cv2.getRotationMatrix2D((cx, cy), degrees, 1.0)
-        map_rotated = cv2.warpAffine(self.map_data.image.arr, r, self.map_data.image.arr.shape[1::-1])
+        map_rotated = cv2.warpAffine(image.arr, r, image.arr.shape[1::-1])
         map_cropped = self._crop_center(map_rotated, self.crop)
         #if visualize:
             #cv2.imshow('padded', self.map_data.image.arr)
@@ -256,6 +267,10 @@ def _crop_center(img: np.ndarray, dimensions: Dim) -> np.ndarray:
     def __post_init__(self):
         """Set computed fields after initialization."""
         object.__setattr__(self, 'image', Img(self._rotate_and_crop_map()))
+        if self.map_data.elevation is not None:
+            object.__setattr__(self, 'elevation', Img(self._rotate_and_crop_map(True)))
+        else:
+            object.__setattr__(self, 'elevation', None)
         object.__setattr__(self, 'pix_to_wgs84', self._pix_to_wgs84())
 
 

gisnav/nodes/base_node.py

@@ -26,7 +26,7 @@
 from gisnav.data import Dim, TimePair, ImageData, MapData, CameraData, Attitude, DataValueError, \
     InputData, ImagePair, AsyncPoseQuery, AsyncWMSQuery, ContextualMapData, FixedCamera, Img, Pose, Position
 from gisnav.geo import GeoPoint, GeoSquare, GeoTrapezoid
-from gisnav.assertions import assert_type, assert_len
+from gisnav.assertions import assert_type, assert_len, assert_ndim, assert_shape
 from gisnav.pose_estimators.pose_estimator import PoseEstimator
 from gisnav.wms import WMSClient
 
@@ -61,6 +61,14 @@ class BaseNode(Node, ABC):
         have just one layer for high resolution aerial or satellite imagery.
     """
 
+    ROS_D_WMS_ELEVATION_LAYERS = []
+    """Default WMS request elevation layers parameter
+
+    .. note::
+        This is an optional elevation layer that makes the pose estimation more accurate especially when flying at low 
+        altitude. It is assumed to be grayscale.
+    """
+
     ROS_D_WMS_STYLES = ['']
     """Default WMS request styles parameter
     
@@ -472,6 +480,16 @@ def _gimbal_device_set_attitude(self, value: Optional[GimbalDeviceSetAttitude])
     # endregion
 
     # region Computed Properties
+    @property
+    def _altitude_scaling(self) -> Optional[float]:
+        """Returns camera focal length divided by camera altitude in meters."""
+        if self._camera_data is not None and self._altitude_agl is not None:
+            return self._camera_data.fx / self._altitude_agl
+        else:
+            self.get_logger().warn('Could not estimate elevation scale because camera focal length and/or vehicle '
+                                   'altitude is unknown.')
+            return None
+
     @property
     def _r_guess(self) -> Optional[np.ndarray]:
         """Gimbal rotation matrix guess (based on :class:`px4_msgs.GimbalDeviceSetAttitude` message)
@@ -679,7 +697,8 @@ def _estimation_inputs(self) -> Optional[Tuple[InputData, ContextualMapData]]:
             self.get_logger().warn(f'Camera yaw unknown, cannot estimate pose.')
             return
 
-        contextual_map_data = ContextualMapData(rotation=camera_yaw, map_data=self._map_data, crop=self._img_dim)
+        contextual_map_data = ContextualMapData(rotation=camera_yaw, map_data=self._map_data, crop=self._img_dim,
+            altitude_scaling=self._altitude_scaling)
         return input_data, contextual_map_data
     # endregion
 
@@ -932,7 +951,7 @@ def _gimbal_device_set_attitude_callback(self, msg: GimbalDeviceSetAttitude) ->
     # endregion
 
     #region WMSWorkerCallbacks
-    def _wms_pool_worker_callback(self, result: List[MapData]) -> None:
+    def _wms_pool_worker_callback(self, result: Tuple[np.ndarray, Optional[np.ndarray]]) -> None:
         """Handles result from :meth:`gisnav.wms.worker`.
 
         Saves received result to :py:attr:`~_map_data. The result should be a collection containing a single
@@ -941,11 +960,17 @@ def _wms_pool_worker_callback(self, result: List[MapData]) -> None:
         :param result: Results from the asynchronous call
         :return:
         """
-        map_ = result
+        map_, elevation = result
         assert_type(map_, np.ndarray)
+        if elevation is not None:
+            assert_ndim(elevation, 2)
+            assert_shape(elevation, map_.shape[0:2])
+
         # Should already have received camera info so _map_size_with_padding should not be None
         assert map_.shape[0:2] == self._map_size_with_padding, 'Decoded map is not of specified size.'
-        map_data = MapData(bbox=self._wms_query.geobbox, image=Img(map_))
+
+        elevation = Img(elevation) if elevation is not None else None
+        map_data = MapData(bbox=self._wms_query.geobbox, image=Img(map_), elevation=elevation)
         self.get_logger().info(f'Map received for bbox: {map_data.bbox.bounds}.')
         self._map_data = map_data
 
@@ -1024,6 +1049,7 @@ def _request_new_map(self, bbox: GeoSquare) -> None:
 
         # Build and send WMS request
         layers = self.get_parameter('wms.layers').get_parameter_value().string_array_value
+        elevation_layers = self.get_parameter('wms.elevation_layers').get_parameter_value().string_array_value
         styles = self.get_parameter('wms.styles').get_parameter_value().string_array_value
         srs_str = self.get_parameter('wms.srs').get_parameter_value().string_value
         image_format = self.get_parameter('wms.image_format').get_parameter_value().string_value
@@ -1036,10 +1062,13 @@ def _request_new_map(self, bbox: GeoSquare) -> None:
                                                                           f'results were not yet ready.'
         self.get_logger().info(f'Requesting map for bbox: {bbox.bounds}, layers: {layers}, srs: {srs_str}, format: '
                                f'{image_format}.')
+        args = [layers, styles, bbox.bounds, self._map_size_with_padding, srs_str, image_format]
+        if len(elevation_layers) > 0:
+            args.append(elevation_layers)
         self._wms_query = AsyncWMSQuery(
             result=self._wms_pool.apply_async(
                 WMSClient.worker,
-                (layers, styles, bbox.bounds, self._map_size_with_padding, srs_str, image_format),
+                tuple(args),
                 callback=self._wms_pool_worker_callback,
                 error_callback=self._wms_pool_worker_error_callback
             ),
@@ -1296,10 +1325,18 @@ def _estimate(self, image_pair: ImagePair, input_data: InputData) -> None:
         """
         assert self._pose_estimation_query is None or self._pose_estimation_query.result.ready()
         pose_guess = None if self._pose_guess is None else tuple(self._pose_guess)
+
+        # Scale elevation raster from meters to camera native pixels
+        elevation = image_pair.ref.elevation.arr if image_pair.ref.elevation is not None else None
+        if elevation is not None:
+            elevation = elevation * image_pair.ref.altitude_scaling if image_pair.ref.altitude_scaling is not None \
+                else None
+
         self._pose_estimation_query = AsyncPoseQuery(
             result=self._pose_estimator_pool.apply_async(
                 self._pose_estimator.worker,
-                (image_pair.qry.image.arr, image_pair.ref.image.arr, image_pair.qry.camera_data.k, pose_guess),
+                (image_pair.qry.image.arr, image_pair.ref.image.arr, image_pair.qry.camera_data.k, pose_guess,
+                 elevation),
                 callback=self._pose_estimation_worker_callback,
                 error_callback=self._pose_estimation_worker_error_callback
             ),

gisnav/pose_estimators/keypoint_pose_estimator.py

@@ -42,7 +42,9 @@ def _find_matching_keypoints(self, query: np.ndarray, reference: np.ndarray) \
         pass
 
     def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray,
-                      guess: Optional[Tuple[np.ndarray, np.ndarray]]) -> Optional[Tuple[np.ndarray, np.ndarray]]:
+                      guess: Optional[Tuple[np.ndarray, np.ndarray]] = None,
+                      elevation_reference: Optional[np.ndarray] = None) \
+            -> Optional[Tuple[np.ndarray, np.ndarray]]:
         """Returns pose between given query and reference images, or None if no pose could not be estimated
 
         Uses :class:`._find_matching_keypoints` to estimate matching keypoints before estimating pose.
@@ -51,15 +53,27 @@ def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray,
         :param reference: The second (reference) image for pose estimation
         :param k: Camera intrinsics matrix (3, 3)
         :param guess: Optional initial guess for camera pose
+        :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale)
         :return: Pose tuple consisting of a rotation (3, 3) and translation (3, 1) np.ndarrays
         """
+        if elevation_reference is not None:
+            assert elevation_reference.shape[0:2] == reference.shape[0:2]
         matched_keypoints = self._find_matching_keypoints(query, reference)
         if matched_keypoints is None or len(matched_keypoints[0]) < self._min_matches:
             return None  # Not enough matching keypoints found
 
         mkp1, mkp2 = matched_keypoints
-        padding = np.array([[0]] * len(mkp1))
-        mkp2_3d = np.hstack((mkp2, padding))  # Set world z-coordinates to zero
+
+        if elevation_reference is None:
+            # Set world z values to zero (assume planar ground surface and estimate homography)
+            z_values = np.array([[0]] * len(mkp1))
+        else:
+            # Z values are assumed to be in camera native coordinates (not in meters)
+            x, y = np.transpose(np.floor(mkp2).astype(int))  # do floor first before converting to int
+            z_values = elevation_reference[y, x].reshape(-1, 1)  # y axis first
+
+        mkp2_3d = np.hstack((mkp2, z_values))
+
         dist_coeffs = np.zeros((4, 1))
         use_guess = guess is not None
         r, t = guess if use_guess else (None, None)

gisnav/pose_estimators/pose_estimator.py

@@ -33,21 +33,23 @@ def initializer(class_: type, *args) -> None:
 
     @staticmethod
     def worker(query: np.ndarray, reference: np.ndarray, k: np.ndarray,
-               guess: Optional[Tuple[np.ndarray, np.ndarray]]) -> Optional[Tuple[np.ndarray, np.ndarray]]:
+               guess: Optional[Tuple[np.ndarray, np.ndarray]] = None,
+               elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]:
         """Returns pose between provided images, or None if pose cannot be estimated
 
         :param query: The first (query) image for pose estimation
         :param reference: The second (reference) image for pose estimation
         :param k: Camera intrinsics matrix of shape (3, 3)
         :param guess: Optional initial guess for camera pose
+        :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale)
         :return: Pose tuple consisting of a rotation (3, 3) and translation (3, 1) numpy arrays
         """
         pose_estimator: PoseEstimator = globals()[PoseEstimator.POSE_ESTIMATOR_GLOBAL_VAR]
 
         if guess is not None:
             assert_pose(guess)
 
-        pose = pose_estimator.estimate_pose(query, reference, k, guess)
+        pose = pose_estimator.estimate_pose(query, reference, k, guess, elevation_reference)
 
         # Do independent check - child class might not check their output
         if pose is not None:
@@ -57,13 +59,15 @@ def worker(query: np.ndarray, reference: np.ndarray, k: np.ndarray,
 
     @abstractmethod
     def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray,
-                      guess: Optional[Tuple[np.ndarray, np.ndarray]]) -> Optional[Tuple[np.ndarray, np.ndarray]]:
+                      guess: Optional[Tuple[np.ndarray, np.ndarray]] = None,
+                      elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]:
         """Returns pose between provided images, or None if pose cannot be estimated
 
         :param query: The first (query) image for pose estimation
         :param reference: The second (reference) image for pose estimation
         :param k: Camera intrinsics matrix of shape (3, 3)
         :param guess: Optional initial guess for camera pose
+        :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale)
         :return: Pose tuple of rotation (3, 3) and translation (3, 1) numpy arrays, or None if could not estimate
         """
         pass

gisnav/wms.py

@@ -4,11 +4,12 @@
 import numpy as np
 import cv2
 
-from typing import Tuple, List
+from typing import Tuple, List, Optional
 from owslib.wms import WebMapService
 from owslib.util import ServiceException
 
 from gisnav.assertions import assert_type, assert_ndim
+from gisnav.data import MapData, Img
 
 
 class WMSClient:
@@ -62,17 +63,22 @@ def initializer(url: str, version_: str, timeout_: int) -> None:
             globals()[WMSClient.WMS_CLIENT_GLOBAL_VAR] = WMSClient(url, version_, timeout_)
 
     @staticmethod
-    def worker(layers: List[str], styles: List[str], bbox: Tuple[float], map_size: Tuple[int, int], srs_str: str, image_format: str) \
-            -> np.ndarray:
-        """Requests one or more map layers from the WMS server
+    def worker(layers: List[str], styles: List[str], bbox: Tuple[float], map_size: Tuple[int, int], srs_str: str,
+               image_format: str, elevation_layers: Optional[List[str]] = None) \
+            -> Tuple[np.ndarray, Optional[np.ndarray]]:
+        """Requests one or more map imagery and optional height raster layers from the WMS server
+        
+        TODO: Currently no support for separate arguments for imagery and height layers. Assumes height layer is 
+        available at same styles and CRS as imagery layer.
 
         :param layers: List of requested map layers
         :param styles: Optional styles of same length as layers, use empty strings for default styles
         :param bbox: Bounding box of the map as tuple (left, bottom, right, top)
         :param map_size: Map size tuple (height, width)
         :param srs_str: WMS server SRS
         :param image_format: WMS server requested image format
-        :return: Requested layers stacked into a numpy array raster
+        :param elevation_layers: Optional layers for height raster
+        :return: Tuple of imagery and height (Optional) rasters
         """
         assert WMSClient.WMS_CLIENT_GLOBAL_VAR in globals()
         wms_client: WMSClient = globals()[WMSClient.WMS_CLIENT_GLOBAL_VAR]
@@ -89,4 +95,21 @@ def worker(layers: List[str], styles: List[str], bbox: Tuple[float], map_size: T
         map_ = cv2.imdecode(map_, cv2.IMREAD_UNCHANGED)
         assert_type(map_, np.ndarray)
         assert_ndim(map_, 3)
-        return map_
+
+        elevation = None
+        if elevation_layers is not None:
+            try:
+                elevation = wms_client._wms.getmap(layers=elevation_layers, styles=styles, srs=srs_str, bbox=bbox,
+                                                   size=map_size, format=image_format,
+                                                   transparent=False)
+            except ServiceException as _:
+                # TODO: handle exception for height layer
+                pass
+
+        if elevation is not None:
+            elevation = np.frombuffer(elevation.read(), np.uint8)  # TODO: handle values above 255?
+            elevation = cv2.imdecode(elevation, cv2.IMREAD_GRAYSCALE)
+            assert_type(elevation, np.ndarray)
+            assert_ndim(elevation, 2)  # Grayscale, no color channels
+
+        return map_, elevation