From 4a525fca5b78302944d6d957d1bf6735bf94d89f Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Thu, 9 May 2024 14:06:16 +0100 Subject: [PATCH 1/7] Enable VO --- gisnav/gisnav/core/pose_node.py | 64 +++++++++++++++++++++++++++++- gisnav/gisnav/core/stereo_node.py | 5 +-- gisnav/launch/params/ekf_node.yaml | 17 ++++---- 3 files changed, 73 insertions(+), 13 deletions(-) diff --git a/gisnav/gisnav/core/pose_node.py b/gisnav/gisnav/core/pose_node.py index 4dfb6586..3fa177bd 100644 --- a/gisnav/gisnav/core/pose_node.py +++ b/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 @@ -23,6 +23,7 @@ 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] @@ -74,6 +75,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 @@ -127,6 +161,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 @@ -202,6 +238,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 +249,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 diff --git a/gisnav/gisnav/core/stereo_node.py b/gisnav/gisnav/core/stereo_node.py index fc48e375..9e1923cb 100644 --- a/gisnav/gisnav/core/stereo_node.py +++ b/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 diff --git a/gisnav/launch/params/ekf_node.yaml b/gisnav/launch/params/ekf_node.yaml index a8046d70..83b0031e 100644 --- a/gisnav/launch/params/ekf_node.yaml +++ b/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,13 @@ 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 + 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 #debug: true #debug_out_file: "/home/hmakelin/robot_localization_debug.txt" From b0e103cd6748f69d4237596d734885a9fa73b920 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 08:17:08 +0100 Subject: [PATCH 2/7] Publish twist instead of pose for VO --- gisnav/gisnav/_transformations.py | 80 ++++++++++++++++++++++++++++++ gisnav/gisnav/constants.py | 4 +- gisnav/gisnav/core/pose_node.py | 26 +++++++--- gisnav/launch/params/ekf_node.yaml | 13 +++-- 4 files changed, 106 insertions(+), 17 deletions(-) diff --git a/gisnav/gisnav/_transformations.py b/gisnav/gisnav/_transformations.py index 6648c0cc..7ce003b3 100644 --- a/gisnav/gisnav/_transformations.py +++ b/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,82 @@ 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, + ] + r1 = tf_transformations.quaternion_matrix(q1) + r2 = tf_transformations.quaternion_matrix(q2) + r_diff = r2 * r1.T + + # TODO do not convert to matrices above, just use quaternions + q_diff = tf_transformations.quaternion_from_matrix(r_diff) + + # 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 diff --git a/gisnav/gisnav/constants.py b/gisnav/gisnav/constants.py index 04fac7c0..5d268422 100644 --- a/gisnav/gisnav/constants.py +++ b/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" diff --git a/gisnav/gisnav/core/pose_node.py b/gisnav/gisnav/core/pose_node.py index 3fa177bd..cd6cea97 100644 --- a/gisnav/gisnav/core/pose_node.py +++ b/gisnav/gisnav/core/pose_node.py @@ -18,7 +18,11 @@ 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 @@ -39,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, @@ -148,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) @@ -196,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( @@ -314,16 +318,22 @@ 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) + x, y, z = 320.0, 180.0, 205.0 # todo do not hard code + previous_pose = tf_.create_identity_pose_stamped(x, y, z) + 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) diff --git a/gisnav/launch/params/ekf_node.yaml b/gisnav/launch/params/ekf_node.yaml index 83b0031e..4356c4b6 100644 --- a/gisnav/launch/params/ekf_node.yaml +++ b/gisnav/launch/params/ekf_node.yaml @@ -44,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" From cb837f00ebf1446c98b5a912a2de3760b42616e1 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 08:17:47 +0100 Subject: [PATCH 3/7] Put back GPS checks and fix EKF2_GPS_CTRL value for px4 simulation --- docker/px4/6011_typhoon_h480 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/px4/6011_typhoon_h480 b/docker/px4/6011_typhoon_h480 index 407bda4d..ac86b38a 100644 --- a/docker/px4/6011_typhoon_h480 +++ b/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 From a442b93769291383f0b9fed4804b722ab3519b70 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 08:53:26 +0100 Subject: [PATCH 4/7] Add scipy dependency --- gisnav/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/gisnav/setup.py b/gisnav/setup.py index 6fe4b6b2..72c3904d 100644 --- a/gisnav/setup.py +++ b/gisnav/setup.py @@ -117,6 +117,7 @@ def parse_package_data(cls, package_file: str) -> PackageData: "torch>=2.1.0", "kornia==0.7.2", # 0.7.2 for LightGlue and DISK "transforms3d", # tf_transformations needs this + "scipy", ], tests_require=["pytest"], extras_require={ From 1e466c4276fdd3058771a588755b757b94050113 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 09:25:57 +0100 Subject: [PATCH 5/7] Move scipy dependency to package.xml --- gisnav/package.xml | 1 + gisnav/setup.py | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/gisnav/package.xml b/gisnav/package.xml index 16ad85b5..c18cdc6d 100644 --- a/gisnav/package.xml +++ b/gisnav/package.xml @@ -46,6 +46,7 @@ python3-setuptools python3-shapely python3-serial + python3-scipy diff --git a/gisnav/setup.py b/gisnav/setup.py index 72c3904d..6fe4b6b2 100644 --- a/gisnav/setup.py +++ b/gisnav/setup.py @@ -117,7 +117,6 @@ def parse_package_data(cls, package_file: str) -> PackageData: "torch>=2.1.0", "kornia==0.7.2", # 0.7.2 for LightGlue and DISK "transforms3d", # tf_transformations needs this - "scipy", ], tests_require=["pytest"], extras_require={ From d60047ae2790c881d67aa0579324671f6fd9e822 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 09:38:00 +0100 Subject: [PATCH 6/7] Scale reference pose when computing twist (differential pose) in VO --- gisnav/gisnav/core/pose_node.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gisnav/gisnav/core/pose_node.py b/gisnav/gisnav/core/pose_node.py index cd6cea97..dd4c4257 100644 --- a/gisnav/gisnav/core/pose_node.py +++ b/gisnav/gisnav/core/pose_node.py @@ -327,7 +327,17 @@ def camera_optical_twist_in_camera_optical_frame( self, msg: MonocularStereoImage ) -> Optional[TwistWithCovarianceStamped]: """Camera pose in visual odometry world frame (i.e. previous query frame)""" - x, y, z = 320.0, 180.0, 205.0 # todo do not hard code + 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) current_pose = self._get_pose(msg) if current_pose is not None: From fc0fc441e66e87a9ac8c393435fedf78a4599339 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 13:20:59 +0100 Subject: [PATCH 7/7] Fix z scaling --- gisnav/gisnav/_transformations.py | 8 +++----- gisnav/gisnav/core/pose_node.py | 3 ++- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/gisnav/gisnav/_transformations.py b/gisnav/gisnav/_transformations.py index 7ce003b3..53c20fd1 100644 --- a/gisnav/gisnav/_transformations.py +++ b/gisnav/gisnav/_transformations.py @@ -443,12 +443,10 @@ def poses_to_twist( pose2.pose.pose.orientation.z, pose2.pose.pose.orientation.w, ] - r1 = tf_transformations.quaternion_matrix(q1) - r2 = tf_transformations.quaternion_matrix(q2) - r_diff = r2 * r1.T - # TODO do not convert to matrices above, just use quaternions - q_diff = tf_transformations.quaternion_from_matrix(r_diff) + 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 diff --git a/gisnav/gisnav/core/pose_node.py b/gisnav/gisnav/core/pose_node.py index dd4c4257..47b79225 100644 --- a/gisnav/gisnav/core/pose_node.py +++ b/gisnav/gisnav/core/pose_node.py @@ -336,9 +336,10 @@ def camera_optical_twist_in_camera_optical_frame( x, y, z = ( scaling * 320.0, scaling * 180.0, - scaling * 205.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)