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 diff --git a/gisnav/gisnav/_transformations.py b/gisnav/gisnav/_transformations.py index 6648c0cc..53c20fd1 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,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 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 4dfb6586..47b79225 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 @@ -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) 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..4356c4b6 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,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" 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