JeVoisBase  1.6
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
PythonObject6D.py
Go to the documentation of this file.
1 ######################################################################################################################
2 #
3 # JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2018 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 object detection using ORB keypoints followed by 6D pose estimation in Python
23 #
24 # This module implements an object detector using ORB keypoints using OpenCV in Python. Its main goal is to also
25 # demonstrate full 6D pose recovery of the detected object, in Python, as well as locating in 3D a sub-element of the
26 # detected object (here, a window within a larger textured wall). See \jvmod{ObjectDetect} for more info about object
27 # detection using keypoints. This module is available with \jvversion{1.6.3} and later.
28 #
29 # The algorithm consists of 5 phases:
30 # - detect keypoint locations, typically corners or other distinctive texture elements or markings;
31 # - compute keypoint descriptors, which are summary representations of the image neighborhood around each keypoint;
32 # - match descriptors from current image to descriptors previously extracted from training images;
33 # - if enough matches are found between the current image and a given training image, and they are of good enough
34 # quality, compute the homography (geometric transformation) between keypoint locations in that training image and
35 # locations of the matching keypoints in the current image. If it is well conditioned (i.e., a 3D viewpoint change
36 # could well explain how the keypoints moved between the training and current images), declare that a match was
37 # found, and draw a pink rectangle around the detected whole object.
38 # - finally perform 6D pose estimation (3D translation + 3D rotation), here for a window located at a specific position
39 # within the whole object, given the known physical sizes of both the whole object and the window within. A green
40 # parallelepiped is drawn at that window's location, sinking into the whole object (as it is representing a tunnel
41 # or port into the object).
42 #
43 # For more information about ORB keypoint detection and matching in OpenCV, see, e.g.,
44 # https://docs.opencv.org/3.4.0/d1/d89/tutorial_py_orb.html
45 #
46 # This module is provided for inspiration. It has no pretension of actually solving the FIRST Robotics Power Up (sm)
47 # vision problem in a complete and reliable way. It is released in the hope that FRC teams will try it out and get
48 # inspired to develop something much better for their own robot.
49 #
50 # Note how, contrary to \jvmod{FirstVision}, \jvmod{DemoArUco}, etc, the green parallelepiped is drawn going into the
51 # object instead of sticking out of it, as it is depicting a tunnel at the window location.
52 #
53 # Using this module
54 # -----------------
55 #
56 # This module is for now specific to the "exchange" of the FIRST Robotics 2018 Power Up (sm) challenge. See
57 # https://www.firstinspires.org/resource-library/frc/competition-manual-qa-system
58 #
59 # The exchange is a large textured structure with a window at the bottom into which robots should deliver foam cubes.
60 #
61 # A reference picture of the whole exchange (taken from the official rules) is in
62 # <b>JEVOIS:/modules/JeVois/PythonObject6D/images/reference.png</b> on your JeVois microSD card. It will be processed
63 # when the module starts. No additional training procedure is needed.
64 #
65 # If you change the reference image, you should also edit:
66 # - values of \p self.owm and \p self.ohm to the width ahd height, in meters, of the actual physical object in your
67 # picture. Square pixels are assumed, so make sure the aspect ratio of your PNG image matches the aspect ratio in
68 # meters given by variables \p self.owm and \p self.ohm in the code.
69 # - values of \p self.wintop, \p self.winleft, \p self.winw, \p self.winh to the location of the top-left corner, in
70 # meters and relative to the top-left corner of the whole reference object, of a window of interest (the tunnel into
71 # which the cubes should be delivered), and width and height, in meters, of the window.
72 #
73 # \b TODO: Add support for multiple images and online training as in \jvmod{ObjectDetect}
74 #
75 # Things to tinker with
76 # ---------------------
77 #
78 # There are a number of limitations and caveats to this module:
79 #
80 # - It does not use color, the input image is converted to grayscale before processing. One could use a different
81 # approach to object detection that would make use of color.
82 # - Results are often quite noisy. Maybe using another detector, like SIFT which provides subpixel accuracy, and better
83 # pruning of false matches (e.g., David Lowe's ratio of the best to second-best match scores) would help.
84 # - This algorithm is slow in this single-threaded Python example, and frame rate depends on image complexity (it gets
85 # slower when more keypoints are detected). One should explore parallelization, as was done in C++ for the
86 # \jvmod{ObjectDetect} module. One could also alternate between full detection using this algorithm once in a while,
87 # and much faster tracking of previous detections at a higher framerate (e.g., using the very robust TLD tracker
88 # (track-learn-detect), also supported in OpenCV).
89 # - If you want to detect smaller objects or pieces of objects, and you do not need 6D pose, you may want to use modules
90 # \jvmod{ObjectDetect} or \jvmod{SaliencySURF} as done, for example, by JeVois user Bill Kendall at
91 # https://www.youtube.com/watch?v=8wYhOnsNZcc
92 #
93 #
94 # @author Laurent Itti
95 #
96 # @displayname Python Object 6D
97 # @videomapping YUYV 320 262 15.0 YUYV 320 240 15.0 JeVois PythonObject6D
98 # @email itti\@usc.edu
99 # @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
100 # @copyright Copyright (C) 2018 by Laurent Itti, iLab and the University of Southern California
101 # @mainurl http://jevois.org
102 # @supporturl http://jevois.org/doc
103 # @otherurl http://iLab.usc.edu
104 # @license GPL v3
105 # @distribution Unrestricted
106 # @restrictions None
107 # @ingroup modules
109  # ###################################################################################################
110  ## Constructor
111  def __init__(self):
112  # Full file name of the training image:
113  self.fname = "/jevois/modules/JeVois/PythonObject6D/images/reference.png"
114 
115  # Measure your object (in meters) and set its size here:
116  self.owm = 48 * 0.0254 # width in meters (specs call for 48 inches)
117  self.ohm = 77.75 * 0.0254 # height in meters (specs call for 77.75 inches)
118 
119  # Window within the object for which we will compute 3D pose: top-left corner in meters relative to the top-left
120  # corner of the full reference object, and window width and height in meters:
121  self.wintop = (77.75 - 18) * 0.0254 # top of exchange window is 18in from ground
122  self.winleft = 6.88 * 0.0254 # left of exchange window is 6.88in from left edge
123  self.winw = (12 + 9) * 0.0254 # exchange window is 1ft 9in wide
124  self.winh = (12 + 4.25) * 0.0254 # exchange window is 1ft 4-1/4in tall
125 
126  # Other parameters:
127  self.distth = 50.0 # Descriptor distance threshold (lower is stricter for exact matches)
128 
129  # Instantiate a JeVois Timer to measure our processing framerate:
130  self.timer = jevois.Timer("PythonObject6D", 100, jevois.LOG_INFO)
131 
132  # ###################################################################################################
133  ## Load camera calibration from JeVois share directory
134  def loadCameraCalibration(self, w, h):
135  cpf = "/jevois/share/camera/calibration{}x{}.yaml".format(w, h)
136  fs = cv2.FileStorage(cpf, cv2.FILE_STORAGE_READ)
137  if (fs.isOpened()):
138  self.camMatrix = fs.getNode("camera_matrix").mat()
139  self.distCoeffs = fs.getNode("distortion_coefficients").mat()
140  jevois.LINFO("Loaded camera calibration from {}".format(cpf))
141  else:
142  jevois.LFATAL("Failed to read camera parameters from file [{}]".format(cpf))
143 
144  # ###################################################################################################
145  ## Detect objects using keypoints
146  def detect(self, imggray, outimg = None):
147  h, w = imggray.shape
148  hlist = []
149 
150  # Create a keypoint detector if needed:
151  if not hasattr(self, 'detector'):
152  self.detector = cv2.ORB_create()
153 
154  # Load training image and detect keypoints on it if needed:
155  if not hasattr(self, 'refkp'):
156  refimg = cv2.imread(self.fname, 0)
157  self.refkp, self.refdes = self.detector.detectAndCompute(refimg, None)
158 
159  # Also store corners of reference image and of window for homography mapping:
160  refh, refw = refimg.shape
161  self.refcorners = np.float32([ [ 0.0, 0.0 ], [ 0.0, refh ], [refw, refh ], [ refw, 0.0 ] ]).reshape(-1,1,2)
162  self.wincorners = np.float32([
163  [ self.winleft * refw / self.owm, self.wintop * refh / self.ohm ],
164  [ self.winleft * refw / self.owm, (self.wintop + self.winh) * refh / self.ohm ],
165  [ (self.winleft + self.winw) * refw / self.owm, (self.wintop + self.winh) * refh / self.ohm ],
166  [ (self.winleft + self.winw) * refw / self.owm, self.wintop * refh / self.ohm ] ]).reshape(-1,1,2)
167  jevois.LINFO("Extracted {} keypoints and descriptors from {}".format(len(self.refkp), self.fname))
168 
169  # Compute keypoints and descriptors:
170  kp, des = self.detector.detectAndCompute(imggray, None)
171  str = "{} keypoints".format(len(kp))
172 
173  # Create a matcher if needed:
174  if not hasattr(self, 'matcher'):
175  self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck = True)
176 
177  # Compute matches between reference image and camera image, then sort them by distance:
178  matches = self.matcher.match(des, self.refdes)
179  matches = sorted(matches, key = lambda x:x.distance)
180  str += ", {} matches".format(len(matches))
181 
182  # Keep only good matches:
183  lastidx = 0
184  for m in matches:
185  if m.distance < self.distth: lastidx += 1
186  else: break
187  matches = matches[0:lastidx]
188  str += ", {} good".format(len(matches))
189 
190  # If we have enough matches, compute homography:
191  corners = []
192  wincorners = []
193  if len(matches) >= 10:
194  obj = []
195  scene = []
196 
197  # Localize the object (see JeVois C++ class ObjectMatcher for details):
198  for m in matches:
199  obj.append(self.refkp[m.trainIdx].pt)
200  scene.append(kp[m.queryIdx].pt)
201 
202  # compute the homography
203  hmg, mask = cv2.findHomography(np.array(obj), np.array(scene), cv2.RANSAC, 5.0)
204 
205  # Check homography conditioning using SVD:
206  u, s, v = np.linalg.svd(hmg, full_matrices = False)
207 
208  # We need the smallest eigenvalue to not be too small, and the ratio of largest to smallest eigenvalue to be
209  # quite large for our homography to be declared good here. Note that linalg.svd returns the eigenvalues in
210  # descending order already:
211  if s[-1] > 0.001 and s[0] / s[-1] > 100:
212  # Project the reference image corners to the camera image:
213  corners = cv2.perspectiveTransform(self.refcorners, hmg)
214  wincorners = cv2.perspectiveTransform(self.wincorners, hmg)
215 
216  # Display any results requested by the users:
217  if outimg is not None and outimg.valid():
218  if len(corners) == 4:
219  jevois.drawLine(outimg, int(corners[0][0,0] + 0.5), int(corners[0][0,1] + 0.5),
220  int(corners[1][0,0] + 0.5), int(corners[1][0,1] + 0.5),
221  2, jevois.YUYV.LightPink)
222  jevois.drawLine(outimg, int(corners[1][0,0] + 0.5), int(corners[1][0,1] + 0.5),
223  int(corners[2][0,0] + 0.5), int(corners[2][0,1] + 0.5),
224  2, jevois.YUYV.LightPink)
225  jevois.drawLine(outimg, int(corners[2][0,0] + 0.5), int(corners[2][0,1] + 0.5),
226  int(corners[3][0,0] + 0.5), int(corners[3][0,1] + 0.5),
227  2, jevois.YUYV.LightPink)
228  jevois.drawLine(outimg, int(corners[3][0,0] + 0.5), int(corners[3][0,1] + 0.5),
229  int(corners[0][0,0] + 0.5), int(corners[0][0,1] + 0.5),
230  2, jevois.YUYV.LightPink)
231  jevois.writeText(outimg, str, 3, h+4, jevois.YUYV.White, jevois.Font.Font6x10)
232 
233  # Return window corners if we did indeed detect the object:
234  hlist = []
235  if len(wincorners) == 4: hlist.append(wincorners)
236 
237  return hlist
238 
239  # ###################################################################################################
240  ## Estimate 6D pose of each of the quadrilateral objects in hlist:
241  def estimatePose(self, hlist):
242  rvecs = []
243  tvecs = []
244 
245  # set coordinate system in the middle of the window, with Z pointing out
246  objPoints = np.array([ ( -self.winw * 0.5, -self.winh * 0.5, 0 ),
247  ( -self.winw * 0.5, self.winh * 0.5, 0 ),
248  ( self.winw * 0.5, self.winh * 0.5, 0 ),
249  ( self.winw * 0.5, -self.winh * 0.5, 0 ) ])
250 
251  for detection in hlist:
252  det = np.array(detection, dtype=np.float).reshape(4,2,1)
253  (ok, rv, tv) = cv2.solvePnP(objPoints, det, self.camMatrix, self.distCoeffs)
254  if ok:
255  rvecs.append(rv)
256  tvecs.append(tv)
257  else:
258  rvecs.append(np.array([ (0.0), (0.0), (0.0) ]))
259  tvecs.append(np.array([ (0.0), (0.0), (0.0) ]))
260 
261  return (rvecs, tvecs)
262 
263  # ###################################################################################################
264  ## Send serial messages, one per object
265  def sendAllSerial(self, w, h, hlist, rvecs, tvecs):
266  idx = 0
267  for c in hlist:
268  # Compute quaternion: FIXME need to check!
269  tv = tvecs[idx]
270  axis = rvecs[idx]
271  angle = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]) ** 0.5
272 
273  # This code lifted from pyquaternion from_axis_angle:
274  mag_sq = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]
275  if (abs(1.0 - mag_sq) > 1e-12): axis = axis / (mag_sq ** 0.5)
276  theta = angle / 2.0
277  r = math.cos(theta)
278  i = axis * math.sin(theta)
279  q = (r, i[0], i[1], i[2])
280 
281  jevois.sendSerial("D3 {} {} {} {} {} {} {} {} {} {} OBJ6D".
282  format(np.asscalar(tv[0]), np.asscalar(tv[1]), np.asscalar(tv[2]), # position
283  self.owm, self.ohm, 1.0, # size
284  r, np.asscalar(i[0]), np.asscalar(i[1]), np.asscalar(i[2]))) # pose
285  idx += 1
286 
287  # ###################################################################################################
288  ## Draw all detected objects in 3D
289  def drawDetections(self, outimg, hlist, rvecs = None, tvecs = None):
290  # Show trihedron and parallelepiped centered on object:
291  hw = self.winw * 0.5
292  hh = self.winh * 0.5
293  dd = -max(hw, hh)
294  i = 0
295  empty = np.array([ (0.0), (0.0), (0.0) ])
296 
297  # NOTE: this code similar to FirstVision, but in the present module we only have at most one object in the list
298  # (the window, if detected):
299  for obj in hlist:
300  # skip those for which solvePnP failed:
301  if np.array_equal(rvecs[i], empty):
302  i += 1
303  continue
304  # This could throw some overflow errors as we convert the coordinates to int, if the projection gets
305  # singular because of noisy detection:
306  try:
307  # Project axis points:
308  axisPoints = np.array([ (0.0, 0.0, 0.0), (hw, 0.0, 0.0), (0.0, hh, 0.0), (0.0, 0.0, dd) ])
309  imagePoints, jac = cv2.projectPoints(axisPoints, rvecs[i], tvecs[i], self.camMatrix, self.distCoeffs)
310 
311  # Draw axis lines:
312  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
313  int(imagePoints[1][0,0] + 0.5), int(imagePoints[1][0,1] + 0.5),
314  2, jevois.YUYV.MedPurple)
315  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
316  int(imagePoints[2][0,0] + 0.5), int(imagePoints[2][0,1] + 0.5),
317  2, jevois.YUYV.MedGreen)
318  jevois.drawLine(outimg, int(imagePoints[0][0,0] + 0.5), int(imagePoints[0][0,1] + 0.5),
319  int(imagePoints[3][0,0] + 0.5), int(imagePoints[3][0,1] + 0.5),
320  2, jevois.YUYV.MedGrey)
321 
322  # Also draw a parallelepiped: NOTE: contrary to FirstVision, here we draw it going into the object, as
323  # opposed to sticking out of it (we just negate Z for that):
324  cubePoints = np.array([ (-hw, -hh, 0.0), (hw, -hh, 0.0), (hw, hh, 0.0), (-hw, hh, 0.0),
325  (-hw, -hh, -dd), (hw, -hh, -dd), (hw, hh, -dd), (-hw, hh, -dd) ])
326  cu, jac2 = cv2.projectPoints(cubePoints, rvecs[i], tvecs[i], self.camMatrix, self.distCoeffs)
327 
328  # Round all the coordinates and cast to int for drawing:
329  cu = np.rint(cu)
330 
331  # Draw parallelepiped lines:
332  jevois.drawLine(outimg, int(cu[0][0,0]), int(cu[0][0,1]), int(cu[1][0,0]), int(cu[1][0,1]),
333  1, jevois.YUYV.LightGreen)
334  jevois.drawLine(outimg, int(cu[1][0,0]), int(cu[1][0,1]), int(cu[2][0,0]), int(cu[2][0,1]),
335  1, jevois.YUYV.LightGreen)
336  jevois.drawLine(outimg, int(cu[2][0,0]), int(cu[2][0,1]), int(cu[3][0,0]), int(cu[3][0,1]),
337  1, jevois.YUYV.LightGreen)
338  jevois.drawLine(outimg, int(cu[3][0,0]), int(cu[3][0,1]), int(cu[0][0,0]), int(cu[0][0,1]),
339  1, jevois.YUYV.LightGreen)
340  jevois.drawLine(outimg, int(cu[4][0,0]), int(cu[4][0,1]), int(cu[5][0,0]), int(cu[5][0,1]),
341  1, jevois.YUYV.LightGreen)
342  jevois.drawLine(outimg, int(cu[5][0,0]), int(cu[5][0,1]), int(cu[6][0,0]), int(cu[6][0,1]),
343  1, jevois.YUYV.LightGreen)
344  jevois.drawLine(outimg, int(cu[6][0,0]), int(cu[6][0,1]), int(cu[7][0,0]), int(cu[7][0,1]),
345  1, jevois.YUYV.LightGreen)
346  jevois.drawLine(outimg, int(cu[7][0,0]), int(cu[7][0,1]), int(cu[4][0,0]), int(cu[4][0,1]),
347  1, jevois.YUYV.LightGreen)
348  jevois.drawLine(outimg, int(cu[0][0,0]), int(cu[0][0,1]), int(cu[4][0,0]), int(cu[4][0,1]),
349  1, jevois.YUYV.LightGreen)
350  jevois.drawLine(outimg, int(cu[1][0,0]), int(cu[1][0,1]), int(cu[5][0,0]), int(cu[5][0,1]),
351  1, jevois.YUYV.LightGreen)
352  jevois.drawLine(outimg, int(cu[2][0,0]), int(cu[2][0,1]), int(cu[6][0,0]), int(cu[6][0,1]),
353  1, jevois.YUYV.LightGreen)
354  jevois.drawLine(outimg, int(cu[3][0,0]), int(cu[3][0,1]), int(cu[7][0,0]), int(cu[7][0,1]),
355  1, jevois.YUYV.LightGreen)
356  except:
357  pass
358 
359  i += 1
360 
361  # ###################################################################################################
362  ## Process function with no USB output
363  def processNoUSB(self, inframe):
364  # Get the next camera image (may block until it is captured) as OpenCV GRAY:
365  imggray = inframe.getCvGray()
366  h, w = imggray.shape
367 
368  # Start measuring image processing time:
369  self.timer.start()
370 
371  # Get a list of quadrilateral convex hulls for all good objects:
372  hlist = self.detect(imggray)
373 
374  # Load camera calibration if needed:
375  if not hasattr(self, 'camMatrix'): self.loadCameraCalibration(w, h)
376 
377  # Map to 6D (inverse perspective):
378  (rvecs, tvecs) = self.estimatePose(hlist)
379 
380  # Send all serial messages:
381  self.sendAllSerial(w, h, hlist, rvecs, tvecs)
382 
383  # Log frames/s info (will go to serlog serial port, default is None):
384  self.timer.stop()
385 
386  # ###################################################################################################
387  ## Process function with USB output
388  def process(self, inframe, outframe):
389  # Get the next camera image (may block until it is captured). To avoid wasting much time assembling a composite
390  # output image with multiple panels by concatenating numpy arrays, in this module we use raw YUYV images and
391  # fast paste and draw operations provided by JeVois on those images:
392  inimg = inframe.get()
393 
394  # Start measuring image processing time:
395  self.timer.start()
396 
397  # Convert input image to GRAY:
398  imggray = jevois.convertToCvGray(inimg)
399  h, w = imggray.shape
400 
401  # Get pre-allocated but blank output image which we will send over USB:
402  outimg = outframe.get()
403  outimg.require("output", w, h + 22, jevois.V4L2_PIX_FMT_YUYV)
404  jevois.paste(inimg, outimg, 0, 0)
405  jevois.drawFilledRect(outimg, 0, h, outimg.width, outimg.height-h, jevois.YUYV.Black)
406 
407  # Let camera know we are done using the input image:
408  inframe.done()
409 
410  # Get a list of quadrilateral convex hulls for all good objects:
411  hlist = self.detect(imggray, outimg)
412 
413  # Load camera calibration if needed:
414  if not hasattr(self, 'camMatrix'): self.loadCameraCalibration(w, h)
415 
416  # Map to 6D (inverse perspective):
417  (rvecs, tvecs) = self.estimatePose(hlist)
418 
419  # Send all serial messages:
420  self.sendAllSerial(w, h, hlist, rvecs, tvecs)
421 
422  # Draw all detections in 3D:
423  self.drawDetections(outimg, hlist, rvecs, tvecs)
424 
425  # Write frames/s info from our timer into the edge map (NOTE: does not account for output conversion time):
426  fps = self.timer.stop()
427  jevois.writeText(outimg, fps, 3, h-10, jevois.YUYV.White, jevois.Font.Font6x10)
428 
429  # We are done with the output, ready to send it to host over USB:
430  outframe.send()
431 
def __init__(self)
Constructor.
def estimatePose(self, hlist)
Estimate 6D pose of each of the quadrilateral objects in hlist:
def process(self, inframe, outframe)
Process function with USB output.
def sendAllSerial(self, w, h, hlist, rvecs, tvecs)
Send serial messages, one per object.
def processNoUSB(self, inframe)
Process function with no USB output.
def drawDetections(self, outimg, hlist, rvecs=None, tvecs=None)
Draw all detected objects in 3D.
def loadCameraCalibration(self, w, h)
Load camera calibration from JeVois share directory.
Simple example of object detection using ORB keypoints followed by 6D pose estimation in Python...
def detect(self, imggray, outimg=None)
Detect objects using keypoints.