fixed camera res update and some nicer visualizations

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)

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)