JeVoisBase  1.3
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
RoadFinder.H
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 // This code adapted from:
19 
20 // //////////////////////////////////////////////////////////////////// //
21 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
22 // University of Southern California (USC) and the iLab at USC. //
23 // See http://iLab.usc.edu for information about this project. //
24 // //////////////////////////////////////////////////////////////////// //
25 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
26 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
27 // in Visual Environments, and Applications'' by Christof Koch and //
28 // Laurent Itti, California Institute of Technology, 2001 (patent //
29 // pending; application number 09/912,225 filed July 23, 2001; see //
30 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status). //
31 // //////////////////////////////////////////////////////////////////// //
32 // This file is part of the iLab Neuromorphic Vision C++ Toolkit. //
33 // //
34 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can //
35 // redistribute it and/or modify it under the terms of the GNU General //
36 // Public License as published by the Free Software Foundation; either //
37 // version 2 of the License, or (at your option) any later version. //
38 // //
39 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope //
40 // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
41 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
42 // PURPOSE. See the GNU General Public License for more details. //
43 // //
44 // You should have received a copy of the GNU General Public License //
45 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write //
46 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, //
47 // Boston, MA 02111-1307 USA. //
48 // //////////////////////////////////////////////////////////////////// //
49 
50 #pragma once
51 
53 #include <jevois/Image/RawImage.H>
54 #include <opencv2/core/core.hpp>
55 #define INVT_TYPEDEF_INT64
56 #define INVT_TYPEDEF_UINT64
58 #include <opencv2/video/tracking.hpp> // for kalman filter
59 
60 // ######################################################################
61 
62 //! a segment is defined by the two end-points
63 /*! \relates RoadFinder*/
64 struct Segment
65 {
66  Segment(Point2D<int> in_p1, Point2D<int> in_p2, float in_angle, float in_length) :
67  p1(in_p1), p2(in_p2), angle(in_angle), length(in_length)
68  { }
69 
72  float angle;
73  float length;
74 
75  bool operator<(const Segment & s) { return length < s.length; }
76 };
77 
78 //! Keeps all the supporting information about a specific vanishing point
79 /*! \relates RoadFinder*/
81 {
82  VanishingPoint(Point2D<int> in_vp, float in_likelihood) :
83  vp(in_vp), likelihood(in_likelihood)
84  { }
85 
87  float prior;
88  float likelihood;
89  float posterior;
90  std::vector<Segment> supportingSegments;
91 };
92 
93 //! keeps all the ready to use information of a supporting line as it pertains to describing the road
94 /*! \relates RoadFinder*/
95 struct Line
96 {
97  Line() : length(0.0F), angle(0.0F), score(0.0F), isActive(false), index(-1)
98  { }
99 
100  //! basic information to specify the line
101  float length;
102  float angle;
103  float score;
104 
105  //! the points that are fit to the line
106  std::vector<Point2D<int> > points;
107 
108  //! quick information for various locations with respect to the road
115 
116  //! original supporting segments out of sync after initial frame
117  std::vector<Segment> segments;
118 
119  //! tracking information to monitor health of the line
120  std::vector<float> scores;
121  std::vector<float> start_scores;
122 
123  //! tracks whether the line can be used for finding the road center
124  bool isActive;
127  float offset;
128 
129  int index;
130 };
131 
132 //! Store information about the road
133 /*! \relates RoadFinder*/
134 struct RoadModel
135 {
136  std::vector<Line> lines;
137  std::vector<int> lastActiveIndex;
138  std::vector<Point2D<float> > lastSeenHorizonPoint;
139  std::vector<Point2D<float> > lastSeenLocation;
140  std::vector<int> numMatches;
141 };
142 
143 namespace roadfinder
144 {
145  static jevois::ParameterCategory const ParamCateg("RoadFinder Options");
146 
147  //! Parameter \relates RoadFinder
148  JEVOIS_DECLARE_PARAMETER(horizon, int, "Estimated vertical (Y) position of the horizon (pixels, "
149  "with 0 at the top of the frame). Adjust this depending on the tilt angle of your camera "
150  "and video input resolution.",
151  70, ParamCateg);
152 
153  //! Parameter \relates RoadFinder
154  JEVOIS_DECLARE_PARAMETER(support, int, "Offset (in pixels) between horizon line and horizon support line (positive "
155  "values means support line is below horizon line.",
156  20, ParamCateg);
157 
158  //! Parameter \relates RoadFinder
159  JEVOIS_DECLARE_PARAMETER(spacing, unsigned int, "Spacing between vanishing point candidates (pixels).",
160  20, ParamCateg);
161 
162  //! Parameter \relates RoadFinder
163  JEVOIS_DECLARE_PARAMETER(distthresh, unsigned int, "Vanishing point distance threshold (pixels).",
164  40, ParamCateg);
165 } // namespace roadfinder
166 
167 
168 //! Navigation by finding road
169 /*! This algorithm implements the algorithm described in: C.-K. Chang, C. Siagian, L. Itti, Mobile Robot Monocular
170  Vision Navigation Based on Road Region and Boundary Estimation, In: Proc. IEEE/RSJ International Conference on
171  Intelligent Robots and Systems (IROS), pp. 1043-1050, Oct 2012.
172 
173  See the research paper at http://ilab.usc.edu/publications/doc/Chang_etal12iros.pdf
174 
175  \ingroup components */
177  public jevois::Parameter<roadfinder::horizon, roadfinder::support,
178  roadfinder::spacing, roadfinder::distthresh>
179 {
180  public:
181  //! constructor
182  RoadFinder(std::string const & instance);
183 
184  //! desctructor
185  virtual ~RoadFinder();
186 
187  //! Compute the vanishing point location using the full blown algorithm
188  /*! img should be greyscale. If visual is valid, it should be YUYV with same or larger dims, with the assumption
189  that it contains a color copy of the input frame in the top-left corner (this is used for debug visualizations
190  of the various things computed). */
191  void process(cv::Mat const & img, jevois::RawImage & visual);
192 
193  //! Get the current vanishing point and confidence
194  std::pair<Point2D<int>, float> getCurrVanishingPoint() const;
195 
196  //! Get the current road center point
198 
199  //! Get the current target point
201 
202  //! Get the kalman-fitered target X, can be used to set robot steering
203  float getFilteredTargetX() const;
204 
205  //! Reset all tracker internals and start fresh (e.g., when changing goal direction)
206  /*! Thread-safe, ok to call concurrently with process(). */
207  void resetRoadModel();
208 
209  protected:
210  //! This class has state and does not support some online param changes
211  void postInit() override;
212 
213  //! This class has state and does not support some online param changes
214  void preUninit() override;
215 
216  //! compute the hough segments in the image
217  void computeHoughSegments(cv::Mat const & cvImage);
218 
219  //! main function to detect the road
220  std::vector<Line> computeVanishingLines(cv::Mat const & edgeMap, Point2D<int> const & vanishingPoint,
221  jevois::RawImage & visual);
222 
223  //! computes the road center point to servo to
224  Point2D<float> computeRoadCenterPoint(cv::Mat const & edgeMap, std::vector<Line> & lines,
225  Point2D<int> & vanishing_point,
226  Point2D<float> & road_center_point, float & confidence);
227 
228  //! update road model and incoming lines NOTE: also change the line parameters to sync them this avoids drifts
229  void updateRoadModel(std::vector<Line> & lines, int index);
230 
231  //! estimate the vanishing point from the tracked lines
232  Point2D<int> getVanishingPoint(std::vector<Line> const & lines, float &confidence);
233 
234  //! track vanishing lines by to fit to the new, inputted, edgemap
235  void trackVanishingLines(cv::Mat const & edgeMap, std::vector<Line> & currentLines, jevois::RawImage & visual);
236 
237  //! get pixels for segment defined by p1 and p2 have added complexity to search within 1.5 pixels of the line
238  std::vector<Point2D<int> >
239  getPixels(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap);
240 
241  //! get pixels for segment defined by p1 and p2 have added complexity to search within 1.5 pixels of the line
242  std::vector<Point2D<int> >
243  getPixels(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap,
244  std::vector<uint>& startIndexes);
245 
246  //! get pixels that make up the segment defined by p1 and p2
247  std::vector<Point2D<int> >
248  getPixelsQuick(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap);
249 
250  //! find lines given the found supporting segments
251  Line findLine2(Segment const & s, cv::Mat const & edgeMap, std::list<Segment> const & supportingSegments,
252  std::vector<bool> & is_used, float & totalLength, uint & numSegments);
253 
254  //! openCV wrapper function to fit a line to an input vector of points
255  void fitLine(std::vector<Point2D<int> > const & points, Point2D<float> & p1,Point2D<float> & p2,
256  int const width, int const height);
257 
258  //! compute how well the line equation fit the edgels in edgemap
259  float getLineFitness(Point2D<int> const & horizonPoint, Point2D<int> const & roadBottomPoint,
260  cv::Mat const & edgeMap, jevois::RawImage & visual);
261 
262  //! compute how well the line equation fit the edgels in edgemap
263  float getLineFitness(Point2D<int> const & horizonPoint, Point2D<int> const & roadBottomPoint,
264  cv::Mat const & edgeMap, std::vector<Point2D<int> > & points, jevois::RawImage & visual);
265 
266  //! update the information in by updating the input points, score and various handy coordinate locations
267  void updateLine(Line & l, std::vector<Point2D<int> > const & points, float score,
268  int const width, int const height);
269 
270  //! update the lines with the inputted set of edgemaps
271  void projectForwardVanishingLines(std::vector<Line> & lines, std::vector<cv::Mat> const & edgeMaps,
272  jevois::RawImage & visual);
273 
274  //! combine two lines sets, discard duplicates and overlaps
275  std::vector<Line> combine(std::vector<Line> & prevLines, std::vector<Line> const & currentLines,
276  int width, int height);
277 
278  //! discard duplicate lines in a set
279  std::vector<Line> discardDuplicates(std::vector<Line> const & currentLines);
280 
281  //! the current road heading
283 
284  //! the accumulated trajectory
286 
287  //! locking accunulated trajectory
288  std::mutex itsAccTrajMtx;
289 
290  //! current segments found using CVHoughlines
291  std::vector<Segment> itsCurrentSegments;
292 
293  std::mutex itsTrackMtx; //!< locking line trackers vars
294 
295  //! indicate whether tracking
297 
298  //! the current lines being tracked
299  std::vector<Line> itsCurrentLines;
300 
301  //! indicate how many unique lines have been identified NOTE: never reset
303 
305 
306  //! vanishing points being considered
307  std::vector<VanishingPoint> itsVanishingPoints;
308 
309  std::mutex itsRoadMtx;
310  Point2D<int> itsVanishingPoint; //!< current vanishing point
311  Point2D<float> itsCenterPoint; //!< current center of road point
312  Point2D<float> itsTargetPoint; //!< target servo point
313  float itsVanishingPointConfidence; //!< current vanishing point
314  std::vector<bool> itsVanishingPointStability; //!< vanishing point score tracker
315 
316  //! for visualizer
318 
319  cv::KalmanFilter itsTPXfilter;
322 };
bool isActive
tracks whether the line can be used for finding the road center
Definition: RoadFinder.H:124
virtual ~RoadFinder()
desctructor
Definition: RoadFinder.C:209
std::vector< int > lastActiveIndex
Definition: RoadFinder.H:137
float getLineFitness(Point2D< int > const &horizonPoint, Point2D< int > const &roadBottomPoint, cv::Mat const &edgeMap, jevois::RawImage &visual)
compute how well the line equation fit the edgels in edgemap
Definition: RoadFinder.C:1230
cv::Mat cvImage(RawImage const &src)
std::vector< Segment > supportingSegments
Definition: RoadFinder.H:90
Point2D< float > onScreenHorizonSupportPoint
Definition: RoadFinder.H:114
unsigned int uint
Canonical unsigned int.
Definition: Types.H:134
float angle
Definition: RoadFinder.H:102
void fitLine(std::vector< Point2D< int > > const &points, Point2D< float > &p1, Point2D< float > &p2, int const width, int const height)
openCV wrapper function to fit a line to an input vector of points
Definition: RoadFinder.C:1204
Point2D< float > pointToServo
Definition: RoadFinder.H:126
std::vector< int > numMatches
Definition: RoadFinder.H:140
std::vector< Point2D< int > > getPixels(Point2D< int > const &p1, Point2D< int > const &p2, cv::Mat const &edgeMap)
get pixels for segment defined by p1 and p2 have added complexity to search within 1...
Definition: RoadFinder.C:954
RoadModel itsRoadModel
Definition: RoadFinder.H:304
void updateRoadModel(std::vector< Line > &lines, int index)
update road model and incoming lines NOTE: also change the line parameters to sync them this avoids d...
Definition: RoadFinder.C:594
std::vector< bool > itsVanishingPointStability
vanishing point score tracker
Definition: RoadFinder.H:314
Point2D< float > horizonPoint
quick information for various locations with respect to the road
Definition: RoadFinder.H:109
Point2D< int > p1
Definition: RoadFinder.H:70
JEVOIS_DECLARE_PARAMETER(thresh1, double,"First threshold for hysteresis", 50.0, ParamCateg)
void trackVanishingLines(cv::Mat const &edgeMap, std::vector< Line > &currentLines, jevois::RawImage &visual)
track vanishing lines by to fit to the new, inputted, edgemap
Definition: RoadFinder.C:1318
std::vector< float > scores
tracking information to monitor health of the line
Definition: RoadFinder.H:120
Line findLine2(Segment const &s, cv::Mat const &edgeMap, std::list< Segment > const &supportingSegments, std::vector< bool > &is_used, float &totalLength, uint &numSegments)
find lines given the found supporting segments
Definition: RoadFinder.C:1093
Point2D< float > onScreenRoadBottomPoint
Definition: RoadFinder.H:112
void preUninit() override
This class has state and does not support some online param changes.
Definition: RoadFinder.C:221
float angleToCenter
Definition: RoadFinder.H:125
Point2D< float > itsTargetPoint
target servo point
Definition: RoadFinder.H:312
std::mutex itsTrackMtx
locking line trackers vars
Definition: RoadFinder.H:293
Point2D< float > horizonSupportPoint
Definition: RoadFinder.H:110
std::mutex itsAccTrajMtx
locking accunulated trajectory
Definition: RoadFinder.H:288
Point2D< float > onScreenHorizonPoint
Definition: RoadFinder.H:113
float itsFilteredTPX
Definition: RoadFinder.H:320
std::vector< Line > lines
Definition: RoadFinder.H:136
Point2D< float > itsAccumulatedTrajectory
the accumulated trajectory
Definition: RoadFinder.H:285
float offset
Definition: RoadFinder.H:127
cv::KalmanFilter itsTPXfilter
Definition: RoadFinder.H:319
std::vector< VanishingPoint > itsVanishingPoints
vanishing points being considered
Definition: RoadFinder.H:307
void postInit() override
This class has state and does not support some online param changes.
Definition: RoadFinder.C:213
Point2D< float > computeRoadCenterPoint(cv::Mat const &edgeMap, std::vector< Line > &lines, Point2D< int > &vanishing_point, Point2D< float > &road_center_point, float &confidence)
computes the road center point to servo to
Definition: RoadFinder.C:433
std::vector< Line > computeVanishingLines(cv::Mat const &edgeMap, Point2D< int > const &vanishingPoint, jevois::RawImage &visual)
main function to detect the road
Definition: RoadFinder.C:819
Store information about the road.
Definition: RoadFinder.H:134
float length
Definition: RoadFinder.H:73
void resetRoadModel()
Reset all tracker internals and start fresh (e.g., when changing goal direction)
Definition: RoadFinder.C:245
Point2D< int > getVanishingPoint(std::vector< Line > const &lines, float &confidence)
estimate the vanishing point from the tracked lines
Definition: RoadFinder.C:757
bool operator<(const Segment &s)
Definition: RoadFinder.H:75
std::vector< Point2D< int > > points
the points that are fit to the line
Definition: RoadFinder.H:106
Point2D< int > vp
Definition: RoadFinder.H:86
std::vector< Point2D< float > > lastSeenHorizonPoint
Definition: RoadFinder.H:138
std::vector< Point2D< float > > lastSeenLocation
Definition: RoadFinder.H:139
std::vector< Line > combine(std::vector< Line > &prevLines, std::vector< Line > const &currentLines, int width, int height)
combine two lines sets, discard duplicates and overlaps
Definition: RoadFinder.C:1418
std::vector< float > start_scores
Definition: RoadFinder.H:121
a segment is defined by the two end-points
Definition: RoadFinder.H:64
Point2D< float > itsCenterPoint
current center of road point
Definition: RoadFinder.H:311
VanishingPoint(Point2D< int > in_vp, float in_likelihood)
Definition: RoadFinder.H:82
float posterior
Definition: RoadFinder.H:89
float score
Definition: RoadFinder.H:103
keeps all the ready to use information of a supporting line as it pertains to describing the road ...
Definition: RoadFinder.H:95
std::vector< Line > itsCurrentLines
the current lines being tracked
Definition: RoadFinder.H:299
float length
basic information to specify the line
Definition: RoadFinder.H:101
Keeps all the supporting information about a specific vanishing point.
Definition: RoadFinder.H:80
Point2D< float > getCurrCenterPoint() const
Get the current road center point.
Definition: RoadFinder.C:233
Point2D< float > roadBottomPoint
Definition: RoadFinder.H:111
std::vector< Segment > itsCurrentSegments
current segments found using CVHoughlines
Definition: RoadFinder.H:291
std::vector< Line > discardDuplicates(std::vector< Line > const &currentLines)
discard duplicate lines in a set
Definition: RoadFinder.C:1486
RoadFinder(std::string const &instance)
constructor
Definition: RoadFinder.C:181
Point2D< float > getCurrTargetPoint() const
Get the current target point.
Definition: RoadFinder.C:237
float angle
Definition: RoadFinder.H:72
bool itsTrackingFlag
indicate whether tracking
Definition: RoadFinder.H:296
void projectForwardVanishingLines(std::vector< Line > &lines, std::vector< cv::Mat > const &edgeMaps, jevois::RawImage &visual)
update the lines with the inputted set of edgemaps
Definition: RoadFinder.C:1410
uint itsNumIdentifiedLines
indicate how many unique lines have been identified NOTE: never reset
Definition: RoadFinder.H:302
void process(cv::Mat const &img, jevois::RawImage &visual)
Compute the vanishing point location using the full blown algorithm.
Definition: RoadFinder.C:256
std::pair< Point2D< int >, float > getCurrVanishingPoint() const
Get the current vanishing point and confidence.
Definition: RoadFinder.C:229
int index
Definition: RoadFinder.H:129
float itsVanishingPointConfidence
current vanishing point
Definition: RoadFinder.H:313
void computeHoughSegments(cv::Mat const &cvImage)
compute the hough segments in the image
Definition: RoadFinder.C:786
Navigation by finding road.
Definition: RoadFinder.H:176
Line()
Definition: RoadFinder.H:97
int itsCurrentMessageID
for visualizer
Definition: RoadFinder.H:317
Point2D< int > p2
Definition: RoadFinder.H:71
Point2D< int > itsVanishingPoint
current vanishing point
Definition: RoadFinder.H:310
std::mutex itsRoadMtx
Definition: RoadFinder.H:309
std::vector< Point2D< int > > getPixelsQuick(Point2D< int > const &p1, Point2D< int > const &p2, cv::Mat const &edgeMap)
get pixels that make up the segment defined by p1 and p2
Definition: RoadFinder.C:1057
bool itsKalmanNeedInit
Definition: RoadFinder.H:321
float getFilteredTargetX() const
Get the kalman-fitered target X, can be used to set robot steering.
Definition: RoadFinder.C:241
Segment(Point2D< int > in_p1, Point2D< int > in_p2, float in_angle, float in_length)
Definition: RoadFinder.H:66
float likelihood
Definition: RoadFinder.H:88
double itsRoadHeading
the current road heading
Definition: RoadFinder.H:282
std::vector< Segment > segments
original supporting segments out of sync after initial frame
Definition: RoadFinder.H:117
void updateLine(Line &l, std::vector< Point2D< int > > const &points, float score, int const width, int const height)
update the information in by updating the input points, score and various handy coordinate locations ...
Definition: RoadFinder.C:1151