Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
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 
57 template <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 ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) // Check if cxx14 or higher
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 ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
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 // cxx98
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 ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L))) // Check if cxx14 or higher
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 ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
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 // cxx98
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 
123 vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
124  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
125 {
126  if (corners.size() < 3) {
127  _goodPoly = false;
128  }
129  init(corners);
130 }
131 
140 vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
141  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
142 {
143  if (corners.size() < 3) {
144  _goodPoly = false;
145  }
146  init(corners);
147 }
148 
155  : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
156  m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
157 { }
158 
163 
170 {
171  _corners = poly._corners;
172  _center = poly._center;
173  _area = poly._area;
174  _goodPoly = poly._goodPoly;
175  _bbox = poly._bbox;
176  m_PnPolyConstants = poly.m_PnPolyConstants;
177  m_PnPolyMultiples = poly.m_PnPolyMultiples;
178  return *this;
179 }
180 
181 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
192 void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners, const bool create_convex_hull)
193 {
194  build(corners, create_convex_hull);
195 }
196 
207 void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners, const bool create_convex_hull)
208 {
209  build(corners, create_convex_hull);
210 }
211 
225 void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam,
226  const bool create_convex_hull)
227 {
228  build(corners, cam, create_convex_hull);
229 }
230 #endif
231 
241 vpPolygon &vpPolygon::build(const std::vector<vpImagePoint> &corners, const bool &create_convex_hull)
242 {
243  if (create_convex_hull) {
244 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
245  init(convexHull(corners));
246 #else
247  vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
248 #endif
249  }
250  else {
251  init(corners);
252  }
253  return *this;
254 }
255 
265 vpPolygon &vpPolygon::build(const std::list<vpImagePoint> &corners, const bool &create_convex_hull)
266 {
267  if (create_convex_hull) {
268 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
269  init(convexHull(corners));
270 #else
271  vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
272 #endif
273  }
274  else {
275  init(corners);
276  }
277  return *this;
278 }
279 
292 vpPolygon &vpPolygon::build(const std::vector<vpPoint> &corners, const vpCameraParameters &cam,
293  const bool &create_convex_hull)
294 {
295  std::vector<vpImagePoint> ipCorners(corners.size());
296  unsigned int corners_size = corners.size();
297  for (unsigned int i = 0; i < corners_size; ++i) {
298  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
299  }
300  build(ipCorners, create_convex_hull);
301  return *this;
302 }
303 
313 void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
314  unsigned int thickness)
315 {
317  vpImagePoint ip;
318 
319  std::vector<vpImagePoint> cornersClick;
320 
321  while (button == vpMouseButton::button1) {
322  bool ret = vpDisplay::getClick(I, ip, button, true);
323  if (ret && (button == vpMouseButton::button1)) {
324  vpDisplay::displayCross(I, ip, size, color, thickness);
325  cornersClick.push_back(ip);
326  vpDisplay::flush(I);
327  }
328  }
329 
330  build(cornersClick);
331 }
332 
342 void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
343 {
345  vpImagePoint ip;
346 
347  std::vector<vpImagePoint> cornersClick;
348 
349  while (button == vpMouseButton::button1) {
350  bool ret = vpDisplay::getClick(I, ip, button, true);
351  if (ret && (button == vpMouseButton::button1)) {
352  vpDisplay::displayCross(I, ip, size, color, thickness);
353  cornersClick.push_back(ip);
354  vpDisplay::flush(I);
355  }
356  }
357 
358  build(cornersClick);
359 }
360 
370 void vpPolygon::init(const std::vector<vpImagePoint> &corners)
371 {
372  _corners = corners;
373 
375  updateArea();
376  updateCenter();
377 
378  precalcValuesPnPoly();
379 }
380 
390 void vpPolygon::init(const std::list<vpImagePoint> &corners)
391 {
392  _corners.insert(_corners.begin(), corners.begin(), corners.end());
393 
395  updateArea();
396  updateCenter();
397 
398  precalcValuesPnPoly();
399 }
400 
412 bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
413  const vpImagePoint &ip4) const
414 {
415  double di1 = ip2.get_i() - ip1.get_i();
416  double dj1 = ip2.get_j() - ip1.get_j();
417 
418  double di2 = ip4.get_i() - ip3.get_i();
419  double dj2 = ip4.get_j() - ip3.get_j();
420 
421  double denominator = (di1 * dj2) - (dj1 * di2);
422 
423  if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
424  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
425  }
426 
427  double alpha = -(((ip1.get_i() - ip3.get_i()) * dj2) + (di2 * (ip3.get_j() - ip1.get_j()))) / denominator;
428  if ((alpha < 0) || (alpha >= 1)) {
429  return false;
430  }
431 
432  double beta = -((di1 * (ip3.get_j() - ip1.get_j())) + (dj1 * (ip1.get_i() - ip3.get_i()))) / denominator;
433  if ((beta < 0) || (beta >= 1)) {
434  return false;
435  }
436 
437  return true;
438 }
439 
448 bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
449 {
450  if (_corners.size() < 3) {
451  return false;
452  }
453 
454  bool test = false;
455  // comment: previous implementation: switch method
456  // case PnPolySegmentIntersection:
457  if (method == PnPolySegmentIntersection) {
458  vpImagePoint infPoint(100000, 100000); // take a point at 'infinity'
459  // we add random since it appears that sometimes infPoint may cause a degenerated case (so relaunch and
460  // hope that result will be different).
461  vpUniRand generator;
462  infPoint.set_i(infPoint.get_i() + (1000 * generator()));
463  infPoint.set_j(infPoint.get_j() + (1000 * generator()));
464 
465  bool oddNbIntersections = false;
466  unsigned int v_corners_size = _corners.size();
467  for (unsigned int i = 0; i < v_corners_size; ++i) {
468  vpImagePoint ip1 = _corners[i];
469  vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
470  bool intersection = false;
471 
472  // If the points are the same we continue without trying to found
473  // an intersection
474  if (ip1 == ip2) {
475  // continue
476  }
477  else {
478 
479  try {
480  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
481  }
482  catch (...) {
483  return isInside(ip);
484  }
485 
486  if (intersection) {
487  oddNbIntersections = !oddNbIntersections;
488  }
489  }
490  }
491 
492  test = oddNbIntersections;
493  }
494  else {
495  // Reference: http://alienryderflex.com/polygon/
496  // comment: case PnPolyRayCasting
497  // comment: default
498  bool oddNodes = false;
499  size_t v_corners_size = _corners.size();
500  for (size_t i = 0, j = (v_corners_size - 1); i < v_corners_size; ++i) {
501  if (((_corners[i].get_v() < ip.get_v()) && (_corners[j].get_v() >= ip.get_v())) ||
502  ((_corners[j].get_v() < ip.get_v()) && (_corners[i].get_v() >= ip.get_v()))) {
503  oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
504  }
505 
506  j = i;
507  }
508 
509  test = oddNodes;
510  // comment: }
511  // comment: break
512  }
513 
514  return test;
515 }
516 
517 void vpPolygon::precalcValuesPnPoly()
518 {
519  if (_corners.size() < 3) {
520  return;
521  }
522 
523  m_PnPolyConstants.resize(_corners.size());
524  m_PnPolyMultiples.resize(_corners.size());
525 
526  size_t v_corners_size = _corners.size();
527  for (size_t i = 0, j = (v_corners_size - 1); i < v_corners_size; ++i) {
528  if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
529  m_PnPolyConstants[i] = _corners[i].get_u();
530  m_PnPolyMultiples[i] = 0.0;
531  }
532  else {
533  m_PnPolyConstants[i] = (_corners[i].get_u() -
534  ((_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()))) +
535  ((_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v()));
536  m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
537  }
538 
539  j = i;
540  }
541 }
542 
553 {
554  if (_corners.size() == 0) {
555  _area = 0;
556  _goodPoly = false;
557  return;
558  }
559  _area = 0;
560 
561  unsigned int v_corners_size = _corners.size();
562  for (unsigned int i = 0; i < v_corners_size; ++i) {
563  unsigned int i_p_1 = (i + 1) % _corners.size();
564  _area += (_corners[i].get_j() * _corners[i_p_1].get_i()) - (_corners[i_p_1].get_j() * _corners[i].get_i());
565  }
566 
567  _area /= 2;
568  if (_area < 0) {
569  _area = -_area;
570  }
571 }
572 
585 {
586  if (_corners.size() == 0) {
587  _center = vpImagePoint(0, 0);
588  _goodPoly = false;
589  return;
590  }
591  double i_tmp = 0;
592  double j_tmp = 0;
593 #if 0
594  for (unsigned int i = 0; i < (_corners.size() - 1); ++i) {
595  i_tmp += (_corners[i].get_i() + _corners[i + 1].get_i()) *
596  (_corners[i + 1].get_i() * _corners[i].get_j() - _corners[i + 1].get_j() * _corners[i].get_i());
597 
598  j_tmp += (_corners[i].get_j() + _corners[i + 1].get_j()) *
599  (_corners[i + 1].get_i() * _corners[i].get_j() - _corners[i + 1].get_j() * _corners[i].get_i());
600  }
601 #else
602  unsigned int v_corners_size = _corners.size();
603  for (unsigned int i = 0; i < v_corners_size; ++i) {
604  unsigned int i_p_1 = (i + 1) % _corners.size();
605  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
606  ((_corners[i_p_1].get_i() * _corners[i].get_j()) - (_corners[i_p_1].get_j() * _corners[i].get_i()));
607 
608  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
609  ((_corners[i_p_1].get_i() * _corners[i].get_j()) - (_corners[i_p_1].get_j() * _corners[i].get_i()));
610  }
611 #endif
612 
613  if (_area > 0) {
614  _center.set_i(fabs(i_tmp / (6 * _area)));
615  _center.set_j(fabs(j_tmp / (6 * _area)));
616  }
617  else {
618  _center = _corners[0];
619  _goodPoly = false;
620  }
621 }
622 
629 {
630  if (_corners.size() == 0) {
633  _goodPoly = false;
634  return;
635  }
636 
637  std::set<double> setI;
638  std::set<double> setJ;
639  unsigned int v_corners_size = _corners.size();
640  for (unsigned int i = 0; i < v_corners_size; ++i) {
641  setI.insert(_corners[i].get_i());
642  setJ.insert(_corners[i].get_j());
643  }
644  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
645  std::set<double>::const_iterator iterI = setI.end();
646  std::set<double>::const_iterator iterJ = setJ.end();
647  --iterI;
648  --iterJ;
649  vpImagePoint br(*iterI, *iterJ);
650 
651  if (tl == br) {
652  _goodPoly = false;
653  }
654  _bbox.setTopLeft(tl);
655  _bbox.setBottomRight(br);
656 }
657 
666 void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
667 {
668  const unsigned int N = static_cast<unsigned int>(_corners.size());
669  for (unsigned int i = 0; i < N; ++i) {
670  vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
671  }
672 }
673 
685 bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
686  const PointInPolygonMethod &method)
687 {
688  vpPolygon poly(roi);
689  return poly.isInside(vpImagePoint(i, j), method);
690 }
691 
695 unsigned int vpPolygon::getSize() const
696 {
697  return (static_cast<unsigned int>(_corners.size()));
698 }
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:72
@ notImplementedError
Not implemented.
Definition: vpException.h:68
@ divideByZeroError
Division by zero.
Definition: vpException.h:69
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:305
double get_j() const
Definition: vpImagePoint.h:125
void set_i(double ii)
Definition: vpImagePoint.h:294
double get_u() const
Definition: vpImagePoint.h:136
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:454
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Defines a generic 2D polygon.
Definition: vpPolygon.h:98
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:666
bool _goodPoly
Definition: vpPolygon.h:185
void updateBoundingBox()
Definition: vpPolygon.cpp:628
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:370
void updateArea()
Definition: vpPolygon.cpp:552
vpPolygon & build(const std::vector< vpImagePoint > &corners, const bool &create_convex_hull=false)
Definition: vpPolygon.cpp:241
void updateCenter()
Definition: vpPolygon.cpp:584
vpImagePoint _center
Definition: vpPolygon.h:180
vpRect _bbox
Bounding box containing the polygon.
Definition: vpPolygon.h:187
PointInPolygonMethod
Definition: vpPolygon.h:102
@ PnPolySegmentIntersection
Definition: vpPolygon.h:103
vp_deprecated void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
Definition: vpPolygon.cpp:192
virtual ~vpPolygon()
Definition: vpPolygon.cpp:162
unsigned int getSize() const
Definition: vpPolygon.cpp:695
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:177
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:169
double _area
Area of the polygon.
Definition: vpPolygon.h:182
void initClick(const vpImage< unsigned char > &I, unsigned int size=5, const vpColor &color=vpColor::red, unsigned int thickness=1)
Definition: vpPolygon.cpp:313
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:448
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:123