#789 Detection ok for all patterns. Renaming to riwbot

Author: miguelriemoliveira

atom_calibration/src/atom_calibration/collect/patterns.py

@@ -37,16 +37,17 @@ def detect(self, image, equalize_histogram=False):
 
         return {"detected": True, 'keypoints': sub_pixel_corners, 'ids': range(0, len(sub_pixel_corners))}
 
-    def drawKeypoints(self, image, result, K=None, D=None):
+    def drawKeypoints(self, image, result, length=0.2, color=(1, 0, 0), K=None, D=None, pattern_name=None):
+
         if result['keypoints'] is None or len(result['keypoints']) == 0:
             return
 
         if not result['detected']:
             return
 
         for point in result['keypoints']:
-            cv2.drawMarker(image, (int(point[0][0]), int(point[0][1])), (0, 0, 255), cv2.MARKER_CROSS, 14)
-            cv2.circle(image, (int(point[0][0]), int(point[0][1])), 7, (0, 255, 0), lineType=cv2.LINE_AA)
+            cv2.drawMarker(image, (int(point[0][0]), int(point[0][1])), color, cv2.MARKER_CROSS, 14)
+            cv2.circle(image, (int(point[0][0]), int(point[0][1])), 7, color, lineType=cv2.LINE_AA)
 
         if K is not None and D is not None:  # estimate pose and draw axis on image
 
@@ -71,7 +72,11 @@ def drawKeypoints(self, image, result, K=None, D=None):
             _, rvecs, tvecs = cv2.solvePnP(
                 objp[ids], np.array(corners, dtype=np.float32), K, D)
 
-            cv2.drawFrameAxes(image, K, D, rvecs, tvecs, 0.3)
+            cv2.drawFrameAxes(image, K, D, rvecs, tvecs, length)
+
+        if pattern_name is not None:
+            point = (int(corners[0][0][0]), int(corners[0][0][1]))
+            cv2.putText(image, pattern_name, point, cv2.FONT_HERSHEY_SIMPLEX, 1.0, color, 2)
 
 
 class CharucoPattern(object):
@@ -107,12 +112,12 @@ def __init__(self, size, length, marker_length, dictionary='DICT_5X5_100'):
 
         if cv2.__version__ == '4.6.0':
             self.dictionary = cv2.aruco.Dictionary_get(charuco_dictionary)
