diff --git a/src/assistive_launch/config/paper_depth_processor.yaml b/src/assistive_launch/config/paper_depth_processor.yaml new file mode 100644 index 0000000..5a96092 --- /dev/null +++ b/src/assistive_launch/config/paper_depth_processor.yaml @@ -0,0 +1,20 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +detection_topic_name: "paper_tag_detections" +camera_info_topic_name: "/nuc/rgb/camera_info" +depth_image_topic_name: "/nuc/depth_to_rgb/image_raw" + +depth_observation_topic_name: "paper_tag_depth_observations" +mask_topic_name: "paper_sheet_mask" +point_cloud_topic_name: "paper_sheet_points" +camera_frame_id: "" + +depth_unit_scale: 0.001 +depth_max_age_sec: 0.15 +depth_patch_radius_px: 2 +plane_fit_stride_px: 2 +plane_fit_min_points: 20 +sheet_depth_margin_m: 0.04 +point_cloud_stride_px: 4 +min_support_ratio: 0.15 +mask_morph_kernel_px: 5 diff --git a/src/assistive_launch/config/paper_grid_rviz.yaml b/src/assistive_launch/config/paper_grid_rviz.yaml new file mode 100644 index 0000000..90c8238 --- /dev/null +++ b/src/assistive_launch/config/paper_grid_rviz.yaml @@ -0,0 +1,33 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +image_topic_name: "/nuc/rgb/image_raw" +camera_info_topic_name: "/nuc/rgb/camera_info" +camera_frame_id: "" +target_frame_id: "" +depth_image_topic_name: "/nuc/depth_to_rgb/image_raw" # or your aligned depth topic +use_depth_for_3d_points: true +depth_unit_scale: 0.001 +depth_max_age_sec: 0.15 +depth_patch_radius_px: 2 + +aruco_dict_name: "DICT_4X4_50" +# Minimum detected tags needed to publish the deformable surface. +min_markers_for_surface: 4 +# Maximum per-tag reprojection error in pixels for accepting tag 3D pose. +pnp_reprojection_error: 3.0 +tf_lookup_timeout_sec: 0.05 + +marker_topic_name: "paper_grid_markers" +pose_topic_name: "paper_grid_pose" + +publish_debug_image: true +debug_image_topic_name: "paper_grid_debug_image" + +grid_line_width: 0.003 +outline_line_width: 0.006 +surface_alpha: 0.12 +point_scale: 0.012 +text_height: 0.03 +text_z_offset: 0.01 +normal_length: 0.05 +normal_line_width: 0.003 diff --git a/src/assistive_launch/config/paper_surface_estimator.yaml b/src/assistive_launch/config/paper_surface_estimator.yaml new file mode 100644 index 0000000..41a10ea --- /dev/null +++ b/src/assistive_launch/config/paper_surface_estimator.yaml @@ -0,0 +1,13 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +input_pose_topic_name: "paper_tag_poses" +output_pose_topic_name: "paper_surface_state" + +measurement_weight: 1.0 +temporal_weight: 0.3 +edge_weight: 0.35 +smoothness_weight: 0.15 + +prediction_variance_growth: 1.5 +missing_position_variance_step: 2.5e-4 +missing_orientation_variance_step: 5.0e-3 diff --git a/src/assistive_launch/config/paper_surface_rviz.yaml b/src/assistive_launch/config/paper_surface_rviz.yaml new file mode 100644 index 0000000..3167cf0 --- /dev/null +++ b/src/assistive_launch/config/paper_surface_rviz.yaml @@ -0,0 +1,18 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +pose_topic_name: "paper_surface_state" +marker_topic_name: "paper_surface_markers" + +grid_line_width: 0.003 +outline_line_width: 0.006 +surface_alpha: 0.12 +predicted_surface_alpha_scale: 0.55 + +point_scale: 0.012 +predicted_point_scale: 0.018 +uncertainty_scale_gain: 0.12 + +text_height: 0.03 +text_z_offset: 0.01 +normal_length: 0.05 +normal_line_width: 0.003 diff --git a/src/assistive_launch/config/paper_tag_detector.yaml b/src/assistive_launch/config/paper_tag_detector.yaml new file mode 100644 index 0000000..a1836bb --- /dev/null +++ b/src/assistive_launch/config/paper_tag_detector.yaml @@ -0,0 +1,11 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +image_topic_name: "/nuc/rgb/image_raw" +detection_topic_name: "paper_tag_detections" + +publish_debug_image: true +debug_image_topic_name: "paper_tag_debug_image" + +aruco_dict_name: "DICT_4X4_50" +min_tag_area_px: 0.0 +quality_half_area_px: 2500.0 diff --git a/src/assistive_launch/config/paper_tag_pose_fuser.yaml b/src/assistive_launch/config/paper_tag_pose_fuser.yaml new file mode 100644 index 0000000..bb407bb --- /dev/null +++ b/src/assistive_launch/config/paper_tag_pose_fuser.yaml @@ -0,0 +1,30 @@ +layout_json_path: "$(find assistive_launch)/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json" + +detection_topic_name: "paper_tag_detections" +depth_observation_topic_name: "paper_tag_depth_observations" +pose_topic_name: "paper_tag_poses" + +camera_info_topic_name: "/nuc/rgb/camera_info" +camera_frame_id: "" +target_frame_id: "" + +use_depth_observations: true +depth_observation_max_age_sec: 0.15 +depth_image_topic_name: "" +use_depth_refinement: true +depth_unit_scale: 0.001 +depth_max_age_sec: 0.15 +depth_patch_radius_px: 2 +plane_fit_stride_px: 2 +plane_fit_min_points: 20 + +tf_lookup_timeout_sec: 0.05 +pnp_reprojection_error: 3.0 + +base_position_variance: 1.0e-5 +base_orientation_variance: 2.5e-3 +range_position_variance_scale: 0.03 +angle_position_variance_scale: 0.08 +range_orientation_variance_scale: 0.05 +angle_orientation_variance_scale: 0.2 +depth_fused_variance_scale: 0.6 diff --git a/src/assistive_launch/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json b/src/assistive_launch/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json new file mode 100644 index 0000000..7146e5c --- /dev/null +++ b/src/assistive_launch/vision_parameters/letter_aruco_grid_3x4_60mm_gap8mm_layout.json @@ -0,0 +1,367 @@ +{ + "page": { + "size_in": [ + 8.5, + 11.0 + ], + "dpi": 300 + }, + "grid": { + "rows": 4, + "cols": 3, + "marker_mm": 60.0, + "gap_mm": 8.0, + "start_id": 0, + "dict": "DICT_4X4_50" + }, + "markers": [ + { + "id": 0, + "row": 0, + "col": 0, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.06, + 0.0, + 0.0 + ], + [ + 0.06, + 0.06, + 0.0 + ], + [ + 0.0, + 0.06, + 0.0 + ] + ] + }, + { + "id": 1, + "row": 0, + "col": 1, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.068, + 0.0, + 0.0 + ], + [ + 0.128, + 0.0, + 0.0 + ], + [ + 0.128, + 0.06, + 0.0 + ], + [ + 0.068, + 0.06, + 0.0 + ] + ] + }, + { + "id": 2, + "row": 0, + "col": 2, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.136, + 0.0, + 0.0 + ], + [ + 0.196, + 0.0, + 0.0 + ], + [ + 0.196, + 0.06, + 0.0 + ], + [ + 0.136, + 0.06, + 0.0 + ] + ] + }, + { + "id": 3, + "row": 1, + "col": 0, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.0, + 0.068, + 0.0 + ], + [ + 0.06, + 0.068, + 0.0 + ], + [ + 0.06, + 0.128, + 0.0 + ], + [ + 0.0, + 0.128, + 0.0 + ] + ] + }, + { + "id": 4, + "row": 1, + "col": 1, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.068, + 0.068, + 0.0 + ], + [ + 0.128, + 0.068, + 0.0 + ], + [ + 0.128, + 0.128, + 0.0 + ], + [ + 0.068, + 0.128, + 0.0 + ] + ] + }, + { + "id": 5, + "row": 1, + "col": 2, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.136, + 0.068, + 0.0 + ], + [ + 0.196, + 0.068, + 0.0 + ], + [ + 0.196, + 0.128, + 0.0 + ], + [ + 0.136, + 0.128, + 0.0 + ] + ] + }, + { + "id": 6, + "row": 2, + "col": 0, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.0, + 0.136, + 0.0 + ], + [ + 0.06, + 0.136, + 0.0 + ], + [ + 0.06, + 0.196, + 0.0 + ], + [ + 0.0, + 0.196, + 0.0 + ] + ] + }, + { + "id": 7, + "row": 2, + "col": 1, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.068, + 0.136, + 0.0 + ], + [ + 0.128, + 0.136, + 0.0 + ], + [ + 0.128, + 0.196, + 0.0 + ], + [ + 0.068, + 0.196, + 0.0 + ] + ] + }, + { + "id": 8, + "row": 2, + "col": 2, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.136, + 0.136, + 0.0 + ], + [ + 0.196, + 0.136, + 0.0 + ], + [ + 0.196, + 0.196, + 0.0 + ], + [ + 0.136, + 0.196, + 0.0 + ] + ] + }, + { + "id": 9, + "row": 3, + "col": 0, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.0, + 0.204, + 0.0 + ], + [ + 0.06, + 0.204, + 0.0 + ], + [ + 0.06, + 0.264, + 0.0 + ], + [ + 0.0, + 0.264, + 0.0 + ] + ] + }, + { + "id": 10, + "row": 3, + "col": 1, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.068, + 0.204, + 0.0 + ], + [ + 0.128, + 0.204, + 0.0 + ], + [ + 0.128, + 0.264, + 0.0 + ], + [ + 0.068, + 0.264, + 0.0 + ] + ] + }, + { + "id": 11, + "row": 3, + "col": 2, + "marker_length_mm": 60.0, + "marker_separation_mm": 8.0, + "corners_m": [ + [ + 0.136, + 0.204, + 0.0 + ], + [ + 0.196, + 0.204, + 0.0 + ], + [ + 0.196, + 0.264, + 0.0 + ], + [ + 0.136, + 0.264, + 0.0 + ] + ] + } + ] +} \ No newline at end of file diff --git a/src/assistive_msgs/CMakeLists.txt b/src/assistive_msgs/CMakeLists.txt index 7bb9fda..a847ead 100644 --- a/src/assistive_msgs/CMakeLists.txt +++ b/src/assistive_msgs/CMakeLists.txt @@ -50,6 +50,12 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES State2D.msg + TagDetection.msg + TagDetectionArray.msg + TagDepthObservation.msg + TagDepthObservationArray.msg + TagPose.msg + TagPoseArray.msg ) ## Generate services in the 'srv' folder diff --git a/src/assistive_msgs/msg/TagDepthObservation.msg b/src/assistive_msgs/msg/TagDepthObservation.msg new file mode 100644 index 0000000..e83f724 --- /dev/null +++ b/src/assistive_msgs/msg/TagDepthObservation.msg @@ -0,0 +1,7 @@ +int32 id +geometry_msgs/Point center_cam +geometry_msgs/Vector3 normal_cam +float32 mean_depth_m +float32 valid_point_ratio +uint32 support_point_count +bool plane_fitted diff --git a/src/assistive_msgs/msg/TagDepthObservationArray.msg b/src/assistive_msgs/msg/TagDepthObservationArray.msg new file mode 100644 index 0000000..f1ef964 --- /dev/null +++ b/src/assistive_msgs/msg/TagDepthObservationArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +TagDepthObservation[] observations diff --git a/src/assistive_msgs/msg/TagDetection.msg b/src/assistive_msgs/msg/TagDetection.msg new file mode 100644 index 0000000..27bac09 --- /dev/null +++ b/src/assistive_msgs/msg/TagDetection.msg @@ -0,0 +1,6 @@ +int32 id +string dictionary_name +geometry_msgs/Point32[] corners_px +geometry_msgs/Point32 center_px +float32 tag_area_px +float32 detection_score diff --git a/src/assistive_msgs/msg/TagDetectionArray.msg b/src/assistive_msgs/msg/TagDetectionArray.msg new file mode 100644 index 0000000..602c91f --- /dev/null +++ b/src/assistive_msgs/msg/TagDetectionArray.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 image_width +uint32 image_height +TagDetection[] detections diff --git a/src/assistive_msgs/msg/TagPose.msg b/src/assistive_msgs/msg/TagPose.msg new file mode 100644 index 0000000..8523b5a --- /dev/null +++ b/src/assistive_msgs/msg/TagPose.msg @@ -0,0 +1,9 @@ +int32 id +string dictionary_name +geometry_msgs/PoseWithCovariance pose +geometry_msgs/Vector3 normal +bool observed +float32 tag_area_px +float32 detection_score +float32 reprojection_error_px +bool depth_fused diff --git a/src/assistive_msgs/msg/TagPoseArray.msg b/src/assistive_msgs/msg/TagPoseArray.msg new file mode 100644 index 0000000..9f630e0 --- /dev/null +++ b/src/assistive_msgs/msg/TagPoseArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +TagPose[] poses diff --git a/src/assistive_msgs/package.xml b/src/assistive_msgs/package.xml index c8b9ef7..bfd4573 100644 --- a/src/assistive_msgs/package.xml +++ b/src/assistive_msgs/package.xml @@ -51,6 +51,8 @@ catkin std_msgs geometry_msgs + std_msgs + geometry_msgs std_msgs geometry_msgs diff --git a/src/tf_broadcasters/CMakeLists.txt b/src/tf_broadcasters/CMakeLists.txt index 096d16e..92b8d6a 100644 --- a/src/tf_broadcasters/CMakeLists.txt +++ b/src/tf_broadcasters/CMakeLists.txt @@ -8,10 +8,15 @@ add_compile_options(-std=c++11) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + assistive_msgs + cv_bridge + geometry_msgs roscpp rospy + sensor_msgs tf2 tf2_ros + visualization_msgs ) ## System dependencies are found with CMake's conventions @@ -106,7 +111,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( # INCLUDE_DIRS include # LIBRARIES tf_broadcasters - CATKIN_DEPENDS roscpp rospy tf2 tf2_ros + CATKIN_DEPENDS assistive_msgs cv_bridge geometry_msgs roscpp rospy sensor_msgs tf2 tf2_ros visualization_msgs # DEPENDS system_lib ) @@ -167,10 +172,14 @@ target_link_libraries(tf_camera_body_all_joints_broadcaster ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS + src/paper_depth_processor.py + src/paper_tag_detector.py + src/paper_tag_pose_fuser.py + src/paper_surface_estimator.py + src/paper_surface_rviz_publisher.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html diff --git a/src/tf_broadcasters/launch/paper_depth_processor.launch b/src/tf_broadcasters/launch/paper_depth_processor.launch new file mode 100644 index 0000000..b7758e9 --- /dev/null +++ b/src/tf_broadcasters/launch/paper_depth_processor.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_grid_rviz_publisher.launch b/src/tf_broadcasters/launch/paper_grid_rviz_publisher.launch new file mode 100644 index 0000000..28eca3d --- /dev/null +++ b/src/tf_broadcasters/launch/paper_grid_rviz_publisher.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_surface_estimator.launch b/src/tf_broadcasters/launch/paper_surface_estimator.launch new file mode 100644 index 0000000..4818908 --- /dev/null +++ b/src/tf_broadcasters/launch/paper_surface_estimator.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_surface_pipeline.launch b/src/tf_broadcasters/launch/paper_surface_pipeline.launch new file mode 100644 index 0000000..878f1bf --- /dev/null +++ b/src/tf_broadcasters/launch/paper_surface_pipeline.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_surface_rviz_publisher.launch b/src/tf_broadcasters/launch/paper_surface_rviz_publisher.launch new file mode 100644 index 0000000..c7637cd --- /dev/null +++ b/src/tf_broadcasters/launch/paper_surface_rviz_publisher.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_tag_detector.launch b/src/tf_broadcasters/launch/paper_tag_detector.launch new file mode 100644 index 0000000..fe1d959 --- /dev/null +++ b/src/tf_broadcasters/launch/paper_tag_detector.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/launch/paper_tag_pose_fuser.launch b/src/tf_broadcasters/launch/paper_tag_pose_fuser.launch new file mode 100644 index 0000000..87f7d32 --- /dev/null +++ b/src/tf_broadcasters/launch/paper_tag_pose_fuser.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/tf_broadcasters/package.xml b/src/tf_broadcasters/package.xml index 9dba533..de2ac5d 100644 --- a/src/tf_broadcasters/package.xml +++ b/src/tf_broadcasters/package.xml @@ -51,14 +51,29 @@ catkin roscpp rospy + assistive_msgs + geometry_msgs + sensor_msgs + visualization_msgs + cv_bridge tf2 tf2_ros roscpp rospy + assistive_msgs + geometry_msgs + sensor_msgs + visualization_msgs + cv_bridge tf2 tf2_ros roscpp rospy + assistive_msgs + geometry_msgs + sensor_msgs + visualization_msgs + cv_bridge tf2 tf2_ros diff --git a/src/tf_broadcasters/src/paper_depth_processor.py b/src/tf_broadcasters/src/paper_depth_processor.py new file mode 100644 index 0000000..cfe9780 --- /dev/null +++ b/src/tf_broadcasters/src/paper_depth_processor.py @@ -0,0 +1,367 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os +import threading + +import cv2 +import numpy as np +import rospy +import sensor_msgs.point_cloud2 as pc2 +from assistive_msgs.msg import TagDepthObservation +from assistive_msgs.msg import TagDepthObservationArray +from assistive_msgs.msg import TagDetectionArray +from cv_bridge import CvBridge, CvBridgeError +from geometry_msgs.msg import Point +from geometry_msgs.msg import Vector3 +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 + + +class PaperDepthProcessor(object): + def __init__(self): + rospy.init_node("paper_depth_processor", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.detection_topic_name = rospy.get_param("~detection_topic_name", "paper_tag_detections") + self.camera_info_topic_name = rospy.get_param("~camera_info_topic_name", "/nuc/rgb/camera_info") + self.depth_image_topic_name = rospy.get_param("~depth_image_topic_name", "/nuc/depth_to_rgb/image_raw") + self.depth_observation_topic_name = rospy.get_param( + "~depth_observation_topic_name", "paper_tag_depth_observations" + ) + self.mask_topic_name = rospy.get_param("~mask_topic_name", "paper_sheet_mask") + self.point_cloud_topic_name = rospy.get_param("~point_cloud_topic_name", "paper_sheet_points") + self.camera_frame_id = rospy.get_param("~camera_frame_id", "") + + self.depth_unit_scale = float(rospy.get_param("~depth_unit_scale", 0.001)) + self.depth_max_age_sec = float(rospy.get_param("~depth_max_age_sec", 0.15)) + self.depth_patch_radius_px = int(rospy.get_param("~depth_patch_radius_px", 2)) + self.plane_fit_stride_px = max(1, int(rospy.get_param("~plane_fit_stride_px", 2))) + self.plane_fit_min_points = max(3, int(rospy.get_param("~plane_fit_min_points", 20))) + self.sheet_depth_margin_m = float(rospy.get_param("~sheet_depth_margin_m", 0.04)) + self.point_cloud_stride_px = max(1, int(rospy.get_param("~point_cloud_stride_px", 4))) + self.min_support_ratio = float(rospy.get_param("~min_support_ratio", 0.15)) + self.mask_morph_kernel_px = max(1, int(rospy.get_param("~mask_morph_kernel_px", 5))) + + self._load_layout(self.layout_json_path) + + self.bridge = CvBridge() + self.data_lock = threading.Lock() + self.depth_lock = threading.Lock() + self.K = None + self.camera_info_ready = False + self.latest_depth = None + self.latest_depth_stamp = None + + self.depth_observation_pub = rospy.Publisher( + self.depth_observation_topic_name, TagDepthObservationArray, queue_size=1 + ) + self.mask_pub = rospy.Publisher(self.mask_topic_name, Image, queue_size=1) + self.point_cloud_pub = rospy.Publisher(self.point_cloud_topic_name, PointCloud2, queue_size=1) + + self.camera_info_sub = rospy.Subscriber( + self.camera_info_topic_name, CameraInfo, self._camera_info_cb, queue_size=1 + ) + self.depth_sub = rospy.Subscriber(self.depth_image_topic_name, Image, self._depth_cb, queue_size=1) + self.detection_sub = rospy.Subscriber( + self.detection_topic_name, TagDetectionArray, self._detection_cb, queue_size=1 + ) + + rospy.loginfo("paper_depth_processor initialized.") + rospy.loginfo("Publishing depth observations on: %s", self.depth_observation_topic_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + layout_data = json.load(f) + if not layout_data.get("markers"): + raise ValueError("Layout has no markers") + + def _camera_info_cb(self, msg): + with self.data_lock: + self.K = np.asarray(msg.K, dtype=np.float64).reshape(3, 3) + self.camera_info_ready = True + if not self.camera_frame_id: + self.camera_frame_id = msg.header.frame_id + + def _depth_cb(self, msg): + try: + depth = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + except CvBridgeError: + return + with self.depth_lock: + self.latest_depth = depth + self.latest_depth_stamp = msg.header.stamp + + def _depth_value_to_m(self, val): + if np.isnan(val) or np.isinf(val): + return None + if val <= 0: + return None + return float(val) * self.depth_unit_scale + + @staticmethod + def _pixel_to_cam(K, u, v, z_m): + fx = K[0, 0] + fy = K[1, 1] + cx = K[0, 2] + cy = K[1, 2] + x = (u - cx) * z_m / fx + y = (v - cy) * z_m / fy + return np.asarray([x, y, z_m], dtype=np.float64) + + @staticmethod + def _normalize(vec, fallback=None): + norm = np.linalg.norm(vec) + if norm < 1e-8: + return fallback + return vec / norm + + def _sample_depth_m(self, depth_img, u, v): + h, w = depth_img.shape[:2] + uu = int(round(u)) + vv = int(round(v)) + if uu < 0 or uu >= w or vv < 0 or vv >= h: + return None + + r = max(0, self.depth_patch_radius_px) + u0 = max(0, uu - r) + u1 = min(w, uu + r + 1) + v0 = max(0, vv - r) + v1 = min(h, vv + r + 1) + patch = depth_img[v0:v1, u0:u1] + if patch.size == 0: + return None + + values_m = [] + for val in patch.reshape(-1): + depth_m = self._depth_value_to_m(float(val)) + if depth_m is not None: + values_m.append(depth_m) + if not values_m: + return None + return float(np.median(np.asarray(values_m, dtype=np.float64))) + + def _fit_depth_plane(self, depth_img, corners_px, K): + h, w = depth_img.shape[:2] + xs = corners_px[:, 0] + ys = corners_px[:, 1] + u0 = max(0, int(np.floor(np.min(xs)))) + u1 = min(w - 1, int(np.ceil(np.max(xs)))) + v0 = max(0, int(np.floor(np.min(ys)))) + v1 = min(h - 1, int(np.ceil(np.max(ys)))) + if u1 <= u0 or v1 <= v0: + return None + + polygon = corners_px.astype(np.float32) + points = [] + attempted_count = 0 + for v in range(v0, v1 + 1, self.plane_fit_stride_px): + for u in range(u0, u1 + 1, self.plane_fit_stride_px): + if cv2.pointPolygonTest(polygon, (float(u), float(v)), False) < 0: + continue + attempted_count += 1 + depth_m = self._depth_value_to_m(float(depth_img[v, u])) + if depth_m is None: + continue + points.append(self._pixel_to_cam(K, u, v, depth_m)) + + valid_ratio = float(len(points)) / float(max(1, attempted_count)) + if len(points) < self.plane_fit_min_points or valid_ratio < self.min_support_ratio: + return { + "center_cam": None, + "normal_cam": None, + "mean_depth_m": 0.0, + "valid_point_ratio": valid_ratio, + "support_point_count": len(points), + "plane_fitted": False, + } + + pts = np.asarray(points, dtype=np.float64) + centroid = np.mean(pts, axis=0) + centered = pts - centroid + try: + _, _, vh = np.linalg.svd(centered, full_matrices=False) + except np.linalg.LinAlgError: + return { + "center_cam": None, + "normal_cam": None, + "mean_depth_m": float(np.mean(pts[:, 2])), + "valid_point_ratio": valid_ratio, + "support_point_count": len(points), + "plane_fitted": False, + } + + normal = self._normalize(vh[-1, :], fallback=np.asarray([0.0, 0.0, -1.0], dtype=np.float64)) + if np.dot(normal, centroid) > 0.0: + normal = -normal + return { + "center_cam": centroid, + "normal_cam": normal, + "mean_depth_m": float(np.mean(pts[:, 2])), + "valid_point_ratio": valid_ratio, + "support_point_count": len(points), + "plane_fitted": True, + } + + def _build_sheet_mask(self, depth_img, detections): + h, w = depth_img.shape[:2] + if not detections: + return np.zeros((h, w), dtype=np.uint8), [] + + hull_points = [] + tag_depths = [] + for detection in detections: + corners_px = np.asarray([[pt.x, pt.y] for pt in detection.corners_px], dtype=np.float32) + if corners_px.shape != (4, 2): + continue + hull_points.append(corners_px) + center_px = np.mean(corners_px, axis=0) + center_depth_m = self._sample_depth_m(depth_img, center_px[0], center_px[1]) + if center_depth_m is not None: + tag_depths.append(center_depth_m) + + if not hull_points: + return np.zeros((h, w), dtype=np.uint8), [] + + hull = cv2.convexHull(np.vstack(hull_points)) + base_mask = np.zeros((h, w), dtype=np.uint8) + cv2.fillConvexPoly(base_mask, hull.astype(np.int32), 255) + + median_depth_m = float(np.median(np.asarray(tag_depths, dtype=np.float64))) if tag_depths else None + valid_mask = np.zeros((h, w), dtype=np.uint8) + yy, xx = np.nonzero(base_mask) + if median_depth_m is not None: + for v, u in zip(yy.tolist(), xx.tolist()): + depth_m = self._depth_value_to_m(float(depth_img[v, u])) + if depth_m is None: + continue + if abs(depth_m - median_depth_m) <= self.sheet_depth_margin_m: + valid_mask[v, u] = 255 + + if np.count_nonzero(valid_mask) == 0: + valid_mask = base_mask + + kernel = np.ones((self.mask_morph_kernel_px, self.mask_morph_kernel_px), dtype=np.uint8) + valid_mask = cv2.morphologyEx(valid_mask, cv2.MORPH_CLOSE, kernel) + valid_mask = cv2.morphologyEx(valid_mask, cv2.MORPH_OPEN, kernel) + return valid_mask, tag_depths + + def _mask_to_point_cloud(self, mask, depth_img, K): + points = [] + h, w = mask.shape[:2] + for v in range(0, h, self.point_cloud_stride_px): + for u in range(0, w, self.point_cloud_stride_px): + if mask[v, u] == 0: + continue + depth_m = self._depth_value_to_m(float(depth_img[v, u])) + if depth_m is None: + continue + point = self._pixel_to_cam(K, u, v, depth_m) + points.append([float(point[0]), float(point[1]), float(point[2])]) + return points + + @staticmethod + def _to_point(vec): + msg = Point() + msg.x = float(vec[0]) + msg.y = float(vec[1]) + msg.z = float(vec[2]) + return msg + + @staticmethod + def _to_vector3(vec): + msg = Vector3() + msg.x = float(vec[0]) + msg.y = float(vec[1]) + msg.z = float(vec[2]) + return msg + + def _detection_cb(self, msg): + with self.data_lock: + if not self.camera_info_ready: + return + K = self.K.copy() + camera_frame_id = self.camera_frame_id + + with self.depth_lock: + if self.latest_depth is None or self.latest_depth_stamp is None: + return + depth_img = self.latest_depth.copy() + depth_stamp = self.latest_depth_stamp + + stamp = msg.header.stamp if msg.header.stamp != rospy.Time(0) else rospy.Time.now() + age = abs((stamp - depth_stamp).to_sec()) + if age > self.depth_max_age_sec: + rospy.logwarn_throttle(2.0, "Skipping depth processing, depth age %.3fs exceeds limit.", age) + return + + header = msg.header + if camera_frame_id: + header.frame_id = camera_frame_id + + observations_msg = TagDepthObservationArray() + observations_msg.header = header + + valid_detections = [] + for detection in msg.detections: + if len(detection.corners_px) != 4: + continue + valid_detections.append(detection) + corners_px = np.asarray([[pt.x, pt.y] for pt in detection.corners_px], dtype=np.float64) + plane_result = self._fit_depth_plane(depth_img, corners_px, K) + center_cam = plane_result["center_cam"] + normal_cam = plane_result["normal_cam"] + + if center_cam is None: + center_px = np.mean(corners_px, axis=0) + center_depth_m = self._sample_depth_m(depth_img, center_px[0], center_px[1]) + if center_depth_m is not None: + center_cam = self._pixel_to_cam(K, center_px[0], center_px[1], center_depth_m) + plane_result["mean_depth_m"] = center_depth_m + if center_cam is None: + continue + + if normal_cam is None: + normal_cam = np.asarray([0.0, 0.0, -1.0], dtype=np.float64) + + observation = TagDepthObservation() + observation.id = int(detection.id) + observation.center_cam = self._to_point(center_cam) + observation.normal_cam = self._to_vector3(normal_cam) + observation.mean_depth_m = float(plane_result["mean_depth_m"]) + observation.valid_point_ratio = float(plane_result["valid_point_ratio"]) + observation.support_point_count = int(plane_result["support_point_count"]) + observation.plane_fitted = bool(plane_result["plane_fitted"]) + observations_msg.observations.append(observation) + + observations_msg.observations.sort(key=lambda obs: obs.id) + self.depth_observation_pub.publish(observations_msg) + + mask, _ = self._build_sheet_mask(depth_img, valid_detections) + try: + mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding="mono8") + mask_msg.header = header + self.mask_pub.publish(mask_msg) + except CvBridgeError: + rospy.logwarn_throttle(5.0, "Failed to publish paper sheet mask.") + + cloud_points = self._mask_to_point_cloud(mask, depth_img, K) + cloud_msg = pc2.create_cloud_xyz32(header, cloud_points) + self.point_cloud_pub.publish(cloud_msg) + + +if __name__ == "__main__": + try: + PaperDepthProcessor() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_depth_processor failed: %s", str(exc)) diff --git a/src/tf_broadcasters/src/paper_grid_rviz_publisher.py b/src/tf_broadcasters/src/paper_grid_rviz_publisher.py new file mode 100644 index 0000000..423f5cd --- /dev/null +++ b/src/tf_broadcasters/src/paper_grid_rviz_publisher.py @@ -0,0 +1,744 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os +import threading + +import cv2 +import numpy as np +import rospy +import tf2_ros +from cv_bridge import CvBridge, CvBridgeError +from geometry_msgs.msg import Point +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + + +ARUCO_DICT = { + "DICT_4X4_50": cv2.aruco.DICT_4X4_50, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + "DICT_4X4_250": cv2.aruco.DICT_4X4_250, + "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000, + "DICT_5X5_50": cv2.aruco.DICT_5X5_50, + "DICT_5X5_100": cv2.aruco.DICT_5X5_100, + "DICT_5X5_250": cv2.aruco.DICT_5X5_250, + "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000, + "DICT_6X6_50": cv2.aruco.DICT_6X6_50, + "DICT_6X6_100": cv2.aruco.DICT_6X6_100, + "DICT_6X6_250": cv2.aruco.DICT_6X6_250, + "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000, + "DICT_7X7_50": cv2.aruco.DICT_7X7_50, + "DICT_7X7_100": cv2.aruco.DICT_7X7_100, + "DICT_7X7_250": cv2.aruco.DICT_7X7_250, + "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000, + "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL, +} + + +class PaperGridRvizPublisher(object): + def __init__(self): + rospy.init_node("paper_grid_rviz_publisher", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.image_topic_name = rospy.get_param("~image_topic_name", "/nuc/rgb/image_raw") + self.camera_info_topic_name = rospy.get_param("~camera_info_topic_name", "/nuc/rgb/camera_info") + self.camera_frame_id = rospy.get_param("~camera_frame_id", "") + self.target_frame_id = rospy.get_param("~target_frame_id", "") + + self.marker_topic_name = rospy.get_param("~marker_topic_name", "paper_grid_markers") + self.pose_topic_name = rospy.get_param("~pose_topic_name", "paper_grid_pose") + self.publish_debug_image = rospy.get_param("~publish_debug_image", True) + self.debug_image_topic_name = rospy.get_param("~debug_image_topic_name", "paper_grid_debug_image") + self.depth_image_topic_name = rospy.get_param("~depth_image_topic_name", "") + self.use_depth_for_3d_points = rospy.get_param("~use_depth_for_3d_points", False) + self.depth_unit_scale = float(rospy.get_param("~depth_unit_scale", 0.001)) # mm->m for 16UC1 + self.depth_max_age_sec = float(rospy.get_param("~depth_max_age_sec", 0.15)) + self.depth_patch_radius_px = int(rospy.get_param("~depth_patch_radius_px", 2)) + + self.min_markers_for_surface = int( + rospy.get_param("~min_markers_for_surface", rospy.get_param("~min_markers_for_pose", 4)) + ) + self.tf_lookup_timeout_sec = float(rospy.get_param("~tf_lookup_timeout_sec", 0.05)) + self.pnp_reprojection_error = float(rospy.get_param("~pnp_reprojection_error", 3.0)) + + self.grid_line_width = float(rospy.get_param("~grid_line_width", 0.003)) + self.outline_line_width = float(rospy.get_param("~outline_line_width", 0.006)) + self.surface_alpha = float(rospy.get_param("~surface_alpha", 0.12)) + self.point_scale = float(rospy.get_param("~point_scale", 0.012)) + self.text_height = float(rospy.get_param("~text_height", 0.03)) + self.text_z_offset = float(rospy.get_param("~text_z_offset", 0.01)) + self.normal_length = float(rospy.get_param("~normal_length", 0.05)) + self.normal_line_width = float(rospy.get_param("~normal_line_width", 0.003)) + + self.layout_data = self._load_layout(self.layout_json_path) + self.grid_rows, self.grid_cols = self._read_grid_shape(self.layout_data) + self.layout_marker_ids = [] + self.id_to_rowcol = {} + self.rowcol_to_id = {} + self.id_to_size_m = {} + self.default_marker_size_m = float(self.layout_data.get("grid", {}).get("marker_mm", 60.0)) / 1000.0 + self._read_marker_layout(self.layout_data) + + self.boundary_pairs = self._build_boundary_pairs() + self.surface_triangles = self._build_surface_triangles() + self.mesh_line_pairs = self._build_mesh_line_pairs() + + self.obj_corners_cache = {} + + layout_dict_name = self.layout_data.get("grid", {}).get("dict", "DICT_4X4_50") + self.aruco_dict_name = rospy.get_param("~aruco_dict_name", layout_dict_name) + self.aruco_dict = self._make_aruco_dict(self.aruco_dict_name) + self.aruco_params = self._make_aruco_params() + self.aruco_detector = None + if hasattr(cv2.aruco, "ArucoDetector"): + self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params) + + self.pnp_square_flag = cv2.SOLVEPNP_ITERATIVE + if hasattr(cv2, "SOLVEPNP_IPPE_SQUARE"): + self.pnp_square_flag = cv2.SOLVEPNP_IPPE_SQUARE + + self.bridge = CvBridge() + self.marker_pub = rospy.Publisher(self.marker_topic_name, MarkerArray, queue_size=1) + self.pose_pub = rospy.Publisher(self.pose_topic_name, PoseStamped, queue_size=1) + self.debug_image_pub = None + if self.publish_debug_image: + self.debug_image_pub = rospy.Publisher(self.debug_image_topic_name, Image, queue_size=1) + + self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10.0)) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + self.K = None + self.D = None + self.camera_info_ready = False + self.markers_visible = False + self.data_lock = threading.Lock() + self.depth_lock = threading.Lock() + self.latest_depth = None + self.latest_depth_stamp = None + + self.camera_info_sub = rospy.Subscriber( + self.camera_info_topic_name, CameraInfo, self._camera_info_cb, queue_size=1 + ) + self.image_sub = rospy.Subscriber(self.image_topic_name, Image, self._image_cb, queue_size=1) + self.depth_sub = None + if self.depth_image_topic_name: + self.depth_sub = rospy.Subscriber(self.depth_image_topic_name, Image, self._depth_cb, queue_size=1) + rospy.loginfo("Depth subscription enabled: %s", self.depth_image_topic_name) + if not self.use_depth_for_3d_points: + rospy.logwarn("Depth topic provided but ~use_depth_for_3d_points is false.") + + rospy.loginfo("paper_grid_rviz_publisher initialized.") + rospy.loginfo("Layout markers loaded: %d", len(self.layout_marker_ids)) + rospy.loginfo("Grid shape: %d rows x %d cols", self.grid_rows, self.grid_cols) + rospy.loginfo("Aruco dictionary: %s", self.aruco_dict_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + return json.load(f) + + def _read_grid_shape(self, layout_data): + grid = layout_data.get("grid", {}) + rows = int(grid.get("rows", 0)) + cols = int(grid.get("cols", 0)) + if rows <= 0 or cols <= 0: + raise ValueError("Invalid rows/cols in layout json") + return rows, cols + + def _read_marker_layout(self, layout_data): + markers = layout_data.get("markers", []) + if not markers: + raise ValueError("Layout has no markers") + + for marker in markers: + marker_id = int(marker["id"]) + row = int(marker["row"]) + col = int(marker["col"]) + size_m = float(marker.get("marker_length_mm", self.default_marker_size_m * 1000.0)) / 1000.0 + + if not (0 <= row < self.grid_rows and 0 <= col < self.grid_cols): + raise ValueError("Marker {} row/col out of bounds".format(marker_id)) + + self.layout_marker_ids.append(marker_id) + self.id_to_rowcol[marker_id] = (row, col) + self.rowcol_to_id[(row, col)] = marker_id + self.id_to_size_m[marker_id] = size_m + + self.layout_marker_ids = sorted(self.layout_marker_ids) + + def _build_boundary_pairs(self): + pairs = [] + + # Top row + for c in range(self.grid_cols - 1): + pairs.append(((0, c), (0, c + 1))) + # Right column + for r in range(self.grid_rows - 1): + pairs.append(((r, self.grid_cols - 1), (r + 1, self.grid_cols - 1))) + # Bottom row (reverse) + for c in range(self.grid_cols - 1, 0, -1): + pairs.append(((self.grid_rows - 1, c), (self.grid_rows - 1, c - 1))) + # Left column (reverse) + for r in range(self.grid_rows - 1, 0, -1): + pairs.append(((r, 0), (r - 1, 0))) + + id_pairs = [] + for a_rc, b_rc in pairs: + a_id = self.rowcol_to_id.get(a_rc) + b_id = self.rowcol_to_id.get(b_rc) + if a_id is not None and b_id is not None: + id_pairs.append((a_id, b_id)) + return id_pairs + + def _build_surface_triangles(self): + triangles = [] + for r in range(self.grid_rows - 1): + for c in range(self.grid_cols - 1): + id00 = self.rowcol_to_id.get((r, c)) + id01 = self.rowcol_to_id.get((r, c + 1)) + id10 = self.rowcol_to_id.get((r + 1, c)) + id11 = self.rowcol_to_id.get((r + 1, c + 1)) + if id00 is None or id01 is None or id10 is None or id11 is None: + continue + triangles.append((id00, id01, id11)) + triangles.append((id00, id11, id10)) + return triangles + + def _build_mesh_line_pairs(self): + pairs = [] + for r in range(self.grid_rows): + for c in range(self.grid_cols - 1): + a_id = self.rowcol_to_id.get((r, c)) + b_id = self.rowcol_to_id.get((r, c + 1)) + if a_id is not None and b_id is not None: + pairs.append((a_id, b_id)) + for r in range(self.grid_rows - 1): + for c in range(self.grid_cols): + a_id = self.rowcol_to_id.get((r, c)) + b_id = self.rowcol_to_id.get((r + 1, c)) + if a_id is not None and b_id is not None: + pairs.append((a_id, b_id)) + return pairs + + def _make_aruco_dict(self, name): + if name not in ARUCO_DICT: + raise ValueError("Unsupported aruco dictionary: {}".format(name)) + dict_id = ARUCO_DICT[name] + if hasattr(cv2.aruco, "getPredefinedDictionary"): + return cv2.aruco.getPredefinedDictionary(dict_id) + return cv2.aruco.Dictionary_get(dict_id) + + def _make_aruco_params(self): + if hasattr(cv2.aruco, "DetectorParameters_create"): + return cv2.aruco.DetectorParameters_create() + return cv2.aruco.DetectorParameters() + + def _camera_info_cb(self, msg): + with self.data_lock: + self.K = np.asarray(msg.K, dtype=np.float64).reshape(3, 3) + self.D = np.asarray(msg.D, dtype=np.float64) + self.camera_info_ready = True + if not self.camera_frame_id: + self.camera_frame_id = msg.header.frame_id + + def _depth_cb(self, msg): + try: + depth = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + except CvBridgeError: + return + with self.depth_lock: + self.latest_depth = depth + self.latest_depth_stamp = msg.header.stamp + + def _detect_markers(self, gray): + if self.aruco_detector is not None: + return self.aruco_detector.detectMarkers(gray) + return cv2.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruco_params) + + def _marker_object_corners(self, marker_size_m): + key = round(float(marker_size_m), 6) + if key in self.obj_corners_cache: + return self.obj_corners_cache[key] + + half = key * 0.5 + obj = np.asarray( + [ + [-half, half, 0.0], # top-left + [half, half, 0.0], # top-right + [half, -half, 0.0], # bottom-right + [-half, -half, 0.0], # bottom-left + ], + dtype=np.float64, + ) + self.obj_corners_cache[key] = obj + return obj + + def _estimate_marker_center_cam(self, marker_corners_img, marker_size_m, K, D): + obj = self._marker_object_corners(marker_size_m) + success, rvec, tvec = cv2.solvePnP( + obj, + marker_corners_img, + K, + D, + flags=self.pnp_square_flag, + ) + if not success and self.pnp_square_flag != cv2.SOLVEPNP_ITERATIVE: + success, rvec, tvec = cv2.solvePnP( + obj, + marker_corners_img, + K, + D, + flags=cv2.SOLVEPNP_ITERATIVE, + ) + if not success: + return None, None, None + + projected, _ = cv2.projectPoints(obj, rvec, tvec, K, D) + reproj_error = np.mean(np.linalg.norm(projected.reshape(-1, 2) - marker_corners_img, axis=1)) + if reproj_error > self.pnp_reprojection_error: + return None, None, None + + center_cam = np.asarray(tvec, dtype=np.float64).reshape(3) + return center_cam, rvec, tvec + + def _depth_value_to_m(self, val): + if np.isnan(val) or np.isinf(val): + return None + if val <= 0: + return None + return float(val) * self.depth_unit_scale + + def _sample_depth_m(self, depth_img, u, v): + h, w = depth_img.shape[:2] + uu = int(round(u)) + vv = int(round(v)) + if uu < 0 or uu >= w or vv < 0 or vv >= h: + return None + + r = max(0, self.depth_patch_radius_px) + u0 = max(0, uu - r) + u1 = min(w, uu + r + 1) + v0 = max(0, vv - r) + v1 = min(h, vv + r + 1) + + patch = depth_img[v0:v1, u0:u1] + if patch.size == 0: + return None + + vals = patch.reshape(-1) + depth_m_vals = [] + for x in vals: + d = self._depth_value_to_m(float(x)) + if d is not None: + depth_m_vals.append(d) + + if not depth_m_vals: + return None + return float(np.median(np.asarray(depth_m_vals, dtype=np.float64))) + + @staticmethod + def _pixel_to_cam(K, u, v, z_m): + fx = K[0, 0] + fy = K[1, 1] + cx = K[0, 2] + cy = K[1, 2] + x = (u - cx) * z_m / fx + y = (v - cy) * z_m / fy + return np.asarray([x, y, z_m], dtype=np.float64) + + def _image_cb(self, msg): + with self.data_lock: + if not self.camera_info_ready: + return + K = self.K.copy() + D = self.D.copy() + camera_frame_id = self.camera_frame_id + + if not camera_frame_id: + rospy.logwarn_throttle(5.0, "Camera frame is empty; waiting for CameraInfo.") + return + + try: + frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + except CvBridgeError as exc: + rospy.logwarn_throttle(5.0, "cv_bridge conversion failed: %s", str(exc)) + return + + stamp = msg.header.stamp if msg.header.stamp != rospy.Time(0) else rospy.Time.now() + target_frame = self.target_frame_id if self.target_frame_id else camera_frame_id + R_tc, t_tc = self._camera_to_target_transform(camera_frame_id, target_frame, stamp) + if R_tc is None: + return + + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + corners, ids, _ = self._detect_markers(gray) + + detected_points_target = {} + detected_normals_target = {} + depth_img = None + depth_stamp = None + if self.use_depth_for_3d_points: + with self.depth_lock: + if self.latest_depth is not None: + depth_img = self.latest_depth.copy() + depth_stamp = self.latest_depth_stamp + + if ids is not None and len(ids) > 0: + ids = ids.flatten().tolist() + for i, marker_id in enumerate(ids): + if marker_id not in self.id_to_rowcol: + continue + marker_size_m = self.id_to_size_m.get(marker_id, self.default_marker_size_m) + marker_corners_img = np.asarray(corners[i], dtype=np.float64).reshape(4, 2) + + center_cam, rvec, tvec = self._estimate_marker_center_cam( + marker_corners_img, marker_size_m, K, D + ) + if center_cam is None: + continue + + R_cm, _ = cv2.Rodrigues(rvec) + normal_cam = R_cm[:, 2] + if np.linalg.norm(normal_cam) > 1e-8: + normal_cam = normal_cam / np.linalg.norm(normal_cam) + + # Optional depth override for better non-planar reconstruction. + if self.use_depth_for_3d_points and depth_img is not None and depth_stamp is not None: + age = abs((stamp - depth_stamp).to_sec()) + if age <= self.depth_max_age_sec: + center_px = np.mean(marker_corners_img, axis=0) + depth_m = self._sample_depth_m(depth_img, center_px[0], center_px[1]) + if depth_m is not None: + center_cam = self._pixel_to_cam(K, center_px[0], center_px[1], depth_m) + + center_target = np.matmul(R_tc, center_cam.reshape(3, 1)) + t_tc + detected_points_target[marker_id] = center_target.reshape(3) + normal_target = np.matmul(R_tc, normal_cam.reshape(3, 1)).reshape(3) + if np.linalg.norm(normal_target) > 1e-8: + normal_target = normal_target / np.linalg.norm(normal_target) + detected_normals_target[marker_id] = normal_target + + if self.publish_debug_image and hasattr(cv2.aruco, "drawDetectedMarkers"): + cv2.aruco.drawDetectedMarkers(frame, [corners[i]], np.asarray([[marker_id]], dtype=np.int32)) + if hasattr(cv2.aruco, "drawAxis"): + cv2.aruco.drawAxis(frame, K, D, rvec, tvec, marker_size_m * 0.5) + + if self.publish_debug_image and self.debug_image_pub is not None: + self._publish_debug_image(frame, stamp) + + if len(detected_points_target) < self.min_markers_for_surface: + self._clear_markers_if_needed(stamp, target_frame) + rospy.logwarn_throttle( + 2.0, + "Detected %d layout markers, need at least %d for deformable surface.", + len(detected_points_target), + self.min_markers_for_surface, + ) + return + + self._publish_pose_from_points(stamp, target_frame, detected_points_target) + self._publish_deformed_markers(stamp, target_frame, detected_points_target, detected_normals_target) + self.markers_visible = True + + def _camera_to_target_transform(self, camera_frame_id, target_frame_id, stamp): + if target_frame_id == camera_frame_id: + return np.eye(3), np.zeros((3, 1)) + try: + tf_msg = self.tf_buffer.lookup_transform( + target_frame_id, + camera_frame_id, + stamp, + rospy.Duration(self.tf_lookup_timeout_sec), + ) + except Exception as exc: + rospy.logwarn_throttle( + 2.0, + "TF lookup failed: %s -> %s (%s)", + target_frame_id, + camera_frame_id, + str(exc), + ) + return None, None + + q = tf_msg.transform.rotation + t = tf_msg.transform.translation + R = self._quat_to_rot(q.x, q.y, q.z, q.w) + T = np.asarray([[t.x], [t.y], [t.z]], dtype=np.float64) + return R, T + + @staticmethod + def _quat_to_rot(x, y, z, w): + xx = x * x + yy = y * y + zz = z * z + xy = x * y + xz = x * z + yz = y * z + wx = w * x + wy = w * y + wz = w * z + return np.asarray( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ], + dtype=np.float64, + ) + + @staticmethod + def _rot_to_quat(R): + m = R + trace = float(m[0, 0] + m[1, 1] + m[2, 2]) + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * s + qx = (m[2, 1] - m[1, 2]) / s + qy = (m[0, 2] - m[2, 0]) / s + qz = (m[1, 0] - m[0, 1]) / s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0 + qw = (m[2, 1] - m[1, 2]) / s + qx = 0.25 * s + qy = (m[0, 1] + m[1, 0]) / s + qz = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0 + qw = (m[0, 2] - m[2, 0]) / s + qx = (m[0, 1] + m[1, 0]) / s + qy = 0.25 * s + qz = (m[1, 2] + m[2, 1]) / s + else: + s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0 + qw = (m[1, 0] - m[0, 1]) / s + qx = (m[0, 2] + m[2, 0]) / s + qy = (m[1, 2] + m[2, 1]) / s + qz = 0.25 * s + return qx, qy, qz, qw + + @staticmethod + def _to_point(x, y, z): + p = Point() + p.x = float(x) + p.y = float(y) + p.z = float(z) + return p + + def _publish_pose_from_points(self, stamp, frame_id, points_by_id): + if len(points_by_id) < 3: + return + + pts = np.asarray(list(points_by_id.values()), dtype=np.float64) + centroid = np.mean(pts, axis=0) + centered = pts - centroid + + try: + _, _, vh = np.linalg.svd(centered, full_matrices=False) + except np.linalg.LinAlgError: + return + + x_axis = vh[0, :] + normal = vh[2, :] + + if np.linalg.norm(x_axis) < 1e-8 or np.linalg.norm(normal) < 1e-8: + return + + x_axis = x_axis / np.linalg.norm(x_axis) + normal = normal / np.linalg.norm(normal) + y_axis = np.cross(normal, x_axis) + if np.linalg.norm(y_axis) < 1e-8: + return + y_axis = y_axis / np.linalg.norm(y_axis) + x_axis = np.cross(y_axis, normal) + x_axis = x_axis / np.linalg.norm(x_axis) + + R = np.column_stack((x_axis, y_axis, normal)) + qx, qy, qz, qw = self._rot_to_quat(R) + + msg = PoseStamped() + msg.header.stamp = stamp + msg.header.frame_id = frame_id + msg.pose.position.x = float(centroid[0]) + msg.pose.position.y = float(centroid[1]) + msg.pose.position.z = float(centroid[2]) + msg.pose.orientation.x = qx + msg.pose.orientation.y = qy + msg.pose.orientation.z = qz + msg.pose.orientation.w = qw + self.pose_pub.publish(msg) + + def _publish_deformed_markers(self, stamp, frame_id, points_by_id, normals_by_id): + msg = MarkerArray() + + # Deformed surface from detected tag points. + surface = Marker() + surface.header.stamp = stamp + surface.header.frame_id = frame_id + surface.ns = "paper_surface" + surface.id = 0 + surface.type = Marker.TRIANGLE_LIST + surface.action = Marker.ADD + surface.pose.orientation.w = 1.0 + surface.scale.x = 1.0 + surface.scale.y = 1.0 + surface.scale.z = 1.0 + surface.color.r = 0.1 + surface.color.g = 0.5 + surface.color.b = 1.0 + surface.color.a = self.surface_alpha + + for tri in self.surface_triangles: + if tri[0] in points_by_id and tri[1] in points_by_id and tri[2] in points_by_id: + p0 = points_by_id[tri[0]] + p1 = points_by_id[tri[1]] + p2 = points_by_id[tri[2]] + surface.points.append(self._to_point(*p0)) + surface.points.append(self._to_point(*p1)) + surface.points.append(self._to_point(*p2)) + msg.markers.append(surface) + + # Mesh lines between neighboring tags. + grid = Marker() + grid.header.stamp = stamp + grid.header.frame_id = frame_id + grid.ns = "aruco_grid" + grid.id = 1 + grid.type = Marker.LINE_LIST + grid.action = Marker.ADD + grid.pose.orientation.w = 1.0 + grid.scale.x = self.grid_line_width + grid.color.r = 0.0 + grid.color.g = 1.0 + grid.color.b = 0.2 + grid.color.a = 0.95 + for a_id, b_id in self.mesh_line_pairs: + if a_id in points_by_id and b_id in points_by_id: + grid.points.append(self._to_point(*points_by_id[a_id])) + grid.points.append(self._to_point(*points_by_id[b_id])) + msg.markers.append(grid) + + # Boundary lines. + outline = Marker() + outline.header.stamp = stamp + outline.header.frame_id = frame_id + outline.ns = "paper_outline" + outline.id = 2 + outline.type = Marker.LINE_LIST + outline.action = Marker.ADD + outline.pose.orientation.w = 1.0 + outline.scale.x = self.outline_line_width + outline.color.r = 1.0 + outline.color.g = 0.2 + outline.color.b = 0.2 + outline.color.a = 1.0 + for a_id, b_id in self.boundary_pairs: + if a_id in points_by_id and b_id in points_by_id: + outline.points.append(self._to_point(*points_by_id[a_id])) + outline.points.append(self._to_point(*points_by_id[b_id])) + msg.markers.append(outline) + + # Tag points. + points_marker = Marker() + points_marker.header.stamp = stamp + points_marker.header.frame_id = frame_id + points_marker.ns = "paper_points" + points_marker.id = 3 + points_marker.type = Marker.SPHERE_LIST + points_marker.action = Marker.ADD + points_marker.pose.orientation.w = 1.0 + points_marker.scale.x = self.point_scale + points_marker.scale.y = self.point_scale + points_marker.scale.z = self.point_scale + points_marker.color.r = 1.0 + points_marker.color.g = 0.8 + points_marker.color.b = 0.0 + points_marker.color.a = 1.0 + for marker_id in sorted(points_by_id.keys()): + points_marker.points.append(self._to_point(*points_by_id[marker_id])) + msg.markers.append(points_marker) + + # Tag IDs on top of detected points. + for marker_id in sorted(points_by_id.keys()): + p = points_by_id[marker_id] + text = Marker() + text.header.stamp = stamp + text.header.frame_id = frame_id + text.ns = "aruco_ids" + text.id = 1000 + int(marker_id) + text.type = Marker.TEXT_VIEW_FACING + text.action = Marker.ADD + text.pose.position.x = float(p[0]) + text.pose.position.y = float(p[1]) + text.pose.position.z = float(p[2] + self.text_z_offset) + text.pose.orientation.w = 1.0 + text.scale.z = self.text_height + text.color.r = 1.0 + text.color.g = 1.0 + text.color.b = 1.0 + text.color.a = 1.0 + text.text = str(marker_id) + msg.markers.append(text) + + # Per-tag surface normals. + normals = Marker() + normals.header.stamp = stamp + normals.header.frame_id = frame_id + normals.ns = "paper_normals" + normals.id = 4 + normals.type = Marker.LINE_LIST + normals.action = Marker.ADD + normals.pose.orientation.w = 1.0 + normals.scale.x = self.normal_line_width + normals.color.r = 1.0 + normals.color.g = 0.0 + normals.color.b = 1.0 + normals.color.a = 1.0 + for marker_id in sorted(points_by_id.keys()): + if marker_id not in normals_by_id: + continue + p = points_by_id[marker_id] + n = normals_by_id[marker_id] + p2 = p + n * self.normal_length + normals.points.append(self._to_point(*p)) + normals.points.append(self._to_point(*p2)) + msg.markers.append(normals) + + self.marker_pub.publish(msg) + + def _clear_markers_if_needed(self, stamp, frame_id): + if not self.markers_visible: + return + clear = MarkerArray() + marker = Marker() + marker.header.stamp = stamp + marker.header.frame_id = frame_id + marker.action = Marker.DELETEALL + clear.markers.append(marker) + self.marker_pub.publish(clear) + self.markers_visible = False + + def _publish_debug_image(self, cv_img, stamp): + try: + msg = self.bridge.cv2_to_imgmsg(cv_img, encoding="bgr8") + msg.header.stamp = stamp + self.debug_image_pub.publish(msg) + except CvBridgeError: + pass + + +if __name__ == "__main__": + try: + PaperGridRvizPublisher() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_grid_rviz_publisher failed: %s", str(exc)) diff --git a/src/tf_broadcasters/src/paper_surface_estimator.py b/src/tf_broadcasters/src/paper_surface_estimator.py new file mode 100644 index 0000000..d650a4e --- /dev/null +++ b/src/tf_broadcasters/src/paper_surface_estimator.py @@ -0,0 +1,394 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os + +import numpy as np +import rospy +from assistive_msgs.msg import TagPose +from assistive_msgs.msg import TagPoseArray + + +class PaperSurfaceEstimator(object): + def __init__(self): + rospy.init_node("paper_surface_estimator", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.input_pose_topic_name = rospy.get_param("~input_pose_topic_name", "paper_tag_poses") + self.output_pose_topic_name = rospy.get_param("~output_pose_topic_name", "paper_surface_state") + self.measurement_weight = float(rospy.get_param("~measurement_weight", 1.0)) + self.temporal_weight = float(rospy.get_param("~temporal_weight", 0.3)) + self.edge_weight = float(rospy.get_param("~edge_weight", 0.35)) + self.smoothness_weight = float(rospy.get_param("~smoothness_weight", 0.15)) + self.prediction_variance_growth = float(rospy.get_param("~prediction_variance_growth", 1.5)) + self.missing_position_variance_step = float(rospy.get_param("~missing_position_variance_step", 2.5e-4)) + self.missing_orientation_variance_step = float(rospy.get_param("~missing_orientation_variance_step", 5.0e-3)) + + layout_data = self._load_layout(self.layout_json_path) + self.layout_dict_name = layout_data.get("grid", {}).get("dict", "DICT_4X4_50") + self.id_to_center, self.id_to_rowcol, self.rowcol_to_id = self._read_layout(layout_data) + self.layout_ids = sorted(self.id_to_center.keys()) + self.id_to_index = {marker_id: idx for idx, marker_id in enumerate(self.layout_ids)} + self.neighbors = self._build_neighbors() + self.edges = self._build_edges() + + self.prev_points = None + self.prev_normals = None + self.prev_covariances = {} + self.prev_frame_id = None + + self.pose_pub = rospy.Publisher(self.output_pose_topic_name, TagPoseArray, queue_size=1) + self.pose_sub = rospy.Subscriber(self.input_pose_topic_name, TagPoseArray, self._pose_cb, queue_size=1) + + rospy.loginfo("paper_surface_estimator initialized.") + rospy.loginfo("Publishing surface state on: %s", self.output_pose_topic_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + return json.load(f) + + def _read_layout(self, layout_data): + id_to_center = {} + id_to_rowcol = {} + rowcol_to_id = {} + for marker in layout_data.get("markers", []): + marker_id = int(marker["id"]) + row = int(marker["row"]) + col = int(marker["col"]) + corners = np.asarray(marker.get("corners_m", []), dtype=np.float64) + if corners.shape != (4, 3): + raise ValueError("Marker {} is missing corners_m".format(marker_id)) + center = np.mean(corners, axis=0) + id_to_center[marker_id] = center + id_to_rowcol[marker_id] = (row, col) + rowcol_to_id[(row, col)] = marker_id + if not id_to_center: + raise ValueError("Layout has no markers") + return id_to_center, id_to_rowcol, rowcol_to_id + + def _build_neighbors(self): + neighbors = {marker_id: [] for marker_id in self.layout_ids} + for marker_id, (row, col) in self.id_to_rowcol.items(): + candidate_rcs = [ + (row - 1, col), + (row + 1, col), + (row, col - 1), + (row, col + 1), + ] + for candidate_rc in candidate_rcs: + neighbor_id = self.rowcol_to_id.get(candidate_rc) + if neighbor_id is not None: + neighbors[marker_id].append(neighbor_id) + return neighbors + + def _build_edges(self): + edges = [] + for marker_id, neighbor_ids in self.neighbors.items(): + for neighbor_id in neighbor_ids: + if marker_id < neighbor_id: + edges.append((marker_id, neighbor_id)) + return edges + + @staticmethod + def _quat_to_rot(x, y, z, w): + xx = x * x + yy = y * y + zz = z * z + xy = x * y + xz = x * z + yz = y * z + wx = w * x + wy = w * y + wz = w * z + return np.asarray( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ], + dtype=np.float64, + ) + + @staticmethod + def _rot_to_quat(R): + trace = float(R[0, 0] + R[1, 1] + R[2, 2]) + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * s + qx = (R[2, 1] - R[1, 2]) / s + qy = (R[0, 2] - R[2, 0]) / s + qz = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0 + qw = (R[2, 1] - R[1, 2]) / s + qx = 0.25 * s + qy = (R[0, 1] + R[1, 0]) / s + qz = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0 + qw = (R[0, 2] - R[2, 0]) / s + qx = (R[0, 1] + R[1, 0]) / s + qy = 0.25 * s + qz = (R[1, 2] + R[2, 1]) / s + else: + s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0 + qw = (R[1, 0] - R[0, 1]) / s + qx = (R[0, 2] + R[2, 0]) / s + qy = (R[1, 2] + R[2, 1]) / s + qz = 0.25 * s + return qx, qy, qz, qw + + @staticmethod + def _normalize(vec, fallback=None): + norm = np.linalg.norm(vec) + if norm < 1e-8: + return fallback + return vec / norm + + def _pose_msg_to_point(self, pose_msg): + position = pose_msg.pose.pose.position + return np.asarray([position.x, position.y, position.z], dtype=np.float64) + + def _pose_msg_to_normal(self, pose_msg): + normal = np.asarray([pose_msg.normal.x, pose_msg.normal.y, pose_msg.normal.z], dtype=np.float64) + return self._normalize(normal, fallback=np.asarray([0.0, 0.0, 1.0], dtype=np.float64)) + + def _compute_rigid_initialization(self, observed_points, observed_normals): + all_points = {marker_id: self.id_to_center[marker_id].copy() for marker_id in self.layout_ids} + if len(observed_points) >= 3: + obs_ids = sorted(observed_points.keys()) + rest_pts = np.asarray([self.id_to_center[marker_id] for marker_id in obs_ids], dtype=np.float64) + obs_pts = np.asarray([observed_points[marker_id] for marker_id in obs_ids], dtype=np.float64) + rest_centroid = np.mean(rest_pts, axis=0) + obs_centroid = np.mean(obs_pts, axis=0) + rest_centered = rest_pts - rest_centroid + obs_centered = obs_pts - obs_centroid + H = np.matmul(rest_centered.T, obs_centered) + try: + U, _, Vt = np.linalg.svd(H, full_matrices=False) + R = np.matmul(Vt.T, U.T) + except np.linalg.LinAlgError: + R = np.eye(3) + if np.linalg.det(R) < 0.0: + Vt[-1, :] *= -1.0 + R = np.matmul(Vt.T, U.T) + t = obs_centroid - np.matmul(R, rest_centroid.reshape(3, 1)).reshape(3) + for marker_id in self.layout_ids: + all_points[marker_id] = np.matmul(R, self.id_to_center[marker_id].reshape(3, 1)).reshape(3) + t + avg_normal = np.mean(np.asarray(list(observed_normals.values()), dtype=np.float64), axis=0) + avg_normal = self._normalize(avg_normal, fallback=R[:, 2]) + elif observed_points: + avg_point = np.mean(np.asarray(list(observed_points.values()), dtype=np.float64), axis=0) + avg_rest = np.mean(np.asarray(list(self.id_to_center.values()), dtype=np.float64), axis=0) + translation = avg_point - avg_rest + for marker_id in self.layout_ids: + all_points[marker_id] = self.id_to_center[marker_id] + translation + avg_normal = np.mean(np.asarray(list(observed_normals.values()), dtype=np.float64), axis=0) + avg_normal = self._normalize(avg_normal, fallback=np.asarray([0.0, 0.0, 1.0], dtype=np.float64)) + else: + avg_normal = np.asarray([0.0, 0.0, 1.0], dtype=np.float64) + return all_points, avg_normal + + def _solve_axis(self, axis_index, observed_points, measurement_weights, reference_points): + num_nodes = len(self.layout_ids) + rows = [] + rhs = [] + + for marker_id, observed_point in observed_points.items(): + row = np.zeros(num_nodes, dtype=np.float64) + row[self.id_to_index[marker_id]] = np.sqrt(measurement_weights[marker_id]) + rows.append(row) + rhs.append(np.sqrt(measurement_weights[marker_id]) * observed_point[axis_index]) + + for marker_id in self.layout_ids: + row = np.zeros(num_nodes, dtype=np.float64) + row[self.id_to_index[marker_id]] = np.sqrt(self.temporal_weight) + rows.append(row) + rhs.append(np.sqrt(self.temporal_weight) * reference_points[marker_id][axis_index]) + + for marker_a, marker_b in self.edges: + row = np.zeros(num_nodes, dtype=np.float64) + row[self.id_to_index[marker_a]] = np.sqrt(self.edge_weight) + row[self.id_to_index[marker_b]] = -np.sqrt(self.edge_weight) + rows.append(row) + target_delta = reference_points[marker_a][axis_index] - reference_points[marker_b][axis_index] + rhs.append(np.sqrt(self.edge_weight) * target_delta) + + for marker_id in self.layout_ids: + neighbor_ids = self.neighbors.get(marker_id, []) + if not neighbor_ids: + continue + row = np.zeros(num_nodes, dtype=np.float64) + row[self.id_to_index[marker_id]] = np.sqrt(self.smoothness_weight) + coeff = -np.sqrt(self.smoothness_weight) / float(len(neighbor_ids)) + for neighbor_id in neighbor_ids: + row[self.id_to_index[neighbor_id]] = coeff + rows.append(row) + neighbor_mean = np.mean( + np.asarray([reference_points[neighbor_id] for neighbor_id in neighbor_ids], dtype=np.float64), + axis=0, + ) + laplacian_target = reference_points[marker_id][axis_index] - neighbor_mean[axis_index] + rhs.append(np.sqrt(self.smoothness_weight) * laplacian_target) + + A = np.asarray(rows, dtype=np.float64) + b = np.asarray(rhs, dtype=np.float64) + solution, _, _, _ = np.linalg.lstsq(A, b, rcond=None) + return solution + + def _estimate_normals(self, estimated_points, observed_normals, reference_normal): + normals = {} + for marker_id in self.layout_ids: + points = [estimated_points[marker_id]] + for neighbor_id in self.neighbors.get(marker_id, []): + points.append(estimated_points[neighbor_id]) + if len(points) >= 3: + pts = np.asarray(points, dtype=np.float64) + centered = pts - np.mean(pts, axis=0) + try: + _, _, vh = np.linalg.svd(centered, full_matrices=False) + normal = self._normalize(vh[-1, :], fallback=reference_normal) + except np.linalg.LinAlgError: + normal = reference_normal + else: + normal = reference_normal + + hint = observed_normals.get(marker_id, reference_normal) + if np.dot(normal, hint) < 0.0: + normal = -normal + normals[marker_id] = normal + return normals + + def _orientation_from_normal(self, normal, marker_id, estimated_points): + normal = self._normalize(normal, fallback=np.asarray([0.0, 0.0, 1.0], dtype=np.float64)) + x_axis = None + row, col = self.id_to_rowcol[marker_id] + right_id = self.rowcol_to_id.get((row, col + 1)) + left_id = self.rowcol_to_id.get((row, col - 1)) + if right_id is not None: + x_axis = estimated_points[right_id] - estimated_points[marker_id] + elif left_id is not None: + x_axis = estimated_points[marker_id] - estimated_points[left_id] + if x_axis is None or np.linalg.norm(x_axis) < 1e-8: + x_axis = np.asarray([1.0, 0.0, 0.0], dtype=np.float64) + x_axis = x_axis - np.dot(x_axis, normal) * normal + x_axis = self._normalize(x_axis, fallback=np.asarray([1.0, 0.0, 0.0], dtype=np.float64)) + y_axis = np.cross(normal, x_axis) + y_axis = self._normalize(y_axis, fallback=np.asarray([0.0, 1.0, 0.0], dtype=np.float64)) + x_axis = np.cross(y_axis, normal) + x_axis = self._normalize(x_axis, fallback=np.asarray([1.0, 0.0, 0.0], dtype=np.float64)) + return np.column_stack((x_axis, y_axis, normal)) + + def _inflate_covariance(self, marker_id, observed_pose): + if observed_pose is not None: + return list(observed_pose.pose.covariance) + + covariance = np.asarray( + self.prev_covariances.get(marker_id, np.zeros(36, dtype=np.float64)), + dtype=np.float64, + ) + covariance[0] = covariance[0] * self.prediction_variance_growth + self.missing_position_variance_step + covariance[7] = covariance[7] * self.prediction_variance_growth + self.missing_position_variance_step + covariance[14] = covariance[14] * self.prediction_variance_growth + self.missing_position_variance_step + covariance[21] = covariance[21] * self.prediction_variance_growth + self.missing_orientation_variance_step + covariance[28] = covariance[28] * self.prediction_variance_growth + self.missing_orientation_variance_step + covariance[35] = covariance[35] * self.prediction_variance_growth + self.missing_orientation_variance_step + return list(covariance) + + def _pose_cb(self, msg): + if self.prev_frame_id is not None and msg.header.frame_id != self.prev_frame_id: + self.prev_points = None + self.prev_normals = None + self.prev_covariances = {} + self.prev_frame_id = msg.header.frame_id + + observed_pose_msgs = {int(pose.id): pose for pose in msg.poses} + observed_points = {marker_id: self._pose_msg_to_point(pose) for marker_id, pose in observed_pose_msgs.items()} + observed_normals = {marker_id: self._pose_msg_to_normal(pose) for marker_id, pose in observed_pose_msgs.items()} + + initial_points, initial_normal = self._compute_rigid_initialization(observed_points, observed_normals) + if self.prev_points is None: + reference_points = initial_points + reference_normal = initial_normal + else: + reference_points = self.prev_points + reference_normal = np.mean(np.asarray(list(self.prev_normals.values()), dtype=np.float64), axis=0) + reference_normal = self._normalize(reference_normal, fallback=initial_normal) + + measurement_weights = {} + for marker_id, pose_msg in observed_pose_msgs.items(): + covariance = np.asarray(pose_msg.pose.covariance, dtype=np.float64) + pos_var = float(np.mean(covariance[[0, 7, 14]])) if covariance.size >= 15 else 1.0e-4 + pos_var = max(pos_var, 1.0e-6) + measurement_weights[marker_id] = self.measurement_weight * max(0.1, pose_msg.detection_score) / pos_var + + estimated_points = {} + for axis_index in range(3): + axis_solution = self._solve_axis(axis_index, observed_points, measurement_weights, reference_points) + for marker_id in self.layout_ids: + if marker_id not in estimated_points: + estimated_points[marker_id] = np.zeros(3, dtype=np.float64) + estimated_points[marker_id][axis_index] = axis_solution[self.id_to_index[marker_id]] + + estimated_normals = self._estimate_normals(estimated_points, observed_normals, reference_normal) + + output_msg = TagPoseArray() + output_msg.header = msg.header + output_msg.header.frame_id = msg.header.frame_id + + current_covariances = {} + for marker_id in self.layout_ids: + observed_pose = observed_pose_msgs.get(marker_id) + covariance = self._inflate_covariance(marker_id, observed_pose) + current_covariances[marker_id] = covariance + + R = self._orientation_from_normal(estimated_normals[marker_id], marker_id, estimated_points) + qx, qy, qz, qw = self._rot_to_quat(R) + + pose_msg = TagPose() + pose_msg.id = marker_id + pose_msg.dictionary_name = observed_pose.dictionary_name if observed_pose is not None else self.layout_dict_name + pose_msg.pose.pose.position.x = float(estimated_points[marker_id][0]) + pose_msg.pose.pose.position.y = float(estimated_points[marker_id][1]) + pose_msg.pose.pose.position.z = float(estimated_points[marker_id][2]) + pose_msg.pose.pose.orientation.x = qx + pose_msg.pose.pose.orientation.y = qy + pose_msg.pose.pose.orientation.z = qz + pose_msg.pose.pose.orientation.w = qw + pose_msg.pose.covariance = covariance + pose_msg.normal.x = float(estimated_normals[marker_id][0]) + pose_msg.normal.y = float(estimated_normals[marker_id][1]) + pose_msg.normal.z = float(estimated_normals[marker_id][2]) + pose_msg.observed = observed_pose is not None + if observed_pose is not None: + pose_msg.tag_area_px = observed_pose.tag_area_px + pose_msg.detection_score = observed_pose.detection_score + pose_msg.reprojection_error_px = observed_pose.reprojection_error_px + pose_msg.depth_fused = observed_pose.depth_fused + else: + pose_msg.tag_area_px = 0.0 + pose_msg.detection_score = 0.0 + pose_msg.reprojection_error_px = 0.0 + pose_msg.depth_fused = False + output_msg.poses.append(pose_msg) + + self.prev_points = {marker_id: estimated_points[marker_id].copy() for marker_id in self.layout_ids} + self.prev_normals = {marker_id: estimated_normals[marker_id].copy() for marker_id in self.layout_ids} + self.prev_covariances = current_covariances + self.pose_pub.publish(output_msg) + + +if __name__ == "__main__": + try: + PaperSurfaceEstimator() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_surface_estimator failed: %s", str(exc)) diff --git a/src/tf_broadcasters/src/paper_surface_rviz_publisher.py b/src/tf_broadcasters/src/paper_surface_rviz_publisher.py new file mode 100644 index 0000000..3adbfa5 --- /dev/null +++ b/src/tf_broadcasters/src/paper_surface_rviz_publisher.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os + +import numpy as np +import rospy +from assistive_msgs.msg import TagPoseArray +from geometry_msgs.msg import Point +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + + +class PaperSurfaceRvizPublisher(object): + def __init__(self): + rospy.init_node("paper_surface_rviz_publisher", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.pose_topic_name = rospy.get_param("~pose_topic_name", "paper_surface_state") + self.marker_topic_name = rospy.get_param("~marker_topic_name", "paper_surface_markers") + self.grid_line_width = float(rospy.get_param("~grid_line_width", 0.003)) + self.outline_line_width = float(rospy.get_param("~outline_line_width", 0.006)) + self.surface_alpha = float(rospy.get_param("~surface_alpha", 0.12)) + self.predicted_surface_alpha_scale = float(rospy.get_param("~predicted_surface_alpha_scale", 0.55)) + self.point_scale = float(rospy.get_param("~point_scale", 0.012)) + self.predicted_point_scale = float(rospy.get_param("~predicted_point_scale", 0.018)) + self.uncertainty_scale_gain = float(rospy.get_param("~uncertainty_scale_gain", 0.12)) + self.text_height = float(rospy.get_param("~text_height", 0.03)) + self.text_z_offset = float(rospy.get_param("~text_z_offset", 0.01)) + self.normal_length = float(rospy.get_param("~normal_length", 0.05)) + self.normal_line_width = float(rospy.get_param("~normal_line_width", 0.003)) + + layout_data = self._load_layout(self.layout_json_path) + self.grid_rows, self.grid_cols = self._read_grid_shape(layout_data) + self.rowcol_to_id = self._read_marker_layout(layout_data) + self.boundary_pairs = self._build_boundary_pairs() + self.surface_triangles = self._build_surface_triangles() + self.mesh_line_pairs = self._build_mesh_line_pairs() + + self.marker_pub = rospy.Publisher(self.marker_topic_name, MarkerArray, queue_size=1) + self.pose_sub = rospy.Subscriber(self.pose_topic_name, TagPoseArray, self._pose_cb, queue_size=1) + self.markers_visible = False + + rospy.loginfo("paper_surface_rviz_publisher initialized.") + rospy.loginfo("Publishing markers on: %s", self.marker_topic_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + return json.load(f) + + @staticmethod + def _read_grid_shape(layout_data): + grid = layout_data.get("grid", {}) + rows = int(grid.get("rows", 0)) + cols = int(grid.get("cols", 0)) + if rows <= 0 or cols <= 0: + raise ValueError("Invalid rows/cols in layout json") + return rows, cols + + def _read_marker_layout(self, layout_data): + rowcol_to_id = {} + for marker in layout_data.get("markers", []): + rowcol_to_id[(int(marker["row"]), int(marker["col"]))] = int(marker["id"]) + if not rowcol_to_id: + raise ValueError("Layout has no markers") + return rowcol_to_id + + def _build_boundary_pairs(self): + pairs = [] + for c in range(self.grid_cols - 1): + pairs.append(((0, c), (0, c + 1))) + for r in range(self.grid_rows - 1): + pairs.append(((r, self.grid_cols - 1), (r + 1, self.grid_cols - 1))) + for c in range(self.grid_cols - 1, 0, -1): + pairs.append(((self.grid_rows - 1, c), (self.grid_rows - 1, c - 1))) + for r in range(self.grid_rows - 1, 0, -1): + pairs.append(((r, 0), (r - 1, 0))) + + id_pairs = [] + for a_rc, b_rc in pairs: + a_id = self.rowcol_to_id.get(a_rc) + b_id = self.rowcol_to_id.get(b_rc) + if a_id is not None and b_id is not None: + id_pairs.append((a_id, b_id)) + return id_pairs + + def _build_surface_triangles(self): + triangles = [] + for r in range(self.grid_rows - 1): + for c in range(self.grid_cols - 1): + id00 = self.rowcol_to_id.get((r, c)) + id01 = self.rowcol_to_id.get((r, c + 1)) + id10 = self.rowcol_to_id.get((r + 1, c)) + id11 = self.rowcol_to_id.get((r + 1, c + 1)) + if id00 is None or id01 is None or id10 is None or id11 is None: + continue + triangles.append((id00, id01, id11)) + triangles.append((id00, id11, id10)) + return triangles + + def _build_mesh_line_pairs(self): + pairs = [] + for r in range(self.grid_rows): + for c in range(self.grid_cols - 1): + a_id = self.rowcol_to_id.get((r, c)) + b_id = self.rowcol_to_id.get((r, c + 1)) + if a_id is not None and b_id is not None: + pairs.append((a_id, b_id)) + for r in range(self.grid_rows - 1): + for c in range(self.grid_cols): + a_id = self.rowcol_to_id.get((r, c)) + b_id = self.rowcol_to_id.get((r + 1, c)) + if a_id is not None and b_id is not None: + pairs.append((a_id, b_id)) + return pairs + + @staticmethod + def _to_point(x, y, z): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = float(z) + return point + + def _pose_cb(self, msg): + if not msg.poses: + self._clear_markers_if_needed(msg.header.stamp, msg.header.frame_id) + return + + points_by_id = {} + normals_by_id = {} + observed_by_id = {} + position_var_by_id = {} + for pose in msg.poses: + marker_id = int(pose.id) + points_by_id[marker_id] = np.asarray( + [ + pose.pose.pose.position.x, + pose.pose.pose.position.y, + pose.pose.pose.position.z, + ], + dtype=np.float64, + ) + normals_by_id[marker_id] = np.asarray([pose.normal.x, pose.normal.y, pose.normal.z], dtype=np.float64) + observed_by_id[marker_id] = bool(pose.observed) + covariance = np.asarray(pose.pose.covariance, dtype=np.float64) + position_var_by_id[marker_id] = float(np.mean(covariance[[0, 7, 14]])) if covariance.size >= 15 else 0.0 + + self._publish_markers( + msg.header.stamp, + msg.header.frame_id, + points_by_id, + normals_by_id, + observed_by_id, + position_var_by_id, + ) + self.markers_visible = True + + def _publish_markers(self, stamp, frame_id, points_by_id, normals_by_id, observed_by_id, position_var_by_id): + msg = MarkerArray() + total_count = max(1, len(points_by_id)) + observed_count = sum(1 for observed in observed_by_id.values() if observed) + observed_ratio = float(observed_count) / float(total_count) + + surface = Marker() + surface.header.stamp = stamp + surface.header.frame_id = frame_id + surface.ns = "paper_surface" + surface.id = 0 + surface.type = Marker.TRIANGLE_LIST + surface.action = Marker.ADD + surface.pose.orientation.w = 1.0 + surface.scale.x = 1.0 + surface.scale.y = 1.0 + surface.scale.z = 1.0 + surface.color.r = 0.1 + surface.color.g = 0.5 + surface.color.b = 1.0 + surface.color.a = self.surface_alpha * ( + self.predicted_surface_alpha_scale + (1.0 - self.predicted_surface_alpha_scale) * observed_ratio + ) + for tri in self.surface_triangles: + if tri[0] in points_by_id and tri[1] in points_by_id and tri[2] in points_by_id: + surface.points.append(self._to_point(*points_by_id[tri[0]])) + surface.points.append(self._to_point(*points_by_id[tri[1]])) + surface.points.append(self._to_point(*points_by_id[tri[2]])) + msg.markers.append(surface) + + observed_grid = Marker() + observed_grid.header.stamp = stamp + observed_grid.header.frame_id = frame_id + observed_grid.ns = "aruco_grid_observed" + observed_grid.id = 1 + observed_grid.type = Marker.LINE_LIST + observed_grid.action = Marker.ADD + observed_grid.pose.orientation.w = 1.0 + observed_grid.scale.x = self.grid_line_width + observed_grid.color.r = 0.0 + observed_grid.color.g = 1.0 + observed_grid.color.b = 0.2 + observed_grid.color.a = 0.95 + + predicted_grid = Marker() + predicted_grid.header.stamp = stamp + predicted_grid.header.frame_id = frame_id + predicted_grid.ns = "aruco_grid_predicted" + predicted_grid.id = 2 + predicted_grid.type = Marker.LINE_LIST + predicted_grid.action = Marker.ADD + predicted_grid.pose.orientation.w = 1.0 + predicted_grid.scale.x = self.grid_line_width + predicted_grid.color.r = 1.0 + predicted_grid.color.g = 0.7 + predicted_grid.color.b = 0.0 + predicted_grid.color.a = 0.7 + + for marker_a, marker_b in self.mesh_line_pairs: + if marker_a not in points_by_id or marker_b not in points_by_id: + continue + line_marker = observed_grid if observed_by_id.get(marker_a, False) and observed_by_id.get(marker_b, False) else predicted_grid + line_marker.points.append(self._to_point(*points_by_id[marker_a])) + line_marker.points.append(self._to_point(*points_by_id[marker_b])) + msg.markers.append(observed_grid) + msg.markers.append(predicted_grid) + + outline = Marker() + outline.header.stamp = stamp + outline.header.frame_id = frame_id + outline.ns = "paper_outline" + outline.id = 3 + outline.type = Marker.LINE_LIST + outline.action = Marker.ADD + outline.pose.orientation.w = 1.0 + outline.scale.x = self.outline_line_width + outline.color.r = 1.0 + outline.color.g = 0.2 + outline.color.b = 0.2 + outline.color.a = 1.0 + for marker_a, marker_b in self.boundary_pairs: + if marker_a in points_by_id and marker_b in points_by_id: + outline.points.append(self._to_point(*points_by_id[marker_a])) + outline.points.append(self._to_point(*points_by_id[marker_b])) + msg.markers.append(outline) + + observed_points = Marker() + observed_points.header.stamp = stamp + observed_points.header.frame_id = frame_id + observed_points.ns = "paper_points_observed" + observed_points.id = 4 + observed_points.type = Marker.SPHERE_LIST + observed_points.action = Marker.ADD + observed_points.pose.orientation.w = 1.0 + observed_points.scale.x = self.point_scale + observed_points.scale.y = self.point_scale + observed_points.scale.z = self.point_scale + observed_points.color.r = 1.0 + observed_points.color.g = 0.8 + observed_points.color.b = 0.0 + observed_points.color.a = 1.0 + for marker_id, point in sorted(points_by_id.items()): + if observed_by_id.get(marker_id, False): + observed_points.points.append(self._to_point(*point)) + msg.markers.append(observed_points) + + observed_normals = Marker() + observed_normals.header.stamp = stamp + observed_normals.header.frame_id = frame_id + observed_normals.ns = "paper_normals_observed" + observed_normals.id = 5 + observed_normals.type = Marker.LINE_LIST + observed_normals.action = Marker.ADD + observed_normals.pose.orientation.w = 1.0 + observed_normals.scale.x = self.normal_line_width + observed_normals.color.r = 0.0 + observed_normals.color.g = 0.8 + observed_normals.color.b = 1.0 + observed_normals.color.a = 1.0 + + predicted_normals = Marker() + predicted_normals.header.stamp = stamp + predicted_normals.header.frame_id = frame_id + predicted_normals.ns = "paper_normals_predicted" + predicted_normals.id = 6 + predicted_normals.type = Marker.LINE_LIST + predicted_normals.action = Marker.ADD + predicted_normals.pose.orientation.w = 1.0 + predicted_normals.scale.x = self.normal_line_width + predicted_normals.color.r = 1.0 + predicted_normals.color.g = 0.0 + predicted_normals.color.b = 1.0 + predicted_normals.color.a = 0.7 + + for marker_id, point in sorted(points_by_id.items()): + normal = normals_by_id.get(marker_id) + if normal is None: + continue + endpoint = point + normal * self.normal_length + line_marker = observed_normals if observed_by_id.get(marker_id, False) else predicted_normals + line_marker.points.append(self._to_point(*point)) + line_marker.points.append(self._to_point(*endpoint)) + msg.markers.append(observed_normals) + msg.markers.append(predicted_normals) + + marker_id_offset = 1000 + for marker_id, point in sorted(points_by_id.items()): + if not observed_by_id.get(marker_id, False): + variance = max(0.0, position_var_by_id.get(marker_id, 0.0)) + predicted_scale = self.predicted_point_scale + self.uncertainty_scale_gain * np.sqrt(variance) + predicted_marker = Marker() + predicted_marker.header.stamp = stamp + predicted_marker.header.frame_id = frame_id + predicted_marker.ns = "paper_points_predicted" + predicted_marker.id = marker_id_offset + marker_id + predicted_marker.type = Marker.SPHERE + predicted_marker.action = Marker.ADD + predicted_marker.pose.position.x = float(point[0]) + predicted_marker.pose.position.y = float(point[1]) + predicted_marker.pose.position.z = float(point[2]) + predicted_marker.pose.orientation.w = 1.0 + predicted_marker.scale.x = predicted_scale + predicted_marker.scale.y = predicted_scale + predicted_marker.scale.z = predicted_scale + predicted_marker.color.r = 1.0 + predicted_marker.color.g = 0.45 + predicted_marker.color.b = 0.15 + predicted_marker.color.a = 0.45 + msg.markers.append(predicted_marker) + + text = Marker() + text.header.stamp = stamp + text.header.frame_id = frame_id + text.ns = "aruco_ids" + text.id = 2000 + marker_id + text.type = Marker.TEXT_VIEW_FACING + text.action = Marker.ADD + text.pose.position.x = float(point[0]) + text.pose.position.y = float(point[1]) + text.pose.position.z = float(point[2] + self.text_z_offset) + text.pose.orientation.w = 1.0 + text.scale.z = self.text_height + if observed_by_id.get(marker_id, False): + text.color.r = 1.0 + text.color.g = 1.0 + text.color.b = 1.0 + text.color.a = 1.0 + text.text = str(marker_id) + else: + text.color.r = 1.0 + text.color.g = 0.65 + text.color.b = 0.0 + text.color.a = 0.9 + text.text = "{}*".format(marker_id) + msg.markers.append(text) + + self.marker_pub.publish(msg) + + def _clear_markers_if_needed(self, stamp, frame_id): + if not self.markers_visible: + return + clear = MarkerArray() + marker = Marker() + marker.header.stamp = stamp + marker.header.frame_id = frame_id + marker.action = Marker.DELETEALL + clear.markers.append(marker) + self.marker_pub.publish(clear) + self.markers_visible = False + + +if __name__ == "__main__": + try: + PaperSurfaceRvizPublisher() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_surface_rviz_publisher failed: %s", str(exc)) diff --git a/src/tf_broadcasters/src/paper_tag_detector.py b/src/tf_broadcasters/src/paper_tag_detector.py new file mode 100644 index 0000000..524d37c --- /dev/null +++ b/src/tf_broadcasters/src/paper_tag_detector.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os + +import cv2 +import numpy as np +import rospy +from assistive_msgs.msg import TagDetection +from assistive_msgs.msg import TagDetectionArray +from cv_bridge import CvBridge, CvBridgeError +from geometry_msgs.msg import Point32 +from sensor_msgs.msg import Image + + +ARUCO_DICT = { + "DICT_4X4_50": cv2.aruco.DICT_4X4_50, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + "DICT_4X4_250": cv2.aruco.DICT_4X4_250, + "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000, + "DICT_5X5_50": cv2.aruco.DICT_5X5_50, + "DICT_5X5_100": cv2.aruco.DICT_5X5_100, + "DICT_5X5_250": cv2.aruco.DICT_5X5_250, + "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000, + "DICT_6X6_50": cv2.aruco.DICT_6X6_50, + "DICT_6X6_100": cv2.aruco.DICT_6X6_100, + "DICT_6X6_250": cv2.aruco.DICT_6X6_250, + "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000, + "DICT_7X7_50": cv2.aruco.DICT_7X7_50, + "DICT_7X7_100": cv2.aruco.DICT_7X7_100, + "DICT_7X7_250": cv2.aruco.DICT_7X7_250, + "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000, + "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL, +} + + +class PaperTagDetector(object): + def __init__(self): + rospy.init_node("paper_tag_detector", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.image_topic_name = rospy.get_param("~image_topic_name", "/nuc/rgb/image_raw") + self.detection_topic_name = rospy.get_param("~detection_topic_name", "paper_tag_detections") + self.publish_debug_image = rospy.get_param("~publish_debug_image", True) + self.debug_image_topic_name = rospy.get_param("~debug_image_topic_name", "paper_tag_debug_image") + self.min_tag_area_px = float(rospy.get_param("~min_tag_area_px", 0.0)) + self.quality_half_area_px = float(rospy.get_param("~quality_half_area_px", 2500.0)) + + layout_data = self._load_layout(self.layout_json_path) + self.layout_marker_ids = self._read_layout_marker_ids(layout_data) + self.aruco_dict_name = rospy.get_param( + "~aruco_dict_name", layout_data.get("grid", {}).get("dict", "DICT_4X4_50") + ) + self.aruco_dict = self._make_aruco_dict(self.aruco_dict_name) + self.aruco_params = self._make_aruco_params() + self.aruco_detector = None + if hasattr(cv2.aruco, "ArucoDetector"): + self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params) + + self.bridge = CvBridge() + self.detection_pub = rospy.Publisher(self.detection_topic_name, TagDetectionArray, queue_size=1) + self.debug_pub = None + if self.publish_debug_image: + self.debug_pub = rospy.Publisher(self.debug_image_topic_name, Image, queue_size=1) + + self.image_sub = rospy.Subscriber(self.image_topic_name, Image, self._image_cb, queue_size=1) + + rospy.loginfo("paper_tag_detector initialized.") + rospy.loginfo("Layout markers loaded: %d", len(self.layout_marker_ids)) + rospy.loginfo("Publishing detections on: %s", self.detection_topic_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + return json.load(f) + + @staticmethod + def _read_layout_marker_ids(layout_data): + markers = layout_data.get("markers", []) + if not markers: + raise ValueError("Layout has no markers") + return set(int(marker["id"]) for marker in markers) + + @staticmethod + def _polygon_area_px(points): + return abs(float(cv2.contourArea(points.astype(np.float32)))) + + def _compute_detection_score(self, area_px): + if area_px <= 0.0: + return 0.0 + return float(area_px / (area_px + self.quality_half_area_px)) + + def _make_aruco_dict(self, name): + if name not in ARUCO_DICT: + raise ValueError("Unsupported aruco dictionary: {}".format(name)) + dict_id = ARUCO_DICT[name] + if hasattr(cv2.aruco, "getPredefinedDictionary"): + return cv2.aruco.getPredefinedDictionary(dict_id) + return cv2.aruco.Dictionary_get(dict_id) + + @staticmethod + def _make_aruco_params(): + if hasattr(cv2.aruco, "DetectorParameters_create"): + return cv2.aruco.DetectorParameters_create() + return cv2.aruco.DetectorParameters() + + def _detect_markers(self, gray): + if self.aruco_detector is not None: + return self.aruco_detector.detectMarkers(gray) + return cv2.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruco_params) + + @staticmethod + def _to_point32(x, y): + point = Point32() + point.x = float(x) + point.y = float(y) + point.z = 0.0 + return point + + def _build_detection_msg(self, marker_id, corners_px): + detection = TagDetection() + detection.id = int(marker_id) + detection.dictionary_name = self.aruco_dict_name + center_px = np.mean(corners_px, axis=0) + detection.center_px = self._to_point32(center_px[0], center_px[1]) + detection.corners_px = [self._to_point32(pt[0], pt[1]) for pt in corners_px] + detection.tag_area_px = self._polygon_area_px(corners_px) + detection.detection_score = self._compute_detection_score(detection.tag_area_px) + return detection + + def _image_cb(self, msg): + try: + frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + except CvBridgeError as exc: + rospy.logwarn_throttle(5.0, "cv_bridge conversion failed: %s", str(exc)) + return + + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + corners, ids, _ = self._detect_markers(gray) + + detections = [] + kept_corners = [] + kept_ids = [] + if ids is not None and len(ids) > 0: + ids = ids.flatten().tolist() + for idx, marker_id in enumerate(ids): + if marker_id not in self.layout_marker_ids: + continue + + marker_corners = np.asarray(corners[idx], dtype=np.float32).reshape(4, 2) + area_px = self._polygon_area_px(marker_corners) + if area_px < self.min_tag_area_px: + continue + + detection = self._build_detection_msg(marker_id, marker_corners) + detections.append(detection) + kept_corners.append(corners[idx]) + kept_ids.append(marker_id) + + detections.sort(key=lambda detection: detection.id) + + detection_array = TagDetectionArray() + detection_array.header = msg.header + detection_array.image_width = int(frame.shape[1]) + detection_array.image_height = int(frame.shape[0]) + detection_array.detections = detections + self.detection_pub.publish(detection_array) + + if self.publish_debug_image and self.debug_pub is not None: + debug_frame = frame.copy() + if kept_corners and hasattr(cv2.aruco, "drawDetectedMarkers"): + debug_ids = np.asarray(kept_ids, dtype=np.int32).reshape(-1, 1) + cv2.aruco.drawDetectedMarkers(debug_frame, kept_corners, debug_ids) + for detection in detections: + label = "{} {:.2f}".format(detection.id, detection.detection_score) + center = (int(round(detection.center_px.x)), int(round(detection.center_px.y))) + cv2.putText( + debug_frame, + label, + (center[0] + 8, center[1] - 8), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 255, 0), + 1, + cv2.LINE_AA, + ) + + try: + debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, encoding="bgr8") + debug_msg.header = msg.header + self.debug_pub.publish(debug_msg) + except CvBridgeError: + rospy.logwarn_throttle(5.0, "Failed to publish paper tag detector debug image.") + + +if __name__ == "__main__": + try: + PaperTagDetector() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_tag_detector failed: %s", str(exc)) diff --git a/src/tf_broadcasters/src/paper_tag_pose_fuser.py b/src/tf_broadcasters/src/paper_tag_pose_fuser.py new file mode 100644 index 0000000..b237fe7 --- /dev/null +++ b/src/tf_broadcasters/src/paper_tag_pose_fuser.py @@ -0,0 +1,565 @@ +#!/usr/bin/env python3 + +from __future__ import print_function + +import json +import os +import threading + +import cv2 +import numpy as np +import rospy +import tf2_ros +from assistive_msgs.msg import TagDepthObservationArray +from assistive_msgs.msg import TagDetectionArray +from assistive_msgs.msg import TagPose +from assistive_msgs.msg import TagPoseArray +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image + + +class PaperTagPoseFuser(object): + def __init__(self): + rospy.init_node("paper_tag_pose_fuser", anonymous=False) + + self.layout_json_path = os.path.expanduser(rospy.get_param("~layout_json_path", "")) + if not self.layout_json_path: + rospy.logfatal("~layout_json_path must be set.") + raise RuntimeError("Missing layout_json_path") + + self.detection_topic_name = rospy.get_param("~detection_topic_name", "paper_tag_detections") + self.depth_observation_topic_name = rospy.get_param( + "~depth_observation_topic_name", "paper_tag_depth_observations" + ) + self.pose_topic_name = rospy.get_param("~pose_topic_name", "paper_tag_poses") + self.camera_info_topic_name = rospy.get_param("~camera_info_topic_name", "/nuc/rgb/camera_info") + self.camera_frame_id = rospy.get_param("~camera_frame_id", "") + self.target_frame_id = rospy.get_param("~target_frame_id", "") + self.use_depth_observations = rospy.get_param("~use_depth_observations", True) + self.depth_observation_max_age_sec = float(rospy.get_param("~depth_observation_max_age_sec", 0.15)) + self.depth_image_topic_name = rospy.get_param("~depth_image_topic_name", "") + self.use_depth_refinement = rospy.get_param("~use_depth_refinement", True) + self.depth_unit_scale = float(rospy.get_param("~depth_unit_scale", 0.001)) + self.depth_max_age_sec = float(rospy.get_param("~depth_max_age_sec", 0.15)) + self.depth_patch_radius_px = int(rospy.get_param("~depth_patch_radius_px", 2)) + self.plane_fit_stride_px = max(1, int(rospy.get_param("~plane_fit_stride_px", 2))) + self.plane_fit_min_points = max(3, int(rospy.get_param("~plane_fit_min_points", 20))) + self.tf_lookup_timeout_sec = float(rospy.get_param("~tf_lookup_timeout_sec", 0.05)) + self.pnp_reprojection_error = float(rospy.get_param("~pnp_reprojection_error", 3.0)) + + self.base_position_variance = float(rospy.get_param("~base_position_variance", 1.0e-5)) + self.base_orientation_variance = float(rospy.get_param("~base_orientation_variance", 2.5e-3)) + self.range_position_variance_scale = float(rospy.get_param("~range_position_variance_scale", 0.03)) + self.angle_position_variance_scale = float(rospy.get_param("~angle_position_variance_scale", 0.08)) + self.range_orientation_variance_scale = float(rospy.get_param("~range_orientation_variance_scale", 0.05)) + self.angle_orientation_variance_scale = float(rospy.get_param("~angle_orientation_variance_scale", 0.2)) + self.depth_fused_variance_scale = float(rospy.get_param("~depth_fused_variance_scale", 0.6)) + + self.layout_data = self._load_layout(self.layout_json_path) + self.default_marker_size_m = float(self.layout_data.get("grid", {}).get("marker_mm", 60.0)) / 1000.0 + self.id_to_size_m = self._read_marker_sizes(self.layout_data) + self.obj_corners_cache = {} + + self.pnp_square_flag = cv2.SOLVEPNP_ITERATIVE + if hasattr(cv2, "SOLVEPNP_IPPE_SQUARE"): + self.pnp_square_flag = cv2.SOLVEPNP_IPPE_SQUARE + + self.bridge = CvBridge() + self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10.0)) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + self.data_lock = threading.Lock() + self.depth_lock = threading.Lock() + self.K = None + self.D = None + self.camera_info_ready = False + self.latest_depth = None + self.latest_depth_stamp = None + self.latest_depth_observations = None + self.latest_depth_observations_stamp = None + + self.pose_pub = rospy.Publisher(self.pose_topic_name, TagPoseArray, queue_size=1) + self.camera_info_sub = rospy.Subscriber( + self.camera_info_topic_name, CameraInfo, self._camera_info_cb, queue_size=1 + ) + self.detection_sub = rospy.Subscriber( + self.detection_topic_name, TagDetectionArray, self._detection_cb, queue_size=1 + ) + self.depth_observation_sub = None + if self.depth_observation_topic_name: + self.depth_observation_sub = rospy.Subscriber( + self.depth_observation_topic_name, + TagDepthObservationArray, + self._depth_observation_cb, + queue_size=1, + ) + self.depth_sub = None + if self.depth_image_topic_name: + self.depth_sub = rospy.Subscriber(self.depth_image_topic_name, Image, self._depth_cb, queue_size=1) + + rospy.loginfo("paper_tag_pose_fuser initialized.") + rospy.loginfo("Publishing per-tag poses on: %s", self.pose_topic_name) + + def _load_layout(self, path): + if not os.path.exists(path): + raise IOError("Layout json not found: {}".format(path)) + with open(path, "r") as f: + return json.load(f) + + def _read_marker_sizes(self, layout_data): + sizes = {} + for marker in layout_data.get("markers", []): + marker_id = int(marker["id"]) + size_m = float(marker.get("marker_length_mm", self.default_marker_size_m * 1000.0)) / 1000.0 + sizes[marker_id] = size_m + if not sizes: + raise ValueError("Layout has no markers") + return sizes + + def _camera_info_cb(self, msg): + with self.data_lock: + self.K = np.asarray(msg.K, dtype=np.float64).reshape(3, 3) + self.D = np.asarray(msg.D, dtype=np.float64) + self.camera_info_ready = True + if not self.camera_frame_id: + self.camera_frame_id = msg.header.frame_id + + def _depth_cb(self, msg): + try: + depth = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + except CvBridgeError: + return + with self.depth_lock: + self.latest_depth = depth + self.latest_depth_stamp = msg.header.stamp + + def _depth_observation_cb(self, msg): + observations = {} + for observation in msg.observations: + observations[int(observation.id)] = observation + with self.depth_lock: + self.latest_depth_observations = observations + self.latest_depth_observations_stamp = msg.header.stamp + + def _marker_object_corners(self, marker_size_m): + key = round(float(marker_size_m), 6) + if key in self.obj_corners_cache: + return self.obj_corners_cache[key] + + half = key * 0.5 + obj = np.asarray( + [ + [-half, half, 0.0], + [half, half, 0.0], + [half, -half, 0.0], + [-half, -half, 0.0], + ], + dtype=np.float64, + ) + self.obj_corners_cache[key] = obj + return obj + + def _estimate_marker_pose(self, marker_corners_img, marker_size_m, K, D): + obj = self._marker_object_corners(marker_size_m) + success, rvec, tvec = cv2.solvePnP( + obj, + marker_corners_img, + K, + D, + flags=self.pnp_square_flag, + ) + if not success and self.pnp_square_flag != cv2.SOLVEPNP_ITERATIVE: + success, rvec, tvec = cv2.solvePnP( + obj, + marker_corners_img, + K, + D, + flags=cv2.SOLVEPNP_ITERATIVE, + ) + if not success: + return None, None, None, None + + projected, _ = cv2.projectPoints(obj, rvec, tvec, K, D) + reproj_error = np.mean(np.linalg.norm(projected.reshape(-1, 2) - marker_corners_img, axis=1)) + if reproj_error > self.pnp_reprojection_error: + return None, None, None, None + + center_cam = np.asarray(tvec, dtype=np.float64).reshape(3) + return center_cam, rvec, tvec, float(reproj_error) + + @staticmethod + def _normalize(vec, fallback=None): + norm = np.linalg.norm(vec) + if norm < 1e-8: + return fallback + return vec / norm + + def _depth_value_to_m(self, val): + if np.isnan(val) or np.isinf(val): + return None + if val <= 0: + return None + return float(val) * self.depth_unit_scale + + def _sample_depth_m(self, depth_img, u, v): + h, w = depth_img.shape[:2] + uu = int(round(u)) + vv = int(round(v)) + if uu < 0 or uu >= w or vv < 0 or vv >= h: + return None + + r = max(0, self.depth_patch_radius_px) + u0 = max(0, uu - r) + u1 = min(w, uu + r + 1) + v0 = max(0, vv - r) + v1 = min(h, vv + r + 1) + patch = depth_img[v0:v1, u0:u1] + if patch.size == 0: + return None + + depth_vals = [] + for val in patch.reshape(-1): + depth_m = self._depth_value_to_m(float(val)) + if depth_m is not None: + depth_vals.append(depth_m) + if not depth_vals: + return None + return float(np.median(np.asarray(depth_vals, dtype=np.float64))) + + @staticmethod + def _pixel_to_cam(K, u, v, z_m): + fx = K[0, 0] + fy = K[1, 1] + cx = K[0, 2] + cy = K[1, 2] + x = (u - cx) * z_m / fx + y = (v - cy) * z_m / fy + return np.asarray([x, y, z_m], dtype=np.float64) + + def _fit_depth_plane(self, depth_img, corners_px, K): + h, w = depth_img.shape[:2] + xs = corners_px[:, 0] + ys = corners_px[:, 1] + u0 = max(0, int(np.floor(np.min(xs)))) + u1 = min(w - 1, int(np.ceil(np.max(xs)))) + v0 = max(0, int(np.floor(np.min(ys)))) + v1 = min(h - 1, int(np.ceil(np.max(ys)))) + if u1 <= u0 or v1 <= v0: + return None, None + + points = [] + polygon = corners_px.astype(np.float32) + for v in range(v0, v1 + 1, self.plane_fit_stride_px): + for u in range(u0, u1 + 1, self.plane_fit_stride_px): + if cv2.pointPolygonTest(polygon, (float(u), float(v)), False) < 0: + continue + depth_m = self._depth_value_to_m(float(depth_img[v, u])) + if depth_m is None: + continue + points.append(self._pixel_to_cam(K, u, v, depth_m)) + + if len(points) < self.plane_fit_min_points: + return None, None + + pts = np.asarray(points, dtype=np.float64) + centroid = np.mean(pts, axis=0) + centered = pts - centroid + try: + _, _, vh = np.linalg.svd(centered, full_matrices=False) + except np.linalg.LinAlgError: + return None, None + normal = self._normalize(vh[-1, :]) + if normal is None: + return None, None + + # Orient the normal to point toward the camera. + if np.dot(normal, centroid) > 0.0: + normal = -normal + return centroid, normal + + def _camera_to_target_transform(self, camera_frame_id, target_frame_id, stamp): + if target_frame_id == camera_frame_id: + return np.eye(3), np.zeros((3, 1)) + try: + tf_msg = self.tf_buffer.lookup_transform( + target_frame_id, + camera_frame_id, + stamp, + rospy.Duration(self.tf_lookup_timeout_sec), + ) + except Exception as exc: + rospy.logwarn_throttle( + 2.0, + "TF lookup failed: %s -> %s (%s)", + target_frame_id, + camera_frame_id, + str(exc), + ) + return None, None + + q = tf_msg.transform.rotation + t = tf_msg.transform.translation + R = self._quat_to_rot(q.x, q.y, q.z, q.w) + T = np.asarray([[t.x], [t.y], [t.z]], dtype=np.float64) + return R, T + + @staticmethod + def _quat_to_rot(x, y, z, w): + xx = x * x + yy = y * y + zz = z * z + xy = x * y + xz = x * z + yz = y * z + wx = w * x + wy = w * y + wz = w * z + return np.asarray( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ], + dtype=np.float64, + ) + + @staticmethod + def _rot_to_quat(R): + trace = float(R[0, 0] + R[1, 1] + R[2, 2]) + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * s + qx = (R[2, 1] - R[1, 2]) / s + qy = (R[0, 2] - R[2, 0]) / s + qz = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0 + qw = (R[2, 1] - R[1, 2]) / s + qx = 0.25 * s + qy = (R[0, 1] + R[1, 0]) / s + qz = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0 + qw = (R[0, 2] - R[2, 0]) / s + qx = (R[0, 1] + R[1, 0]) / s + qy = 0.25 * s + qz = (R[1, 2] + R[2, 1]) / s + else: + s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0 + qw = (R[1, 0] - R[0, 1]) / s + qx = (R[0, 2] + R[2, 0]) / s + qy = (R[1, 2] + R[2, 1]) / s + qz = 0.25 * s + return qx, qy, qz, qw + + def _rotation_from_normal_and_hint(self, normal, x_hint, y_hint): + z_axis = self._normalize(normal) + if z_axis is None: + return None + + x_axis = x_hint - np.dot(x_hint, z_axis) * z_axis + x_axis = self._normalize(x_axis) + if x_axis is None: + x_axis = np.cross(y_hint, z_axis) + x_axis = self._normalize(x_axis) + if x_axis is None: + return None + + y_axis = np.cross(z_axis, x_axis) + y_axis = self._normalize(y_axis) + if y_axis is None: + return None + + x_axis = np.cross(y_axis, z_axis) + x_axis = self._normalize(x_axis) + if x_axis is None: + return None + return np.column_stack((x_axis, y_axis, z_axis)) + + def _compute_covariance(self, center_cam, normal_cam, depth_fused): + range_m = float(np.linalg.norm(center_cam)) + view_alignment = 1.0 + if range_m > 1e-8: + ray_dir = center_cam / range_m + view_alignment = abs(float(np.dot(normal_cam, -ray_dir))) + angle_penalty = max(0.0, 1.0 - view_alignment) + + pos_var = self.base_position_variance * ( + 1.0 + + self.range_position_variance_scale * range_m * range_m + + self.angle_position_variance_scale * angle_penalty * angle_penalty + ) + rot_var = self.base_orientation_variance * ( + 1.0 + + self.range_orientation_variance_scale * range_m * range_m + + self.angle_orientation_variance_scale * angle_penalty * angle_penalty + ) + if depth_fused: + pos_var *= self.depth_fused_variance_scale + rot_var *= self.depth_fused_variance_scale + + covariance = np.zeros(36, dtype=np.float64) + covariance[0] = pos_var + covariance[7] = pos_var + covariance[14] = pos_var + covariance[21] = rot_var + covariance[28] = rot_var + covariance[35] = rot_var + return list(covariance) + + def _detection_cb(self, msg): + with self.data_lock: + if not self.camera_info_ready: + return + K = self.K.copy() + D = self.D.copy() + camera_frame_id = self.camera_frame_id + + if not camera_frame_id: + rospy.logwarn_throttle(5.0, "Camera frame is empty; waiting for CameraInfo.") + return + + stamp = msg.header.stamp if msg.header.stamp != rospy.Time(0) else rospy.Time.now() + target_frame = self.target_frame_id if self.target_frame_id else camera_frame_id + R_tc, t_tc = self._camera_to_target_transform(camera_frame_id, target_frame, stamp) + if R_tc is None: + return + + depth_img = None + depth_stamp = None + depth_observations = None + depth_observations_stamp = None + if self.use_depth_refinement: + with self.depth_lock: + depth_observations = self.latest_depth_observations + depth_observations_stamp = self.latest_depth_observations_stamp + if self.latest_depth is not None: + depth_img = self.latest_depth.copy() + depth_stamp = self.latest_depth_stamp + + pose_array = TagPoseArray() + pose_array.header = msg.header + pose_array.header.frame_id = target_frame + + for detection in msg.detections: + marker_id = int(detection.id) + marker_size_m = self.id_to_size_m.get(marker_id, self.default_marker_size_m) + if len(detection.corners_px) != 4: + continue + + corners_px = np.asarray([[pt.x, pt.y] for pt in detection.corners_px], dtype=np.float64) + center_cam, rvec, _, reproj_error = self._estimate_marker_pose(corners_px, marker_size_m, K, D) + if center_cam is None: + continue + + R_cm, _ = cv2.Rodrigues(rvec) + normal_cam = self._normalize(R_cm[:, 2], fallback=np.asarray([0.0, 0.0, -1.0], dtype=np.float64)) + refined_center_cam = center_cam.copy() + refined_normal_cam = normal_cam.copy() + refined_rotation_cam = R_cm.copy() + depth_fused = False + + if ( + self.use_depth_refinement + and self.use_depth_observations + and depth_observations is not None + and depth_observations_stamp is not None + ): + obs_age = abs((stamp - depth_observations_stamp).to_sec()) + depth_observation = depth_observations.get(marker_id) + if depth_observation is not None and obs_age <= self.depth_observation_max_age_sec: + refined_center_cam = np.asarray( + [ + depth_observation.center_cam.x, + depth_observation.center_cam.y, + depth_observation.center_cam.z, + ], + dtype=np.float64, + ) + refined_normal_cam = self._normalize( + np.asarray( + [ + depth_observation.normal_cam.x, + depth_observation.normal_cam.y, + depth_observation.normal_cam.z, + ], + dtype=np.float64, + ), + fallback=refined_normal_cam, + ) + refined_rotation_cam = self._rotation_from_normal_and_hint( + refined_normal_cam, + R_cm[:, 0], + R_cm[:, 1], + ) + if refined_rotation_cam is None: + refined_rotation_cam = R_cm.copy() + depth_fused = True + + if ( + self.use_depth_refinement + and not depth_fused + and depth_img is not None + and depth_stamp is not None + ): + age = abs((stamp - depth_stamp).to_sec()) + if age <= self.depth_max_age_sec: + plane_center_cam, plane_normal_cam = self._fit_depth_plane(depth_img, corners_px, K) + if plane_center_cam is not None and plane_normal_cam is not None: + if np.dot(plane_normal_cam, refined_normal_cam) < 0.0: + plane_normal_cam = -plane_normal_cam + refined_center_cam = plane_center_cam + refined_normal_cam = plane_normal_cam + refined_rotation_cam = self._rotation_from_normal_and_hint( + refined_normal_cam, + R_cm[:, 0], + R_cm[:, 1], + ) + if refined_rotation_cam is None: + refined_rotation_cam = R_cm.copy() + depth_fused = True + else: + center_px = np.mean(corners_px, axis=0) + center_depth_m = self._sample_depth_m(depth_img, center_px[0], center_px[1]) + if center_depth_m is not None: + refined_center_cam = self._pixel_to_cam(K, center_px[0], center_px[1], center_depth_m) + depth_fused = True + + center_target = (np.matmul(R_tc, refined_center_cam.reshape(3, 1)) + t_tc).reshape(3) + normal_target = np.matmul(R_tc, refined_normal_cam.reshape(3, 1)).reshape(3) + normal_target = self._normalize(normal_target, fallback=np.asarray([0.0, 0.0, 1.0], dtype=np.float64)) + R_tm = np.matmul(R_tc, refined_rotation_cam) + qx, qy, qz, qw = self._rot_to_quat(R_tm) + + pose_msg = TagPose() + pose_msg.id = marker_id + pose_msg.dictionary_name = detection.dictionary_name + pose_msg.pose.pose.position.x = float(center_target[0]) + pose_msg.pose.pose.position.y = float(center_target[1]) + pose_msg.pose.pose.position.z = float(center_target[2]) + pose_msg.pose.pose.orientation.x = qx + pose_msg.pose.pose.orientation.y = qy + pose_msg.pose.pose.orientation.z = qz + pose_msg.pose.pose.orientation.w = qw + pose_msg.pose.covariance = self._compute_covariance(refined_center_cam, refined_normal_cam, depth_fused) + pose_msg.normal.x = float(normal_target[0]) + pose_msg.normal.y = float(normal_target[1]) + pose_msg.normal.z = float(normal_target[2]) + pose_msg.observed = True + pose_msg.tag_area_px = detection.tag_area_px + pose_msg.detection_score = detection.detection_score + pose_msg.reprojection_error_px = reproj_error + pose_msg.depth_fused = depth_fused + pose_array.poses.append(pose_msg) + + pose_array.poses.sort(key=lambda pose: pose.id) + self.pose_pub.publish(pose_array) + + +if __name__ == "__main__": + try: + PaperTagPoseFuser() + rospy.spin() + except Exception as exc: + rospy.logfatal("paper_tag_pose_fuser failed: %s", str(exc))