Merge pull request #120 from hmakelin/enable-vo

Enable visual odometry

docker/px4/6011_typhoon_h480

@@ -16,8 +16,8 @@ param set-default EKF2_REQ_VDRIFT 1.5   # max value
 param set-default EKF2_REQ_SACC 5.0     # max value
 param set-default EKF2_HEAD_NOISE 1.0   # max value
 param set-default SENS_GPS_MASK 7
-param set-default EKF2_GPS_CTRL 8       # lon/lat, alt, 3D vel, heading
-param set-default EKF2_GPS_CHECK 0
+param set-default EKF2_GPS_CTRL 15      # max: lon/lat, alt, 3D vel, heading
+#param set-default EKF2_GPS_CHECK 0
 
 # Secondary GPS (GISNav) at GPS 2 serial port via NMEA
 # https://docs.px4.io/v1.14/en/hardware/serial_port_mapping.html#default-px4board

gisnav/gisnav/_transformations.py

@@ -14,6 +14,7 @@
     PoseWithCovarianceStamped,
     Quaternion,
     TransformStamped,
+    TwistWithCovarianceStamped,
 )
 from pyproj import Proj, transform
 from rclpy.node import Node
@@ -411,3 +412,80 @@ def enu_to_ecef_matrix(lon: float, lat: float) -> np.ndarray:
         ]
     )
     return R
+
+
+def poses_to_twist(
+    pose2: PoseWithCovarianceStamped, pose1: PoseStamped
+) -> TwistWithCovarianceStamped:
+    """pose2 is newer pose with covariances, pose1 is older pose (identity pose in
+    VO)
+    """
+    # Time step between poses
+    t2, t1 = usec_from_header(pose2.header), usec_from_header(pose1.header)
+
+    time_step = (t2 - t1) / 1e6  # seconds
+
+    # Calculate linear velocities
+    dx = pose2.pose.pose.position.x - pose1.pose.position.x
+    dy = pose2.pose.pose.position.y - pose1.pose.position.y
+    dz = pose2.pose.pose.position.z - pose1.pose.position.z
+
+    # Calculate angular velocities using quaternion differences
+    q1 = [
+        pose1.pose.orientation.x,
+        pose1.pose.orientation.y,
+        pose1.pose.orientation.z,
+        pose1.pose.orientation.w,
+    ]
+    q2 = [
+        pose2.pose.pose.orientation.x,
+        pose2.pose.pose.orientation.y,
+        pose2.pose.pose.orientation.z,
+        pose2.pose.pose.orientation.w,
+    ]
+
+    q_diff = tf_transformations.quaternion_multiply(
+        q2, tf_transformations.quaternion_inverse(q1)
+    )
+
+    # Converting quaternion to rotation vector (axis-angle)
+    angle = 2 * np.arccos(q_diff[3])  # Compute the rotation angle
+    axis = q_diff[:3] / np.sin(angle / 2)  # Normalize the axis
+    rotation_vector = angle * axis  # Multiply angle by the normalized axis
+
+    ang_vel = rotation_vector / time_step
+
+    # Create TwistStamped message
+    twist = TwistWithCovarianceStamped()
+    twist.header.stamp = pose2.header.stamp
+    twist.header.frame_id = pose2.header.frame_id
+    twist.twist.twist.linear.x = dx / time_step
+    twist.twist.twist.linear.y = dy / time_step
+    twist.twist.twist.linear.z = dz / time_step
+    twist.twist.twist.angular.x = ang_vel[0]
+    twist.twist.twist.angular.y = ang_vel[1]
+    twist.twist.twist.angular.z = ang_vel[2]
+
+    twist.twist.covariance = pose2.pose.covariance  # todo: could make these smaller?
+
+    return twist
+
+
+def create_identity_pose_stamped(x, y, z):
+    pose_stamped = PoseStamped()
+
+    pose_stamped.header.stamp = rclpy.time.Time().to_msg()
+    pose_stamped.header.frame_id = "camera_optical"
+
+    # Set the position coordinates
+    pose_stamped.pose.position.x = x
+    pose_stamped.pose.position.y = y
+    pose_stamped.pose.position.z = z
+
+    # Set the orientation to identity (no rotation)
+    pose_stamped.pose.orientation.x = 0.0
+    pose_stamped.pose.orientation.y = 0.0
+    pose_stamped.pose.orientation.z = 0.0
+    pose_stamped.pose.orientation.w = 1.0
+
+    return pose_stamped

