Visual Servoing Platform  version 3.5.1 under development (2022-12-01)
vpPolygon.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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 http://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  * Author:
35  * Amaury Dame
36  * Nicolas Melchior
37  * Romain Tallonneau
38  * Fabien Spindler
39  * Julien Dufour
40  *
41  *****************************************************************************/
42 
43 // System
44 #include <limits>
45 #include <set>
46 
47 // Core
48 #include <visp3/core/vpDisplay.h>
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPolygon.h>
52 #include <visp3/core/vpUniRand.h>
53 
54 // Local helper
55 #ifdef VISP_HAVE_OPENCV
56 #include <opencv2/imgproc/imgproc.hpp>
57 
63 template <typename IpContainer> std::vector<vpImagePoint> convexHull(const IpContainer &ips)
64 {
65  if (ips.size() == 0) {
67  "Convex Hull can not be computed as the input does not contain any image point.");
68  }
69 
70  // Visp -> CV
71  std::vector<cv::Point> cv_pts;
72 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
73  std::transform(cbegin(ips), cend(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
74  return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
75  });
76 #elif (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
77  std::transform(begin(ips), end(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
78  return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
79  });
80 #else
81  for (typename IpContainer::const_iterator it = ips.begin(); it != ips.end(); ++it) {
82  cv_pts.push_back(cv::Point(static_cast<int>(it->get_u()), static_cast<int>(it->get_v())));
83  }
84 #endif
85 
86  // Get convex hull from OpenCV
87  std::vector<cv::Point> cv_conv_hull_corners;
88  cv::convexHull(cv_pts, cv_conv_hull_corners);
89 
90  // CV -> Visp
91  std::vector<vpImagePoint> conv_hull_corners;
92 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
93  std::transform(cbegin(cv_conv_hull_corners), cend(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
94  [](const cv::Point &pt) {
95  return vpImagePoint{static_cast<double>(pt.y), static_cast<double>(pt.x)};
96  });
97 #elif (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
98  std::transform(begin(cv_conv_hull_corners), end(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
99  [](const cv::Point &pt) {
100  return vpImagePoint{static_cast<double>(pt.y), static_cast<double>(pt.x)};
101  });
102 #else
103  for (std::vector<cv::Point>::const_iterator it = cv_conv_hull_corners.begin(); it != cv_conv_hull_corners.end();
104  ++it) {
105  conv_hull_corners.push_back(vpImagePoint(static_cast<double>(it->y), static_cast<double>(it->x)));
106  }
107 #endif
108 
109  return conv_hull_corners;
110 }
111 
112 #endif
113 
118  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
119 {
120 }
121 
130 vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
131  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
132 {
133  if (corners.size() < 3) {
134  _goodPoly = false;
135  }
136  init(corners);
137 }
138 
147 vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
148  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
149 {
150  if (corners.size() < 3) {
151  _goodPoly = false;
152  }
153  init(corners);
154 }
155 
162  : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
163  m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
164 {
165 }
166 
171 
178 {
179  _corners = poly._corners;
180  _center = poly._center;
181  _area = poly._area;
182  _goodPoly = poly._goodPoly;
183  _bbox = poly._bbox;
184  m_PnPolyConstants = poly.m_PnPolyConstants;
185  m_PnPolyMultiples = poly.m_PnPolyMultiples;
186  return *this;
187 }
188 
198 void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners, const bool create_convex_hull)
199 {
200  if (create_convex_hull) {
201 #ifdef VISP_HAVE_OPENCV
202  init(convexHull(corners));
203 #else
204  vpException(vpException::notImplementedError, "Cannot build a convex hull without OPENCV");
205 #endif
206  } else {
207  init(corners);
208  }
209 }
210 
220 void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners, const bool create_convex_hull)
221 {
222  if (create_convex_hull) {
223 #ifdef VISP_HAVE_OPENCV
224  init(convexHull(corners));
225 #else
226  vpException(vpException::notImplementedError, "Cannot build a convex hull without OPENCV");
227 #endif
228  } else {
229  init(corners);
230  }
231 }
232 
246 void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam,
247  const bool create_convex_hull)
248 {
249  std::vector<vpImagePoint> ipCorners(corners.size());
250  for (unsigned int i = 0; i < corners.size(); ++i) {
251  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
252  }
253  buildFrom(ipCorners, create_convex_hull);
254 }
255 
265 void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
266  unsigned int thickness)
267 {
269  vpImagePoint ip;
270 
271  std::vector<vpImagePoint> cornersClick;
272 
273  while (button == vpMouseButton::button1) {
274  bool ret = vpDisplay::getClick(I, ip, button, true);
275  if (ret && button == vpMouseButton::button1) {
276  vpDisplay::displayCross(I, ip, size, color, thickness);
277  cornersClick.push_back(ip);
278  vpDisplay::flush(I);
279  }
280  }
281 
282  buildFrom(cornersClick);
283 }
284 
295 void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
296 {
298  vpImagePoint ip;
299 
300  std::vector<vpImagePoint> cornersClick;
301 
302  while (button == vpMouseButton::button1) {
303  bool ret = vpDisplay::getClick(I, ip, button, true);
304  if (ret && button == vpMouseButton::button1) {
305  vpDisplay::displayCross(I, ip, size, color, thickness);
306  cornersClick.push_back(ip);
307  vpDisplay::flush(I);
308  }
309  }
310 
311  buildFrom(cornersClick);
312 }
313 
323 void vpPolygon::init(const std::vector<vpImagePoint> &corners)
324 {
325  _corners = corners;
326 
328  updateArea();
329  updateCenter();
330 
331  precalcValuesPnPoly();
332 }
333 
343 void vpPolygon::init(const std::list<vpImagePoint> &corners)
344 {
345  _corners.insert(_corners.begin(), corners.begin(), corners.end());
346 
348  updateArea();
349  updateCenter();
350 
351  precalcValuesPnPoly();
352 }
353 
365 bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
366  const vpImagePoint &ip4) const
367 {
368  double di1 = ip2.get_i() - ip1.get_i();
369  double dj1 = ip2.get_j() - ip1.get_j();
370 
371  double di2 = ip4.get_i() - ip3.get_i();
372  double dj2 = ip4.get_j() - ip3.get_j();
373 
374  double denominator = di1 * dj2 - dj1 * di2;
375 
376  if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
377  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
378  }
379 
380  double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
381  if (alpha < 0 || alpha >= 1) {
382  return false;
383  }
384 
385  double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
386  if (beta < 0 || beta >= 1) {
387  return false;
388  }
389 
390  return true;
391 }
392 
401 bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
402 {
403  if (_corners.size() < 3) {
404  return false;
405  }
406 
407  bool test = false;
408  switch (method) {
410  vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
411  vpUniRand generator;
412  infPoint.set_i(infPoint.get_i() + 1000 * generator());
413  infPoint.set_j(infPoint.get_j() + 1000 * generator()); // we add random since it appears that
414  // sometimes infPoint may cause a
415  // degenerated case (so realucnch and
416  // hope that result will be
417  // different).
418 
419  bool oddNbIntersections = false;
420  for (unsigned int i = 0; i < _corners.size(); ++i) {
421  vpImagePoint ip1 = _corners[i];
422  vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
423  bool intersection = false;
424 
425  // If the points are the same we continue without trying to found
426  // an intersection
427  if (ip1 == ip2)
428  continue;
429 
430  try {
431  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
432  } catch (...) {
433  return isInside(ip);
434  }
435 
436  if (intersection) {
437  oddNbIntersections = !oddNbIntersections;
438  }
439  }
440 
441  test = oddNbIntersections;
442  } break;
443 
444  // Reference: http://alienryderflex.com/polygon/
445  case PnPolyRayCasting:
446  default: {
447  bool oddNodes = false;
448  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
449  if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) ||
450  (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) {
451  oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
452  }
453 
454  j = i;
455  }
456 
457  test = oddNodes;
458  } break;
459  }
460 
461  return test;
462 }
463 
464 void vpPolygon::precalcValuesPnPoly()
465 {
466  if (_corners.size() < 3) {
467  return;
468  }
469 
470  m_PnPolyConstants.resize(_corners.size());
471  m_PnPolyMultiples.resize(_corners.size());
472 
473  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
474  if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
475  m_PnPolyConstants[i] = _corners[i].get_u();
476  m_PnPolyMultiples[i] = 0.0;
477  } else {
478  m_PnPolyConstants[i] = _corners[i].get_u() -
479  (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) +
480  (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
481  m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
482  }
483 
484  j = i;
485  }
486 }
487 
498 {
499  if (_corners.size() == 0) {
500  _area = 0;
501  _goodPoly = false;
502  return;
503  }
504  _area = 0;
505 
506  for (unsigned int i = 0; i < _corners.size(); ++i) {
507  unsigned int i_p_1 = (i + 1) % _corners.size();
508  _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
509  }
510 
511  _area /= 2;
512  if (_area < 0) {
513  _area = -_area;
514  }
515 }
516 
529 {
530  if (_corners.size() == 0) {
531  _center = vpImagePoint(0, 0);
532  _goodPoly = false;
533  return;
534  }
535  double i_tmp = 0;
536  double j_tmp = 0;
537 #if 0
538  for(unsigned int i=0; i<(_corners.size()-1); ++i){
539  i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
540  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
541 
542  j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
543  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
544  }
545 #else
546  for (unsigned int i = 0; i < _corners.size(); ++i) {
547  unsigned int i_p_1 = (i + 1) % _corners.size();
548  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
549  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
550 
551  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
552  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
553  }
554 #endif
555 
556  if (_area > 0) {
557  _center.set_i(fabs(i_tmp / (6 * _area)));
558  _center.set_j(fabs(j_tmp / (6 * _area)));
559  } else {
560  _center = _corners[0];
561  _goodPoly = false;
562  }
563 }
564 
571 {
572  if (_corners.size() == 0) {
575  _goodPoly = false;
576  return;
577  }
578 
579  std::set<double> setI;
580  std::set<double> setJ;
581  for (unsigned int i = 0; i < _corners.size(); ++i) {
582  setI.insert(_corners[i].get_i());
583  setJ.insert(_corners[i].get_j());
584  }
585  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
586  std::set<double>::const_iterator iterI = setI.end();
587  std::set<double>::const_iterator iterJ = setJ.end();
588  --iterI;
589  --iterJ;
590  vpImagePoint br(*iterI, *iterJ);
591 
592  if (tl == br) {
593  _goodPoly = false;
594  }
595  _bbox.setTopLeft(tl);
596  _bbox.setBottomRight(br);
597 }
598 
607 void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
608 {
609  const unsigned int N = (unsigned int)_corners.size();
610  for (unsigned int i = 0; i < N; ++i) {
611  vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
612  }
613 }
614 
626 bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
627  const PointInPolygonMethod &method)
628 {
629  vpPolygon poly(roi);
630  return poly.isInside(vpImagePoint(i, j), method);
631 }
632 
636 unsigned int vpPolygon::getSize() const { return ((unsigned int)_corners.size()); }
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ notImplementedError
Not implemented.
Definition: vpException.h:93
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:132
void set_i(double ii)
Definition: vpImagePoint.h:298
double get_u() const
Definition: vpImagePoint.h:143
double get_i() const
Definition: vpImagePoint.h:121
double get_v() const
Definition: vpImagePoint.h:154
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Defines a generic 2D polygon.
Definition: vpPolygon.h:106
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:607
bool _goodPoly
Definition: vpPolygon.h:117
void updateBoundingBox()
Definition: vpPolygon.cpp:570
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:323
void updateArea()
Definition: vpPolygon.cpp:497
void updateCenter()
Definition: vpPolygon.cpp:528
vpImagePoint _center
Definition: vpPolygon.h:112
vpRect _bbox
Bounding box containing the polygon.
Definition: vpPolygon.h:119
PointInPolygonMethod
Definition: vpPolygon.h:122
@ PnPolyRayCasting
Definition: vpPolygon.h:124
@ PnPolySegmentIntersection
Definition: vpPolygon.h:123
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
Definition: vpPolygon.cpp:198
virtual ~vpPolygon()
Definition: vpPolygon.cpp:170
unsigned int getSize() const
Definition: vpPolygon.cpp:636
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:109
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:177
double _area
Area of the polygon.
Definition: vpPolygon.h:114
void initClick(const vpImage< unsigned char > &I, unsigned int size=5, const vpColor &color=vpColor::red, unsigned int thickness=1)
Definition: vpPolygon.cpp:265
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:401
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:297
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:367
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101