Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPolygon.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Defines a generic 2D polygon.
33 *
34*****************************************************************************/
35
36// System
37#include <limits>
38#include <set>
39
40// Core
41#include <visp3/core/vpDisplay.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpMeterPixelConversion.h>
44#include <visp3/core/vpPolygon.h>
45#include <visp3/core/vpUniRand.h>
46
47// Local helper
48#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
49
50#include <opencv2/imgproc/imgproc.hpp>
51
57template <typename IpContainer> std::vector<vpImagePoint> convexHull(const IpContainer &ips)
58{
59 if (ips.size() == 0) {
61 "Convex Hull can not be computed as the input does not contain any image point.");
62 }
63
64 // Visp -> CV
65 std::vector<cv::Point> cv_pts;
66#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
67 std::transform(cbegin(ips), cend(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
68 return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
69 });
70#elif (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
71 std::transform(begin(ips), end(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
72 return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
73 });
74#else
75 for (typename IpContainer::const_iterator it = ips.begin(); it != ips.end(); ++it) {
76 cv_pts.push_back(cv::Point(static_cast<int>(it->get_u()), static_cast<int>(it->get_v())));
77 }
78#endif
79
80 // Get convex hull from OpenCV
81 std::vector<cv::Point> cv_conv_hull_corners;
82 cv::convexHull(cv_pts, cv_conv_hull_corners);
83
84 // CV -> Visp
85 std::vector<vpImagePoint> conv_hull_corners;
86#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
87 std::transform(cbegin(cv_conv_hull_corners), cend(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
88 [](const cv::Point &pt) {
89 return vpImagePoint{static_cast<double>(pt.y), static_cast<double>(pt.x)};
90 });
91#elif (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
92 std::transform(begin(cv_conv_hull_corners), end(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
93 [](const cv::Point &pt) {
94 return vpImagePoint{static_cast<double>(pt.y), static_cast<double>(pt.x)};
95 });
96#else
97 for (std::vector<cv::Point>::const_iterator it = cv_conv_hull_corners.begin(); it != cv_conv_hull_corners.end();
98 ++it) {
99 conv_hull_corners.push_back(vpImagePoint(static_cast<double>(it->y), static_cast<double>(it->x)));
100 }
101#endif
102
103 return conv_hull_corners;
104}
105
106#endif
107
112 : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
113{
114}
115
124vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
125 : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
126{
127 if (corners.size() < 3) {
128 _goodPoly = false;
129 }
130 init(corners);
131}
132
141vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
142 : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
143{
144 if (corners.size() < 3) {
145 _goodPoly = false;
146 }
147 init(corners);
148}
149
156 : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
157 m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
158{
159}
160
165
172{
173 _corners = poly._corners;
174 _center = poly._center;
175 _area = poly._area;
176 _goodPoly = poly._goodPoly;
177 _bbox = poly._bbox;
178 m_PnPolyConstants = poly.m_PnPolyConstants;
179 m_PnPolyMultiples = poly.m_PnPolyMultiples;
180 return *this;
181}
182
192void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners, const bool create_convex_hull)
193{
194 if (create_convex_hull) {
195#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
196 init(convexHull(corners));
197#else
198 vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
199#endif
200 } else {
201 init(corners);
202 }
203}
204
214void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners, const bool create_convex_hull)
215{
216 if (create_convex_hull) {
217#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
218 init(convexHull(corners));
219#else
220 vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
221#endif
222 } else {
223 init(corners);
224 }
225}
226
240void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam,
241 const bool create_convex_hull)
242{
243 std::vector<vpImagePoint> ipCorners(corners.size());
244 for (unsigned int i = 0; i < corners.size(); ++i) {
245 vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
246 }
247 buildFrom(ipCorners, create_convex_hull);
248}
249
259void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
260 unsigned int thickness)
261{
263 vpImagePoint ip;
264
265 std::vector<vpImagePoint> cornersClick;
266
267 while (button == vpMouseButton::button1) {
268 bool ret = vpDisplay::getClick(I, ip, button, true);
269 if (ret && button == vpMouseButton::button1) {
270 vpDisplay::displayCross(I, ip, size, color, thickness);
271 cornersClick.push_back(ip);
273 }
274 }
275
276 buildFrom(cornersClick);
277}
278
289void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
290{
292 vpImagePoint ip;
293
294 std::vector<vpImagePoint> cornersClick;
295
296 while (button == vpMouseButton::button1) {
297 bool ret = vpDisplay::getClick(I, ip, button, true);
298 if (ret && button == vpMouseButton::button1) {
299 vpDisplay::displayCross(I, ip, size, color, thickness);
300 cornersClick.push_back(ip);
302 }
303 }
304
305 buildFrom(cornersClick);
306}
307
317void vpPolygon::init(const std::vector<vpImagePoint> &corners)
318{
319 _corners = corners;
320
322 updateArea();
323 updateCenter();
324
325 precalcValuesPnPoly();
326}
327
337void vpPolygon::init(const std::list<vpImagePoint> &corners)
338{
339 _corners.insert(_corners.begin(), corners.begin(), corners.end());
340
342 updateArea();
343 updateCenter();
344
345 precalcValuesPnPoly();
346}
347
359bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
360 const vpImagePoint &ip4) const
361{
362 double di1 = ip2.get_i() - ip1.get_i();
363 double dj1 = ip2.get_j() - ip1.get_j();
364
365 double di2 = ip4.get_i() - ip3.get_i();
366 double dj2 = ip4.get_j() - ip3.get_j();
367
368 double denominator = di1 * dj2 - dj1 * di2;
369
370 if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
371 throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
372 }
373
374 double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
375 if (alpha < 0 || alpha >= 1) {
376 return false;
377 }
378
379 double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
380 if (beta < 0 || beta >= 1) {
381 return false;
382 }
383
384 return true;
385}
386
395bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
396{
397 if (_corners.size() < 3) {
398 return false;
399 }
400
401 bool test = false;
402 switch (method) {
404 vpImagePoint infPoint(100000, 100000); // take a point at 'infinity'
405 vpUniRand generator;
406 infPoint.set_i(infPoint.get_i() + 1000 * generator());
407 infPoint.set_j(infPoint.get_j() + 1000 * generator()); // we add random since it appears that
408 // sometimes infPoint may cause a
409 // degenerated case (so relaunch and
410 // hope that result will be
411 // different).
412
413 bool oddNbIntersections = false;
414 for (unsigned int i = 0; i < _corners.size(); ++i) {
415 vpImagePoint ip1 = _corners[i];
416 vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
417 bool intersection = false;
418
419 // If the points are the same we continue without trying to found
420 // an intersection
421 if (ip1 == ip2)
422 continue;
423
424 try {
425 intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
426 } catch (...) {
427 return isInside(ip);
428 }
429
430 if (intersection) {
431 oddNbIntersections = !oddNbIntersections;
432 }
433 }
434
435 test = oddNbIntersections;
436 } break;
437
438 // Reference: http://alienryderflex.com/polygon/
439 case PnPolyRayCasting:
440 default: {
441 bool oddNodes = false;
442 for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
443 if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) ||
444 (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) {
445 oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
446 }
447
448 j = i;
449 }
450
451 test = oddNodes;
452 } break;
453 }
454
455 return test;
456}
457
458void vpPolygon::precalcValuesPnPoly()
459{
460 if (_corners.size() < 3) {
461 return;
462 }
463
464 m_PnPolyConstants.resize(_corners.size());
465 m_PnPolyMultiples.resize(_corners.size());
466
467 for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
468 if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
469 m_PnPolyConstants[i] = _corners[i].get_u();
470 m_PnPolyMultiples[i] = 0.0;
471 } else {
472 m_PnPolyConstants[i] = _corners[i].get_u() -
473 (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) +
474 (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
475 m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
476 }
477
478 j = i;
479 }
480}
481
492{
493 if (_corners.size() == 0) {
494 _area = 0;
495 _goodPoly = false;
496 return;
497 }
498 _area = 0;
499
500 for (unsigned int i = 0; i < _corners.size(); ++i) {
501 unsigned int i_p_1 = (i + 1) % _corners.size();
502 _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
503 }
504
505 _area /= 2;
506 if (_area < 0) {
507 _area = -_area;
508 }
509}
510
523{
524 if (_corners.size() == 0) {
525 _center = vpImagePoint(0, 0);
526 _goodPoly = false;
527 return;
528 }
529 double i_tmp = 0;
530 double j_tmp = 0;
531#if 0
532 for(unsigned int i=0; i<(_corners.size()-1); ++i){
533 i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
534 (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
535
536 j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
537 (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
538 }
539#else
540 for (unsigned int i = 0; i < _corners.size(); ++i) {
541 unsigned int i_p_1 = (i + 1) % _corners.size();
542 i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
543 (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
544
545 j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
546 (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
547 }
548#endif
549
550 if (_area > 0) {
551 _center.set_i(fabs(i_tmp / (6 * _area)));
552 _center.set_j(fabs(j_tmp / (6 * _area)));
553 } else {
554 _center = _corners[0];
555 _goodPoly = false;
556 }
557}
558
565{
566 if (_corners.size() == 0) {
569 _goodPoly = false;
570 return;
571 }
572
573 std::set<double> setI;
574 std::set<double> setJ;
575 for (unsigned int i = 0; i < _corners.size(); ++i) {
576 setI.insert(_corners[i].get_i());
577 setJ.insert(_corners[i].get_j());
578 }
579 vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
580 std::set<double>::const_iterator iterI = setI.end();
581 std::set<double>::const_iterator iterJ = setJ.end();
582 --iterI;
583 --iterJ;
584 vpImagePoint br(*iterI, *iterJ);
585
586 if (tl == br) {
587 _goodPoly = false;
588 }
589 _bbox.setTopLeft(tl);
591}
592
601void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
602{
603 const unsigned int N = (unsigned int)_corners.size();
604 for (unsigned int i = 0; i < N; ++i) {
605 vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
606 }
607}
608
620bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
621 const PointInPolygonMethod &method)
622{
623 vpPolygon poly(roi);
624 return poly.isInside(vpImagePoint(i, j), method);
625}
626
630unsigned int vpPolygon::getSize() const { return ((unsigned int)_corners.size()); }
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ notImplementedError
Not implemented.
Definition vpException.h:81
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
double get_j() const
void set_i(double ii)
double get_u() const
double get_i() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Defines a generic 2D polygon.
Definition vpPolygon.h:97
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
bool _goodPoly
Definition vpPolygon.h:108
void updateBoundingBox()
void init(const std::vector< vpImagePoint > &corners)
void updateArea()
void updateCenter()
vpImagePoint _center
Definition vpPolygon.h:103
vpRect _bbox
Bounding box containing the polygon.
Definition vpPolygon.h:110
PointInPolygonMethod
Definition vpPolygon.h:113
@ PnPolyRayCasting
Definition vpPolygon.h:115
@ PnPolySegmentIntersection
Definition vpPolygon.h:114
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
virtual ~vpPolygon()
unsigned int getSize() const
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition vpPolygon.h:100
vpPolygon & operator=(const vpPolygon &poly)
double _area
Area of the polygon.
Definition vpPolygon.h:105
void initClick(const vpImage< unsigned char > &I, unsigned int size=5, const vpColor &color=vpColor::red, unsigned int thickness=1)
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
void setBottomRight(const vpImagePoint &bottomRight)
Definition vpRect.h:293
void setTopLeft(const vpImagePoint &topLeft)
Definition vpRect.h:363
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122