gisnav/gisnav/constants.py

@@ -64,9 +64,9 @@
 :attr:`.StereoNode.pose`.
 
 """
-ROS_TOPIC_RELATIVE_QUERY_POSE: Final = "~/vo/pose"
+ROS_TOPIC_RELATIVE_QUERY_TWIST: Final = "~/vo/twist"
 """Relative :term:`topic` into which :class:`.StereoNode` publishes
-:attr:`.StereoNode.camera_optical_pose_in_query_frame`.
+:attr:`.StereoNode.camera_optical_twist_in_query_frame`.
 """
 
 MAVROS_TOPIC_TIME_REFERENCE: Final = "/mavros/time_reference"

gisnav/gisnav/core/pose_node.py

@@ -8,7 +8,7 @@
 The pose is estimated by finding matching keypoints between the query and
 reference images and then solving the resulting :term:`PnP` problem.
 """
-from typing import Optional, Tuple, Union, cast
+from typing import Final, Optional, Tuple, Union, cast
 
 import cv2
 import numpy as np
@@ -18,11 +18,16 @@
 import torch
 from builtin_interfaces.msg import Time
 from cv_bridge import CvBridge
-from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped
+from geometry_msgs.msg import (
+    PoseWithCovariance,
+    PoseWithCovarianceStamped,
+    TwistWithCovarianceStamped,
+)
 from kornia.feature import DISK, LightGlueMatcher, laf_from_center_scale_ori
 from rclpy.node import Node
 from rclpy.qos import QoSPresetProfiles
 from robot_localization.srv import SetPose
