JeVoisBase  1.10
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
FirstPython.py
Go to the documentation of this file.
1 ######################################################################################################################
2 #
3 # JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2017 by Laurent Itti, the University of Southern
4 # California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 #
6 # This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 # redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 # Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 # without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 # License for more details. You should have received a copy of the GNU General Public License along with this program;
11 # if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 #
13 # Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 # Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 ######################################################################################################################
16 
17 import libjevois as jevois
18 import cv2
19 import numpy as np
20 import math # for cos, sin, etc
21 
22 ## Simple example of FIRST Robotics image processing pipeline using OpenCV in Python on JeVois
23 #
24 # This module is a simplified version of the C++ module \jvmod{FirstVision}. It is available with \jvversion{1.6.2} or
25 # later.
26 #
27 # This module implements a simple color-based object detector using OpenCV in Python. Its main goal is to also
28 # demonstrate full 6D pose recovery of the detected object, in Python.
29 #
30 # This module isolates pixels within a given HSV range (hue, saturation, and value of color pixels), does some cleanups,
31 # and extracts object contours. It is looking for a rectangular U shape of a specific size (set by parameters \p owm and
32 # \p ohm for object width and height in meters). See screenshots for an example of shape. It sends information about
33 # detected objects over serial.
34 #
35 # This module usually works best with the camera sensor set to manual exposure, manual gain, manual color balance, etc
36 # so that HSV color values are reliable. See the file \b script.cfg file in this module's directory for an example of
37 # how to set the camera settings each time this module is loaded.
38 #
39 # This module is provided for inspiration. It has no pretension of actually solving the FIRST Robotics vision problem
40 # in a complete and reliable way. It is released in the hope that FRC teams will try it out and get inspired to
41 # develop something much better for their own robot.
42 #
43 # Using this module
44 # -----------------
45 #
46 # Check out [this tutorial](http://jevois.org/tutorials/UserFirstVision.html) first, for the \jvmod{FirstVision} module
47 # written in C++ and also check out the doc for \jvmod{FirstVision}. Then you can just dive in and start editing the
48 # python code of \jvmod{FirstPython}.
49 #
50 # See http://jevois.org/tutorials for tutorials on getting started with programming JeVois in Python without having
51 # to install any development software on your host computer.
52 #
53 # Trying it out
54 # -------------
55 #
56 # Edit the module's file at JEVOIS:/modules/JeVois/FirstPython/FirstPython.py and set the parameters \p self.owm and \p
57 # self.ohm to the physical width and height of your U-shaped object in meters. You should also review and edit the other
58 # parameters in the module's constructor, such as the range of HSV colors.
59 #
60 # @author Laurent Itti
61 #
62 # @displayname FIRST Python
63 # @videomapping YUYV 640 252 60.0 YUYV 320 240 60.0 JeVois FirstPython
64 # @videomapping YUYV 320 252 60.0 YUYV 320 240 60.0 JeVois FirstPython
65 # @email itti\@usc.edu
66 # @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
67 # @copyright Copyright (C) 2018 by Laurent Itti, iLab and the University of Southern California
68 # @mainurl http://jevois.org
69 # @supporturl http://jevois.org/doc
70 # @otherurl http://iLab.usc.edu
71 # @license GPL v3
72 # @distribution Unrestricted
73 # @restrictions None
74 # @ingroup modules
76  # ###################################################################################################
77  ## Constructor
78  def __init__(self):
79  # HSV color range to use:
80  #
81  # H: 0=red/do not use because of wraparound, 30=yellow, 45=light green, 60=green, 75=green cyan, 90=cyan,
82  # 105=light blue, 120=blue, 135=purple, 150=pink
83  # S: 0 for unsaturated (whitish discolored object) to 255 for fully saturated (solid color)
84  # V: 0 for dark to 255 for maximally bright
85  self.HSVmin = np.array([ 20, 50, 180], dtype=np.uint8)
86  self.HSVmax = np.array([ 80, 255, 255], dtype=np.uint8)
87 
88  # Measure your U-shaped object (in meters) and set its size here:
89  self.owm = 0.280 # width in meters
90  self.ohm = 0.175 # height in meters
91 
92  # Other processing parameters:
93  self.epsilon = 0.015 # Shape smoothing factor (higher for smoother)
94  self.hullarea = ( 20*20, 300*300 ) # Range of object area (in pixels) to track
95  self.hullfill = 50 # Max fill ratio of the convex hull (percent)
96  self.ethresh = 900 # Shape error threshold (lower is stricter for exact shape)
97  self.margin = 5 # Margin from from frame borders (pixels)
98 
99  # Instantiate a JeVois Timer to measure our processing framerate:
100  self.timer = jevois.Timer("FirstPython", 100, jevois.LOG_INFO)
101 
102  # CAUTION: The constructor is a time-critical code section. Taking too long here could upset USB timings and/or
103  # video capture software running on the host computer. Only init the strict minimum here, and do not use OpenCV,
104  # read files, etc
105 
106  # ###################################################################################################
107  ## Load camera calibration from JeVois share directory
108  def loadCameraCalibration(self, w, h):
109  cpf = "/jevois/share/camera/calibration{}x{}.yaml".format(w, h)
110  fs = cv2.FileStorage(cpf, cv2.FILE_STORAGE_READ)
111  if (fs.isOpened()):
112  self.camMatrix = fs.getNode("camera_matrix").mat()
113  self.distCoeffs = fs.getNode("distortion_coefficients").mat()
114  jevois.LINFO("Loaded camera calibration from {}".format(cpf))
115  else:
116  jevois.LFATAL("Failed to read camera parameters from file [{}]".format(cpf))
117 
118  # ###################################################################################################
119  ## Detect objects within our HSV range
120  def detect(self, imgbgr, outimg = None):
121  maxn = 5 # max number of objects we will consider
122  h, w, chans = imgbgr.shape
123 
124  # Convert input image to HSV:
125  imghsv = cv2.cvtColor(imgbgr, cv2.COLOR_BGR2HSV)
126 
127  # Isolate pixels inside our desired HSV range:
128  imgth = cv2.inRange(imghsv, self.HSVmin, self.HSVmax)
129  str = "H={}-{} S={}-{} V={}-{} ".format(self.HSVmin[0], self.HSVmax[0], self.HSVmin[1],
130  self.HSVmax[1], self.HSVmin[2], self.HSVmax[2])
131 
132  # Create structuring elements for morpho maths:
133  if not hasattr(self, 'erodeElement'):
134  self.erodeElement = cv2.getStructuringElement(cv2.MORPH_RECT, (2, 2))
135  self.dilateElement = cv2.getStructuringElement(cv2.MORPH_RECT, (2, 2))
136 
137  # Apply morphological operations to cleanup the image noise:
138  imgth = cv2.erode(imgth, self.erodeElement)
139  imgth = cv2.dilate(imgth, self.dilateElement)
140 
141  # Detect objects by finding contours:
142  contours, hierarchy = cv2.findContours(imgth, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
143  str += "N={} ".format(len(contours))
144 
145  # Only consider the 5 biggest objects by area:
146  contours = sorted(contours, key = cv2.contourArea, reverse = True)[:maxn]
147  hlist = [ ] # list of hulls of good objects, which we will return
148  str2 = ""
149  beststr2 = ""
150 
151  # Identify the "good" objects:
152  for c in contours:
153  # Keep track of our best detection so far:
154  if len(str2) > len(beststr2): beststr2 = str2
155  str2 = ""
156 
157  # Compute contour area:
158  area = cv2.contourArea(c, oriented = False)
159 
160  # Compute convex hull:
161  rawhull = cv2.convexHull(c, clockwise = True)
162  rawhullperi = cv2.arcLength(rawhull, closed = True)
163  hull = cv2.approxPolyDP(rawhull, epsilon = self.epsilon * rawhullperi * 3.0, closed = True)
164 
165  # Is it the right shape?
166  if (hull.shape != (4,1,2)): continue # 4 vertices for the rectangular convex outline (shows as a trapezoid)
167  str2 += "H" # Hull is quadrilateral
168 
169  huarea = cv2.contourArea(hull, oriented = False)
170  if huarea < self.hullarea[0] or huarea > self.hullarea[1]: continue
171  str2 += "A" # Hull area ok
172 
173  hufill = area / huarea * 100.0
174  if hufill > self.hullfill: continue
175  str2 += "F" # Fill is ok
176 
177  # Check object shape:
178  peri = cv2.arcLength(c, closed = True)
179  approx = cv2.approxPolyDP(c, epsilon = self.epsilon * peri, closed = True)
180  if len(approx) < 7 or len(approx) > 9: continue # 8 vertices for a U shape
181  str2 += "S" # Shape is ok
182 
183  # Compute contour serr:
184  serr = 100.0 * cv2.matchShapes(c, approx, cv2.CONTOURS_MATCH_I1, 0.0)
185  if serr > self.ethresh: continue
186  str2 += "E" # Shape error is ok
187 
188  # Reject the shape if any of its vertices gets within the margin of the image bounds. This is to avoid
189  # getting grossly incorrect 6D pose estimates as the shape starts getting truncated as it partially exits
190  # the camera field of view:
191  reject = 0
192  for v in c:
193  if v[0,0] < self.margin or v[0,0] >= w-self.margin or v[0,1] < self.margin or v[0,1] >= h-self.margin:
194  reject = 1
195  break
196 
197  if reject == 1: continue
198  str2 += "M" # Margin ok
199 
200  # Re-order the 4 points in the hull if needed: In the pose estimation code, we will assume vertices ordered
201  # as follows:
202  #
203  # 0| |3
204  # | |
205  # | |
206  # 1----------2
207 
208  # v10+v23 should be pointing outward the U more than v03+v12 is:
209  v10p23 = complex(hull[0][0,0] - hull[1][0,0] + hull[3][0,0] - hull[2][0,0],
210  hull[0][0,1] - hull[1][0,1] + hull[3][0,1] - hull[2][0,1])
211  len10p23 = abs(v10p23)
212  v03p12 = complex(hull[3][0,0] - hull[0][0,0] + hull[2][0,0] - hull[1][0,0],
213  hull[3][0,1] - hull[0][0,1] + hull[2][0,1] - hull[1][0,1])
214  len03p12 = abs(v03p12)
215 
216  # Vector from centroid of U shape to centroid of its hull should also point outward of the U:
217  momC = cv2.moments(c)
218  momH = cv2.moments(hull)
219  vCH = complex(momH['m10'] / momH['m00'] - momC['m10'] / momC['m00'],
220  momH['m01'] / momH['m00'] - momC['m01'] / momC['m00'])
221  lenCH = abs(vCH)
222 
223  if len10p23 < 0.1 or len03p12 < 0.1 or lenCH < 0.1: continue
224  str2 += "V" # Shape vectors ok
225 
226  good = (v10p23.real * vCH.real + v10p23.imag * vCH.imag) / (len10p23 * lenCH)
227  bad = (v03p12.real * vCH.real + v03p12.imag * vCH.imag) / (len03p12 * lenCH)
228 
229  # We reject upside-down detections as those are likely to be spurious:
230  if vCH.imag >= -2.0: continue
231  str2 += "U" # U shape is upright
232 
233  # Fixup the ordering of the vertices if needed:
234  if bad > good: hull = np.roll(hull, shift = 1, axis = 0)
235 
236  # This detection is a keeper:
237  str2 += " OK"
238  hlist.append(hull)
239 
240  if len(str2) > len(beststr2): beststr2 = str2
241 
242  # Display any results requested by the users:
243  if outimg is not None and outimg.valid():
244  if (outimg.width == w * 2): jevois.pasteGreyToYUYV(imgth, outimg, w, 0)
245  jevois.writeText(outimg, str + beststr2, 3, h+1, jevois.YUYV.White, jevois.Font.Font6x10)
246 
247  return hlist
248 
249  # ###################################################################################################
250  ## Estimate 6D pose of each of the quadrilateral objects in hlist:
251  def estimatePose(self, hlist):
252  rvecs = []
253  tvecs = []
254 
255  # set coordinate system in the middle of the object, with Z pointing out
256  objPoints = np.array([ ( -self.owm * 0.5, -self.ohm * 0.5, 0 ),
257  ( -self.owm * 0.5, self.ohm * 0.5, 0 ),
258  ( self.owm * 0.5, self.ohm * 0.5, 0 ),
259  ( self.owm * 0.5, -self.ohm * 0.5, 0 ) ])
260 
261  for detection in hlist:
262  det = np.array(detection, dtype=np.float).reshape(4,2,1)
263  (ok, rv, tv) = cv2.solvePnP(objPoints, det, self.camMatrix, self.distCoeffs)
264  if ok:
265  rvecs.append(rv)
266  tvecs.append(tv)
267  else:
268  rvecs.append(np.array([ (0.0), (0.0), (0.0) ]))
269  tvecs.append(np.array([ (0.0), (0.0), (0.0) ]))
270 
271  return (rvecs, tvecs)
272 
273  # ###################################################################################################
274  ## Send serial messages, one per object
275  def sendAllSerial(self, w, h, hlist, rvecs, tvecs):
276  idx = 0
277  for c in hlist:
278  # Compute quaternion: FIXME need to check!
279  tv = tvecs[idx]
280  axis = rvecs[idx]
281  angle = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]) ** 0.5
282 
283  # This code lifted from pyquaternion from_axis_angle:
284  mag_sq = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]
285  if (abs(1.0 - mag_sq) > 1e-12): axis = axis / (mag_sq ** 0.5)
286  theta = angle / 2.0
287  r = math.cos(theta)
288  i = axis * math.sin(theta)
289  q = (r, i[0], i[1], i[2])
290 
291  jevois.sendSerial("D3 {} {} {} {} {} {} {} {} {} {} FIRST".
292  format(np.asscalar(tv[0]), np.asscalar(tv[1]), np.asscalar(tv[2]), # position
293  self.owm, self.ohm, 1.0, # size
294  r, np.asscalar(i[0]), np.asscalar(i[1]), np.asscalar(i[2]))) # pose
295  idx += 1
296 
297  # ###################################################################################################
298  ## Draw all detected objects in 3D
299  def drawDetections(self, outimg, hlist, rvecs = None, tvecs = None):
300  # Show trihedron and parallelepiped centered on object:
301  hw = self.owm * 0.5
302  hh = self.ohm * 0.5
303  dd = -max(hw, hh)
304  i = 0
305  empty = np.array([ (0.0), (0.0), (0.0) ])
306 
307  for obj in hlist:
308  # skip those for which solvePnP failed:
309  if np.array_equal(rvecs[i], empty):
310  i += 1
311  continue
312 
313  # Project axis points:
314  axisPoints = np.array([ (0.0, 0.0, 0.0), (hw, 0.0, 0.0), (0.0, hh, 0.0), (0.0, 0.0, dd) ])
315  imagePoints, jac = cv2.projectPoints(axisPoints, rvecs[i], tvecs[i], self.camMatrix, self.distCoeffs)
316 
317  # Draw axis lines:
318  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
319  int(imagePoints[1][0,0] + 0.5), int(imagePoints[1][0,1] + 0.5),
320  2, jevois.YUYV.MedPurple)
321  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
322  int(imagePoints[2][0,0] + 0.5), int(imagePoints[2][0,1] + 0.5),
323  2, jevois.YUYV.MedGreen)
324  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
325  int(imagePoints[3][0,0] + 0.5), int(imagePoints[3][0,1] + 0.5),
326  2, jevois.YUYV.MedGrey)
327 
328  # Also draw a parallelepiped:
329  cubePoints = np.array([ (-hw, -hh, 0.0), (hw, -hh, 0.0), (hw, hh, 0.0), (-hw, hh, 0.0),
330  (-hw, -hh, dd), (hw, -hh, dd), (hw, hh, dd), (-hw, hh, dd) ])
331  cu, jac2 = cv2.projectPoints(cubePoints, rvecs[i], tvecs[i], self.camMatrix, self.distCoeffs)
332 
333  # Round all the coordinates and cast to int for drawing:
334  cu = np.rint(cu)
335 
336  # Draw parallelepiped lines:
337  jevois.drawLine(outimg, int(cu[0][0,0]), int(cu[0][0,1]), int(cu[1][0,0]), int(cu[1][0,1]),
338  1, jevois.YUYV.LightGreen)
339  jevois.drawLine(outimg, int(cu[1][0,0]), int(cu[1][0,1]), int(cu[2][0,0]), int(cu[2][0,1]),
340  1, jevois.YUYV.LightGreen)
341  jevois.drawLine(outimg, int(cu[2][0,0]), int(cu[2][0,1]), int(cu[3][0,0]), int(cu[3][0,1]),
342  1, jevois.YUYV.LightGreen)
343  jevois.drawLine(outimg, int(cu[3][0,0]), int(cu[3][0,1]), int(cu[0][0,0]), int(cu[0][0,1]),
344  1, jevois.YUYV.LightGreen)
345  jevois.drawLine(outimg, int(cu[4][0,0]), int(cu[4][0,1]), int(cu[5][0,0]), int(cu[5][0,1]),
346  1, jevois.YUYV.LightGreen)
347  jevois.drawLine(outimg, int(cu[5][0,0]), int(cu[5][0,1]), int(cu[6][0,0]), int(cu[6][0,1]),
348  1, jevois.YUYV.LightGreen)
349  jevois.drawLine(outimg, int(cu[6][0,0]), int(cu[6][0,1]), int(cu[7][0,0]), int(cu[7][0,1]),
350  1, jevois.YUYV.LightGreen)
351  jevois.drawLine(outimg, int(cu[7][0,0]), int(cu[7][0,1]), int(cu[4][0,0]), int(cu[4][0,1]),
352  1, jevois.YUYV.LightGreen)
353  jevois.drawLine(outimg, int(cu[0][0,0]), int(cu[0][0,1]), int(cu[4][0,0]), int(cu[4][0,1]),
354  1, jevois.YUYV.LightGreen)
355  jevois.drawLine(outimg, int(cu[1][0,0]), int(cu[1][0,1]), int(cu[5][0,0]), int(cu[5][0,1]),
356  1, jevois.YUYV.LightGreen)
357  jevois.drawLine(outimg, int(cu[2][0,0]), int(cu[2][0,1]), int(cu[6][0,0]), int(cu[6][0,1]),
358  1, jevois.YUYV.LightGreen)
359  jevois.drawLine(outimg, int(cu[3][0,0]), int(cu[3][0,1]), int(cu[7][0,0]), int(cu[7][0,1]),
360  1, jevois.YUYV.LightGreen)
361 
362  i += 1
363 
364  # ###################################################################################################
365  ## Process function with no USB output
366  def processNoUSB(self, inframe):
367  # Get the next camera image (may block until it is captured) as OpenCV BGR:
368  imgbgr = inframe.getCvBGR()
369  h, w, chans = imgbgr.shape
370 
371  # Start measuring image processing time:
372  self.timer.start()
373 
374  # Get a list of quadrilateral convex hulls for all good objects:
375  hlist = self.detect(imgbgr)
376 
377  # Load camera calibration if needed:
378  if not hasattr(self, 'camMatrix'): self.loadCameraCalibration(w, h)
379 
380  # Map to 6D (inverse perspective):
381  (rvecs, tvecs) = self.estimatePose(hlist)
382 
383  # Send all serial messages:
384  self.sendAllSerial(w, h, hlist, rvecs, tvecs)
385 
386  # Log frames/s info (will go to serlog serial port, default is None):
387  self.timer.stop()
388 
389  # ###################################################################################################
390  ## Process function with USB output
391  def process(self, inframe, outframe):
392  # Get the next camera image (may block until it is captured). To avoid wasting much time assembling a composite
393  # output image with multiple panels by concatenating numpy arrays, in this module we use raw YUYV images and
394  # fast paste and draw operations provided by JeVois on those images:
395  inimg = inframe.get()
396 
397  # Start measuring image processing time:
398  self.timer.start()
399 
400  # Convert input image to BGR24:
401  imgbgr = jevois.convertToCvBGR(inimg)
402  h, w, chans = imgbgr.shape
403 
404  # Get pre-allocated but blank output image which we will send over USB:
405  outimg = outframe.get()
406  outimg.require("output", w * 2, h + 12, jevois.V4L2_PIX_FMT_YUYV)
407  jevois.paste(inimg, outimg, 0, 0)
408  jevois.drawFilledRect(outimg, 0, h, outimg.width, outimg.height-h, jevois.YUYV.Black)
409 
410  # Let camera know we are done using the input image:
411  inframe.done()
412 
413  # Get a list of quadrilateral convex hulls for all good objects:
414  hlist = self.detect(imgbgr, outimg)
415 
416  # Load camera calibration if needed:
417  if not hasattr(self, 'camMatrix'): self.loadCameraCalibration(w, h)
418 
419  # Map to 6D (inverse perspective):
420  (rvecs, tvecs) = self.estimatePose(hlist)
421 
422  # Send all serial messages:
423  self.sendAllSerial(w, h, hlist, rvecs, tvecs)
424 
425  # Draw all detections in 3D:
426  self.drawDetections(outimg, hlist, rvecs, tvecs)
427 
428  # Write frames/s info from our timer into the edge map (NOTE: does not account for output conversion time):
429  fps = self.timer.stop()
430  jevois.writeText(outimg, fps, 3, h-10, jevois.YUYV.White, jevois.Font.Font6x10)
431 
432  # We are done with the output, ready to send it to host over USB:
433  outframe.send()
434 
def detect(self, imgbgr, outimg=None)
Detect objects within our HSV range.
Definition: FirstPython.py:120
def estimatePose(self, hlist)
Estimate 6D pose of each of the quadrilateral objects in hlist:
Definition: FirstPython.py:251
Simple example of FIRST Robotics image processing pipeline using OpenCV in Python on JeVois...
Definition: FirstPython.py:75
def process(self, inframe, outframe)
Process function with USB output.
Definition: FirstPython.py:391
def __init__(self)
Constructor.
Definition: FirstPython.py:78
def sendAllSerial(self, w, h, hlist, rvecs, tvecs)
Send serial messages, one per object.
Definition: FirstPython.py:275
def processNoUSB(self, inframe)
Process function with no USB output.
Definition: FirstPython.py:366
def drawDetections(self, outimg, hlist, rvecs=None, tvecs=None)
Draw all detected objects in 3D.
Definition: FirstPython.py:299
def loadCameraCalibration(self, w, h)
Load camera calibration from JeVois share directory.
Definition: FirstPython.py:108