JeVoisBase  1.8
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
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 //! Parameter \relates RoadNavigation
34 JEVOIS_DECLARE_PARAMETER(vpconf, float, "Minimum vanishing point confidence required to send a serial message. "
35  "Note that currently, confidence values are binary (only 0 or 1), and quite conservative "
36  "(many good detections may get a confidence of 0 if they are not very clean).",
37  0.0F, roadfinder::ParamCateg);
38 
39 //! Road finder demo
40 /*! This algorithm detects road using a compination of edge detection and tracking, and texture analysis. The algorithm
41  is an implementation of Chang, Siagian and Itti, IROS 2012, available at
42  http://ilab.usc.edu/publications/doc/Chang_etal12iros.pdf
43 
44  The algorithms combines detection and tracking of line segments at the edges of the road or on the road (e.g., lane
45  dividers), and texture analysis to distinguish the road region from its surroundings.
46 
47  The algorithm outputs the horizontal coordinate of the vanishing point of the road, which usually is a good
48  indication of the road heading (except in very tight bends or corners).
49 
50  Demo display outputs
51  --------------------
52 
53  Detected line segments are shown in black and white, while segments that have been reliably tracked over multiple
54  frames are shown in thick purple. Estimated vanishing point location and confidence is shown as a big green disk on
55  the horizon line.
56 
57  Note that the demo display runs much slower than the algorithm with no video output (which should be used on
58  autonomous cars), simply because drawing all the detected line segments takes a lot of time.
59 
60  Serial Messages
61  ---------------
62 
63  This module can send standardized serial messages as described in \ref UserSerialStyle. One 1D message is issued on
64  every video frame for the vanishing point's horizontal location.
65 
66  - Serial message type: \b 1D
67  - `id`: always \b vp (shorthand for vanishing point)
68  - `x`: standardized 2D horizontal coordinate of the vanishing point
69  - `w`: always 0.
70  - `extra`: none (empty string)
71 
72  Trying it out
73  -------------
74 
75  To casually try out this module, just search the web for pictures of roads and point the JeVois camera to one of
76  them. Make sure that you align the horizon line of the algorithm (which has a number of purple and green disks)
77  roughly with the horizon line in your picture. As you move the camera left and right, the location of the large
78  green disk that marks the detected vanishing point should move left and right, and should point to the vanishing
79  point of the road in your image.
80 
81  When using on a mobile robot in the real world, setting the proper horizon line is essential for good operation of
82  the algorithm. This is determined by parameter \c horizon, which should be tuned according to the height and
83  tilt angle of the JeVois camera on your vehicle.
84 
85 
86  @author Laurent Itti
87 
88  @videomapping NONE 0 0 0 YUYV 320 240 30.0 JeVois RoadNavigation
89  @videomapping NONE 0 0 0 YUYV 176 144 120.0 JeVois RoadNavigation
90  @videomapping YUYV 320 256 30.0 YUYV 320 240 30.0 JeVois RoadNavigation
91  @videomapping YUYV 176 160 120.0 YUYV 176 144 120.0 JeVois RoadNavigation
92  @email itti\@usc.edu
93  @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
94  @copyright Copyright (C) 2016 by Laurent Itti, iLab and the University of Southern California
95  @mainurl http://jevois.org
96  @supporturl http://jevois.org/doc
97  @otherurl http://iLab.usc.edu
98  @license GPL v3
99  @distribution Unrestricted
100  @restrictions None
101  \ingroup modules */
103 {
104  public:
105  // ####################################################################################################
106  //! Constructor
107  // ####################################################################################################
108  RoadNavigation(std::string const & instance) :
109  jevois::StdModule(instance), itsProcessingTimer("Processing", 30, LOG_DEBUG)
110  {
111  itsRoadFinder = addSubComponent<RoadFinder>("roadfinder");
112  }
113 
114  // ####################################################################################################
115  //! Virtual destructor for safe inheritance
116  // ####################################################################################################
117  virtual ~RoadNavigation() { }
118 
119  // ####################################################################################################
120  //! Processing function, no video out
121  // ####################################################################################################
122  virtual void process(jevois::InputFrame && inframe) override
123  {
124  // Wait for next available camera image, convert it to gray, and release it:
125  cv::Mat imggray = inframe.getCvGRAY();
126 
127  // Compute the vanishing point, with no drawings:
128  jevois::RawImage visual; // unallocated pixels, will not draw anything
129  itsRoadFinder->process(imggray, visual);
130 
131  // Get the vanishing point and send to serial:
132  int const w = imggray.cols;
133  std::pair<Point2D<int>, float> vp = itsRoadFinder->getCurrVanishingPoint();
134  if (vp.second >= vpconf::get()) sendSerialImg1Dx(w, vp.first.i, 0.0F, "vp");
135  }
136 
137  // ####################################################################################################
138  //! Processing function with USB video out
139  // ####################################################################################################
140  virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
141  {
142  // Wait for next available camera image:
143  jevois::RawImage inimg = inframe.get(); unsigned int const w = inimg.width, h = inimg.height;
144  inimg.require("input", w, h, V4L2_PIX_FMT_YUYV); // accept any image size but require YUYV pixels
145 
147 
148  // Convert it to gray:
149  cv::Mat imggray = jevois::rawimage::convertToCvGray(inimg);
150 
151  // Compute the vanishing point. Note: the results will be drawn into inimg, so that we don't have to wait for
152  // outimg to be ready. It's ok to modify the input image, its buffer will be sent back to the camera driver for
153  // capture once we are done here, and will be overwritten anyway:
154  itsRoadFinder->process(imggray, inimg);
155 
156  // Wait for an image from our gadget driver into which we will put our results:
157  jevois::RawImage outimg = outframe.get();
158 
159  // Enforce the correct output image size and format:
160  outimg.require("output", w, h + 16, V4L2_PIX_FMT_YUYV);
161 
162  // Paste the original image + drawings to the top-left corner of the display:
163  unsigned short const txtcol = 0xa0ff; //WHITE: 0x80ff;
164  jevois::rawimage::paste(inimg, outimg, 0, 0);
165  jevois::rawimage::writeText(outimg, "JeVois Road Navigation Demo", 3, 3, txtcol);
166 
167  // Let camera know we are done processing the raw YUV input image:
168  inframe.done();
169 
170  // Clear out the bottom section of the image:
171  jevois::rawimage::drawFilledRect(outimg, 0, h, w, outimg.height-h, jevois::yuyv::Black);
172 
173  // Get the vanishing point and send to serial:
174  std::pair<Point2D<int>, float> vp = itsRoadFinder->getCurrVanishingPoint();
175  if (vp.second >= vpconf::get()) sendSerialImg1Dx(w, vp.first.i, 0.0F, "vp");
176 
177  // Write some extra info about the vp:
178  std::ostringstream otxt; otxt << std::fixed << std::setprecision(3);
179  otxt << "VP x=" << vp.first.i << " (" << vp.second << ") CTR=" << std::setprecision(1);
180  auto cp = itsRoadFinder->getCurrCenterPoint();
181  auto tp = itsRoadFinder->getCurrTargetPoint();
182  float const tpx = itsRoadFinder->getFilteredTargetX();
183  otxt << cp.i << " TGT=" << tp.i << " fTPX=" << tpx;
184  jevois::rawimage::writeText(outimg, otxt.str().c_str(), 3, h + 3, jevois::yuyv::White);
185 
186  // Show processing fps:
187  std::string const & fpscpu = itsProcessingTimer.stop();
188  jevois::rawimage::writeText(outimg, fpscpu, 3, h - 13, jevois::yuyv::White);
189 
190  // Send the output image with our processing results to the host over USB:
191  outframe.send();
192  }
193 
194  // ####################################################################################################
195  //! Module internals
196  // ####################################################################################################
197  protected:
199  std::shared_ptr<RoadFinder> itsRoadFinder;
200 };
201 
202 // Allow the module to be loaded as a shared object (.so) file:
RoadNavigation(std::string const &instance)
Constructor.
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
unsigned int height
virtual void process(jevois::InputFrame &&inframe) override
Processing function, no video out.
std::shared_ptr< RoadFinder > itsRoadFinder
StdModule(std::string const &instance)
jevois::Timer itsProcessingTimer
Module internals.
JEVOIS_REGISTER_MODULE(RoadNavigation)
cv::Mat convertToCvGray(RawImage const &src)
std::string const & stop()
JEVOIS_DECLARE_PARAMETER(camparams, std::string, "File stem of camera parameters, or empty. Camera resolution " "will be appended, as well as a .cfg extension. For example, specifying 'camera_para' " "here and running the camera sensor at 320x240 will attempt to load " "camera_para320x240.dat from within the module's directory.", "camera_para", ParamCateg)
Parameter.
void sendSerialImg1Dx(unsigned int camw, float x, float size=0.0F, std::string const &id="", std::string const &extra="")
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
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
Road finder demo.
void require(char const *info, unsigned int w, unsigned int h, unsigned int f) const