JeVoisBase  1.3
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
RoadFinder.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 // 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 
51 #include <jevois/Debug/Log.H>
52 #include <jevois/Debug/Profiler.H>
53 #include <opencv2/imgproc/imgproc.hpp> // for Canny
54 #include <opencv2/imgproc/imgproc_c.h> // for cvFitLine
56 #include <future>
57 
58 // heading difference per unit pixel, it's measured 27 degrees per half image of 160 pixels
59 #define HEADING_DIFFERENCE_PER_PIXEL 27.0/160.0*M_PI/180.0 // in radians
60 
61 // Beobot 2.0 pixel to cm road bottom conversion ratio, that is, 1 pic = 9.525mm
62 #define BEOBOT2_PIXEL_TO_MM_ROAD_BOTTOM_RATIO 9.525
63 
64 namespace
65 {
66  // ######################################################################
67  static inline bool coordsOk(int const x, int const y, int const w, int const h)
68  { return (x >= 0 && x < w && y >= 0 && y < h); }
69 
70  // ######################################################################
71  Point2D<float> intersectPoint(Point2D<float> const & p1, Point2D<float> const & p2,
72  Point2D<float> const & p3, Point2D<float> const & p4)
73  {
74  //Find intersection point Algorithm can be find here :
75  //http://paulbourke.net/geometry/lineline2d/
76 
77  double mua,mub;
78  double denom,numera,numerb;
79  double x,y;
80  double EPS = 0.0001;//Epsilon : a small number to enough to be insignificant
81 
82  denom = (p4.j-p3.j) * (p2.i-p1.i) - (p4.i-p3.i) * (p2.j-p1.j);
83  numera = (p4.i-p3.i) * (p1.j-p3.j) - (p4.j-p3.j) * (p1.i-p3.i);
84  numerb = (p2.i-p1.i) * (p1.j-p3.j) - (p2.j-p1.j) * (p1.i-p3.i);
85 
86  /* Are the lines coincident? */
87  if (fabs(numera) < EPS && fabs(numerb) < EPS && fabs(denom) < EPS)
88  {
89  x = (p1.i + p2.i) / 2;
90  y = (p1.j + p2.j) / 2;
91  return Point2D<float>(x,y);
92  }
93 
94  /* Are the lines parallel */
95  if (fabs(denom) < EPS) {
96  x = 0;
97  y = 0;
98  return Point2D<float>(x,y);
99  }
100 
101  /* Is the intersection along the the segments */
102  mua = numera / denom;
103  mub = numerb / denom;
104  if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
105  x = 0;
106  y = 0;
107 
108  }
109  x = p1.i + mua * (p2.i - p1.i);
110  y = p1.j + mua * (p2.j - p1.j);
111 
112  return Point2D<float>(x,y);
113  }
114 
115  // ######################################################################
116  template<typename T>
117  T side(Point2D<T> const & pt1, Point2D<T> const & pt2, Point2D<T> const & pt)
118  {
119  T i = pt.i;
120  T j = pt.j;
121 
122  T Ax = pt1.i; T Ay = pt1.j;
123  T Bx = pt2.i; T By = pt2.j;
124 
125  // 0 means on the line
126  // positive value means to the left
127  // negative value means to the right
128  return (Bx-Ax)*(j-Ay) - (By-Ay)*(i-Ax);
129  }
130 
131  // ######################################################################
132  template<typename T>
133  double distance(Point2D<T> const & pt1, Point2D<T> const & pt2, Point2D<T> const & pt, Point2D<T> & midPt)
134  {
135  double x1 = pt1.i;
136  double y1 = pt1.j;
137 
138  double x2 = pt2.i;
139  double y2 = pt2.j;
140 
141  double x3 = pt.i;
142  double y3 = pt.j;
143 
144  double xi; double yi;
145 
146  if (x1 == x2){ xi = x1; yi = y3; }
147  else if(y1 == y2){ xi = x3; yi = y1; }
148  else
149  {
150  double x2m1 = x2 - x1;
151  double y2m1 = y2 - y1;
152 
153  double x3m1 = x3 - x1;
154  double y3m1 = y3 - y1;
155 
156  double distSq = x2m1*x2m1+y2m1*y2m1;
157  double u = (x3m1*x2m1 + y3m1*y2m1)/distSq;
158 
159  xi = x1 + u*x2m1;
160  yi = y1 + u*y2m1;
161  }
162 
163  midPt = Point2D<T>(xi,yi);
164 
165  double dx = xi - x3;
166  double dy = yi - y3;
167  return pow(dx*dx+dy*dy, .5F);
168  }
169 
170  // ######################################################################
171  template<typename T>
172  double distance(Point2D<T> const & pt1, Point2D<T> const & pt2, Point2D<T> const & pt)
173  {
174  Point2D<T> midPt;
175  return distance(pt1, pt2, pt, midPt);
176  }
177 
178 } // namespace
179 
180 // ######################################################################
181 RoadFinder::RoadFinder(std::string const & instance) :
182  jevois::Component(instance), itsTPXfilter(2, 1, 0), itsKalmanNeedInit(true)
183 {
188 
189  // currently not processing tracker
190  itsTrackingFlag = false;
191 
192  // current accumulated trajectory
195 
196  // indicate how many unique lines have been identified NOTE: never reset
198 
199  // init kalman filter
200  itsTPXfilter.transitionMatrix = (cv::Mat_<float>(2, 2) << 1, 1, 0, 1);
201  cv::setIdentity(itsTPXfilter.measurementMatrix);
202  cv::setIdentity(itsTPXfilter.processNoiseCov, cv::Scalar::all(0.1F * 0.1F));
203  cv::setIdentity(itsTPXfilter.measurementNoiseCov, cv::Scalar::all(2.0F * 2.0F));
204  cv::setIdentity(itsTPXfilter.errorCovPost, cv::Scalar::all(0.1F * 0.1F));
205  itsTPXfilter.statePost.at<float>(1) = 0.0F;
206 }
207 
208 // ######################################################################
210 { }
211 
212 // ######################################################################
214 {
218 }
219 
220 // ######################################################################
222 {
226 }
227 
228 // ######################################################################
229 std::pair<Point2D<int>, float> RoadFinder::getCurrVanishingPoint() const
230 { return std::make_pair(itsVanishingPoint, itsVanishingPointConfidence); }
231 
232 // ######################################################################
234 { return itsCenterPoint; }
235 
236 // ######################################################################
238 { return itsTargetPoint; }
239 
240 // ######################################################################
242 { return itsFilteredTPX; }
243 
244 // ######################################################################
246 {
247  std::lock_guard<std::mutex> _(itsRoadMtx);
248  itsRoadModel.lines.clear();
252  itsRoadModel.numMatches.clear();
253 }
254 
255 // ######################################################################
256 void RoadFinder::process(cv::Mat const & img, jevois::RawImage & visual)
257 {
258  static jevois::Profiler profiler("RoadFinder", 100, LOG_DEBUG);
259  static int currRequestID = 0;
260  ++currRequestID; ///FIXME
261 
262  profiler.start();
263 
264  // Set initial kalman state now that we know image width:
265  if (itsKalmanNeedInit)
266  {
267  itsTPXfilter.statePost.at<float>(0) = img.cols / 2;
268  itsKalmanNeedInit = false;
269  }
270 
271  // If we are just starting, initialize the vanishing point locations:
272  if (itsVanishingPoints.empty())
273  {
274  int const spacing = roadfinder::spacing::get();
275  int const hline = roadfinder::horizon::get();
276  for (int i = -4 * spacing; i <= img.cols + 4 * spacing; i += spacing)
277  itsVanishingPoints.push_back(VanishingPoint(Point2D<int>(i, hline), 0.0F));
278  }
279 
280  // Code from updateMessage() in original code:
281 
282  // Compute Canny edges:
283  int const sobelApertureSize = 7;
284  int const highThreshold = 400 * sobelApertureSize * sobelApertureSize;
285  int const lowThreshold = int(highThreshold * 0.4F);
286  cv::Mat cvEdgeMap;
287  cv::Canny(img, cvEdgeMap, lowThreshold, highThreshold, sobelApertureSize);
288 
289  profiler.checkpoint("Canny done");
290 
291  // Get prior vp while we compute the new one:
292  Point2D<int> prior_vp = itsVanishingPoint;
293 
294  // Track lines in a thread if we have any. Do not access itsCurrentLines or destroy cvEdgeMap until done:
295  std::future<void> track_fut;
296  if (itsCurrentLines.empty() == false)
297  track_fut = std::async(std::launch::async, [&]() {
298  // Track the vanishing lines:
299  trackVanishingLines(cvEdgeMap, itsCurrentLines, visual);
300 
301  // Compute the vanishing point, center point, target point:
302  float confidence; Point2D<int> vp(-1, -1); Point2D<float> cp(-1, -1);
303  Point2D<float> tp = computeRoadCenterPoint(cvEdgeMap, itsCurrentLines, vp, cp, confidence);
304 
305  // update road model:
306  updateRoadModel(itsCurrentLines, currRequestID);
307 
308  itsVanishingPoint = vp;
309  itsCenterPoint = cp;
310  itsTargetPoint = tp;
311  itsVanishingPointConfidence = confidence;
312  });
313  else
314  {
319  }
320 
321  profiler.checkpoint("Tracker launched");
322 
323  // Code from evolve() in original code:
324  computeHoughSegments(cvEdgeMap);
325 
326  profiler.checkpoint("Hough done");
327 
328  std::vector<Line> newLines = computeVanishingLines(cvEdgeMap, prior_vp, visual);
329 
330  profiler.checkpoint("Vanishing lines done");
331 
332  // wait until the tracking thread is done, get the trackers and 'disable' it during project forward
333  if (track_fut.valid()) track_fut.get();
334 
335  profiler.checkpoint("Tracker done");
336 
337  // integrate the two sets of lines
338  itsCurrentLines = combine(newLines, itsCurrentLines, cvEdgeMap.cols, cvEdgeMap.rows);
339 
340  profiler.checkpoint("Combine done");
341 
342  // Do some demo visualization if desired:
343  if (visual.valid())
344  {
345  // find the most likely vanishing point location
346  size_t max_il = 0; float max_l = itsVanishingPoints[max_il].likelihood;
347  size_t max_ip = 0; float max_p = itsVanishingPoints[max_ip].posterior;
348  for (size_t i = 0; i < itsVanishingPoints.size(); ++i)
349  {
350  float const likelihood = itsVanishingPoints[i].likelihood;
351  float const posterior = itsVanishingPoints[i].posterior;
352  if (max_l < likelihood) { max_l = likelihood; max_il = i; }
353  if (max_p < posterior) { max_p = posterior; max_ip = i; }
354  }
355 
356  // draw the vanishing point likelihoods
357  for (size_t i = 0; i < itsVanishingPoints.size(); ++i)
358  {
359  float const likelihood = itsVanishingPoints[i].likelihood;
360  float const posterior = itsVanishingPoints[i].posterior;
361  Point2D<int> const & vp = itsVanishingPoints[i].vp;
362  int l_size = likelihood / max_l * 10;
363  int p_size = posterior / max_l * 10;
364 
365  if (l_size < 2) l_size = 2;
366  if (p_size < 2) p_size = 2;
367 
368  Point2D<int> pt = vp;
369  if (i == max_il) jevois::rawimage::drawDisk(visual, pt.i, pt.j, l_size, jevois::yuyv::LightPink); // orange
370  else jevois::rawimage::drawDisk(visual, pt.i, pt.j, l_size, jevois::yuyv::DarkPink);
371 
372  if (i == max_ip) jevois::rawimage::drawDisk(visual, pt.i, pt.j, p_size, jevois::yuyv::LightGreen);
373  else jevois::rawimage::drawDisk(visual, pt.i, pt.j, p_size, jevois::yuyv::DarkGreen);
374  }
375 
376  // Draw all the segments found:
377  for (Segment const & s : itsCurrentSegments)
378  jevois::rawimage::drawLine(visual, s.p1.i, s.p1.j, s.p2.i, s.p2.j, 0, jevois::yuyv::DarkGrey);
379 
380  // Draw the supporting segments
381  for (Segment const & s : itsVanishingPoints[max_ip].supportingSegments)
382  jevois::rawimage::drawLine(visual, s.p1.i, s.p1.j, s.p2.i, s.p2.j, 1, jevois::yuyv::LightGrey);
383 
384  // Draw current tracked lines
385  for (Line const & line : itsCurrentLines)
386  {
387  unsigned short color = jevois::yuyv::DarkGreen;
388  if (line.start_scores.size() > 0) color = jevois::yuyv::LightGreen;
389  else if (line.scores.size() > 0) color = jevois::yuyv::MedGreen;
390 
391  jevois::rawimage::drawLine(visual, line.horizonPoint.i, line.horizonPoint.j,
392  line.roadBottomPoint.i, line.roadBottomPoint.j, 0, color);
393  jevois::rawimage::drawLine(visual, line.onScreenHorizonSupportPoint.i, line.onScreenHorizonSupportPoint.j,
394  line.onScreenRoadBottomPoint.i, line.onScreenRoadBottomPoint.j, 0, color);
395 
396  // Highlight the segment points:
397  for (Point2D<int> const & p : line.points) jevois::rawimage::drawDisk(visual, p.i, p.j, 1, jevois::yuyv::White);
398  }
399  int slack = 0;
400 
401  // Lateral position information
402  //Point2D<int> vp = itsVanishingPoint;
405 
406  // draw the lateral position point
407  Point2D<int> cp_i(cp.i+slack, cp.j);
408  Point2D<int> tp_i(tp.i+slack, tp.j);
409  Point2D<int> cp_i0(cp.i+slack, cvEdgeMap.rows-20);
410  Point2D<int> tp_i0(tp.i+slack, cvEdgeMap.rows-20);
411  if (cp_i.isValid())
412  jevois::rawimage::drawLine(visual, cp_i0.i, cp_i0.j, cp_i.i, cp_i.j, 2, jevois::yuyv::LightGreen);
413  if (tp_i.isValid())
414  jevois::rawimage::drawLine(visual, tp_i0.i, tp_i0.j, tp_i.i, tp_i.j, 2, jevois::yuyv::LightPink);
415  }
416 
417  // Filter the target point: Predict:
418  cv::Mat prediction = itsTPXfilter.predict();
419 
420  // Update:
421  if (itsTargetPoint.isValid())
422  {
423  cv::Mat measurement = (cv::Mat_<float>(1, 1) << itsTargetPoint.i);
424  cv::Mat estimated = itsTPXfilter.correct(measurement);
425  itsFilteredTPX = estimated.at<float>(0);
426  }
427  else itsFilteredTPX = prediction.at<float>(0);
428 
429  profiler.stop();
430 }
431 
432 //######################################################################
433 Point2D<float> RoadFinder::computeRoadCenterPoint(cv::Mat const & edgeMap, std::vector<Line> & lines,
434  Point2D<int> & vanishing_point,
435  Point2D<float> & road_center_point, float & confidence)
436 {
437  itsRoadMtx.lock();
438  Point2D<int> prev_vanishing_point = itsVanishingPoint;
439  std::vector<bool> vanishingPointStability = itsVanishingPointStability;
440  itsRoadMtx.unlock();
441 
442  Point2D<float> target_point;
443 
444  size_t num_healthy_lines = 0;
445  size_t num_new_lines = 0;
446  size_t num_noisy_lines = 0;
447  size_t num_healthy_active = 0;
448  int const width = edgeMap.cols; int const height = edgeMap.rows;
449  int const horiline = roadfinder::horizon::get();
450 
451  for (Line const & line : lines)
452  {
453  if (line.scores.size() > 0) ++num_noisy_lines;
454  else if (line.start_scores.size() > 0) ++num_new_lines;
455  else { ++num_healthy_lines; if (line.isActive) ++num_healthy_active; }
456  }
457 
458  if (num_healthy_lines == 0 && num_new_lines == 0 && num_healthy_lines == 0)
459  {
460  vanishing_point = prev_vanishing_point;
461  road_center_point = Point2D<float>(width/2, height-1);
462  target_point = road_center_point;
463  confidence = .4;
464  }
465  else
466  {
467  std::vector<Line> temp_lines; float weight = 0.0;
468  if (num_healthy_lines > 0)
469  {
470  for (Line const & line : lines)
471  if (line.scores.size() == 0 && line.start_scores.size() == 0) temp_lines.push_back(line);
472  vanishing_point = getVanishingPoint(temp_lines, weight);
473  }
474  else if (num_new_lines > 0)
475  {
476  for (Line const & line : lines) if (line.start_scores.size() > 0) temp_lines.push_back(line);
477  vanishing_point = getVanishingPoint(temp_lines, weight);
478  weight -= .1;
479  }
480  else if (num_noisy_lines > 0)
481  {
482  for (Line const & line : lines) if (line.scores.size() > 0) temp_lines.push_back(line);
483  vanishing_point = getVanishingPoint(temp_lines, weight);
484  weight -= .2;
485  }
486 
487  road_center_point = Point2D<float>(width/2, height-1);
488  target_point = road_center_point;
489  confidence = weight;
490  }
491 
492  if (num_healthy_lines > 0 && num_healthy_active == 0)
493  {
494  // reset the angle to the center lines
495  for (Line & line : lines)
496  {
497  if (line.scores.size() == 0 && line.start_scores.size() == 0)
498  {
499  line.isActive = true;
500  line.angleToCenter = M_PI/2 - line.angle;
501  line.pointToServo = line.onScreenRoadBottomPoint;
502  line.offset = 0.0f;
503  }
504  }
505  }
506 
507  // get the average active center angle
508  float total_weight = 0.0f;
509  float total_angle = 0.0f;
510  int num_a_line = 0;
511 
512  float total_curr_offset = 0.0f;
513  for (Line const & line : lines)
514  if (line.scores.size() == 0 && line.start_scores.size() == 0 && line.isActive)
515  {
516  num_a_line++;
517 
518  // compute angle to road center
519  float angle = line.angle;
520  float cangle = line.angleToCenter;
521  float weight = line.score;
522 
523  float ccangle = cangle + angle;
524 
525  total_angle += ccangle * weight;
526  total_weight += weight;
527 
528  // compute center point
529  float dist = height - horiline;
530  float tpi = 0.0;
531  float cos_ang = cos(M_PI/2 - ccangle);
532  if (cos_ang != 0.0F) tpi = dist / cos_ang * sin(M_PI/2 - ccangle);
533  tpi += vanishing_point.i;
534 
535  // compute offset from point to servo
536  float c_offset = 0.0f;
537 
538  Point2D<float> c_pt = line.onScreenRoadBottomPoint;
539  Point2D<float> pt_ts = line.pointToServo;
540  Point2D<float> h_pt = line.onScreenHorizonPoint;
541  float offset = line.offset;
542  if (fabs(pt_ts.i) < 0.05F || fabs(pt_ts.i - width) < 0.05F)
543  {
544  // figure out where the point would be for the specified 'j' component
545  Point2D<float> h1(0 , pt_ts.j);
546  Point2D<float> h2(width, pt_ts.j);
547  c_pt = intersectPoint(h_pt, c_pt, h1, h2);
548  }
549  c_offset = c_pt.i - pt_ts.i;
550  total_curr_offset += (c_offset + offset) * weight;
551  }
552 
553  // calculate confidence
554  float avg_weight = 0.0F;
555  if (num_a_line > 0) avg_weight = total_weight / num_a_line;
556  confidence = avg_weight;
557  if (avg_weight == 0) return road_center_point;
558 
559  // angle-based road center estimation
560  float avg_angle = 0.0F;
561  if (total_weight > 0) avg_angle = total_angle / total_weight;
562  float dist = height - horiline;
563  float tpi = 0.0;
564  float cos_ang = cos(M_PI/2 - avg_angle);
565  if (fabs(cos_ang) > 0.001F) tpi = dist / cos_ang * sin(M_PI/2 - avg_angle);
566  tpi += float(width) / 2.0F;
567  target_point = Point2D<float>(tpi, height-1);
568 
569  // offset point-based road center estimation
570  float avg_offset = 0.0F;
571  if (total_weight > 0) avg_offset = total_curr_offset / total_weight;
572  float cpi = float(width) / 2.0F + avg_offset;
573  road_center_point = Point2D<float>(cpi, height-1);
574 
575  // if at least one line is active activate all the healthy but inactive lines
576  for (Line & line : lines)
577  {
578  if (avg_weight > 0.0 && line.scores.size() == 0 && line.start_scores.size() == 0 && !line.isActive)
579  {
580  // calculate angle
581  float angle = line.angle;
582  float ccangle = avg_angle - angle;
583  line.angleToCenter = ccangle;
584  line.pointToServo = line.onScreenRoadBottomPoint;
585  line.offset = avg_offset;
586  line.isActive = true;
587  }
588  }
589 
590  return road_center_point;
591 }
592 
593 //######################################################################
594 void RoadFinder::updateRoadModel(std::vector<Line> & lines, int index)
595 {
596  size_t n_road_lines = itsRoadModel.lines.size();
597  size_t n_in_lines = lines.size();
598 
599  // update only on the healthy input lines
600  std::vector<bool> is_healthy(n_in_lines);
601  for (size_t i = 0; i < n_in_lines; ++i)
602  is_healthy[i] = (lines[i].scores.size() == 0 && lines[i].start_scores.size() == 0);
603 
604  std::vector<int> road_match_index(n_road_lines, -1);
605  std::vector<int> in_match_index(n_in_lines, -1);
606 
607  std::vector<std::vector<float> > match_dists;
608  for (size_t i = 0; i < n_in_lines; ++i) match_dists.push_back(std::vector<float>(n_road_lines));
609 
610  // go through each input and road line combination to get the match score which is a simple closest point proximity
611  for (size_t i = 0; i < n_in_lines; ++i)
612  {
613  if (!is_healthy[i]) continue;
614 
615  Point2D<float> ipt = lines[i].onScreenRoadBottomPoint;
616  Point2D<float> hpt = lines[i].horizonPoint;
617 
618  for (size_t j = 0; j < n_road_lines; ++j)
619  {
622 
623  float dist = lsl.distance(ipt);
624  float hdist = hpt.distance(lshpt);
625 
626  if (hdist > 50) match_dists[i][j] = dist + hdist;
627  else match_dists[i][j] = dist;
628  }
629  }
630 
631  // calculate the best match and add it
632  for (size_t i = 0; i < n_in_lines; ++i)
633  {
634  if (!is_healthy[i]) continue;
635 
636  // get the (best and second best) match and scores
637  int m1_index = -1; float m1_dist = -1.0;
638  int m2_index = -1; float m2_dist = -1.0;
639  for (size_t j = 0; j < n_road_lines; ++j)
640  {
641  if (road_match_index[j] != -1) continue;
642 
643  float dist = match_dists[i][j];
644  if (m1_index == -1 || dist < m1_dist)
645  {
646  m2_index = m1_index; m2_dist = m1_dist;
647  m1_index = j ; m1_dist = dist;
648  }
649  else if (m2_index == -1 || dist < m2_dist)
650  {
651  m2_index = j ; m2_dist = dist;
652  }
653  }
654 
655  // get the best match for road with many evidences
656  int ml1_index = -1; float ml1_dist = -1.0;
657  int ml2_index = -1; float ml2_dist = -1.0;
658  for (size_t j = 0; j < n_road_lines; ++j)
659  {
660  int nmatch = itsRoadModel.numMatches[j];
661  if (road_match_index[j] != -1 || nmatch < 10) continue;
662 
663  float dist = match_dists[i][j];
664  if (ml1_index == -1 || dist < ml1_dist)
665  {
666  ml2_index = ml1_index; ml2_dist = ml1_dist;
667  ml1_index = j ; ml1_dist = dist;
668  }
669  else if (ml2_index == -1 || dist < ml2_dist)
670  {
671  ml2_index = j ; ml2_dist = dist;
672  }
673  }
674 
675  // if there first
676  int j = -1;
677 
678  // check the large matches first
679  if (ml1_dist != -1.0F)
680  if (ml1_dist < 15.0F || ((ml2_dist != -1.0 || ml1_dist/ml2_dist < .1) && ml1_dist < 30.0F))
681  j = ml1_index;
682 
683  // then the smaller matches
684  if (j == -1 && m1_dist != -1.0F)
685  if (m1_dist < 5.0F || ((m2_dist != -1.0 || m1_dist/m2_dist < .1) && m1_dist < 20.0F))
686  j = m1_index;
687 
688  if (j != -1)
689  {
690  road_match_index[j] = i;
691  in_match_index[i] = j;
692 
693  // use the model parameters
694  lines[i].angleToCenter = itsRoadModel.lines[j].angleToCenter;
695  lines[i].pointToServo = itsRoadModel.lines[j].pointToServo;
696  lines[i].offset = itsRoadModel.lines[j].offset;
697  lines[i].index = itsRoadModel.lines[j].index;
698  lines[i].isActive = true;
699 
700  // update the road model
701  Point2D<float> hpt = lines[i].horizonPoint;
702  Point2D<float> ipt = lines[i].onScreenRoadBottomPoint;
703 
706  itsRoadModel.lastActiveIndex[j] = index;
707 
708  int nmatch = itsRoadModel.numMatches[j];
709  itsRoadModel.numMatches[j] = nmatch+1;
710  }
711  }
712 
713  // delete inactive road model lines
714  std::vector<Line>::iterator l_itr = itsRoadModel.lines.begin();
715  std::vector<Point2D<float> >::iterator h_itr = itsRoadModel.lastSeenHorizonPoint.begin();
716  std::vector<Point2D<float> >::iterator p_itr = itsRoadModel.lastSeenLocation.begin();
717  std::vector<int>::iterator i_itr = itsRoadModel.lastActiveIndex.begin();
718  std::vector<int>::iterator n_itr = itsRoadModel.numMatches.begin();
719 
720  while (l_itr != itsRoadModel.lines.end())
721  {
722  int lindex = *i_itr;
723  if (index > lindex+300)
724  {
725  l_itr = itsRoadModel.lines.erase(l_itr);
726  h_itr = itsRoadModel.lastSeenHorizonPoint.erase(h_itr);
727  p_itr = itsRoadModel.lastSeenLocation.erase(p_itr);
728  i_itr = itsRoadModel.lastActiveIndex.erase(i_itr);
729  n_itr = itsRoadModel.numMatches.erase(n_itr);
730  }
731  else { l_itr++; p_itr++; i_itr++; n_itr++; }
732  }
733 
734  n_road_lines = itsRoadModel.lines.size();
735 
736  // add all the lines not yet added to the road model
737  for (size_t i = 0; i < n_in_lines; ++i)
738  {
739  if (!is_healthy[i]) continue;
740  if (in_match_index[i] != -1) continue;
741 
742  lines[i].index = itsNumIdentifiedLines++;
743 
744  Point2D<float> hpt = lines[i].horizonPoint;
745  Point2D<float> ipt = lines[i].onScreenRoadBottomPoint;
746 
747  // update the road model
748  itsRoadModel.lines.push_back(lines[i]);
749  itsRoadModel.lastSeenHorizonPoint.push_back(hpt);
750  itsRoadModel.lastSeenLocation.push_back(ipt);
751  itsRoadModel.lastActiveIndex.push_back(index);
752  itsRoadModel.numMatches.push_back(1);
753  }
754 }
755 
756 //######################################################################
757 Point2D<int> RoadFinder::getVanishingPoint(std::vector<Line> const & lines, float & confidence)
758 {
759  // get the horizon points, do a weighted average
760  float total_weight = 0.0F;
761  float total_hi = 0.0F;
762  int num_hi = 0;
763  for (Line const & l : lines)
764  {
765  float hi = l.horizonPoint.i;
766  float weight = l.score;
767 
768  total_hi += hi*weight;
769  total_weight += weight;
770  num_hi++;
771  }
772 
773  float wavg_hi = 0.0F;
774  float avg_weight = 0.0F;
775  if (num_hi > 0)
776  {
777  wavg_hi = total_hi / total_weight;
778  avg_weight = total_weight / num_hi;
779  }
780  confidence = avg_weight;
781 
782  return Point2D<int>(wavg_hi, roadfinder::horizon::get());
783 }
784 
785 //######################################################################
787 {
788  int const threshold = 10, minLineLength = 5, maxGap = 2;
789  std::vector<cv::Vec4i> segments;
790  cv::HoughLinesP(cvImage, segments, 1, CV_PI/180, threshold, minLineLength, maxGap);
791 
792  itsCurrentSegments.clear();
793 
794  for (cv::Vec4i const & seg : segments)
795  {
796  Point2D<int> pt1(seg[0], seg[1]);
797  Point2D<int> pt2(seg[2], seg[3]);
798 
799  int dx = pt2.i - pt1.i;
800  int dy = pt2.j - pt1.j;
801 
802  float length = pow(dx * dx + dy * dy, .5);
803  float angle = atan2(dy, dx) * 180.0F /M_PI;
804 
805  int horizon_y = roadfinder::horizon::get();
806  int horizon_s_y = horizon_y + roadfinder::support::get();
807  bool good_horizon_support =
808  (pt1.j > horizon_y && pt2.j > horizon_y) && (pt1.j > horizon_s_y || pt2.j > horizon_s_y);
809 
810  bool non_vertical = !((angle > 80.0F && angle < 100.0F) || (angle < -80.0F && angle > -100.0F));
811 
812  if (length > 5.0F && good_horizon_support && non_vertical)
813  itsCurrentSegments.push_back(Segment(pt1, pt2, angle, length));
814  }
815 }
816 
817 //######################################################################
818 std::vector<Line>
819 RoadFinder::computeVanishingLines(cv::Mat const & edgeMap, Point2D<int> const & vanishingPoint,
820  jevois::RawImage & visual)
821 {
822  int const horiline = roadfinder::horizon::get();
823  int const vpdt = roadfinder::distthresh::get();
824 
825  Point2D<float> h1(0, horiline);
826  Point2D<float> h2(edgeMap.cols, horiline);
827 
828  uint num_vp = itsVanishingPoints.size();
829  std::vector<float> curr_vp_likelihood(num_vp, 0.0F);
830  std::vector<std::vector<uint> > curr_vp_support(num_vp);
831 
832  for(size_t j = 0; j < itsCurrentSegments.size(); j++)
833  {
834  Segment const & s = itsCurrentSegments[j];
835  Point2D<float> p1(s.p1);
836  Point2D<float> p2(s.p2);
837  if (p2.j > p1.j)
838  {
839  p1 = Point2D<float>(s.p2.i, s.p2.j);
840  p2 = Point2D<float>(s.p1.i, s.p1.j);
841  }
842 
843  float length = s.length;
844 
845  // compute intersection to vanishing point vertical
846  Point2D<float> p_int = intersectPoint(p1, p2, h1, h2);
847 
848  // for each vanishing point
849  for (size_t i = 0; i < itsVanishingPoints.size(); ++i)
850  {
851  Point2D<int> const & vp = itsVanishingPoints[i].vp;
852  int p_int_i = int(p_int.i);
853  if (!((p1.i <= p2.i && p2.i <= p_int_i && p_int_i <= vp.i+10) ||
854  (p1.i >= p2.i && p2.i >= p_int_i && p_int_i >= vp.i-10) ))
855  continue;
856 
857  float dist = p_int.distance(Point2D<float>(vp.i, vp.j));
858  float d_val = 1.0 - dist / vpdt;
859  if (d_val > 0.0F)
860  {
861  curr_vp_support[i].push_back(j);
862 
863  // accumulate likelihood values
864  curr_vp_likelihood[i] += d_val*length;
865  }
866  }
867  }
868 
869  // integrate with previous values: FIXXX
870  for (size_t i = 0; i < itsVanishingPoints.size(); ++i)
871  {
872  Point2D<int> const & vp = itsVanishingPoints[i].vp;
873 
874  float likelihood = curr_vp_likelihood[i];
875  itsVanishingPoints[i].likelihood = likelihood;
876 
877  // compute prior
878  float prior = 0.1;
879  if (!(vanishingPoint.i == -1 && vanishingPoint.j == -1))
880  {
881  float di = fabs(vp.i - vanishingPoint.i);
882  prior = 1.0 - di / (edgeMap.cols / 4);
883  if (prior < .1) prior = .1;
884  }
885 
886  itsVanishingPoints[i].prior = prior;
887  itsVanishingPoints[i].posterior = prior*likelihood;
888 
889  itsVanishingPoints[i].supportingSegments.clear();
890 
891  for (uint j = 0; j < curr_vp_support[i].size(); ++j)
892  itsVanishingPoints[i].supportingSegments.push_back(itsCurrentSegments[curr_vp_support[i][j]]);
893  }
894 
895  uint max_i = 0;
896  float max_p = itsVanishingPoints[max_i].posterior;
897  for (uint i = 0; i < itsVanishingPoints.size(); ++i)
898  {
899  float posterior = itsVanishingPoints[i].posterior;
900  if (max_p < posterior) { max_p = posterior; max_i = i; }
901  }
902 
903  // create vanishing lines
904 
905  // sort the supporting segments on length
906  std::list<Segment> supporting_segments;
907  uint n_segments = itsVanishingPoints[max_i].supportingSegments.size();
908  for (uint i = 0; i < n_segments; ++i) supporting_segments.push_back(itsVanishingPoints[max_i].supportingSegments[i]);
909  supporting_segments.sort();
910  supporting_segments.reverse();
911 
912  std::vector<Line> current_lines;
913  std::vector<bool> is_used(n_segments, false);
914 
915  // create lines
916  std::list<Segment>::iterator itr = supporting_segments.begin(), stop = supporting_segments.end();
917  uint index = 0;
918  while (itr != stop)
919  {
920  Segment const & s2 = *itr;
921  itr++;
922 
923  if (is_used[index]){ index++; continue; }
924  is_used[index] = true;
925  index++;
926 
927  // find other segments with this angle
928  float total_length = 0.0F; uint num_segments = 0;
929  Line l = findLine2(s2, edgeMap, supporting_segments, is_used, total_length, num_segments);
930 
931  // check for line fitness
933  Point2D<float> const & osrbp = l.onScreenRoadBottomPoint;
934 
935  Point2D<int> hpt(oshsp + .5);
936  Point2D<int> rpt(osrbp + .5);
937 
938  float score = getLineFitness(hpt, rpt, edgeMap, visual);
939  l.score = score;
940  l.start_scores.push_back(score);
941  if (score >= .5) current_lines.push_back(l);
942  }
943 
944  // save the vanishing point
945  itsRoadMtx.lock();
947  itsRoadMtx.unlock();
948 
949  return current_lines;
950 }
951 
952 // ######################################################################
953 std::vector<Point2D<int> >
954 RoadFinder::getPixels(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap)
955 {
956  std::vector<uint> startIndexes;
957  return getPixels(p1, p2, edgeMap, startIndexes);
958 }
959 
960 // ######################################################################
961 std::vector<Point2D<int> >
962 RoadFinder::getPixels(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap,
963  std::vector<uint> & startIndexes)
964 {
965  std::vector<Point2D<int> > points;
966 
967  // from Graphics Gems / Paul Heckbert
968  const int w = edgeMap.cols, h = edgeMap.rows;
969  int dx = p2.i - p1.i, ax = abs(dx) << 1, sx = dx < 0 ? -1 : 1;
970  int dy = p2.j - p1.j, ay = abs(dy) << 1, sy = dy < 0 ? -1 : 1;
971  int x = p1.i, y = p1.j;
972 
973  // flag to start new segment for the next hit
974  bool start_segment = true; startIndexes.clear();
975 
976  if (ax > ay)
977  {
978  int d = ay - (ax >> 1);
979  for (;;)
980  {
981  bool adding = false;
982 
983  if (coordsOk(x, y, w, h))
984  {
985  if (edgeMap.at<byte>(y, x) > 0)
986  { points.push_back(Point2D<int>(x,y)); adding = true; }
987  else if (points.size() > 0)
988  {
989  uint size = points.size();
990  Point2D<int> ppt = points[size-1];
991 
992  // get points that are neighbors of the previous point
993  for (int di = 0; di <= 1; di++)
994  for (int dj = 0; dj <= 1; dj++)
995  {
996  if (di == 0 && dj == 0) continue;
997  if (!coordsOk(x+di, y+dj, w, h)) continue;
998 
999  if (edgeMap.at<byte>(y+dj, x+di) && abs((x+di)-ppt.i) <= 1 && abs((y+dj)-ppt.j) <= 1)
1000  { points.push_back(Point2D<int>(x+di, y+dj)); adding = true; }
1001  }
1002  }
1003 
1004  if (start_segment && adding) { startIndexes.push_back(points.size()-1); start_segment = false; }
1005  else if (!start_segment && !adding && Point2D<int>(x, y).distance(points[points.size()-1]) > 1.5)
1006  start_segment = true;
1007  }
1008 
1009  if (x == p2.i) break;
1010  if (d >= 0) { y += sy; d -= ax; }
1011  x += sx; d += ay;
1012  }
1013  }
1014  else
1015  {
1016  int d = ax - (ay >> 1);
1017  for (;;)
1018  {
1019  bool adding = false;
1020 
1021  if (x >= 0 && x < w && y >= 0 && y < h)
1022  {
1023  if (edgeMap.at<byte>(y, x) > 0)
1024  { points.push_back(Point2D<int>(x,y)); adding = true; }
1025  else if (points.size() > 0)
1026  {
1027  uint size = points.size();
1028  Point2D<int> ppt = points[size-1];
1029 
1030  // get points that are neighbors of the previous point
1031  for (int di = 0; di <= 1; di++)
1032  for (int dj = 0; dj <= 1; dj++)
1033  {
1034  if (di == 0 && dj == 0) continue;
1035  if (!coordsOk(x+di, y+dj, w, h)) continue;
1036 
1037  if (edgeMap.at<byte>(y+dj, x+di) && abs((x+di)-ppt.i) <= 1 && abs((y+dj)-ppt.j) <= 1)
1038  { points.push_back(Point2D<int>(x+di,y+dj)); adding = true; }
1039  }
1040  }
1041 
1042  if (start_segment && adding) { startIndexes.push_back(points.size()-1); start_segment = false; }
1043  else if (!start_segment && !adding && Point2D<int>(x, y).distance(points[points.size()-1]) > 1.5)
1044  start_segment = true;
1045  }
1046 
1047  if (y == p2.j) break;
1048  if (d >= 0) { x += sx; d -= ay; }
1049  y += sy; d += ax;
1050  }
1051  }
1052  return points;
1053 }
1054 
1055 // ######################################################################
1056 std::vector<Point2D<int> >
1057 RoadFinder::getPixelsQuick(Point2D<int> const & p1, Point2D<int> const & p2, cv::Mat const & edgeMap)
1058 {
1059  std::vector<Point2D<int> > points;
1060 
1061  // from Graphics Gems / Paul Heckbert
1062  const int w = edgeMap.cols, h = edgeMap.rows;
1063  int dx = p2.i - p1.i, ax = abs(dx) << 1, sx = dx < 0 ? -1 : 1;
1064  int dy = p2.j - p1.j, ay = abs(dy) << 1, sy = dy < 0 ? -1 : 1;
1065  int x = p1.i, y = p1.j;
1066 
1067  // flag to start new segment for the next hit
1068  if (ax > ay)
1069  {
1070  int d = ay - (ax >> 1);
1071  for (;;)
1072  {
1073  if (x >= 0 && x < w && y >= 0 && y < h && edgeMap.at<byte>(y, x) > 0) points.push_back(Point2D<int>(x, y));
1074  if (x == p2.i) break; else if (d >= 0) { y += sy; d -= ax; }
1075  x += sx; d += ay;
1076  }
1077  }
1078  else
1079  {
1080  int d = ax - (ay >> 1);
1081  for (;;)
1082  {
1083  if (x >= 0 && x < w && y >= 0 && y < h && edgeMap.at<byte>(y, x) > 0) points.push_back(Point2D<int>(x, y));
1084  if (y == p2.j) break; else if (d >= 0) { x += sx; d -= ay; }
1085  y += sy; d += ax;
1086  }
1087  }
1088  return points;
1089 }
1090 
1091 
1092 // ######################################################################
1093 Line RoadFinder::findLine2(Segment const & s, cv::Mat const & edgeMap, std::list<Segment> const & supportingSegments,
1094  std::vector<bool> & is_used, float & totalLength, uint & numSegments)
1095 {
1096  Point2D<int> const & p1 = s.p1; Point2D<int> const & p2 = s.p2;
1097 
1098  Line l; l.segments.push_back(s); l.length = s.length;
1099  std::vector<Point2D<int> > points = getPixels(p1, p2, edgeMap);
1100 
1101  float const distance_threshold = 7.0F; float const distance_threshold2 = 5.0F;
1102 
1103  // find points within distance
1104  size_t index = 0; totalLength = s.length; numSegments = 1;
1105 
1106  std::list<Segment>::const_iterator itr = supportingSegments.begin();
1107  for (size_t i = 0; i < is_used.size(); ++i)
1108  {
1109  Segment const & s2 = (*itr);
1110  Point2D<int> const & p2_1 = s2.p1; Point2D<int> const & p2_2 = s2.p2; float const length = s2.length;
1111  ++itr; ++index;
1112 
1113  if (is_used[i]) continue;
1114 
1115  int mid_left_count = 0, mid_right_count = 0;
1116  bool is_inline = true, is_close_inline = true;
1117  std::vector<Point2D<int> > curr_points = getPixels(p2_1, p2_2, edgeMap);
1118 
1119  for (size_t j = 0; j < curr_points.size(); ++j)
1120  {
1121  float const dist = distance(p1, p2, curr_points[j]);
1122  int const wsid = side(p1, p2, curr_points[j]);
1123 
1124  if (wsid <= 0) ++mid_left_count;
1125  if (wsid >= 0) ++mid_right_count;
1126  if (dist > distance_threshold2) { is_close_inline = false; }
1127  if (dist > distance_threshold) { is_inline = false; j = curr_points.size(); }
1128  }
1129 
1130  // include
1131  if (is_close_inline || (is_inline && mid_left_count >= 2 && mid_right_count >= 2))
1132  {
1133  for (size_t j = 0; j < curr_points.size(); ++j) points.push_back(curr_points[j]);
1134  is_used[i] = true; totalLength += length; ++numSegments;
1135 
1136  std::vector<Point2D<int> > curr_points2 = getPixels(p2_1, p2_2, edgeMap);
1137  for (size_t j = 0; j < curr_points2.size(); ++j) points.push_back(curr_points2[j]);
1138  }
1139  }
1140 
1141  updateLine(l, points, totalLength, edgeMap.cols, edgeMap.rows);
1142  Point2D<float> const & point1 = l.onScreenHorizonSupportPoint;
1143  Point2D<float> const & point2 = l.onScreenRoadBottomPoint;
1144  float dist = point1.distance(point2);
1145  float score = l.score / dist; // FIXME div by zero??
1146  l.score = score;
1147  return l;
1148 }
1149 
1150 // ######################################################################
1151 void RoadFinder::updateLine(Line & l, std::vector<Point2D<int> > const & points, float score,
1152  int const width, int const height)
1153 {
1154  if (points.empty()) { l.score = -1.0F; return; }
1155 
1156  int const horiline = roadfinder::horizon::get();
1157  int const horisupp = horiline + roadfinder::support::get();
1158 
1159  // fit a line using all the points
1160  Point2D<float> lp1, lp2; fitLine(points, lp1, lp2, width, height);
1161  l.points = points;
1162  l.score = score;
1163 
1164  Point2D<float> tr1 = intersectPoint(lp1, lp2, Point2D<float>(0, horiline), Point2D<float>(width, horiline));
1165  Point2D<float> tr2 = intersectPoint(lp1, lp2, Point2D<float>(0, height-1), Point2D<float>(width, height-1));
1166  Point2D<float> tr3 = intersectPoint(lp1, lp2, Point2D<float>(0, horisupp), Point2D<float>(width, horisupp));
1167 
1168  l.horizonPoint = tr1;
1169  l.horizonSupportPoint = tr3;
1170  l.roadBottomPoint = tr2;
1171 
1172  if (tr2.i >= 0 && tr2.i <= width) l.onScreenRoadBottomPoint = tr2;
1173  else if (tr2.i < 0)
1174  l.onScreenRoadBottomPoint = intersectPoint(lp1, lp2, Point2D<float>(0, 0), Point2D<float>(0,height));
1175  else if (tr2.i > width)
1176  l.onScreenRoadBottomPoint = intersectPoint(lp1, lp2, Point2D<float>(width, 0), Point2D<float>(width,height));
1177 
1178  if (tr1.i >= 0 && tr1.i <= width) l.onScreenHorizonPoint = tr1;
1179  else if (tr1.i < 0)
1180  l.onScreenHorizonPoint = intersectPoint(lp1, lp2, Point2D<float>(0, 0), Point2D<float>(0,height));
1181  else if (tr1.i > width)
1182  l.onScreenHorizonPoint = intersectPoint(lp1, lp2, Point2D<float>(width, 0), Point2D<float>(width,height));
1183 
1184  if (tr3.i >= 0 && tr3.i <= width) l.onScreenHorizonSupportPoint = tr3;
1185  else if (tr3.i < 0)
1186  l.onScreenHorizonSupportPoint = intersectPoint(lp1, lp2, Point2D<float>(0, 0), Point2D<float>(0,height));
1187  else if (tr3.i > width)
1188  l.onScreenHorizonSupportPoint = intersectPoint(lp1, lp2, Point2D<float>(width, 0), Point2D<float>(width,height));
1189 
1190  Point2D<float> const & p1 = l.horizonPoint;
1191  Point2D<float> const & p2 = l.roadBottomPoint;
1192 
1193  float dy = p2.j - p1.j;
1194  float dx = p2.i - p1.i;
1195 
1196  // set it to 0 to M_PI
1197  float angle = atan2(dy, dx);
1198  if (angle < 0.0) angle = M_PI + angle;
1199 
1200  l.angle = angle;
1201 }
1202 
1203 // ######################################################################
1204 void RoadFinder::fitLine(std::vector<Point2D<int> > const & points, Point2D<float> & p1,Point2D<float> & p2,
1205  int const width, int const height)
1206 {
1207  float line[4];
1208 
1209  CvPoint* cvPoints = (CvPoint*)malloc(points.size() * sizeof(Point2D<int>));
1210  for (size_t i = 0; i < points.size(); ++i) { cvPoints[i].x = points[i].i; cvPoints[i].y = points[i].j; }
1211 
1212  {
1213  CvMat point_mat = cvMat(1, points.size(), CV_32SC2, cvPoints);
1214  cvFitLine(&point_mat, CV_DIST_L2, 0, 0.01, 0.01, line);
1215  }
1216 
1217  free(cvPoints);
1218 
1219  float const d = sqrtf(line[0]*line[0] + line[1]*line[1]);
1220  line[0] /= d; line[1] /= d;
1221 
1222  float const t = width + height;
1223  p1.i = line[2] - line[0] * t;
1224  p1.j = line[3] - line[1] * t;
1225  p2.i = line[2] + line[0] * t;
1226  p2.j = line[3] + line[1] * t;
1227 }
1228 
1229 // ######################################################################
1230 float RoadFinder::getLineFitness(Point2D<int> const & horizonPoint, Point2D<int> const & roadBottomPoint,
1231  cv::Mat const & edgeMap, jevois::RawImage & visual)
1232 {
1233  std::vector<Point2D<int> > points;
1234  return getLineFitness(horizonPoint, roadBottomPoint, edgeMap, points, visual);
1235 }
1236 
1237 // ######################################################################
1238 float RoadFinder::getLineFitness(Point2D<int> const & horizonPoint, Point2D<int> const & roadBottomPoint,
1239  cv::Mat const & edgeMap, std::vector<Point2D<int> > & points,
1240  jevois::RawImage & visual)
1241 {
1242  float score = 0;
1243  int min_effective_segment_size = 5;
1244 
1245  // go through the points in the line
1246  Point2D<int> p1 = horizonPoint;
1247  Point2D<int> p2 = roadBottomPoint;
1248  float dist = p1.distance(p2);
1249 
1250  points.clear();
1251  std::vector<uint> start_indexes;
1252  points = getPixels(p1, p2, edgeMap, start_indexes);
1253 
1254  int sp = 4;
1255  std::vector<Point2D<int> > lpoints = getPixelsQuick(p1+Point2D<int>(-sp,0), p2+Point2D<int>(-sp,0), edgeMap);
1256  std::vector<Point2D<int> > rpoints = getPixelsQuick(p1+Point2D<int>(sp,0), p2+Point2D<int>(sp,0), edgeMap);
1257 
1258  uint num_segments = start_indexes.size();
1259  float max_length = 0.0F;
1260  int total = 0; int max = 0;
1261  float eff_length = 0;
1262  for (uint i = 1; i < num_segments; ++i)
1263  {
1264  int size = start_indexes[i] - start_indexes[i-1];
1265  if (max < size) max = size;
1266  total += size;
1267 
1268  uint i1 = start_indexes[i-1];
1269  uint i2 = start_indexes[i ]-1;
1270  Point2D<int> pt1 = points[i1];
1271  Point2D<int> pt2 = points[i2];
1272  float length = pt1.distance(pt2);
1273  if (max_length < length) max_length = length;
1274 
1275  if (size >= min_effective_segment_size) eff_length+= length;
1276  }
1277 
1278  if (num_segments > 0)
1279  {
1280  int size = int(points.size()) - int(start_indexes[num_segments-1]);
1281 
1282  if (max < size) max = size;
1283  total += size;
1284 
1285  uint i1 = start_indexes[num_segments-1 ];
1286  uint i2 = points.size()-1;
1287 
1288  Point2D<int> pt1 = points[i1];
1289  Point2D<int> pt2 = points[i2];
1290  float length = pt1.distance(pt2);
1291 
1292  if (max_length < length) max_length = length;
1293 
1294  if (size >= min_effective_segment_size) eff_length+= length;
1295  }
1296 
1297  if (max_length > 0.0)
1298  {
1299  uint lsize = lpoints.size();
1300  uint rsize = rpoints.size();
1301 
1302  // can't be bigger than 15 degrees or bad point
1303  if (dist <= 50.0 || points.size() < 2*(lsize+rsize)) score = 0.0;
1304  else score = eff_length/dist;
1305  }
1306 
1307  if (visual.valid())
1308  {
1309  jevois::rawimage::drawLine(visual, p1.i, p1.j, p2.i, p2.j, 0, 0x80ff);
1310  for (Point2D<int> const & p : points) jevois::rawimage::drawDisk(visual, p.i, p.j, 1, jevois::yuyv::LightTeal);
1311  for (Point2D<int> const & p : lpoints) jevois::rawimage::drawDisk(visual, p.i, p.j, 1, jevois::yuyv::LightTeal);
1312  for (Point2D<int> const & p : rpoints) jevois::rawimage::drawDisk(visual, p.i, p.j, 1, jevois::yuyv::LightTeal);
1313  }
1314  return score;
1315 }
1316 
1317 // ######################################################################
1318 void RoadFinder::trackVanishingLines(cv::Mat const & edgeMap, std::vector<Line> & currentLines,
1319  jevois::RawImage & visual)
1320 {
1321  for (Line & line : currentLines)
1322  {
1323  Point2D<int> pi1(line.onScreenHorizonSupportPoint + 0.5F);
1324  Point2D<int> pi2(line.onScreenRoadBottomPoint + 0.5F);
1325 
1326  float max_score = 0.0F;
1327  std::vector<Point2D<int> > max_points;
1328 
1329  for (int di1 = -10; di1 <= 10; di1 += 2)
1330  for (int di2 = -10; di2 <= 10; di2 += 2)
1331  {
1332  Point2D<int> pn1(pi1.i + di1, pi1.j);
1333  Point2D<int> pn2(pi2.i + di2, pi2.j);
1334  std::vector<Point2D<int> > points;
1335 
1336  float score = getLineFitness(pn1, pn2, edgeMap, points, visual);
1337 
1338  // Debug drawing:
1339  if (visual.valid())
1340  {
1341  Line l; updateLine(l, points, score, edgeMap.cols, edgeMap.rows);
1342  jevois::rawimage::drawLine(visual, pn1.i, pn1.j, pn2.i, pn2.j, 0, jevois::yuyv::MedPurple);
1343  for (Point2D<int> const & p : points)
1344  jevois::rawimage::drawDisk(visual, p.i, p.j, 1, jevois::yuyv::MedPurple);
1345  }
1346 
1347  if (score > max_score) { max_score = score; max_points = points; }
1348  }
1349 
1350  // update the vanishing line
1351  if (max_score > 0) updateLine(line, max_points, max_score, edgeMap.cols, edgeMap.rows); else line.score = max_score;
1352  line.segments.clear();
1353  }
1354 
1355  // check for start values
1356  std::vector<Line> temp_lines;
1357  for (Line & line : currentLines)
1358  {
1359  uint num_sscore = line.start_scores.size();
1360 
1361  // still in starting stage
1362  if (num_sscore > 0)
1363  {
1364  line.start_scores.push_back(line.score);
1365  ++num_sscore;
1366 
1367  uint num_high = 0;
1368  for (uint j = 0; j < num_sscore; ++j) if (line.start_scores[j] > .5) num_high++;
1369 
1370  if (num_high > 5) { line.start_scores.clear(); num_sscore = 0; }
1371  if (num_sscore < 7) temp_lines.push_back(line);
1372  }
1373  else temp_lines.push_back(line);
1374  }
1375  currentLines.clear();
1376  for (uint i = 0; i < temp_lines.size(); ++i) currentLines.push_back(temp_lines[i]);
1377 
1378  // check lines to see if any lines are below the threshold
1379  temp_lines.clear();
1380  for (Line & line : currentLines)
1381  {
1382  if (line.score < 0.3 || line.scores.size() > 0) line.scores.push_back(line.score);
1383 
1384  uint num_low = 0; int size = line.scores.size(); bool all_good_values = true;
1385  for (int j = 0; j < size; ++j) if (line.scores[j] < 0.3) { num_low++; all_good_values = false; }
1386 
1387  // keep until 5 of 7 bad values
1388  if (num_low < 5)
1389  {
1390  // update the values
1391  if (all_good_values) line.scores.clear();
1392  else
1393  {
1394  std::vector<float> vals;
1395  if (size > 7)
1396  {
1397  for (int j = size-7; j < size; ++j) vals.push_back(line.scores[j]);
1398  line.scores = vals;
1399  }
1400  }
1401  temp_lines.push_back(line);
1402  }
1403  }
1404 
1405  currentLines.clear();
1406  for (uint i = 0; i < temp_lines.size(); ++i) currentLines.push_back(temp_lines[i]);
1407 }
1408 
1409 // ######################################################################
1410 void RoadFinder::projectForwardVanishingLines(std::vector<Line> & lines, std::vector<cv::Mat> const & edgeMaps,
1411  jevois::RawImage & visual)
1412 {
1413  // project forward the lines using all the frames that are just passed
1414  for (cv::Mat const & em : edgeMaps) trackVanishingLines(em, lines, visual);
1415 }
1416 
1417 // ######################################################################
1418 std::vector<Line> RoadFinder::combine(std::vector<Line> & prevLines, std::vector<Line> const & currentLines,
1419  int width, int height)
1420 {
1421  std::vector<Line> combLines;
1422  std::vector<bool> cline_isadded(currentLines.size(), false);
1423  prevLines = discardDuplicates(prevLines);
1424 
1425  // integrate the two trackers
1426  for (size_t j = 0; j < prevLines.size(); ++j)
1427  {
1428  Point2D<float> const & pp1 = prevLines[j].onScreenHorizonSupportPoint;
1429  Point2D<float> const & pp2 = prevLines[j].onScreenRoadBottomPoint;
1430  float const score_pl = prevLines[j].score;
1431 
1432  float min_dist = 1.0e30F; int min_i = -1;
1433  std::vector<size_t> match_index;
1434  for (size_t i = 0; i < currentLines.size(); ++i)
1435  {
1436  Point2D<float> const & cp1 = currentLines[i].onScreenHorizonSupportPoint;
1437  Point2D<float> const & cp2 = currentLines[i].onScreenRoadBottomPoint;
1438 
1439  // check the two ends of the vanishing points
1440  float const dist = cp1.distance(pp1) + cp2.distance(pp2);
1441 
1442  // if the lines are close enough
1443  if (dist < 7.0F) { match_index.push_back(i); if (dist < min_dist) { min_dist = dist; min_i = i; } }
1444  }
1445 
1446  // combine lines if there are duplicates
1447  if (match_index.size() > 0)
1448  {
1449  // if matched with more than 1 pick the closest one and use the one with the higher score
1450  float score_mcl = currentLines[min_i].score;
1451  Line l;
1452  if (score_pl > score_mcl)
1453  combLines.push_back(prevLines[j]);
1454  else
1455  {
1456  updateLine(l, currentLines[min_i].points, score_mcl, width, height);
1457 
1458  l.start_scores = prevLines[j].start_scores;
1459  size_t ss_size = l.start_scores.size();
1460  if (ss_size > 0) l.start_scores[ss_size-1] = score_mcl;
1461 
1462  l.scores = prevLines[j].start_scores;
1463  size_t s_size = l.scores.size();
1464  if (s_size > 0) l.scores[s_size-1] = score_mcl;
1465 
1466  l.isActive = prevLines[j].isActive;
1467  l.angleToCenter = prevLines[j].angleToCenter;
1468  l.pointToServo = prevLines[j].pointToServo;
1469  l.offset = prevLines[j].offset;
1470  l.index = prevLines[j].index;
1471  combLines.push_back(l);
1472  }
1473 
1474  // but all the other lines are discarded
1475  for (uint i = 0; i < match_index.size(); ++i) cline_isadded[match_index[i]] = true;
1476  }
1477  else combLines.push_back(prevLines[j]);
1478  }
1479 
1480  for (uint i = 0; i < cline_isadded.size(); ++i) if (!cline_isadded[i]) combLines.push_back(currentLines[i]);
1481 
1482  return combLines;
1483 }
1484 
1485 // ######################################################################
1486 std::vector<Line> RoadFinder::discardDuplicates(std::vector<Line> const & lines)
1487 {
1488  std::vector<Line> newLines; std::vector<bool> line_isadded(lines.size(), false);
1489 
1490  for (size_t j = 0; j < lines.size(); ++j)
1491  {
1492  if (line_isadded[j]) continue;
1493 
1494  Line line_added = lines[j];
1495  Point2D<float> const & pp1 = line_added.onScreenHorizonSupportPoint;
1496  Point2D<float> const & pp2 = line_added.onScreenRoadBottomPoint;
1497 
1498  for (size_t i = j + 1; i < lines.size(); ++i)
1499  {
1500  if (line_isadded[i]) continue;
1501 
1502  Point2D<float> const & cp1 = lines[i].onScreenHorizonSupportPoint;
1503  Point2D<float> const & cp2 = lines[i].onScreenRoadBottomPoint;
1504  float const score_cl2 = lines[i].score;
1505 
1506  // check the two ends of the vanishing points
1507  float const dist = cp1.distance(pp1) + cp2.distance(pp2);
1508 
1509  // if the lines are close enough
1510  if (dist < 3.0F)
1511  {
1512  line_isadded[i] = true;
1513  if (line_added.score < score_cl2) line_added = lines[i];
1514  }
1515  }
1516  newLines.push_back(line_added);
1517  }
1518  return newLines;
1519 }
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)
Point2D< float > onScreenHorizonSupportPoint
Definition: RoadFinder.H:114
unsigned int uint
Canonical unsigned int.
Definition: Types.H:134
void unFreeze()
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
T i
2D coordinates
Definition: Point2D.H:157
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 drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
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
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
Point2D< float > horizonSupportPoint
Definition: RoadFinder.H:110
void checkpoint(char const *description)
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
bool valid() const
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
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
std::vector< Point2D< int > > points
the points that are fit to the line
Definition: RoadFinder.H:106
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
bool isValid() const
test whether i and j are both positive
Definition: Point2D.H:341
void freeze()
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
type_with_N_bits< unsigned char, 8 >::type byte
8-bit unsigned integer
Definition: Types.H:86
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
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
promote_trait< T, float >::TP distance(const Point2D< T > &p) const
the euclidean distance from p
Definition: Point2D.H:357
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
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
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
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
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