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/
70 
71  To make your own, for example, using another dictionary, see the documentation of the ArUco component of JeVoisBase.
72 
73  Serial Messages
74  ---------------
75 
76  This module can send the following messages over serial port (make sure you set the \c serout parameter to
77  designate the serial port to which you want to send these messages, see the documentation for \c serout under
78  \ref UserCli).
79 
80  - When serstyle is 0:
81  \verbatim
82  ArUco ID
83  \endverbatim
84  where
85  + ID is the decoded ID of the ArUco that was detected
86 
87  - When serstyle is 1:
88  \verbatim
89  ArUco ID X,Y
90  \endverbatim
91  where
92  + ID is the decoded ID of the ArUco that was detected
93  + X,Y are the coordinates of the marker's center
94 
95  - When serstyle is 2:
96  \verbatim
97  ArUco ID X1,Y1 X2,Y2 X3,Y3 X4,Y4
98  \endverbatim
99  where
100  + ID is the decoded ID of the ArUco that was detected
101  + X1,Y1 are the coordinates of the first corner
102  + X2,Y2 are the coordinates of the second corner
103  + X3,Y3 are the coordinates of the third corner
104  + X4,Y4 are the coordinates of the fourth corner
105 
106  The coordinates are normalized so that the serial outputs are independent of camera resolution. x=0, y=0 is the
107  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
108  at y=-750, and the bottom edge at y=750 (since the camera has a 4:3 aspect ratio).
109 
110  One message is issued for each detected ArUco.
111 
112  @author Laurent Itti
113 
114  @displayname Demo ArUco
115  @videomapping NONE 0 0 0 YUYV 320 240 30.0 JeVois DemoArUco
116  @videomapping YUYV 320 260 30.0 YUYV 320 240 30.0 JeVois DemoArUco
117  @videomapping YUYV 640 500 20.0 YUYV 640 480 20.0 JeVois DemoArUco
118  @email itti\@usc.edu
119  @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
120  @copyright Copyright (C) 2016 by Laurent Itti, iLab and the University of Southern California
121  @mainurl http://jevois.org
122  @supporturl http://jevois.org/doc
123  @otherurl http://iLab.usc.edu
124  @license GPL v3
125  @distribution Unrestricted
126  @restrictions None
127  \ingroup modules */
128 class DemoArUco : public jevois::Module,
129  public jevois::Parameter<showpose, markerlen, serstyle>
130 {
131  public:
132  // ####################################################################################################
133  //! Constructor
134  // ####################################################################################################
135  DemoArUco(std::string const & instance) : jevois::Module(instance)
136  {
137  itsArUco = addSubComponent<ArUco>("aruco");
138  itsArUco->camparams::set("calibration.yaml"); // use camera calibration parameters in module path
139  }
140 
141  // ####################################################################################################
142  //! Virtual destructor for safe inheritance
143  // ####################################################################################################
144  virtual ~DemoArUco()
145  { }
146 
147  // ####################################################################################################
148  //! Processing function, no video output
149  // ####################################################################################################
150  virtual void process(jevois::InputFrame && inframe) override
151  {
152  // Wait for next available camera image, any format and resolution ok here:
153  jevois::RawImage const inimg = inframe.get(); unsigned int const w = inimg.width, h = inimg.height;
154 
155  // Convert the image to grayscale and process:
156  cv::Mat cvimg = jevois::rawimage::convertToCvGray(inimg);
157  std::vector<int> ids;
158  std::vector<std::vector<cv::Point2f> > corners;
159  itsArUco->detectMarkers(cvimg, ids, corners);
160 
161  // Let camera know we are done processing the input image:
162  inframe.done();
163 
164  // Send serial output:
165  sendAllSerial(ids, corners, w, h);
166  }
167 
168  // ####################################################################################################
169  //! Send the serial messages
170  // ####################################################################################################
171  void sendAllSerial(std::vector<int> ids, std::vector<std::vector<cv::Point2f> > corners,
172  unsigned int w, unsigned int h)
173  {
174  int const ss = serstyle::get(); int const nMarkers = int(corners.size());
175  for (int i = 0; i < nMarkers; ++i)
176  {
177  std::vector<cv::Point2f> const & currentMarker = corners[i];
178  std::ostringstream serout; serout << "ArUco " << ids[i] << std::fixed << std::setprecision(1);
179  switch (ss)
180  {
181  case 0: // Send only the ID
182  break;
183 
184  case 1: // Send the coordinates of the center
185  {
186  float cx = 0.0F, cy = 0.0F;
187  for (cv::Point2f cm : currentMarker)
188  {
189  // Normalize the coordinates:
190  float x = cm.x, y = cm.y; jevois::coords::imgToStd(x, y, w, h);
191  cx += x; cy += y;
192  }
193  serout << ' ' << 0.25F * cx << ',' << 0.25F * cy;
194  }
195  break;
196 
197  default: // Send the 4 corner coordinates:
198  for (cv::Point2f cm : currentMarker)
199  {
200  // Normalize the coordinates:
201  float x = cm.x, y = cm.y; jevois::coords::imgToStd(x, y, w, h);
202  serout << ' ' << x << ',' << y;
203  }
204  }
205 
206  // Send to serial:
207  sendSerial(serout.str());
208  }
209  }
210 
211  // ####################################################################################################
212  //! Processing function with video output to USB
213  // ####################################################################################################
214  virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
215  {
216  static jevois::Timer timer("processing");
217 
218  // Wait for next available camera image:
219  jevois::RawImage const inimg = inframe.get();
220 
221  timer.start();
222 
223  // We only handle one specific pixel format, any size in this demo:
224  unsigned int const w = inimg.width, h = inimg.height;
225  inimg.require("input", w, h, V4L2_PIX_FMT_YUYV);
226 
227  // While we process it, start a thread to wait for out frame and paste the input into it:
228  jevois::RawImage outimg;
229  auto paste_fut = std::async(std::launch::async, [&]() {
230  outimg = outframe.get();
231  outimg.require("output", w, h + 20, inimg.fmt);
232  jevois::rawimage::paste(inimg, outimg, 0, 0);
233  jevois::rawimage::writeText(outimg, "JeVois ArUco Demo", 3, 3, jevois::yuyv::White);
234  jevois::rawimage::drawFilledRect(outimg, 0, h, w, outimg.height-h, jevois::yuyv::Black);
235  });
236 
237  // Convert the image to grayscale and process:
238  cv::Mat cvimg = jevois::rawimage::convertToCvGray(inimg);
239  std::vector<int> ids;
240  std::vector<std::vector<cv::Point2f> > corners;
241  std::vector<cv::Vec3d> rvecs, tvecs;
242  itsArUco->detectMarkers(cvimg, ids, corners);
243 
244  if (showpose::get() && ids.empty() == false)
245  itsArUco->estimatePoseSingleMarkers(corners, markerlen::get(), rvecs, tvecs);
246 
247  // Wait for paste to finish up:
248  paste_fut.get();
249 
250  // Let camera know we are done processing the input image:
251  inframe.done();
252 
253  // Show all the results:
254 
255  // This code is like drawDetectedMarkers() in cv::aruco, but for YUYV output image:
256  int nMarkers = int(corners.size());
257  for (int i = 0; i < nMarkers; ++i)
258  {
259  std::vector<cv::Point2f> const & currentMarker = corners[i];
260 
261  // draw marker sides and prepare serial out string:
262  for (int j = 0; j < 4; ++j)
263  {
264  cv::Point2f const & p0 = currentMarker[j];
265  cv::Point2f const & p1 = currentMarker[ (j+1) % 4 ];
266  jevois::rawimage::drawLine(outimg, int(p0.x + 0.4999F), int(p0.y + 0.4999F),
267  int(p1.x + 0.4999F), int(p1.y + 0.3999F), 1, jevois::yuyv::LightGreen);
268  }
269 
270  // draw first corner mark
271  jevois::rawimage::drawDisk(outimg, int(currentMarker[0].x + 0.4999F), int(currentMarker[0].y + 0.4999F),
272  3, jevois::yuyv::LightGreen);
273 
274  // draw ID
275  if (ids.empty() == false)
276  {
277  cv::Point2f cent(0.0F, 0.0F); for (int p = 0; p < 4; ++p) cent += currentMarker[p] * 0.25F;
278  jevois::rawimage::writeText(outimg, std::string("id=") + std::to_string(ids[i]),
279  int(cent.x + 0.4999F), int(cent.y + 0.4999F) - 5, jevois::yuyv::LightPink);
280  }
281  }
282 
283  // Send serial output:
284  sendAllSerial(ids, corners, w, h);
285 
286  // This code is like drawAxis() in cv::aruco, but for YUYV output image:
287  if (showpose::get() && ids.empty() == false)
288  {
289  float const length = markerlen::get() * 0.5F;
290 
291  for (size_t i = 0; i < ids.size(); ++i)
292  {
293  // project axis points
294  std::vector<cv::Point3f> axisPoints;
295  axisPoints.push_back(cv::Point3f(0.0F, 0.0F, 0.0F));
296  axisPoints.push_back(cv::Point3f(length, 0.0F, 0.0F));
297  axisPoints.push_back(cv::Point3f(0.0F, length, 0.0F));
298  axisPoints.push_back(cv::Point3f(0.0F, 0.0F, length));
299 
300  std::vector<cv::Point2f> imagePoints;
301  cv::projectPoints(axisPoints, rvecs[i], tvecs[i], itsArUco->itsCamMatrix,
302  itsArUco->itsDistCoeffs, imagePoints);
303 
304  // draw axis lines
305  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
306  int(imagePoints[1].x + 0.4999F), int(imagePoints[1].y + 0.4999F),
307  2, jevois::yuyv::MedPurple);
308  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
309  int(imagePoints[2].x + 0.4999F), int(imagePoints[2].y + 0.4999F),
310  2, jevois::yuyv::MedGreen);
311  jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.4999F), int(imagePoints[0].y + 0.4999F),
312  int(imagePoints[3].x + 0.4999F), int(imagePoints[3].y + 0.4999F),
313  2, jevois::yuyv::MedGrey);
314  }
315  }
316 
317  jevois::rawimage::writeText(outimg, "Detected " + std::to_string(ids.size()) + " ArUco markers.",
318  3, h + 5, jevois::yuyv::White);
319 
320  // Show processing fps:
321  std::string const & fpscpu = timer.stop();
322  jevois::rawimage::writeText(outimg, fpscpu, 3, h - 13, jevois::yuyv::White);
323 
324  // Send the output image with our processing results to the host over USB:
325  outframe.send();
326  }
327 
328  // ####################################################################################################
329  protected:
330  std::shared_ptr<ArUco> itsArUco;
331 };
332 
333 // 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:150
JEVOIS_REGISTER_MODULE(DemoArUco)
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
#define JEVOIS_DECLARE_PARAMETER(ParamName, ParamType,...)
Parameter.
unsigned int fmt
DemoArUco(std::string const &instance)
Constructor.
Definition: DemoArUco.C:135
Simple demo of ArUco augmented reality markers detection and decoding.
Definition: DemoArUco.C:128
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:171
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:330
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:214
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:144