From 540f83ee0dcfa82e770a52e044cff89e53038062 Mon Sep 17 00:00:00 2001
From: paddy <paddy-hofmann@web.de>
Date: Fri, 2 Feb 2024 16:34:27 +0100
Subject: [PATCH] fixed camera res update and some nicer visualizations
---
minibot_vision/scripts/SegmentSign.py | 56 ++++++++++++++------------
minibot_vision/scripts/SignDetector.py | 4 +-
2 files changed, 34 insertions(+), 26 deletions(-)
diff --git a/minibot_vision/scripts/SegmentSign.py b/minibot_vision/scripts/SegmentSign.py
index 4ef9bd1..fc25cac 100755
--- a/minibot_vision/scripts/SegmentSign.py
+++ b/minibot_vision/scripts/SegmentSign.py
@@ -79,7 +79,7 @@ def fetch_rosparams():
def image_color_callback(data):
- global img_rgb_stream, cv_bridge
+ global img_rgb_stream, cv_bridge, IMG_RES
try:
img_rgb_stream = cv_bridge.imgmsg_to_cv2(data, "bgr8")
@@ -89,7 +89,7 @@ def image_color_callback(data):
def image_depth_callback(data):
- global img_depth_stream, cv_bridge
+ global img_depth_stream, cv_bridge, DEPTH_RES
try:
img_depth_stream = cv_bridge.imgmsg_to_cv2(data, "16UC1")
@@ -119,7 +119,7 @@ def circular_mean(p, r, arr : np.array):
return sum_px_values / count_px
-def do_hough_circle_detection(img_rgb, img_depth, VISUALIZE=False):
+def do_hough_circle_detection(img_rgb, img_depth, VISUALIZE=False, pub_all=False):
global canny, accum_thresh
gray = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2GRAY)
@@ -134,25 +134,32 @@ def do_hough_circle_detection(img_rgb, img_depth, VISUALIZE=False):
minRadius=MIN_RADIUS, maxRadius=MAX_RADIUS)
keypoint = []
if circles is not None:
- circles = np.uint16(np.around(circles))
- i = circles[0, 0]
- center = (i[0], i[1])
- radius = i[2]
-
- # get depth in [m] (was radius * 0.4 in real world)
- d = circular_mean(center, radius * 0.2, copy(img_depth)) / 1000 # todo this is a slow implementation. You might want to speedup
- # filter if sign to close (circle detector will struggle) or to far (background)
- # was 0.2 and 1.0
- if d < MIN_DEPTH or d > MAX_DEPTH:
- return []
- keypoint.append({"center": center, "radius": radius, "depth": d})
-
- # circle center
- if VISUALIZE:
- cv2.putText(img_rgb, "d:{:1.3f} r:{:1.0f}".format(d, radius), (center[0], center[1] - radius - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), thickness=1)
- cv2.circle(img_rgb, center, 1, (0, 100, 100), 3)
- # circle outline
- cv2.circle(img_rgb, center, radius, (255, 0, 255), 3)
+ for k in range(circles.shape[1]):
+ circles = np.uint16(np.around(circles))
+ i = circles[0, k]
+ center = (i[0], i[1])
+ radius = i[2]
+
+ # get depth in [m] (was radius * 0.4 in real world)
+ d = circular_mean(center, radius * 0.2, copy(img_depth)) / 1000 # todo this is a slow implementation. You might want to speedup
+ # filter if sign to close (circle detector will struggle) or to far (background)
+ # was 0.2 and 1.0
+ if (d < MIN_DEPTH or d > MAX_DEPTH) and not pub_all:
+ return []
+ keypoint.append({"center": center, "radius": radius, "depth": d})
+
+ # circle center
+ if VISUALIZE:
+ cv2.putText(img_rgb, "d:{:1.3f} r:{:1.0f}".format(d, radius), (center[0], center[1] - radius - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), thickness=1)
+ cv2.circle(img_rgb, center, 1, (0, 100, 100), 3)
+ # circle outline
+ color = (255, 0, 255)
+ if k == 0:
+ color = (0, 255, 0)
+ cv2.circle(img_rgb, center, radius, color, 3)
+
+ if not pub_all:
+ break
return keypoint
@@ -355,7 +362,7 @@ def publish_image(img, pub_cmpr_img):
if __name__=="__main__":
- rospy.init_node("hough_detection")
+ rospy.init_node("hough_transform")
ros_pack = rospkg.RosPack()
ns = rospy.get_namespace()
segment_rate = rospy.Rate(5)
@@ -382,8 +389,7 @@ if __name__=="__main__":
accum_thresh = rospy.get_param("sign_detector/canny_param2") #30
img_processed = copy(img_rgb_stream)
- rospy.loginfo("img_processed: {}".format(img_processed))
- keypoints = do_hough_circle_detection(img_processed, copy(img_depth_stream), VISUALIZE=True)
+ keypoints = do_hough_circle_detection(img_processed, copy(img_depth_stream), VISUALIZE=True, pub_all=not toggle_patch_visualization)
if toggle_patch_visualization:
img_processed = copy(img_rgb_stream)
diff --git a/minibot_vision/scripts/SignDetector.py b/minibot_vision/scripts/SignDetector.py
index 4aef9ca..acc7d2f 100755
--- a/minibot_vision/scripts/SignDetector.py
+++ b/minibot_vision/scripts/SignDetector.py
@@ -51,6 +51,7 @@ def image_color_callback(data):
try:
img_rgb_stream = bridge.imgmsg_to_cv2(data, "bgr8")
img_rgb_timestamp = rospy.Time.now()
+ SegmentSign.IMG_RES = (data.height, data.width)
except CvBridgeError as e:
print(e)
@@ -60,6 +61,7 @@ def image_depth_callback(data):
try:
img_depth_stream = bridge.imgmsg_to_cv2(data, "16UC1")
+ SegmentSign.DEPTH_RES = (data.height, data.width)
except CvBridgeError as e:
print(e)
@@ -289,7 +291,7 @@ if __name__ == "__main__":
pub_raw_img = rospy.Publisher("capture_imgs/result_image", Image, queue_size=10) # TODO migrate service uri to this node!
pub_cmpr_img = rospy.Publisher("capture_imgs/result_image/compressed", CompressedImage, queue_size=10) # TODO migrate service uri to this node!
- SegmentSign.update_camera_params()
+ # SegmentSign.update_camera_params()
rospy.loginfo("({}) Sign classification running with params fps: {}, visualization_compression_factor: {}".format(rospy.get_name(), fps, visualization_compression_factor))
call_rvr_color_event(event_id=TriggerLedEventRequest.VISION_STARTUP, stop_current_event=False)
--
GitLab