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