raspberry-pi/config.ini

@@ -39,17 +39,18 @@
 [mag_sensor]
 
 [opt_sensor]
-  capture_device = /dev/video1
+  capture_device = -1
   debug_image = yes
 
   # distance between center of fiducials in mm
-  target_distance = 165
+  target_distance = 55
 
   # x offset when target is in the center of camera (at offset=50%) in mm
   x_offset = 195
 
-  # FOV of the camera in °
-  fov = 75
+  # horizontal FoV of the camera in °
+  # RaspiCam datasheet: https://www.raspberrypi.org/documentation/hardware/camera/
+  fov = 53.50
 
 [gui]
   fullscreen = yes

raspberry-pi/sensors/opticalSensor.py

@@ -30,14 +30,20 @@ parameters =  aruco.DetectorParameters_create()
 
 
 def find_marker(image, debug=True):
-    # Our operations on the frame come here
   gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-  im_bw = cv2.equalizeHist(gray)
   if debug:
-    image[:,:,0] = im_bw
-    image[:,:,1] = im_bw
-    image[:,:,2] = im_bw
-  corners, ids, rejectedImgPoints = aruco.detectMarkers(im_bw, aruco_dict, parameters=parameters)
+    image[:,:,0] = gray
+    image[:,:,1] = gray
+    image[:,:,2] = gray
+  corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
+
+  if debug:
+    for corner in rejectedImgPoints:
+      c = corner[0]
+      cv2.line(image, tuple(c[0]), tuple(c[1]), (0, 255, 255), 1)
+      cv2.line(image, tuple(c[1]), tuple(c[2]), (0, 255, 255), 1)
+      cv2.line(image, tuple(c[2]), tuple(c[3]), (0, 255, 255), 1)
+      cv2.line(image, tuple(c[3]), tuple(c[0]), (0, 255, 255), 1)
   
   markers = [None] * 4
 
@@ -45,12 +51,13 @@ def find_marker(image, debug=True):
     return markers
 
   for corner, id in zip(corners, ids.flatten()):
-    # draw the bounding box of the ArUCo detection
+    # calculate the average of all 4 corners
     c = corner[0]
     cX = sum(c[:,0]) / 4
     cY = sum(c[:,1]) / 4
 
     if debug:
+      # draw the bounding box of the ArUCo detection
       cv2.line(image, tuple(c[0]), tuple(c[1]), (0, 255, 0), 2)
       cv2.line(image, tuple(c[1]), tuple(c[2]), (0, 255, 0), 2)
       cv2.line(image, tuple(c[2]), tuple(c[3]), (0, 255, 0), 2)
@@ -68,8 +75,9 @@ def measureDistances(image, debug=True):
     markers = find_marker(image, debug)
 
     if markers[0] and markers[1]:
-      
+      # the distance between marker 0 and marker 1
       distance = np.sqrt((markers[0][0]-markers[1][0])**2 + (markers[0][1]-markers[1][1])**2)
+      # the center point between marker 0 and 1
       centerX = (markers[0][0]+markers[1][0])/2
       centerY = (markers[0][1]+markers[1][1])/2
 
@@ -81,6 +89,7 @@ def measureDistances(image, debug=True):
         cv2.putText(image, "%.2fpx" % (distance), (int(markers[0][0]), int(markers[0][1]+25)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
         cv2.putText(image, "%.2fpx" % (centerX) , (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
         
+      # normalize pixels to percent of horizontal resolution
       return centerX/image.shape[1], distance/image.shape[1]
 
     else:
@@ -98,19 +107,22 @@ class OpticalSensor():
     self.x_offset               = float(conf["opt_sensor"]["x_offset"])
     self.fov                    = float(conf["opt_sensor"]["fov"])
     self.log_handler            = logHandler.get_log_handler()
-    self._t  = None
+    self.showImage              = conf["opt_sensor"]["debug_image"] == "yes"
+    self._thread = threading.Thread(target=self._getFrames, args=())
     self.success = False
     self.running = True
-    self.showImage = True
 
   def start(self):
     self.log_handler.log_and_print("start optical sensor")
-    if not self._t:
-      self._t = threading.Thread(target=self._getFrames, args=())
-      self._t.start()
+    self._thread.start()
 
   def _getFrames(self):
-    self.cap = cv2.VideoCapture(self.conf["opt_sensor"]["capture_device"])
+    try:
+      dev = int(self.conf["opt_sensor"]["capture_device"])
+    except ValueError:
+      dev = self.conf["opt_sensor"]["capture_device"]
+    self.cap = cv2.VideoCapture(dev)
+
     while self.running:
       self.success, image = self.cap.read()
       if self.success:
@@ -121,7 +133,7 @@ class OpticalSensor():
           if position != None:
             self.pass_to_gui(position + values)
         if self.showImage:
-          cv2.imshow("image", image)
+          cv2.imshow("Camera capture with ARUCO detection", image)
           if cv2.waitKey(1) & 0xFF == ord('q'):
             self.showImage = False
             cv2.destroyAllWindows()
@@ -134,14 +146,22 @@ class OpticalSensor():
 
   def stop(self):
     self.running = False
-    if self._t:
-      self._t.join()
-    self.log_handler.log_and_print("stop optical sensor")
+    self._thread.join()
+    self.log_handler.log_and_print("stopped optical sensor")
 
   def calculate_position(self, values):
     centerX_perc, distance_perc = values
-
+    #       distance (as a percentage of visible_width_at_y)
+    # \    <-------->    /<= beta
+    #   \       |  /   /
+    #     \    y| /  /
+    #       \   |/ /
+    #  x_off  \__/ 
+    #<---------># <= camera with FoV/2 = alpha
+
+    # extrapolate total width from measured fiducial distance
     visible_width_at_y = self.target_distance / distance_perc
+    # 50% is is center of camera, so we shift it by x_offset to align with the system
     x = self.x_offset + visible_width_at_y * (centerX_perc-0.5)
     alpha = (self.fov/2) / 180 * math.pi
     beta = (90 - self.fov/2) / 180 * math.pi