diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 91d258a..8fa23a2 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -3,35 +3,24 @@ import numpy as np import rclpy import tf2_ros -from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import Pose, Transform, TransformStamped from nav_msgs.msg import Odometry +from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge from rclpy.node import Node -from scipy.spatial.transform import Rotation as R +from rclpy.time import Time +from scipy.spatial.transform import Rotation def _invert_pose(p): p_inv = np.zeros((4, 4)) p_inv[:3, :3] = p[:3, :3].T p_inv[:3, 3] = -p[:3, :3].T @ p[:3, 3] - p_inv[3, 3] = 1 + p_inv[3, 3] = 1.0 return p_inv -def _mat_from_pose(pose): - pose_out = np.zeros((4, 4)) - q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] - t = [pose.position.x, pose.position.y, pose.position.z] - Rmat = R.from_quat(q).as_matrix() - pose_out[:3, :3] = Rmat - pose_out[:3, 3] = t - pose_out[3, 3] = 1 - return pose_out - - -def Exp(rho, theta): - cos = np.cos - sin = np.sin - norm = np.linalg.norm +def _exp(rho, theta): + theta_norm = np.linalg.norm(theta) xi_hat = np.zeros((4, 4)) theta_hat = np.array( [[0, -theta[2], theta[1]], [theta[2], 0, -theta[0]], [-theta[1], theta[0], 0]] @@ -41,20 +30,54 @@ def Exp(rho, theta): return ( np.eye(4) + xi_hat - + (1 - cos(norm(theta))) / (norm(theta) ** 2) * xi_hat @ xi_hat - + (norm(theta) - sin(norm(theta))) - / (norm(theta) ** 3) - * xi_hat - @ xi_hat - @ xi_hat + + (1 - np.cos(theta_norm)) / (theta_norm**2) * xi_hat @ xi_hat + + (theta_norm - np.sin(theta_norm)) / (theta_norm**3) * xi_hat @ xi_hat @ xi_hat ) -def _calc_odom_delta(odom_old, odom_new): - w_T_old = _mat_from_pose(odom_old.pose.pose) - w_T_new = _mat_from_pose(odom_new.pose.pose) - delta = _invert_pose(w_T_old) @ w_T_new - return delta +def _translation_distance(pose1, pose2): + return np.linalg.norm(pose1[:3, 3] - pose2[:3, 3]) + + +def _rot_from_pose(pose): + q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + return Rotation.from_quat(q).as_matrix() + + +def _mat_from_pose(pose): + pose_out = np.zeros((4, 4)) + pose_out[:3, :3] = _rot_from_pose(pose) + pose_out[:3, 3] = [pose.position.x, pose.position.y, pose.position.z] + pose_out[3, 3] = 1.0 + return pose_out + + +def _pose_from_mat(p): + q = Rotation.from_matrix(p[:3, :3]).as_quat() + + msg = Pose() + msg.position.x = p[0, 3] + msg.position.y = p[1, 3] + msg.position.z = p[2, 3] + msg.orientation.x = q[0] + msg.orientation.y = q[1] + msg.orientation.z = q[2] + msg.orientation.w = q[3] + return msg + + +def _transform_from_mat(p): + q = Rotation.from_matrix(p[:3, :3]).as_quat() + + msg = Transform() + msg.translation.x = p[0, 3] + msg.translation.y = p[1, 3] + msg.translation.z = p[2, 3] + msg.rotation.x = q[0] + msg.rotation.y = q[1] + msg.rotation.z = q[2] + msg.rotation.w = q[3] + return msg class NoisyTransformPublisher(Node): @@ -65,106 +88,140 @@ class NoisyTransformPublisher(Node): def __init__(self): super().__init__("noisy_tf_publisher_node") - self._yaw_std_dev = self._get_param("yaw_std_dev", 0.01).double_value - self._xyz_std_dev = self._get_param("xyz_std_dev", 0.01).double_value - self._wrong_velocity_frame = self._get_param( - "wrong_velocity_frame", False - ).bool_value - self._start_from_origin = self._get_param("start_from_origin", True).bool_value - self._add_noise = self._get_param("add_noise", True).bool_value + self._last_noisy_pose = None + self._last_loop_closure_stamp = None + self._poses = [] + self._stamps = [] + self._parent_frame = self._get_param("parent_frame", "odom").string_value self._child_frame = self._get_param("child_frame", "base_link").string_value - self._br = tf2_ros.TransformBroadcaster(self) - self._sub = self.create_subscription(Odometry, "~/odom", self._odom_cb, 10) - self._pub = self.create_publisher(Odometry, "noisy_odom_out", 10) - self._last_gt_odom = None - self._last_noisy_odom = None - - def _odom_cb(self, odom_msg): - if self._last_gt_odom is None: - self._last_gt_odom = odom_msg - self._last_noisy_odom = Odometry() if self._start_from_origin else odom_msg - return - - pose_delta = _calc_odom_delta(self._last_gt_odom, odom_msg) + self._yaw_std_dev = self._get_param("yaw_std_dev", 0.001).double_value + self._xy_std_dev = self._get_param("xy_std_dev", 0.001).double_value + self._z_std_dev = self._get_param("z_std_dev", 0.001).double_value - self._last_noisy_odom = self._add_odom_noise( - self._last_noisy_odom, odom_msg, pose_delta - ) - - self._publish_noisy_transform( - self._parent_frame, - self._child_frame, - odom_msg.header.stamp, - self._last_noisy_odom.pose.pose, - ) - - self._pub.publish(self._last_noisy_odom) - self._last_gt_odom = odom_msg - - def _publish_noisy_transform(self, fixed_frame, child_frame, time, noisy_pose): - t = TransformStamped() - t.header.stamp = time - t.header.frame_id = fixed_frame - t.child_frame_id = child_frame + self._start_from_origin = self._get_param("start_from_origin", True).bool_value + self._add_noise = self._get_param("add_noise", True).bool_value + self._wrong_velocity_frame = self._get_param( + "wrong_velocity_frame", False + ).bool_value - # Set position from noisy pose - t.transform.translation.x = noisy_pose.position.x - t.transform.translation.y = noisy_pose.position.y - t.transform.translation.z = noisy_pose.position.z + self._enable_pseudo_loop_closures = self._get_param( + "enable_pseudo_loop_closures", True + ).bool_value + self._robot_id = self._get_param("robot_id", 0).integer_value + self._loop_closure_threshold_m = self._get_param( + "loop_closure_threshold_m", 0.5 + ).double_value + min_lc_sep_s = self._get_param( + "minimum_loop_closure_separation_s", 1.0 + ).double_value + self._min_loop_closure_separation_ns = int(1.0e9 * min_lc_sep_s) + lc_window_s = self._get_param("loop_closure_window_ns", 30.0).double_value + self._loop_closure_window_ns = int(1.0e9 * lc_window_s) + + seed = self._get_param("random_seed", 0).integer_value + self._rng = np.random.default_rng(seed=seed if seed > 0 else None) - # Set orientation from noisy pose - t.transform.rotation = noisy_pose.orientation + self._br = tf2_ros.TransformBroadcaster(self) + self._sub = self.create_subscription(Odometry, "odom", self._odom_cb, 10) + self._pub = self.create_publisher(Odometry, "noisy_odom", 10) + if self._enable_pseudo_loop_closures: + self._lc_pub = self.create_publisher(PoseGraph, "loop_closures", 10) + + def _odom_cb(self, msg): + map_T_new = _mat_from_pose(msg.pose.pose) + self._poses.append(map_T_new) + self._stamps.append(Time.from_msg(msg.header.stamp).nanoseconds) + if len(self._poses) == 1: + self._last_noisy_pose = np.eye(4) if self._start_from_origin else map_T_new + return - # Broadcast the noisy transform - self._br.sendTransform(t) + map_T_old = self._poses[-2] + old_T_new = _invert_pose(map_T_old) @ map_T_new + if self._add_noise: + old_T_new @= _exp(*self._sample_noise()) - def _add_odom_noise(self, prev_noisy_odom, cur_gt_odom, pose_delta): - updated_odom = Odometry() - updated_odom.header = cur_gt_odom.header + self._last_noisy_pose = self._last_noisy_pose @ old_T_new + self._publish_noisy_transform(msg.header.stamp, self._last_noisy_pose) + self._publish_noisy_odom(self._last_noisy_pose, msg) + if not self._enable_pseudo_loop_closures: + return - prev_pose = _mat_from_pose(prev_noisy_odom.pose.pose) - updated_pose = prev_pose @ pose_delta - if self._add_noise: - noisy_trans = np.random.normal(scale=self._xyz_std_dev) - noisy_rot = [0, 0, np.random.normal(scale=self._yaw_std_dev)] - updated_pose @= Exp(noisy_trans, noisy_rot) - - updated_odom.pose.pose.position.x = updated_pose[0, 3] - updated_odom.pose.pose.position.y = updated_pose[1, 3] - updated_odom.pose.pose.position.z = updated_pose[2, 3] - - q = R.from_matrix(updated_pose[:3, :3]).as_quat() - updated_odom.pose.pose.orientation.x = q[0] - updated_odom.pose.pose.orientation.y = q[1] - updated_odom.pose.pose.orientation.z = q[2] - updated_odom.pose.pose.orientation.w = q[3] - - q_gt = [ - cur_gt_odom.pose.pose.orientation.x, - cur_gt_odom.pose.pose.orientation.y, - cur_gt_odom.pose.pose.orientation.z, - cur_gt_odom.pose.pose.orientation.w, - ] - - R_gt = R.from_quat(q_gt).as_matrix() - R_noisy = updated_pose[:3, :3] - - vel = np.array( - [ - cur_gt_odom.twist.twist.linear.x, - cur_gt_odom.twist.twist.linear.y, - cur_gt_odom.twist.twist.linear.z, - ] - ) + candidate = self._find_pseudo_loop_closure() + if candidate is None: + return + self._last_loop_closure_stamp = self._stamps[-1] + self._publish_loop_closure(candidate) + + def _find_pseudo_loop_closure(self): + curr_stamp = self._stamps[-1] + if self._last_loop_closure_stamp is not None: + last_diff_ns = curr_stamp - self._last_loop_closure_stamp + if last_diff_ns < self._min_loop_closure_separation_ns: + return None + + curr_pose = self._poses[-1] + for idx, prev_pose in enumerate(self._poses[:-1]): + prev_stamp = self._stamps[idx] + time_diff_ns = curr_stamp - prev_stamp + if time_diff_ns < self._loop_closure_window_ns: + return None + + diff_m = _translation_distance(curr_pose, prev_pose) + if diff_m < self._loop_closure_threshold_m: + return idx + + def _publish_loop_closure(self, candidate): + map_T_ref = self._poses[candidate] + map_T_query = self._poses[-1] + ref_T_query = _invert_pose(map_T_ref) @ map_T_query + + edge = PoseGraphEdge() + edge.header.stamp = Time(nanoseconds=self._stamps[-1]).to_msg() + edge.key_from = self._stamps[candidate] + edge.key_to = self._stamps[-1] + edge.robot_from = self._robot_id + edge.robot_to = self._robot_id + edge.type = PoseGraphEdge.LOOPCLOSE + edge.pose = _pose_from_mat(ref_T_query) + + msg = PoseGraph() + msg.edges.append(edge) + self._lc_pub.publish(msg) + + def _sample_noise(self): + t_noise = np.zeros(3) + t_noise[:2] = self._rng.normal(scale=self._xy_std_dev) + if self._z_std_dev > 0.0: + t_noise[2] = self._rng.normal(scale=self._z_std_dev) + + R_noise = [0, 0, self._rng.normal(scale=self._yaw_std_dev)] + return t_noise, R_noise + + def _publish_noisy_transform(self, time, noisy_pose): + msg = TransformStamped() + msg.header.stamp = time + msg.header.frame_id = self._parent_frame + msg.child_frame_id = self._child_frame + msg.transform = _transform_from_mat(noisy_pose) + self._br.sendTransform(msg) + + def _publish_noisy_odom(self, noisy_pose, cur_gt_odom): + curr_twist = cur_gt_odom.twist.twist + vel = np.array([curr_twist.linear.x, curr_twist.linear.y, curr_twist.linear.z]) if self._wrong_velocity_frame: - vel = (R_gt.T @ R_noisy).T @ vel - - updated_odom.twist.twist.linear.x = vel[0] - updated_odom.twist.twist.linear.y = vel[1] - updated_odom.twist.twist.linear.z = vel[2] - return updated_odom + R_gt = _rot_from_pose(cur_gt_odom.pose.pose) + R_noisy = noisy_pose[:3, :3] + vel = (R_noisy.T @ R_gt) @ vel + + msg = Odometry() + msg.header = cur_gt_odom.header + msg.pose.pose = _pose_from_mat(noisy_pose) + msg.twist.twist.linear.x = vel[0] + msg.twist.twist.linear.y = vel[1] + msg.twist.twist.linear.z = vel[2] + self._pub.publish(msg) def main(args=None): diff --git a/hydra_ros/launch/datasets/uhumans2.launch.yaml b/hydra_ros/launch/datasets/uhumans2.launch.yaml index 3fd4185..c322220 100644 --- a/hydra_ros/launch/datasets/uhumans2.launch.yaml +++ b/hydra_ros/launch/datasets/uhumans2.launch.yaml @@ -3,6 +3,7 @@ launch: - arg: {name: scene, default: office, description: uhumans2 scene name} - arg: {name: use_gt_semantics, default: 'true'} - arg: {name: use_openset_places, default: 'false'} + - arg: {name: use_gt_poses, default: 'true'} - arg: {name: labelspace, default: $(if $(var use_gt_semantics) uhumans2_$(var scene) ade20k_full)} - arg: {name: log_prefix, default: uhumans2_$(var scene)} - arg: {name: label_remap_path, default: $(find-pkg-share hydra)/config/label_remaps/uhumans2_$(var scene).yaml} @@ -13,6 +14,11 @@ launch: - set_remap: {from: hydra/input/left_cam/feature, to: /tesse/left_cam/semantic/feature} - let: {name: extra_yaml_gt, value: '{semantic_label_remap_filepath: $(var label_remap_path)}'} - let: {name: extra_yaml_no_gt, value: '{}'} + - node: + pkg: tf2_ros + name: world_to_map + exec: static_transform_publisher + args: --frame-id world --child-frame-id map - group: - push_ros_namespace: {namespace: tesse/left_cam} - set_remap: {from: color/image_raw, to: rgb/image_raw} @@ -26,20 +32,20 @@ launch: file: $(find-pkg-share semantic_inference_ros)/launch/image_embedding_node.launch.yaml arg: - {name: min_period_s, value: '0.2'} + - group: + - set_remap: {from: odom, to: /tesse/odom} + - set_remap: {from: loop_closures, to: /hydra/external_loop_closures} + - include: + if: $(not $(var use_gt_poses)) + file: $(find-pkg-share hydra_ros)/launch/noisy_tf_publisher_node.launch.yaml - include: file: $(find-pkg-share hydra_ros)/launch/hydra.launch.yaml arg: - {name: dataset, value: uhumans2} - {name: labelspace, value: $(var labelspace)} - {name: log_prefix, value: $(var log_prefix)} - - {name: sensor_frame, value: left_cam} - - {name: robot_frame, value: base_link_gt} - - {name: odom_frame, value: world} + - {name: robot_frame, value: $(if $(var use_gt_poses) base_link_gt base_link)} + - {name: odom_frame, value: $(if $(var use_gt_poses) world odom)} - {name: map_frame, value: map} - {name: lcd_config_path, value: $(find-pkg-share hydra)/config/lcd/uhumans2.yaml} - {name: extra_yaml, value: $(if $(var use_gt_semantics) $(var extra_yaml_gt) $(var extra_yaml_no_gt))} - - node: - pkg: tf2_ros - name: world_to_map - exec: static_transform_publisher - args: --frame-id world --child-frame-id map diff --git a/hydra_ros/launch/hydra.launch.yaml b/hydra_ros/launch/hydra.launch.yaml index 11bf965..09abc83 100644 --- a/hydra_ros/launch/hydra.launch.yaml +++ b/hydra_ros/launch/hydra.launch.yaml @@ -10,7 +10,6 @@ launch: - arg: {name: dataset, description: Dataset hydra is running} - arg: {name: labelspace, description: Closed-set semantics description} - arg: {name: robot_id, default: '0', description: Unique robot identifier} - - arg: {name: sensor_frame, default: left_cam, description: Camera frame} - arg: {name: robot_frame, default: base_link_gt, description: Robot body tf frame} - arg: {name: odom_frame, default: world, description: Odometry (map) frame} - arg: {name: map_frame, default: map, description: Backend scene graph frame} diff --git a/hydra_ros/launch/noisy_tf_publisher_node.launch.yaml b/hydra_ros/launch/noisy_tf_publisher_node.launch.yaml index dadacd1..7bd37a9 100644 --- a/hydra_ros/launch/noisy_tf_publisher_node.launch.yaml +++ b/hydra_ros/launch/noisy_tf_publisher_node.launch.yaml @@ -2,10 +2,10 @@ launch: - arg: {name: parent_frame, default: odom} - arg: {name: child_frame, default: base_link} - - arg: {name: yaw_std_dev, default: '0.01'} - - arg: {name: xyz_std_dev, default: '0.01'} - - arg: {name: start_from_origin, default: 'True'} - - arg: {name: wrong_velocity_frame, default: 'False'} + - arg: {name: yaw_std_dev, default: '0.001'} + - arg: {name: xyz_std_dev, default: '0.001'} + - arg: {name: start_from_origin, default: 'true'} + - arg: {name: wrong_velocity_frame, default: 'false'} - node: pkg: hydra_ros exec: noisy_tf_publisher @@ -15,8 +15,7 @@ launch: - {name: parent_frame, value: $(var parent_frame)} - {name: child_frame, value: $(var child_frame)} - {name: yaw_std_dev, value: $(var yaw_std_dev)} - - {name: xyz_std_dev, value: $(var xyz_std_dev)} + - {name: xy_std_dev, value: $(var xyz_std_dev)} + - {name: z_std_dev, value: $(var xyz_std_dev)} - {name: start_from_origin, value: $(var start_from_origin)} - {name: wrong_velocity_frame, value: $(var wrong_velocity_frame)} - remap: - - {from: odom, to: /tesse/odom}