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