JeVoisBase  1.21
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
Loading...
Searching...
No Matches
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>
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
64namespace
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// ######################################################################
181RoadFinder::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// ######################################################################
211
212// ######################################################################
214{
215 roadfinder::horizon::freeze(true);
216 roadfinder::support::freeze(true);
217 roadfinder::spacing::freeze(true);
218}
219
220// ######################################################################
222{
223 roadfinder::horizon::freeze(false);
224 roadfinder::support::freeze(false);
225 roadfinder::spacing::freeze(false);
226}
227
228// ######################################################################
229std::pair<Point2D<int>, float> RoadFinder::getCurrVanishingPoint() const
230{ return std::make_pair(itsVanishingPoint, itsVanishingPointConfidence); }
231
232// ######################################################################
235
236// ######################################################################
239
240// ######################################################################
243
244// ######################################################################
246{
247 std::lock_guard<std::mutex> _(itsRoadMtx);
248 itsRoadModel.lines.clear();
252 itsRoadModel.numMatches.clear();
253}
254
255// ######################################################################
256void 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:
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:
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 = jevois::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
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 JEVOIS_WAIT_GET_FUTURE(track_fut);
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:
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//######################################################################
436Point2D<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//######################################################################
597void 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//######################################################################
760Point2D<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//######################################################################
789void RoadFinder::computeHoughSegments(cv::Mat const & cvImage)
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//######################################################################
821std::vector<Line>
822RoadFinder::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// ######################################################################
956std::vector<Point2D<int> >
957RoadFinder::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// ######################################################################
964std::vector<Point2D<int> >
965RoadFinder::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// ######################################################################
1059std::vector<Point2D<int> >
1060RoadFinder::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// ######################################################################
1096Line 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);
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// ######################################################################
1154void 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// ######################################################################
1207void 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// ######################################################################
1233float 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// ######################################################################
1241float 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// ######################################################################
1321void 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)
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// ######################################################################
1414void 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// ######################################################################
1422std::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// ######################################################################
1490std::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}
int h
#define JEVOIS_WAIT_GET_FUTURE(f)
unsigned int uint
Canonical unsigned int.
Definition Types.H:134
This is a basic class to encode 2D integer coordinates.
Definition Point2D.H:67
promote_trait< T, float >::TP distance(const Point2D< T > &p) const
the euclidean distance from p
Definition Point2D.H:357
T i
2D coordinates
Definition Point2D.H:157
bool isValid() const
test whether i and j are both positive
Definition Point2D.H:341
std::mutex itsRoadMtx
Definition RoadFinder.H:309
void trackVanishingLines(cv::Mat const &edgeMap, std::vector< Line > &currentLines, jevois::RawImage &visual)
track vanishing lines by to fit to the new, inputted, edgemap
std::vector< VanishingPoint > itsVanishingPoints
vanishing points being considered
Definition RoadFinder.H:307
std::vector< Segment > itsCurrentSegments
current segments found using CVHoughlines
Definition RoadFinder.H:291
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
void process(cv::Mat const &img, jevois::RawImage &visual)
Compute the vanishing point location using the full blown algorithm.
Definition RoadFinder.C:256
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
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
std::pair< Point2D< int >, float > getCurrVanishingPoint() const
Get the current vanishing point and confidence.
Definition RoadFinder.C:229
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
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
Point2D< float > itsTargetPoint
target servo point
Definition RoadFinder.H:312
void resetRoadModel()
Reset all tracker internals and start fresh (e.g., when changing goal direction)
Definition RoadFinder.C:245
Point2D< float > getCurrCenterPoint() const
Get the current road center point.
Definition RoadFinder.C:233
void projectForwardVanishingLines(std::vector< Line > &lines, std::vector< cv::Mat > const &edgeMaps, jevois::RawImage &visual)
update the lines with the inputted set of edgemaps
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
Point2D< int > getVanishingPoint(std::vector< Line > const &lines, float &confidence)
estimate the vanishing point from the tracked lines
Definition RoadFinder.C:760
bool itsKalmanNeedInit
Definition RoadFinder.H:321
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
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
RoadModel itsRoadModel
Definition RoadFinder.H:304
std::vector< Line > itsCurrentLines
the current lines being tracked
Definition RoadFinder.H:299
uint itsNumIdentifiedLines
indicate how many unique lines have been identified NOTE: never reset
Definition RoadFinder.H:302
RoadFinder(std::string const &instance)
constructor
Definition RoadFinder.C:181
void preUninit() override
This class has state and does not support some online param changes.
Definition RoadFinder.C:221
Point2D< int > itsVanishingPoint
current vanishing point
Definition RoadFinder.H:310
cv::KalmanFilter itsTPXfilter
Definition RoadFinder.H:319
virtual ~RoadFinder()
desctructor
Definition RoadFinder.C:209
void computeHoughSegments(cv::Mat const &cvImage)
compute the hough segments in the image
Definition RoadFinder.C:789
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
std::vector< bool > itsVanishingPointStability
vanishing point score tracker
Definition RoadFinder.H:314
Point2D< float > itsAccumulatedTrajectory
the accumulated trajectory
Definition RoadFinder.H:285
void postInit() override
This class has state and does not support some online param changes.
Definition RoadFinder.C:213
bool itsTrackingFlag
indicate whether tracking
Definition RoadFinder.H:296
float itsFilteredTPX
Definition RoadFinder.H:320
float itsVanishingPointConfidence
current vanishing point
Definition RoadFinder.H:313
Point2D< float > getCurrTargetPoint() const
Get the current target point.
Definition RoadFinder.C:237
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
Point2D< float > itsCenterPoint
current center of road point
Definition RoadFinder.H:311
float getFilteredTargetX() const
Get the kalman-fitered target X, can be used to set robot steering.
Definition RoadFinder.C:241
std::vector< Line > discardDuplicates(std::vector< Line > const &currentLines)
discard duplicate lines in a set
void checkpoint(char const *description)
bool valid() const
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
unsigned short constexpr MedPurple
unsigned short constexpr LightPink
unsigned short constexpr White
unsigned short constexpr DarkGrey
unsigned short constexpr LightTeal
unsigned short constexpr MedGreen
unsigned short constexpr DarkPink
unsigned short constexpr DarkGreen
unsigned short constexpr LightGreen
unsigned short constexpr LightGrey
keeps all the ready to use information of a supporting line as it pertains to describing the road
Definition RoadFinder.H:96
std::vector< float > scores
tracking information to monitor health of the line
Definition RoadFinder.H:120
std::vector< float > start_scores
Definition RoadFinder.H:121
Point2D< float > onScreenRoadBottomPoint
Definition RoadFinder.H:112
float angle
Definition RoadFinder.H:102
float score
Definition RoadFinder.H:103
Point2D< float > onScreenHorizonSupportPoint
Definition RoadFinder.H:114
Point2D< float > horizonPoint
quick information for various locations with respect to the road
Definition RoadFinder.H:109
float offset
Definition RoadFinder.H:127
Point2D< float > onScreenHorizonPoint
Definition RoadFinder.H:113
bool isActive
tracks whether the line can be used for finding the road center
Definition RoadFinder.H:124
float angleToCenter
Definition RoadFinder.H:125
std::vector< Point2D< int > > points
the points that are fit to the line
Definition RoadFinder.H:106
Point2D< float > roadBottomPoint
Definition RoadFinder.H:111
float length
basic information to specify the line
Definition RoadFinder.H:101
std::vector< Segment > segments
original supporting segments out of sync after initial frame
Definition RoadFinder.H:117
Point2D< float > pointToServo
Definition RoadFinder.H:126
Point2D< float > horizonSupportPoint
Definition RoadFinder.H:110
int index
Definition RoadFinder.H:129
std::vector< Point2D< float > > lastSeenHorizonPoint
Definition RoadFinder.H:138
std::vector< Line > lines
Definition RoadFinder.H:136
std::vector< Point2D< float > > lastSeenLocation
Definition RoadFinder.H:139
std::vector< int > numMatches
Definition RoadFinder.H:140
std::vector< int > lastActiveIndex
Definition RoadFinder.H:137
a segment is defined by the two end-points
Definition RoadFinder.H:65
float length
Definition RoadFinder.H:73
Point2D< int > p1
Definition RoadFinder.H:70
Point2D< int > p2
Definition RoadFinder.H:71
Keeps all the supporting information about a specific vanishing point.
Definition RoadFinder.H:81