JeVoisBase  1.0
JeVois Smart Embedded Machine Vision Toolkit Base Modules
RoadNavigation.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, CA 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 
20 #include <jevois/Debug/Log.H>
21 #include <jevois/Debug/Timer.H>
24 
25 #include <opencv2/core/core.hpp>
26 #include <opencv2/imgproc/imgproc.hpp>
27 
28 #include <future>
29 #include <linux/videodev2.h> // for v4l2 pixel types
30 
31 // icon by Dave Gandy in transport at flaticon
32 
33 //! Road finder demo
34 /*! This algorithm detects road using a compination of edge detection and tracking, and texture analysis. The algorithm
35  is an implementation of Chang, Siagian and Itti, IROS 2012, available at
36  http://ilab.usc.edu/publications/doc/Chang_etal12iros.pdf
37 
38  The algorithms combines detection and tracking of line segments at the edges of the road or on the road (e.g., lane
39  dividers), and texture analysis to distinguish the road region from its surroundings.
40 
41  The algorithm outputs the horizontal coordinate of the vanishing point of the road, which usually is a good
42  indication of the road heading (except in very tight bends or corners).
43 
44  Demo display outputs
45  --------------------
46 
47  Detected line segments are shown in black and white, while segments that have been reliably tracked over multiple
48  frames are shown in thick purple. Estimated vanishing point location and confidence is shown as a big green disk on
49  the horizon line.
50 
51  Serial outputs
52  --------------
53 
54  Issues a message of the form
55  \verbatim
56  T1D x
57  \endverbatim
58  where x is the standardized horizontal coordinate (between -1000 for full left to 1000 for full right) of the
59  vanishing point. See \ref coordhelpers for standardized coordinates.
60 
61  Trying it out
62  -------------
63 
64  To casually try out this module, just search the web for pictures of roads and point the JeVois camera to one of
65  them. Make sure that you align the horizon line of the algorithm (which has a bumber of purple and green disks)
66  roughly with the horizon line in your picture. As you move the camera left and right, the location of the large
67  green disk that marks the detected vanishing point should move left and right, and should point to the vanishing
68  point of the road in your image.
69 
70  When using on a mobile robot in th ereal world, setting the proper horizon line is essential for good operation of
71  the algorithm. This is determined by parameter \c horizon, which should be tuned according to the height and
72  tilt angle of the JeVois camera on your vehicle.
73 
74  @author Laurent Itti
75 
76  @videomapping NONE 0 0 0 YUYV 320 240 30.0 JeVois RoadNavigation
77  @videomapping NONE 0 0 0 YUYV 176 144 120.0 JeVois RoadNavigation
78  @videomapping YUYV 320 256 30.0 YUYV 320 240 30.0 JeVois RoadNavigation
79  @videomapping YUYV 176 160 120.0 YUYV 176 144 120.0 JeVois RoadNavigation
80  @email itti\@usc.edu
81  @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
82  @copyright Copyright (C) 2016 by Laurent Itti, iLab and the University of Southern California
83  @mainurl http://jevois.org
84  @supporturl http://jevois.org/doc
85  @otherurl http://iLab.usc.edu
86  @license GPL v3
87  @distribution Unrestricted
88  @restrictions None
89  \ingroup modules */
91 {
92  public:
93  // ####################################################################################################
94  //! Constructor
95  // ####################################################################################################
96  RoadNavigation(std::string const & instance) :
97  jevois::Module(instance), itsProcessingTimer("Processing", 30, LOG_DEBUG)
98  {
99  itsRoadFinder = addSubComponent<RoadFinder>("roadfinder");
100  }
101 
102  // ####################################################################################################
103  //! Virtual destructor for safe inheritance
104  // ####################################################################################################
105  virtual ~RoadNavigation() { }
106 
107  // ####################################################################################################
108  //! Processing function, no video out
109  // ####################################################################################################
110  virtual void process(jevois::InputFrame && inframe) override
111  {
112  // Wait for next available camera image:
113  jevois::RawImage inimg = inframe.get();
114 
115  // Convert it to gray:
116  cv::Mat imggray = jevois::rawimage::convertToCvGray(inimg);
117 
118  // Compute the vanishing point, with no drawings:
119  jevois::RawImage visual; // unallocated pixels, will not draw anything
120  itsRoadFinder->process(imggray, visual);
121 
122  // Let camera know we are done processing the input image:
123  inframe.done();
124 
125  // Get the filtered target point x and send to serial:
126  int const tpx = int(itsRoadFinder->getFilteredTargetX() * 2000.0F / inimg.width - 1000.0F);
127  sendSerial(jevois::sformat("T1D %d", int(tpx + 0.5F)));
128  }
129 
130  // ####################################################################################################
131  //! Processing function with USB video out
132  // ####################################################################################################
133  virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
134  {
135  // Wait for next available camera image:
136  jevois::RawImage inimg = inframe.get(); unsigned int const w = inimg.width, h = inimg.height;
137  inimg.require("input", w, h, V4L2_PIX_FMT_YUYV); // accept any image size but require YUYV pixels
138 
140 
141  // Convert it to gray:
142  cv::Mat imggray = jevois::rawimage::convertToCvGray(inimg);
143 
144  // Compute the vanishing point. Note: the results will be drawn into inimg, so that we don't have to wait for
145  // outimg to be ready. It's ok to modify the input image, its buffer will be sent back to the camera driver for
146  // capture once we are done here, and will be overwritten anyway:
147  itsRoadFinder->process(imggray, inimg);
148 
149  // Wait for an image from our gadget driver into which we will put our results:
150  jevois::RawImage outimg = outframe.get();
151 
152  // Enforce the correct output image size and format:
153  outimg.require("output", w, h + 16, V4L2_PIX_FMT_YUYV);
154 
155  // Paste the original image + drawings to the top-left corner of the display:
156  unsigned short const txtcol = 0xa0ff; //WHITE: 0x80ff;
157  jevois::rawimage::paste(inimg, outimg, 0, 0);
158  jevois::rawimage::writeText(outimg, "JeVois Road Navigation Demo", 3, 3, txtcol);
159 
160  // Let camera know we are done processing the raw YUV input image:
161  inframe.done();
162 
163  // Clear out the bottom section of the image:
164  jevois::rawimage::drawFilledRect(outimg, 0, h, w, outimg.height-h, jevois::yuyv::Black);
165 
166  // Get the filtered target point x and send to serial:
167  int const tpx = int(itsRoadFinder->getFilteredTargetX() * 2000.0F / w - 1000.0F);
168  sendSerial(jevois::sformat("T1D %d", int(tpx + 0.5F)));
169 
170  // Write some extra info about the vp:
171  std::ostringstream otxt; otxt << std::fixed << std::setprecision(3);
172  auto vp = itsRoadFinder->getCurrVanishingPoint();
173  otxt << "VP x=" << vp.first.i << " (" << vp.second << ") CTR=" << std::setprecision(1);
174  auto cp = itsRoadFinder->getCurrCenterPoint();
175  auto tp = itsRoadFinder->getCurrTargetPoint();
176  otxt << cp.i << " TGT=" << tp.i << " fTPX=" << tpx;
177  jevois::rawimage::writeText(outimg, otxt.str().c_str(), 3, h + 3, jevois::yuyv::White);
178 
179  // Show processing fps:
180  std::string const & fpscpu = itsProcessingTimer.stop();
181  jevois::rawimage::writeText(outimg, fpscpu, 3, h - 13, jevois::yuyv::White);
182 
183  // Send the output image with our processing results to the host over USB:
184  outframe.send();
185  }
186 
187  // ####################################################################################################
188  //! Module internals
189  // ####################################################################################################
190  protected:
192  std::shared_ptr<RoadFinder> itsRoadFinder;
193 };
194 
195 // Allow the module to be loaded as a shared object (.so) file:
RoadNavigation(std::string const &instance)
Constructor.
friend friend class Module
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 out.
std::shared_ptr< RoadFinder > itsRoadFinder
std::string sformat(char const *fmt,...) __attribute__((format(__printf__
jevois::Timer itsProcessingTimer
Module internals.
JEVOIS_REGISTER_MODULE(RoadNavigation)
cv::Mat convertToCvGray(RawImage const &src)
virtual void sendSerial(std::string const &str)
void drawFilledRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int col)
virtual void process(jevois::InputFrame &&inframe, jevois::OutputFrame &&outframe) override
Processing function with USB video out.
virtual ~RoadNavigation()
Virtual destructor for safe inheritance.
unsigned int width
std::string const & stop()
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
Road finder demo.