JeVoisBase  1.0
JeVois Smart Embedded Machine Vision Toolkit Base Modules
DemoArUco.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 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, BA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
18 #include <jevois/Core/Module.H>
19 #include <jevois/Debug/Timer.H>
23 #include <linux/videodev2.h> // for v4l2 pixel types
24 #include <opencv2/core/core.hpp>
25 #include <opencv2/imgproc/imgproc.hpp>
26 #include <opencv2/calib3d.hpp> // for projectPoints()
27 #include <sstream>
28 
29 //! Parameter \relates DemoArUco
30 JEVOIS_DECLARE_PARAMETER(showpose, bool, "Show pose vectors, requires a valid camera calibration matrix",
31  false, aruco::ParamCateg);
32 
33 //! Parameter \relates DemoArUco
34 JEVOIS_DECLARE_PARAMETER(markerlen, float, "Marker side length (meters), used only for pose estimation",
35  0.1F, aruco::ParamCateg);
36 
37 //! Parameter \relates DemoArUco
38 JEVOIS_DECLARE_PARAMETER(serstyle, int, "Serial output style for strings issued for each detected ArUco marker: "
39  "0: [ArUco ID] where ID is the decoded ArUco ID; "
40  "1: [ArUco ID X,Y] sends ID and standardized coordinates of the marker center;"
41  "2: [ArUco ID X1,Y1 X2,Y2 X3,Y3 X4,Y4] sends ID and standardized coords of the 4 corners.",
42  0, jevois::Range<int>(0, 2), aruco::ParamCateg);
43 
44 //! Simple demo of ArUco augmented reality markers detection and decoding
45 /*! Detect and decode patterns known as ArUco markers, which are small 2D barcodes often used in augmented
46  reality and robotics.
47 
48  ArUco markers are small 2D barcodes. Each ArUco marker corresponds to a number, encoded into a small grid of black
49  and white pixels. The ArUco decoding algorithm is capable of locating, decoding, and of estimating the pose
50  (location and orientation in space) of any ArUco markers in the camera's field of view.
51 
52  ArUcos are very useful as tags for many robotics and augmented reality applications. For example, one may place an
53  ArUco next to a robot's charging station, an elevator button, or an object that a robot should manipulate.
54 
55  For more information about ArUco, see https://www.uco.es/investiga/grupos/ava/node/26
56 
57  The implementation of ArUco used by JeVois is the one of OpenCV-Contrib, documented here:
58  http://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html
59 
60  ArUco markers can be created with several standard dictionaries. Different disctionaries give rise to different
61  numbers of pixels in the markers, and to different numbers of possible symbols that can be created using the
62  dictionary. The default dictionary used by JeVois is 4x4 with 50 symbols. Other dictionaries are also supported by
63  setting the appropriate parameter over serial port or in a config file, up to 7x7 with 1000 symbols.
64 
65  Creating and printing markers
66  -----------------------------
67 
68  We have created the 50 markers available in the default dictionary (4x4_50) as PNG images that you can download and
69  print, at http://jevois.org/data/ArUco.zip
70 
71  To make your own, for example, using another dictionary, see the documentation of the ArUco component of
72  JeVoisBase. Some utilities are provided with the component.
73 
74  Serial Messages
75  ---------------
76 
77  This module can send the following messages over serial port (make sure you set the \p serout parameter to
78  designate the serial port to which you want to send these messages, see the documentation for \p serout under
79  the \ref UserCli).
80 
81  - When serstyle is 0:
82  \verbatim
83  ArUco ID
84  \endverbatim
85  where
86  + ID is the decoded ID of the ArUco that was detected
87 
88  - When serstyle is 1:
89  \verbatim
90  ArUco ID X,Y
91  \endverbatim
92  where
93  + ID is the decoded ID of the ArUco that was detected
94  + X,Y are the coordinates of the marker's center
95 
96  - When serstyle is 2:
97  \verbatim
98  ArUco ID X1,Y1 X2,Y2 X3,Y3 X4,Y4
99  \endverbatim
100  where
101  + ID is the decoded ID of the ArUco that was detected
102  + X1,Y1 are the coordinates of the first corner
103  + X2,Y2 are the coordinates of the second corner
104  + X3,Y3 are the coordinates of the third corner
105  + X4,Y4 are the coordinates of the fourth corner
106 
107  The coordinates are normalized so that the serial outputs are independent of camera resolution. x=0, y=0 is the
108  center of the field of view, the left edge of the camera image is at x=-1000, the right edge at x=1000, the top edge
109  at y=-750, and the bottom edge at y=750 (since the camera has a 4:3 aspect ratio). See \ref coordhelpers for more
110  information on standardized coordinates in JeVois.
111 
112  One message is issued for every detected ArUco, on every video frame.
113 
114  Things to try
115  -------------
116 
117  - First, use a video viewer software on a host computer and select one of the video modes with video output over
118  USB. Point your JeVois camera towards one of the screenshots provided with this module, or towards some ArUco
119  markers that you find on the web or that you have printed from the collection above (note: the default dictionary
120  is 4x4_50, see parameter \p dictionary).
121 
122  - Then try it with no video output, as it would be used by a robot. Connect to the command-line interface of your
123  JeVois camera through the serial-over-USB connection (see \ref UserCli; on Linux, you would use <b>sudo screen
124  /dev/ttyACM0 115200</b>) and try:
125  \verbatim
126  setpar serout USB
127  setmapping2 YUYV 320 240 30.0 JeVois DemoArUco
128  streamon
129  \endverbatim
130  and point the camera to some markers; the camera should issue messages about all the markers it identifies.
131 
132  Showing the pose vectors
133  ------------------------
134 
135  The OpenCV ArUco module can also compute normals to each marker, which allows you to recover the marker's
136  orientation and distance. The requires that the camera be calibrated, see the documentation of the ArUco component
137  in JeVois. A generic calibration that is for a JeVois camera with standard lens is included in file \b
138  calibration.yaml in the module's directory (on the MicroSD, this is
139  <b>JEVOIS:/modules/JeVois/DemoArUco/calibration.yaml</b>).
140 
141  FIXME this crashes since the update to opencv 3.2, need to figure out what changed since 3.1.
142 
143 
144  @author Laurent Itti
145 
146  @displayname Demo ArUco
147  @videomapping NONE 0 0 0 YUYV 320 240 30.0 JeVois DemoArUco
148  @videomapping YUYV 320 260 30.0 YUYV 320 240 30.0 JeVois DemoArUco
149  @videomapping YUYV 640 500 20.0 YUYV 640 480 20.0 JeVois DemoArUco
150  @email itti\@usc.edu
151  @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
152  @copyright Copyright (C) 2016 by Laurent Itti, iLab and the University of Southern California
153  @mainurl http://jevois.org
154  @supporturl http://jevois.org/doc
155  @otherurl http://iLab.usc.edu
156  @license GPL v3
157  @distribution Unrestricted
158  @restrictions None
159  \ingroup modules */
160 class DemoArUco : public jevois::Module,
161  public jevois::Parameter<showpose, markerlen, serstyle>
162 {
163  public:
164  // ####################################################################################################
165  //! Constructor
166  // ####################################################################################################
167  DemoArUco(std::string const & instance) : jevois::Module(instance)
168  {
169  itsArUco = addSubComponent<ArUco>("aruco");
170  itsArUco->camparams::set("calibration.yaml"); // use camera calibration parameters in module path
171  }
172 
173  // ####################################################################################################
174  //! Virtual destructor for safe inheritance
175  // ####################################################################################################
176  virtual ~DemoArUco()
177  { }
178 
179  // ####################################################################################################
180  //! Processing function, no video output
181  // ####################################################################################################
182  virtual void process(jevois::InputFrame && inframe) override
183  {
184  // Wait for next available camera image, any format and resolution ok here:
185  jevois::RawImage const inimg = inframe.get(); unsigned int const w = inimg.width, h = inimg.height;
186 
187  // Convert the image to grayscale and process:
188  cv::Mat cvimg = jevois::rawimage::convertToCvGray(inimg);
189  std::vector<int> ids;
190  std::vector<std::vector<cv::Point2f> > corners;
191  itsArUco->detectMarkers(cvimg, ids, corners);
192 
193  // Let camera know we are done processing the input image:
194  inframe.done();
195 
196  // Send serial output:
197  sendAllSerial(ids, corners, w, h);
198  }
199 
200  // ####################################################################################################
201  //! Send the serial messages
202  // ####################################################################################################
203  void sendAllSerial(std::vector<int> ids, std::vector<std::vector<cv::Point2f> > corners,
204  unsigned int w, unsigned int h)
205  {
206  int const ss = serstyle::get(); int const nMarkers = int(corners.size());
207  for (int i = 0; i < nMarkers; ++i)
208  {
209  std::vector<cv::Point2f> const & currentMarker = corners[i];
210  std::ostringstream serout; serout << "ArUco " << ids[i] << std::fixed << std::setprecision(1);
211  switch (ss)
212  {
213  case 0: // Send only the ID
214  break;
215 
216  case 1: // Send the coordinates of the center
217  {
218  float cx = 0.0F, cy = 0.0F;
219  for (cv::Point2f cm : currentMarker)
220  {
221  // Normalize the coordinates:
222  float x = cm.x, y = cm.y; jevois::coords::imgToStd(x, y, w, h);
223  cx += x; cy += y;
224  }
225  serout << ' ' << int(0.25F * cx + 0.5F) << ',' << int(0.25F * cy + 0.5F);
226  }
227  break;
228 
229  default: // Send the 4 corner coordinates:
230  for (cv::Point2f cm : currentMarker)
231  {
232  // Normalize the coordinates:
233  float x = cm.x, y = cm.y; jevois::coords::imgToStd(x, y, w, h, 1.0F);
234  serout << ' ' << int(x) << ',' << int(y);
235  }
236  }
237 
238  // Send to serial:
239  sendSerial(serout.str());
240  }
241  }
242 
243  // ####################################################################################################
244  //! Processing function with video output to USB
245  // ####################################################################################################
246  virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
247  {
248  static jevois::Timer timer("processing");
249 
250  // Wait for next available camera image:
251  jevois::RawImage const inimg = inframe.get();
252 
253  timer.start();
254 
255  // We only handle one specific pixel format, any size in this demo:
256  unsigned int const w = inimg.width, h = inimg.height;
257  inimg.require("input", w, h, V4L2_PIX_FMT_YUYV);
258 
259  // While we process it, start a thread to wait for out frame and paste the input into it:
260  jevois::RawImage outimg;
261  auto paste_fut = std::async(std::launch::async, [&]() {
262  outimg = outframe.get();
263  outimg.require("output", w, h + 20, inimg.fmt);
264  jevois::rawimage::paste(inimg, outimg, 0, 0);
265  jevois::rawimage::writeText(outimg, "JeVois ArUco Demo", 3, 3, jevois::yuyv::White);
266  jevois::rawimage::drawFilledRect(outimg, 0, h, w, outimg.height-h, jevois::yuyv::Black);
267  });
268 
269  // Convert the image to grayscale and process:
270  cv::Mat cvimg = jevois::rawimage::convertToCvGray(inimg);
271  std::vector<int> ids;
272  std::vector<std::vector<cv::Point2f> > corners;
273  std::vector<cv::Vec3d> rvecs, tvecs;
274  itsArUco->detectMarkers(cvimg, ids, corners);
275 
276  if (showpose::get() && ids.empty() == false)
277  itsArUco->estimatePoseSingleMarkers(corners, markerlen::get(), rvecs, tvecs);
278 
279  // Wait for paste to finish up:
280  paste_fut.get();
281 
282  // Let camera know we are done processing the input image:
283  inframe.done();
284 
285  // Show all the results:
286 
287  // This code is like drawDetectedMarkers() in cv::aruco, but for YUYV output image:
288  int nMarkers = int(corners.size());
289  for (int i = 0; i < nMarkers; ++i)
290  {
291  std::vector<cv::Point2f> const & currentMarker = corners[i];
292 
293  // draw marker sides and prepare serial out string:
294  for (int j = 0; j < 4; ++j)
295  {
296  cv::Point2f const & p0 = currentMarker[j];
297  cv::Point2f const & p1 = currentMarker[ (j+1) % 4 ];
298  jevois::rawimage::drawLine(outimg, int(p0.x + 0.4999F), int(p0.y + 0.4999F),
299  int(p1.x + 0.4999F), int(p1.y + 0.3999F), 1, jevois::yuyv::LightGreen);
300  }
301 
302  // draw first corner mark
303  jevois::rawimage::drawDisk(outimg, int(currentMarker[0].x + 0.4999F), int(currentMarker[0].y + 0.4999F),
304  3, jevois::yuyv::LightGreen);
305 
306  // draw ID
307  if (ids.empty() == false)
308  {
309  cv::Point2f cent(0.0F, 0.0F); for (int p = 0; p < 4; ++p) cent += currentMarker[p] * 0.25F;
310  jevois::rawimage::writeText(outimg, std::string("id=") + std::to_string(ids[i]),
311  int(cent.x + 0.4999F), int(cent.y + 0.4999F) - 5, jevois::yuyv::LightPink);
312  }
313  }
314 
315  // Send serial output:
316  sendAllSerial(ids, corners, w, h);
317 
318  // This code is like drawAxis() in cv::aruco, but for YUYV output image:
319  if (showpose::get() && ids.empty() == false)
320  {
321  float const length = markerlen::get() * 0.5F;
322 
323  for (size_t i = 0; i < ids.size(); ++i)
324  {
325  // project axis points
326  std::vector<cv::Point3f> axisPoints;
327  axisPoints.push_back(cv::Point3f(0.0F, 0.0F, 0.0F));
328  axisPoints.push_back(cv::Point3f(length, 0.0F, 0.0F));
329  axisPoints.push_back(cv::Point3f(0.0F, length, 0.0F));
330  axisPoints.push_back(cv::Point3f(0.0F, 0.0F, length));
331 
332  std::vector<cv::Point2f> imagePoints;
333  cv::projectPoints(axisPoints, rvecs[i], tvecs[i], itsArUco->itsCamMatrix,
334  itsArUco->itsDistCoeffs, imagePoints);
335 
336  // draw axis lines
337  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
338  int(imagePoints[1].x + 0.4999F), int(imagePoints[1].y + 0.4999F),
339  2, jevois::yuyv::MedPurple);
340  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
341  int(imagePoints[2].x + 0.4999F), int(imagePoints[2].y + 0.4999F),
342  2, jevois::yuyv::MedGreen);
343  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
344  int(imagePoints[3].x + 0.4999F), int(imagePoints[3].y + 0.4999F),
345  2, jevois::yuyv::MedGrey);
346  }
347  }
348 
349  jevois::rawimage::writeText(outimg, "Detected " + std::to_string(ids.size()) + " ArUco markers.",
350  3, h + 5, jevois::yuyv::White);
351 
352  // Show processing fps:
353  std::string const & fpscpu = timer.stop();
354  jevois::rawimage::writeText(outimg, fpscpu, 3, h - 13, jevois::yuyv::White);
355 
356  // Send the output image with our processing results to the host over USB:
357  outframe.send();
358  }
359 
360  // ####################################################################################################
361  protected:
362  std::shared_ptr<ArUco> itsArUco;
363 };
364 
365 // Allow the module to be loaded as a shared object (.so) file:
friend friend class Module
void imgToStd(float &x, float &y, RawImage const &camimg, float const eps=0.1F)
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
void require(char const *info, unsigned int w, unsigned int h, unsigned int f) const
unsigned int height
virtual void process(jevois::InputFrame &&inframe) override
Processing function, no video output.
Definition: DemoArUco.C:182
JEVOIS_REGISTER_MODULE(DemoArUco)
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
unsigned int fmt
JEVOIS_DECLARE_PARAMETER(camparams, std::string,"Filename of camera parameters, or empty","", ParamCateg)
Parameter.
DemoArUco(std::string const &instance)
Constructor.
Definition: DemoArUco.C:167
Simple demo of ArUco augmented reality markers detection and decoding.
Definition: DemoArUco.C:160
cv::Mat convertToCvGray(RawImage const &src)
virtual void sendSerial(std::string const &str)
void sendAllSerial(std::vector< int > ids, std::vector< std::vector< cv::Point2f > > corners, unsigned int w, unsigned int h)
Send the serial messages.
Definition: DemoArUco.C:203
void drawFilledRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int col)
std::string to_string(T const &val)
std::shared_ptr< ArUco > itsArUco
Definition: DemoArUco.C:362
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
virtual void process(jevois::InputFrame &&inframe, jevois::OutputFrame &&outframe) override
Processing function with video output to USB.
Definition: DemoArUco.C:246
unsigned int width
std::string const & stop()
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
virtual ~DemoArUco()
Virtual destructor for safe inheritance.
Definition: DemoArUco.C:176