double counting filtering

evaluation/numpy_stats.py

@@ -10,19 +10,58 @@
 
 print('sample size {0:d}'.format(len(data)))
 
-print('mean {0:.2f} std {1:.2f}'.format(np.mean(data), np.std(data)))
-print('min {0:.2f} max {1:.2f} range {2:.2f}'.format(np.min(data), np.max(data), np.max(data) - np.min(data)))
+bin_nr = 20
+bin_range = (70,210) # None for auto
 
-print('median {0:.2f}'.format(np.median(data)))
-print('qunatile: qnt_05% {0:.2f} qnt_95% {1:.2f} range {2:.2f}'.format(np.quantile(data,0.05), np.quantile(data,0.95),np.quantile(data,0.95)-np.quantile(data,0.05)))
-print('quartile: lower {0:.2f} upper {1:.2f} range {2:.2f}'.format(np.quantile(data,0.25), 
-                                                                                        np.quantile(data,0.75),np.quantile(data,0.75)-np.quantile(data,0.25)))
+# calculate all stats
+st_mean = np.mean(data)
+st_std = np.std(data)
+st_min = np.min(data)
+st_max = np.max(data)
+st_median = np.median(data)
+st_qnt_05 = np.quantile(data, 0.05)
+st_qnt_95 = np.quantile(data, 0.95)
+st_loqrt = np.quantile(data, 0.25)
+st_upqrt = np.quantile(data, 0.75)
+h_count, h_range = np.histogram(data, bins=bin_nr, range=bin_range)
+
+# print out the results
+print(f'mean {st_mean:.2f} std {st_std:.2f}')
+print(f'min {st_min:.2f} max {st_max:.2f} range {st_max-st_min:.2f}')
+
+print(f'median {st_median:.2f}')
+print(f'quantile: qnt_05% {st_qnt_05:.2f} qnt_95% {st_qnt_95:.2f} range {st_qnt_95-st_qnt_05:.2f}')
+print(f'quartile: lower {st_loqrt:.2f} upper {st_upqrt:.2f} range {st_upqrt-st_loqrt:.2f}')
 
-h_count, h_range = np.histogram(data, bins=20, range=(70,210))
 print('histogram:')
 print(' - bin counts ', h_count)
 print(' - bin ranges ', h_range)
 
 #plotting histograms with the same settings as above
-plt.hist(data, bins=20, range=(60,220))
+plt.hist(data, bins=bin_nr, range=bin_range)
+
+# plot the stats, uncomment if desired
+
+# plt.axvline(st_mean, linestyle='dashed')
+# plt.axvline(st_min, linestyle='dashed')
+# plt.axvline(st_max, linestyle='dashed')
+
+# plt.axvline(st_median, color='tab:orange', linestyle='dotted')
+# plt.axvline(st_qnt_05, color='tab:orange', linestyle='dotted')
+# plt.axvline(st_qnt_95, color='tab:orange', linestyle='dotted')
+
+# min_ylim, max_ylim = plt.ylim()
+# x_offset = 1.01
+# y_offset = max_ylim*0.9
+# plt.text(st_mean*x_offset, y_offset, f'mean = {st_mean:.1f}')
+# plt.text(st_min*x_offset, y_offset, f'min = {st_min:.1f}')
+# plt.text(st_max*x_offset, y_offset, f'max = {st_max:.1f}')
+
+# y_offset = max_ylim*0.7
+# plt.text(st_median*x_offset, y_offset, f'median = {st_median:.1f}', color='tab:orange')
+# plt.text(st_qnt_05*x_offset, y_offset, f'qnt_05% = {st_qnt_05:.1f}', color='tab:orange')
+# plt.text(st_qnt_95*x_offset, y_offset, f'qnt_95% = {st_qnt_95:.1f}', color='tab:orange')
+
+plt.title(f'histogram bin_nr = {bin_nr:d}')
+
 plt.show()

src/rob2002_tutorial/rob2002_tutorial/counter_basic.py

@@ -12,7 +12,7 @@ def __init__(self):
         self.object_counter = 0
         self.frame_counter = -1
 