-            self.board = cv2.aruco.CharucoBoard_create(size["y"] + 1, size["x"] + 1, length, marker_length,
+            self.board = cv2.aruco.CharucoBoard_create(size["x"] + 1, size["y"] + 1, length, marker_length,
                                                        self.dictionary)
-
+            # self.image_board = self.board.draw((787, 472))
         else:  # all versions from 4.7.0 onward
             self.dictionary = cv2.aruco.getPredefinedDictionary(charuco_dictionary)
-            self.board = cv2.aruco.CharucoBoard((size["y"] + 1, size["x"] + 1), length, marker_length,
+            self.board = cv2.aruco.CharucoBoard((size["x"] + 1, size["y"] + 1), length, marker_length,
                                                 self.dictionary)
 
     def detect(self, image, equalize_histogram=False):
@@ -129,13 +134,25 @@ def detect(self, image, equalize_histogram=False):
         if cv2.__version__ == '4.6.0':
             params = cv2.aruco.DetectorParameters_create()
             corners, ids, rejected = cv2.aruco.detectMarkers(gray, self.dictionary, parameters=params)
+
+            # Debug
+            # image_gui = deepcopy(image)
+#             cv2.aruco.drawDetectedMarkers(image_gui, corners, ids)
+#             cv2.namedWindow('Debug image', cv2.WINDOW_NORMAL)
+#             cv2.imshow('Debug image', image_gui)
+#
+#             cv2.namedWindow('image board', cv2.WINDOW_NORMAL)
+#             cv2.imshow('image board', self.image_board)
+#
+#             cv2.waitKey(30)
+
             # cv2.aruco.refineDetectedMarkers(gray, self.board, corners, ids, rejected)
         else:
             params = cv2.aruco.DetectorParameters()
             detector = cv2.aruco.ArucoDetector(self.dictionary, params)
             corners, ids, rejected = detector.detectMarkers(gray)
 
-        if len(corners) <= 4:  # Must have more than 3 corner detections
+        if len(corners) <= 8:  # Must have more than 3 corner detections
             return {"detected": False, 'keypoints': np.array([]), 'ids': []}
 
         # Interpolation of charuco corners
@@ -153,7 +170,7 @@ def detect(self, image, equalize_histogram=False):
         # If all above works, return detected corners.
         return {'detected': True, 'keypoints': ccorners, 'ids': cids.ravel().tolist()}
 
-    def drawKeypoints(self, image, result, K=None, D=None):
+    def drawKeypoints(self, image, result, length=0.2, color=(255, 0, 0), K=None, D=None, pattern_name=None):
         if result['keypoints'] is None or len(result['keypoints']) == 0:
             return
 
@@ -164,6 +181,7 @@ def drawKeypoints(self, image, result, K=None, D=None):
         # Build a numpy array with the chessboard corners
         ccorners = np.zeros((len(result['keypoints']), 1, 2), dtype=float)
         cids = list(range(0, len(result['keypoints'])))
+        num_corners = len(cids)
 
         points = result['keypoints'].astype(np.int32)
         for idx, (point, id) in enumerate(zip(result['keypoints'], result['ids'])):
@@ -175,15 +193,20 @@ def drawKeypoints(self, image, result, K=None, D=None):
         np_ccorners = np.array(ccorners, dtype=np.float32)
 
         # Draw charuco corner detection
-        image = cv2.aruco.drawDetectedCornersCharuco(image, np_ccorners, np_cids, (0, 0, 255))
+        image = cv2.aruco.drawDetectedCornersCharuco(image, np_ccorners, np_cids, color)
 
         if K is not None and D is not None:  # estimate pose and draw axis on image
             rvecs, tvecs = None, None
             _, rvecs, tvecs = cv2.aruco.estimatePoseCharucoBoard(np_ccorners,
                                                                  np_cids, self.board,
                                                                  K, D, rvecs, tvecs)
             # Draw frame on the image
-            cv2.drawFrameAxes(image, K, D, rvecs, tvecs, 0.3)
+            cv2.drawFrameAxes(image, K, D, rvecs, tvecs, length)
+
+        if pattern_name is not None:
+            point = tuple(ccorners[0][0])
+            point = (int(point[0]), int(point[1]))
+            cv2.putText(image, pattern_name, point, cv2.FONT_HERSHEY_SIMPLEX, 1.0, color, 2)
 
 
 def initializePatternsDict(config, step=0.02):

atom_calibration/src/atom_calibration/collect/rgb_labeler

@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+
+
+# stdlib
+import argparse
+from copy import deepcopy
+import os
+import sys
+
+import cv2
+from atom_core.config_io import loadConfig
+
+import atom_core.ros_utils
+
+# 3rd-party
+import rospy
+import numpy as np
+import atom_core.utilities
+from cv_bridge import CvBridge
+from matplotlib import cm
+from sensor_msgs.msg import *
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import CameraInfo
+from colorama import Fore, Style
+
+# local packages
+from atom_calibration.collect import patterns
+from atom_core.utilities import atomError
+
+from atom_msgs.msg import ImageWithRGBLabels, RGBLabels, Detection2D
+
+
+class RGBLabeler:
+    """
+    Handles data labeling for a generic sensor:
+        RGB: Fully automated labeling. Periodically runs a chessboard detection on the newly received image.
+    """
+
+    def __init__(self, config, sensor_name, label_data=True, debug=False):
+
+        print('Starting RGB labeler for sensor ' + str(sensor_name))
+
+        # Store variables to class attributes
+        self.config = config
+        self.label_data = label_data
+        self.sensor_name = sensor_name
+        self.debug = debug
+
+        # Check if sensor exists in config
+        if self.sensor_name not in config['sensors'].keys():
+            atomError('Sensor ' + Fore.BLUE + sensor_name + Style.RESET_ALL + ' not in config. Cannot start labeler.\nAvailable sensors are: ' +
+                      Fore.BLUE + str(list(config['sensors'].keys())) + Style.RESET_ALL)
+
+        self.sensor_config = config['sensors'][sensor_name]
+
+        # Check if modality is rgb
+        if not self.sensor_config['modality'] == 'rgb':
+            atomError('Sensor ' + sensor_name + ' has modality ' +
+                      self.sensor_config['modality'] + ' . Cannot start rgb labeler.')
+
+        # Get the type of message from the message topic of the sensor data, which is given as input.
+        self.msg_type_str, self.msg_type = atom_core.ros_utils.getMessageTypeFromTopic(self.sensor_config['topic_name'])
+
+        # Create a self.detectors dictionary with all one detector class instantiated for each pattern
+        self.detectors = {}
+        for pattern_key, pattern in self.config['calibration_patterns'].items():
+            if pattern['pattern_type'] == 'chessboard':
+                self.detectors[pattern_key] = patterns.ChessboardPattern(pattern['dimension'],
+                                                                         pattern['size'])
+            elif pattern['pattern_type'] == 'charuco':
+                self.detectors[pattern_key] = patterns.CharucoPattern(pattern['dimension'],
+                                                                      pattern['size'],
+                                                                      pattern['inner_size'],
+                                                                      pattern['dictionary'])
+            else:
+                atomError('Unknown pattern type ' + str(pattern['pattern_type']))
+
+        self.cm_patterns = cm.tab10(np.linspace(0, 1, len(self.config['calibration_patterns'].keys())))
+
+        # Set up the labeled image publication
+        self.bridge = CvBridge()  # a CvBridge structure is needed to convert opencv images to ros messages.
+        self.labeled_image_topic = self.sensor_config['topic_name'] + '/labeled'
+        self.publisher_labeled_image = rospy.Publisher(self.labeled_image_topic,
+                                                       sensor_msgs.msg.Image,
+                                                       queue_size=1)  # publish
+
+        # Set up the publication of the produced labels
+        self.publisher_image_with_rgb_labels = rospy.Publisher(self.sensor_name + '/labels',
+                                                               ImageWithRGBLabels, queue_size=1)
+
+        # Receive one camera_info message corresponding to this sensor and store it.
+        # We need this to have the K and D matrices.
+        camera_info_topic = os.path.dirname(self.sensor_config['topic_name']) + '/camera_info'
+        print('Waiting for camera_info message on topic ' + camera_info_topic + ' ...')
+        self.camera_info_msg = rospy.wait_for_message(camera_info_topic, CameraInfo)
+        print('... received!')
+
+        self.K = np.ndarray((3, 3), dtype=float, buffer=np.array(self.camera_info_msg.K))
+        self.D = np.ndarray((5, 1), dtype=float, buffer=np.array(self.camera_info_msg.D))
+
+        # self.camera_info = message_converter.convert_ros_message_to_dictionary(self.camera_info_msg)
+
+        # Subscribe to the message topic containing sensor data
+        self.subscriber = rospy.Subscriber(self.sensor_config['topic_name'], self.msg_type,
+                                           self.labelData, queue_size=1)
+
+    def labelData(self, msg):
+
+        if not self.labelData:  # Nothing to do in this case
+            return
+
+        if self.debug:
+            print('Image received. Labeling data for sensor ' + self.sensor_name)
+
+        # Convert to opencv image and save image to disk
+        image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+        image_gui = deepcopy(image)
+
+        # Create the labels by detecting all patterns in the image
+        image_with_rgb_labels_msg = ImageWithRGBLabels()
+        image_with_rgb_labels_msg.image = msg  # image is the one use for labeling
+        for detector_idx, (detector_key, detector) in enumerate(self.detectors.items()):
+
+            # Initialize labels for this pattern
+            pattern_labels = RGBLabels()
+            pattern_labels.detected = False
+            pattern_labels.idxs = []
+
+            # Detect this pattern in the image
+            result = detector.detect(image, equalize_histogram=False)
+
+            if result['detected']:
+                pattern_labels.detected = True
+                for idx, corner in enumerate(result['keypoints']):
+                    x = float(corner[0][0])
+                    y = float(corner[0][1])
+
+                    # The charuco pattern also returns an ID for each keypoint.
+                    # We can use this information for partial detections.
+                    if 'ids' in result:
+                        id = result['ids'][idx]
+                    else:
+                        id = idx
+
+                    pattern_labels.idxs.append(Detection2D(x=x, y=y, id=id))
+
+            # Add the labels if this pattern to the ImageWithLabels msg
+            image_with_rgb_labels_msg.patterns.append(pattern_labels)
+
+            # For visual debugging
+            color = self.cm_patterns[detector_idx, 0:3]
+            color = int(color[0]*255), int(color[1]*255), int(color[2]*255)
+
+            detector.drawKeypoints(image_gui, result, K=self.K, D=self.D, pattern_name=detector_key, color=color)
+
+        # Publish ImageWithLabels msg
+        self.publisher_image_with_rgb_labels.publish(image_with_rgb_labels_msg)
+        # labels_msg.labels =
+        # self.camera_info = message_converter.convert_ros_message_to_dictionary(self.camera_info_msg)
+
+        # Create labeled image and publish it
+        msg_out = self.bridge.cv2_to_imgmsg(image_gui, encoding="passthrough")
+        msg_out.header.stamp = msg.header.stamp
+        msg_out.header.frame_id = msg.header.frame_id
+        self.publisher_labeled_image.publish(msg_out)
+
+        if self.debug:
+            cv2.namedWindow(self.labeled_image_topic, cv2.WINDOW_NORMAL)
+            cv2.imshow(self.labeled_image_topic, image_gui)
+            key = cv2.waitKey(30)
+            if key & 0xff == ord('q'):
+                rospy.signal_shutdown(1)
+
+
+def main():
+
+    # Parse command line arguments
+    ap = argparse.ArgumentParser()
+    ap.add_argument("-c", "--calibration_file", help='full path to calibration file.', type=str, required=True)
+    ap.add_argument("-sn", "--sensor_name", help='Name of the sensor as given in the config.yml', type=str, required=True)
+    ap.add_argument("-d", "--debug", help='Run in debug mode', action='store_true', default=False)
+
+    args = vars(ap.parse_args(args=atom_core.ros_utils.filterLaunchArguments(sys.argv)))
+    print("\nArgument list=" + str(args) + '\n')
+
+    config = loadConfig(args['calibration_file'])
+
+    # Initialize ROS stuff
+    node_name = args['sensor_name'] + '_rgb_labeler'
+    rospy.init_node(node_name)
+
+    rgb_labeler = RGBLabeler(config, sensor_name=args['sensor_name'], debug=args['debug'])
+
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

atom_examples/rihmpbot/rihmpbot_calibration/calibration/config.yml

@@ -124,7 +124,7 @@ calibration_patterns:
   #   dictionary: "DICT_6X6_100"
   #   mesh_file: "package://atom_worlds/pattern/models/charuco_200x150_4x6_30_22_DICT_6X6/model.dae"
   #   border_size: {'x': 0.01, 'y': 0.015} # measured in meshlab
-  #   dimension: {'x': 3, 'y': 5} # for charuco patterns in atom we put dimension -1
+  #   dimension: {'x': 5, 'y': 3} # for charuco patterns in atom we put dimension -1
   #   size: 0.025
   #   inner_size: 0.018
   charuco_170x100_3x6:
@@ -135,7 +135,7 @@ calibration_patterns:
     dictionary: "DICT_6X6_100"
     mesh_file: "package://atom_worlds/pattern/models/charuco_170x100_3x6_25_18_DICT_6X6/model.dae"
     border_size: {'x': 0.01, 'y': 0.01235} # measured in meshlab
-    dimension: {'x': 2, 'y': 5} # for charuco patterns in atom we put dimension -1
+    dimension: {'x': 5, 'y': 2} # for charuco patterns in atom we put dimension -1
     size: 0.025
     inner_size: 0.018
 
@@ -147,7 +147,7 @@ calibration_patterns:
     dictionary: "DICT_7X7_100"
     mesh_file: "package://atom_worlds/pattern/models/charuco_200x120_3x6_30_22_DICT_7X7/model.dae"
     border_size: {'x': 0.01, 'y': 0.015} # measured in meshlab
-    dimension: {'x': 2, 'y': 5} # for charuco patterns in atom we put dimension -1
+    dimension: {'x': 5, 'y': 2} # for charuco patterns in atom we put dimension -1
     size: 0.030
     inner_size: 0.022
 
@@ -176,7 +176,6 @@ calibration_patterns:
   #   inner_size: 0.045
 
 
-
 # Miscellaneous configuration
 
 # If your calibration problem is not fully constrained you should anchored one of the sensors.