+from scipy.interpolate import interp1d
 from sensor_msgs.msg import CameraInfo, Image, TimeReference
 
 from gisnav_msgs.msg import (  # type: ignore[attr-defined]
@@ -38,7 +43,7 @@
     ROS_TOPIC_CAMERA_INFO,
     ROS_TOPIC_RELATIVE_POSE,
     ROS_TOPIC_RELATIVE_POSE_IMAGE,
-    ROS_TOPIC_RELATIVE_QUERY_POSE,
+    ROS_TOPIC_RELATIVE_QUERY_TWIST,
     ROS_TOPIC_RELATIVE_TWIST_IMAGE,
     STEREO_NODE_NAME,
     FrameID,
@@ -74,6 +79,39 @@ class PoseNode(Node):
     MIN_MATCHES = 30
     """Minimum number of keypoint matches before attempting pose estimation"""
 
+    class ScalingBuffer:
+        """Maintains timestamped query frame to world frame scaling in a sliding windown
+        buffer so that shallow matching (VO) pose can be scaled to meters using
+        scaling information obtained from deep matching
+        """
+
+        _WINDOW_LENGTH: Final = 100
+
+        def __init__(self):
+            self._scaling_arr: np.ndarray = np.ndarray([])
+            self._timestamp_arr: np.ndarray = np.ndarray([])
+
+        def append(self, timestamp_usec: int, scaling: float) -> None:
+            self._scaling_arr = np.append(self._scaling_arr, scaling)
+            self._timestamp_arr = np.append(self._timestamp_arr, timestamp_usec)
+
+            if self._scaling_arr.size > self._WINDOW_LENGTH:
+                self._scaling_arr[-self._WINDOW_LENGTH :]
+            if self._timestamp_arr.size > self._WINDOW_LENGTH:
+                self._timestamp_arr[-self._WINDOW_LENGTH :]
+
+        def interpolate(self, timestamp_usec: int) -> Optional[float]:
+            if self._scaling_arr.size < 2:
+                return None
+
+            interp_function = interp1d(
+                self._timestamp_arr,
+                self._scaling_arr,
+                kind="linear",
+                fill_value="extrapolate",
+            )
+            return interp_function(timestamp_usec)
+
     def __init__(self, *args, **kwargs):
         """Class initializer
 
@@ -114,7 +152,7 @@ def __init__(self, *args, **kwargs):
         # initialize publishers (for launch tests)
         self.pose
         # TODO method does not support none input
-        self.camera_optical_pose_in_query_frame(None)
+        self.camera_optical_twist_in_camera_optical_frame(None)
 
         self._tf_buffer = tf2_ros.Buffer()
         self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self)
@@ -127,6 +165,8 @@ def __init__(self, *args, **kwargs):
         self._set_pose_request = SetPose.Request()
         self._pose_sent = False
 
+        self._scaling_buffer = self.ScalingBuffer()
+
     def _set_initial_pose(self, pose):
         if not self._pose_sent:
             self._set_pose_request.pose = pose
@@ -160,7 +200,7 @@ def _pose_image_cb(self, msg: Image) -> None:
 
     def _twist_image_cb(self, msg: Image) -> None:
         """Callback for :attr:`.twist_image` message"""
-        self.camera_optical_pose_in_query_frame(self.twist_image)
+        self.camera_optical_twist_in_camera_optical_frame(self.twist_image)
 
     @property
     @ROS.publish(
@@ -202,6 +242,7 @@ def _get_pose(
         r, t = pose
 
         r_inv = r.T
+
         camera_optical_position_in_world = -r_inv @ t
 
         tf_.visualize_camera_position(
@@ -212,15 +253,38 @@ def _get_pose(
             f"{'shallow' if shallow_inference else 'deep'} inference",
         )
 
+        if isinstance(msg, MonocularStereoImage):
+            scaling = self._scaling_buffer.interpolate(
+                tf_.usec_from_header(msg.query.header)
+            )
+            if scaling is not None:
+                camera_optical_position_in_world = (
+                    camera_optical_position_in_world * scaling
+                )
+            else:
+                self.get_logger().debug(
+                    "No scaling availble for VO - will not return VO pose"
+                )
+                return None
+
         pose = tf_.create_pose_msg(
             msg.query.header.stamp,
-            cast(FrameID, "earth"),
+            cast(FrameID, "earth")
+            if isinstance(msg, OrthoStereoImage)
+            else cast(FrameID, "camera_optical"),
             r_inv,
             camera_optical_position_in_world,
         )
 
         if isinstance(msg, OrthoStereoImage):
             affine = tf_.proj_to_affine(msg.crs.data)
+
+            # use z scaling value
+            usec = tf_.usec_from_header(msg.query.header)
+            scaling = np.abs(affine[2, 2])
+            if usec is not None and scaling is not None:
+                self._scaling_buffer.append(usec, scaling)
+
             t_wgs84 = affine @ np.append(camera_optical_position_in_world, 1)
             x, y, z = tf_.wgs84_to_ecef(*t_wgs84.tolist())
             pose.pose.position.x = x
@@ -254,16 +318,33 @@ def _get_pose(
         return pose_with_covariance
 
     @ROS.publish(
-        ROS_TOPIC_RELATIVE_QUERY_POSE,
+        ROS_TOPIC_RELATIVE_QUERY_TWIST,
         QoSPresetProfiles.SENSOR_DATA.value,
     )
     # @ROS.transform("camera_optical")  # TODO: enable after scaling to meters
     @narrow_types
-    def camera_optical_pose_in_query_frame(
+    def camera_optical_twist_in_camera_optical_frame(
         self, msg: MonocularStereoImage
-    ) -> Optional[PoseWithCovarianceStamped]:
+    ) -> Optional[TwistWithCovarianceStamped]:
         """Camera pose in visual odometry world frame (i.e. previous query frame)"""
-        return self._get_pose(msg)
+        scaling = self._scaling_buffer.interpolate(
+            tf_.usec_from_header(msg.query.header)
+        )
+        if scaling is None:
+            return None
+
+        x, y, z = (
+            scaling * 320.0,
+            scaling * 180.0,
+            scaling * -205.0,
+        )  # todo do not hard code
+        previous_pose = tf_.create_identity_pose_stamped(x, y, z)
+        previous_pose.header = msg.reference.header
+        current_pose = self._get_pose(msg)
+        if current_pose is not None:
+            return tf_.poses_to_twist(current_pose, previous_pose)
+        else:
+            return None
 
     @property
     # @ROS.max_delay_ms(DELAY_DEFAULT_MS)

gisnav/gisnav/core/stereo_node.py

@@ -71,7 +71,7 @@ def __init__(self, *args, **kwargs) -> None:
         # setup publisher to pass launch test without image callback being
         # triggered
         self.pose_image
-        # self.twist_image  # todo re-enable once scaling is solved
+        self.twist_image
 
         # Initialize the transform broadcaster and listener
         self._tf_buffer = tf2_ros.Buffer()
@@ -99,8 +99,7 @@ def _image_cb(self, msg: Image) -> None:
         """Callback for :attr:`.image` message"""
         self.pose_image  # publish rotated and cropped orthoimage stack
 
-        # TODO: re-enable once proper scaling is implemented for VO
-        # self.twist_image  # publish two subsequent images for VO
+        self.twist_image  # publish two subsequent images for VO
 
         # TODO this is brittle - nothing is enforcing that this is assigned after
         #  publishing stereo_image

gisnav/launch/params/ekf_node.yaml

@@ -29,12 +29,13 @@ robot_localization:
       # Fuse absolute pose estimated from map rasters
       pose0: "/gisnav/pose_node/pose"
       pose0_config: [true, true, true,     # Position XYZ
-                     true, true, true,     # Orientation (roll, pitch, yaw)
+                     false, false, false, #true, true, true,     # Orientation (roll, pitch, yaw)
                      false, false, false,  # Velocity XYZ
                      false, false, false,  # Angular rates XYZ
                      false, false, false]  # Accelerations XYZ
 
       # Fuse smooth relative pose estimated via VO differentially as velocity
+      # - better than a lagging velocity from pose0
       #pose1: "/gisnav/pose_node/vo/pose"
       #pose1_differential: true
       #pose1_config: [true, true, true,     # Position XYZ
@@ -43,13 +44,12 @@ robot_localization:
       #               false, false, false,  # Angular rates XYZ
       #               false, false, false]  # Accelerations XYZ
 
-      #pose1: "/gisnav/pose_node/vo/pose"
-      #pose1_differential: true
-      #pose1_config: [false, false, false,  # Position XYZ
-      #               false, false, false,  # Orientation (roll, pitch, yaw)
-      #               true, true, true,     # Velocity XYZ
-      #               true, true, true,     # Angular rates XYZ
-      #               false, false, false]  # Accelerations XYZ
+      twist1: "/gisnav/pose_node/vo/twist"
+      twist1_config: [false, false, false,  # Position XYZ
+                      false, false, false,  # Orientation (roll, pitch, yaw)
+                      true, true, true,     # Velocity XYZ
+                      true, true, true,     # Angular rates XYZ
+                      false, false, false]  # Accelerations XYZ
 
       #debug: true
       #debug_out_file: "/home/hmakelin/robot_localization_debug.txt"

gisnav/package.xml

@@ -46,6 +46,7 @@
   <depend>python3-setuptools</depend> <!-- setuptools is build dependency only? -->
   <depend>python3-shapely</depend>
   <depend>python3-serial</depend>
+  <depend>python3-scipy</depend>
   <!-- <depend>python3-owslib</depend> not found by rosdep, in setup.py instead -->
 
   <!-- need these to install things from setup.py -->