-        self.subscriber = self.create_subscription(PolygonStamped, "/object_polygon", self.counter_callback, 10)
+        self.subscriber = self.create_subscription(PolygonStamped, "/object_polygon", self.total_counter_callback, 10)
 
     def counter_callback(self, data):
 
@@ -28,6 +28,10 @@ def counter_callback(self, data):
         # keep counting objects
         self.object_counter += 1
 
+    def total_counter_callback(self, data):       
+        # keep counting objects
+        self.object_counter += 1
+        print(f'{self.object_counter:d} objects counted so far.')
 
 def main(args=None):
     rclpy.init(args=args)

src/rob2002_tutorial/rob2002_tutorial/detector_dblcounting.py

@@ -0,0 +1,102 @@
+import rclpy
+from rclpy.node import Node
+from rclpy import qos
+
+import cv2 as cv
+
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Polygon, PolygonStamped, Point32
+from cv_bridge import CvBridge
+
+from .rectangle import Rectangle
+
+class DetectorBasic(Node):
+    visualisation = True
+    data_logging = False
+    log_path = 'evaluation/data/'
+    seq = 0
+    prev_objects = []
+
+    def __init__(self):    
+        super().__init__('detector_basic')
+        self.bridge = CvBridge()
+
+        self.min_area_size = 100.0
+        self.countour_color = (255, 255, 0) # cyan
+        self.countour_width = 1 # in pixels
+
+        self.object_pub = self.create_publisher(PolygonStamped, '/object_polygon', 10)
+        self.image_sub = self.create_subscription(Image, '/limo/depth_camera_link/image_raw', 
+                                                  self.image_color_callback, qos_profile=qos.qos_profile_sensor_data)
+
+    def image_color_callback(self, data):
+        bgr_image = self.bridge.imgmsg_to_cv2(data, "bgr8") # convert ROS Image message to OpenCV format
+
+        # detect a color blob in the color image
+        # provide the right range values for each BGR channel (set to red bright objects)
+        bgr_thresh = cv.inRange(bgr_image, (0, 0, 80), (50, 50, 255))
+
+        # finding all separate image regions in the binary image, using connected components algorithm
+        bgr_contours, _ = cv.findContours( bgr_thresh,
+            cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
+
+        detected_objects = []
+        for contour in bgr_contours:
+            area = cv.contourArea(contour)
+            # detect only large objects
+            if area > self.min_area_size:
+                # get the bounding box of the region
+                bbx, bby, bbw, bbh = cv.boundingRect(contour)
+                # append the bounding box of the region into a list
+                detected_objects.append(Rectangle(bbx, bby, bbx+bbw, bby+bbh))
+                if self.visualisation:
+                    cv.rectangle(bgr_image, (bbx, bby), (bbx+bbw, bby+bbh), self.countour_color,  self.countour_width)
+
+        # double detection filtering
+        # filter out the objects which overlap with detections from the previous frame
+        # assumes slow movement of the robot
+
+        new_objects = []
+        for rectA in detected_objects:
+            detection = True #
+            for rectB in self.prev_objects:
+                if rectA & rectB: # if there is any overlap consider that to be the existing object
+                    detection = False
+            if detection:
+                # append the bounding box of the region into a list
+                new_objects.append(Polygon(points = [Point32(x=float(rectA.x1), y=float(rectA.y1)), Point32(x=float(rectA.width), y=float(rectA.height))]))
+
+        self.prev_objects = detected_objects
+
+        if new_objects:
+            print(f'Got {len(new_objects):d} new object(s).')
+
+        # publish individual objects from the list
+        # the header information is taken from the Image message
+        for polygon in new_objects:
+            self.object_pub.publish(PolygonStamped(polygon=polygon, header=data.header))
+
+        # log the processed images to files
+        if self.data_logging:
+            cv.imwrite(self.log_path + f'colour_{self.seq:06d}.png', bgr_image)
+            cv.imwrite(self.log_path + f'mask_{self.seq:06d}.png', bgr_thresh)
+
+        # visualise the image processing results    
+        if self.visualisation:
+            cv.imshow("colour image", bgr_image)
+            cv.imshow("detection mask", bgr_thresh)
+            cv.waitKey(1)
+
+        self.seq += 1
+
+def main(args=None):
+    rclpy.init(args=args)
+    detector_basic = DetectorBasic()
+
+    rclpy.spin(detector_basic)
+
+    detector_basic.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/rob2002_tutorial/mover_spinner.py

@@ -0,0 +1,38 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+
+class MoverSpinner(Node):
+
+    def __init__(self):
+        super().__init__('mover_spinner')
+        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
+        timer_period = 1.0  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.i = 0
+
+    def timer_callback(self):              
+        msg = Twist()
+        msg.angular.z = 0.1
+        self.publisher_.publish(msg)
+        self.get_logger().info(f'Publishing: "{msg}"')
+        self.i += 1
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover_spinner = MoverSpinner()
+
+    rclpy.spin(mover_spinner)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    mover_spinner.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/rob2002_tutorial/rectangle.py

@@ -0,0 +1,98 @@
+from itertools import product, tee
+
+def pairwise(iterable):
+    "s -> (s0, s1), (s1, s2), (s2, s3), ..."
+    a, b = tee(iterable)
+    next(b, None)
+    return zip(a, b)
+
+class Rectangle:
+
+    __slots__ = '__x1', '__y1', '__x2', '__y2'
+
+    def __init__(self, x1, y1, x2, y2):
+        self.__setstate__((min(x1, x2), min(y1, y2), max(x1, x2), max(y1, y2)))
+
+    def __repr__(self):
+        return '{}({})'.format(type(self).__name__, ', '.join(map(repr, self)))
+
+    def __eq__(self, other):
+        return self.data == other.data
+
+    def __ne__(self, other):
+        return self.data != other.data
+
+    def __hash__(self):
+        return hash(self.data)
+
+    def __len__(self):
+        return 4
+
+    def __getitem__(self, key):
+        return self.data[key]
+
+    def __iter__(self):
+        return iter(self.data)
+
+    def __and__(self, other):
+        x1, y1, x2, y2 = max(self.x1, other.x1), max(self.y1, other.y1), \
+                         min(self.x2, other.x2), min(self.y2, other.y2)
+        if x1 < x2 and y1 < y2:
+            return type(self)(x1, y1, x2, y2)
+
+    def __sub__(self, other):
+        intersection = self & other
+        if intersection is None:
+            yield self
+        else:
+            x, y = {self.x1, self.x2}, {self.y1, self.y2}
+            if self.x1 < other.x1 < self.x2:
+                x.add(other.x1)
+            if self.y1 < other.y1 < self.y2:
+                y.add(other.y1)
+            if self.x1 < other.x2 < self.x2:
+                x.add(other.x2)
+            if self.y1 < other.y2 < self.y2:
+                y.add(other.y2)
+            for (x1, x2), (y1, y2) in product(pairwise(sorted(x)),
+                                              pairwise(sorted(y))):
+                instance = type(self)(x1, y1, x2, y2)
+                if instance != intersection:
+                    yield instance
+
+    def __getstate__(self):
+        return self.x1, self.y1, self.x2, self.y2
+
+    def __setstate__(self, state):
+        self.__x1, self.__y1, self.__x2, self.__y2 = state
+
+    @property
+    def x1(self):
+        return self.__x1
+
+    @property
+    def y1(self):
+        return self.__y1
+
+    @property
+    def x2(self):
+        return self.__x2
+
+    @property
+    def y2(self):
+        return self.__y2
+
+    @property
+    def width(self):
+        return self.x2 - self.x1
+
+    @property
+    def height(self):
+        return self.y2 - self.y1
+
+    intersection = __and__
+
+    difference = __sub__
+
+    data = property(__getstate__)
+

src/rob2002_tutorial/setup.py

@@ -25,7 +25,9 @@
         'console_scripts': [
             'mover_basic = rob2002_tutorial.mover_basic:main',
             'mover_laser = rob2002_tutorial.mover_laser:main',
+            'mover_spinner = rob2002_tutorial.mover_spinner:main',
             'detector_basic = rob2002_tutorial.detector_basic:main',
+            'detector_better = rob2002_tutorial.detector_dblcounting:main',
             'counter_basic = rob2002_tutorial.counter_basic:main',
         ],
     },