From 1f3cc16019a43735797ad3a43e98db47a24cdf11 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sun, 15 Mar 2026 20:39:50 +0000 Subject: [PATCH 1/6] clean up noisy odom node --- hydra_ros/app/noisy_tf_publisher | 180 ++++++++---------- .../launch/datasets/uhumans2.launch.yaml | 21 +- hydra_ros/launch/hydra.launch.yaml | 1 - .../noisy_tf_publisher_node.launch.yaml | 13 +- 4 files changed, 103 insertions(+), 112 deletions(-) diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 91d258a0..412ac2f1 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -3,35 +3,50 @@ import numpy as np import rclpy import tf2_ros -from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import TransformStamped, Pose from nav_msgs.msg import Odometry from rclpy.node import Node -from scipy.spatial.transform import Rotation as R +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 _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)) - 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 + 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 Exp(rho, theta): - cos = np.cos - sin = np.sin - norm = np.linalg.norm +def _pose_from_mat(p): + msg = Pose() + msg.position.x = p[0, 3] + msg.position.y = p[1, 3] + msg.position.z = p[2, 3] + + q = Rotation.from_matrix(p[:3, :3]).as_quat() + msg.orientation.x = q[0] + msg.orientation.y = q[1] + msg.orientation.z = q[2] + msg.orientation.w = q[3] + + return msg + + +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,12 +56,8 @@ 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 ) @@ -65,102 +76,79 @@ 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._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 + self._parent_frame = self._get_param("parent_frame", "odom").string_value + self._child_frame = self._get_param("child_frame", "base_link").string_value - 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._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 - self._publish_noisy_transform( - self._parent_frame, - self._child_frame, - odom_msg.header.stamp, - self._last_noisy_odom.pose.pose, - ) + seed = self._get_param("random_seed", 0).integer_value + self._rng = np.random.default_rng(seed=seed if seed > 0 else None) - self._pub.publish(self._last_noisy_odom) - self._last_gt_odom = odom_msg + 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) - 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 + def _odom_cb(self, msg): + if self._last_gt_odom is None: + self._last_gt_odom = msg + self._last_noisy_odom = Odometry() if self._start_from_origin else msg + return - # 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 + delta = _calc_odom_delta(self._last_gt_odom, msg) + self._last_noisy_odom = self._add_odom_noise(self._last_noisy_odom, msg, delta) + self._last_gt_odom = msg - # Set orientation from noisy pose - t.transform.rotation = noisy_pose.orientation + self._publish_noisy_transform(msg.header.stamp, self._last_noisy_odom.pose.pose) + self._pub.publish(self._last_noisy_odom) - # Broadcast the noisy transform - self._br.sendTransform(t) + 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.translation.x = noisy_pose.position.x + msg.transform.translation.y = noisy_pose.position.y + msg.transform.translation.z = noisy_pose.position.z + msg.transform.rotation = noisy_pose.orientation + self._br.sendTransform(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 _add_odom_noise(self, prev_noisy_odom, cur_gt_odom, pose_delta): - updated_odom = Odometry() - updated_odom.header = cur_gt_odom.header - 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, - ] - ) + t_noise, R_noise = self._sample_noise() + updated_pose @= _exp(t_noise, R_noise) + 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 + R_gt = _rot_from_pose(cur_gt_odom.pose.pose) + R_noisy = updated_pose[:3, :3] + vel = (R_noisy.T @ R_gt) @ vel + updated_odom = Odometry() + updated_odom.header = cur_gt_odom.header + updated_odom.pose.pose = _pose_from_mat(updated_pose) updated_odom.twist.twist.linear.x = vel[0] updated_odom.twist.twist.linear.y = vel[1] updated_odom.twist.twist.linear.z = vel[2] diff --git a/hydra_ros/launch/datasets/uhumans2.launch.yaml b/hydra_ros/launch/datasets/uhumans2.launch.yaml index 3fd41851..cd7e9d0c 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,19 @@ 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} + - 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 11bf9656..09abc830 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 dadacd1a..7bd37a9e 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} From 0931323f3668468df015f67281f0361cb0d1ac8b Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 16 Mar 2026 13:57:14 +0000 Subject: [PATCH 2/6] (wip) start tracking pose history --- hydra_ros/app/noisy_tf_publisher | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 412ac2f1..c4cc78a0 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -31,17 +31,16 @@ def _mat_from_pose(pose): 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] - - q = Rotation.from_matrix(p[:3, :3]).as_quat() msg.orientation.x = q[0] msg.orientation.y = q[1] msg.orientation.z = q[2] msg.orientation.w = q[3] - return msg @@ -76,12 +75,11 @@ class NoisyTransformPublisher(Node): def __init__(self): super().__init__("noisy_tf_publisher_node") - self._last_gt_odom = None self._last_noisy_odom = None + self._poses = [] self._parent_frame = self._get_param("parent_frame", "odom").string_value self._child_frame = self._get_param("child_frame", "base_link").string_value - 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 @@ -100,14 +98,18 @@ class NoisyTransformPublisher(Node): self._pub = self.create_publisher(Odometry, "~/noisy_odom", 10) def _odom_cb(self, msg): - if self._last_gt_odom is None: - self._last_gt_odom = msg + self._poses.append(_mat_from_pose(msg.pose.pose)) + if len(self._poses) == 1: self._last_noisy_odom = Odometry() if self._start_from_origin else msg return - delta = _calc_odom_delta(self._last_gt_odom, msg) - self._last_noisy_odom = self._add_odom_noise(self._last_noisy_odom, msg, delta) - self._last_gt_odom = msg + map_T_new = self._poses[-1] + 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) + + self._last_noisy_odom = self._add_odom_noise(self._last_noisy_odom, msg, old_T_new) self._publish_noisy_transform(msg.header.stamp, self._last_noisy_odom.pose.pose) self._pub.publish(self._last_noisy_odom) From 37978f70ff2a0901b63f1d7ab80f50c0e6cbbe31 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 16 Mar 2026 20:26:24 +0000 Subject: [PATCH 3/6] publish loop closures --- hydra_ros/app/noisy_tf_publisher | 175 ++++++++++++------ .../launch/datasets/uhumans2.launch.yaml | 1 + 2 files changed, 122 insertions(+), 54 deletions(-) diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index c4cc78a0..15891965 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -3,9 +3,11 @@ import numpy as np import rclpy import tf2_ros -from geometry_msgs.msg import TransformStamped, Pose +from geometry_msgs.msg import Transform, TransformStamped, Pose from nav_msgs.msg import Odometry +from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge from rclpy.node import Node +from rclpy.time import Time from scipy.spatial.transform import Rotation @@ -17,6 +19,26 @@ def _invert_pose(p): return p_inv +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]] + ) + xi_hat[:3, :3] = theta_hat + xi_hat[:3, 3] = rho + return ( + np.eye(4) + + 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 _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() @@ -44,27 +66,18 @@ def _pose_from_mat(p): return msg -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]] - ) - xi_hat[:3, :3] = theta_hat - xi_hat[:3, 3] = rho - return ( - np.eye(4) - + 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 _transform_from_mat(p): + q = Rotation.from_matrix(p[:3, :3]).as_quat() -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 + 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): @@ -75,8 +88,10 @@ class NoisyTransformPublisher(Node): def __init__(self): super().__init__("noisy_tf_publisher_node") - self._last_noisy_odom = None + 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 @@ -90,40 +105,90 @@ class NoisyTransformPublisher(Node): "wrong_velocity_frame", False ).bool_value + 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", 10.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) 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) + 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): - self._poses.append(_mat_from_pose(msg.pose.pose)) + 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_odom = Odometry() if self._start_from_origin else msg + self._last_noisy_pose = np.eye(4) if self._start_from_origin else map_T_new return - map_T_new = self._poses[-1] 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) + old_T_new @= _exp(*self._sample_noise()) - self._last_noisy_odom = self._add_odom_noise(self._last_noisy_odom, msg, old_T_new) + 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 - self._publish_noisy_transform(msg.header.stamp, self._last_noisy_odom.pose.pose) - self._pub.publish(self._last_noisy_odom) + candidate = self._find_pseudo_loop_closure() + if candidate is None: + return - 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.translation.x = noisy_pose.position.x - msg.transform.translation.y = noisy_pose.position.y - msg.transform.translation.z = noisy_pose.position.z - msg.transform.rotation = noisy_pose.orientation - self._br.sendTransform(msg) + 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) @@ -134,27 +199,29 @@ class NoisyTransformPublisher(Node): R_noise = [0, 0, self._rng.normal(scale=self._yaw_std_dev)] return t_noise, R_noise - def _add_odom_noise(self, prev_noisy_odom, cur_gt_odom, pose_delta): - prev_pose = _mat_from_pose(prev_noisy_odom.pose.pose) - updated_pose = prev_pose @ pose_delta - if self._add_noise: - t_noise, R_noise = self._sample_noise() - updated_pose @= _exp(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: R_gt = _rot_from_pose(cur_gt_odom.pose.pose) - R_noisy = updated_pose[:3, :3] + R_noisy = noisy_pose[:3, :3] vel = (R_noisy.T @ R_gt) @ vel - updated_odom = Odometry() - updated_odom.header = cur_gt_odom.header - updated_odom.pose.pose = _pose_from_mat(updated_pose) - 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 + 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 cd7e9d0c..7a5429f3 100644 --- a/hydra_ros/launch/datasets/uhumans2.launch.yaml +++ b/hydra_ros/launch/datasets/uhumans2.launch.yaml @@ -34,6 +34,7 @@ launch: - {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 From 2614bb627650f07a55dc0335ac959fb062622ccc Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Thu, 19 Mar 2026 19:20:47 +0000 Subject: [PATCH 4/6] update loop closure parameters --- hydra_ros/app/noisy_tf_publisher | 2 +- hydra_ros/src/hydra_ros_pipeline.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 15891965..5fa11d0d 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -116,7 +116,7 @@ class NoisyTransformPublisher(Node): "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", 10.0).double_value + 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 diff --git a/hydra_ros/src/hydra_ros_pipeline.cpp b/hydra_ros/src/hydra_ros_pipeline.cpp index db555a21..974d337c 100644 --- a/hydra_ros/src/hydra_ros_pipeline.cpp +++ b/hydra_ros/src/hydra_ros_pipeline.cpp @@ -160,7 +160,7 @@ void HydraRosPipeline::stop() { void HydraRosPipeline::initLCD() { // TODO(nathan) push to pipeline config? - auto lcd_config = config::fromContext(); + auto lcd_config = config::fromContext(); lcd_config.detector.num_semantic_classes = GlobalInfo::instance().getTotalLabels(); LOG_IF(INFO, config.verbosity >= 2) << "Number of classes for LCD: " << lcd_config.detector.num_semantic_classes; From 3a0cf38b95d0ea0308239f59bca15e99245b6ec8 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 14:21:54 +0000 Subject: [PATCH 5/6] fix linting --- hydra_ros/app/noisy_tf_publisher | 2 +- hydra_ros/launch/datasets/uhumans2.launch.yaml | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 5fa11d0d..8fa23a2b 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -3,7 +3,7 @@ import numpy as np import rclpy import tf2_ros -from geometry_msgs.msg import Transform, TransformStamped, Pose +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 diff --git a/hydra_ros/launch/datasets/uhumans2.launch.yaml b/hydra_ros/launch/datasets/uhumans2.launch.yaml index 7a5429f3..c322220b 100644 --- a/hydra_ros/launch/datasets/uhumans2.launch.yaml +++ b/hydra_ros/launch/datasets/uhumans2.launch.yaml @@ -33,11 +33,11 @@ launch: 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 + - 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: From e0dc288941d8126d77fb60d92a4b73edd90386a2 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Sat, 13 Jun 2026 14:29:35 +0000 Subject: [PATCH 6/6] revert config name --- hydra_ros/src/hydra_ros_pipeline.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hydra_ros/src/hydra_ros_pipeline.cpp b/hydra_ros/src/hydra_ros_pipeline.cpp index 974d337c..db555a21 100644 --- a/hydra_ros/src/hydra_ros_pipeline.cpp +++ b/hydra_ros/src/hydra_ros_pipeline.cpp @@ -160,7 +160,7 @@ void HydraRosPipeline::stop() { void HydraRosPipeline::initLCD() { // TODO(nathan) push to pipeline config? - auto lcd_config = config::fromContext(); + auto lcd_config = config::fromContext(); lcd_config.detector.num_semantic_classes = GlobalInfo::instance().getTotalLabels(); LOG_IF(INFO, config.verbosity >= 2) << "Number of classes for LCD: " << lcd_config.detector.num_semantic_classes;