Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
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  // Check if std:c++14 or higher
67 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
68  std::transform(cbegin(ips), cend(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
69  return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
70  });
71 #else
72  std::transform(begin(ips), end(ips), std::back_inserter(cv_pts), [](const vpImagePoint &ip) {
73  return cv::Point(static_cast<int>(ip.get_u()), static_cast<int>(ip.get_v()));
74  });
75 #endif
76 
77  // Get convex hull from OpenCV
78  std::vector<cv::Point> cv_conv_hull_corners;
79  cv::convexHull(cv_pts, cv_conv_hull_corners);
80 
81  // CV -> Visp
82  std::vector<vpImagePoint> conv_hull_corners;
83  // Check if std:c++14 or higher
84 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
85  std::transform(cbegin(cv_conv_hull_corners), cend(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
86  [](const cv::Point &pt) {
87  return vpImagePoint { static_cast<double>(pt.y), static_cast<double>(pt.x) };
88  });
89 #else
90  std::transform(begin(cv_conv_hull_corners), end(cv_conv_hull_corners), std::back_inserter(conv_hull_corners),
91  [](const cv::Point &pt) {
92  return vpImagePoint { static_cast<double>(pt.y), static_cast<double>(pt.x) };
93  });
94 #endif
95 
96  return conv_hull_corners;
97 }
98 
99 #endif
100 
105  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
106 { }
107 
116 vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
117  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
118 {
119  if (corners.size() < 3) {
120  _goodPoly = false;
121  }
122  init(corners);
123 }
124 
133 vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
134  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
135 {
136  if (corners.size() < 3) {
137  _goodPoly = false;
138  }
139  init(corners);
140 }
141 
148  : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
149  m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
150 { }
151 
156 
163 {
164  _corners = poly._corners;
165  _center = poly._center;
166  _area = poly._area;
167  _goodPoly = poly._goodPoly;
168  _bbox = poly._bbox;
169  m_PnPolyConstants = poly.m_PnPolyConstants;
170  m_PnPolyMultiples = poly.m_PnPolyMultiples;
171  return *this;
172 }
173 
183 void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners, const bool create_convex_hull)
184 {
185  if (create_convex_hull) {
186 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
187  init(convexHull(corners));
188 #else
189  vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
190 #endif
191  }
192  else {
193  init(corners);
194  }
195 }
196 
206 void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners, const bool create_convex_hull)
207 {
208  if (create_convex_hull) {
209 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
210  init(convexHull(corners));
211 #else
212  vpException(vpException::notImplementedError, "Cannot build a convex hull without OpenCV imgproc module");
213 #endif
214  }
215  else {
216  init(corners);
217  }
218 }
219 
233 void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam,
234  const bool create_convex_hull)
235 {
236  std::vector<vpImagePoint> ipCorners(corners.size());
237  for (unsigned int i = 0; i < corners.size(); ++i) {
238  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
239  }
240  buildFrom(ipCorners, create_convex_hull);
241 }
242 
252 void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
253  unsigned int thickness)
254 {
256  vpImagePoint ip;
257 
258  std::vector<vpImagePoint> cornersClick;
259 
260  while (button == vpMouseButton::button1) {
261  bool ret = vpDisplay::getClick(I, ip, button, true);
262  if (ret && button == vpMouseButton::button1) {
263  vpDisplay::displayCross(I, ip, size, color, thickness);
264  cornersClick.push_back(ip);
265  vpDisplay::flush(I);
266  }
267  }
268 
269  buildFrom(cornersClick);
270 }
271 
282 void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
283 {
285  vpImagePoint ip;
286 
287  std::vector<vpImagePoint> cornersClick;
288 
289  while (button == vpMouseButton::button1) {
290  bool ret = vpDisplay::getClick(I, ip, button, true);
291  if (ret && button == vpMouseButton::button1) {
292  vpDisplay::displayCross(I, ip, size, color, thickness);
293  cornersClick.push_back(ip);
294  vpDisplay::flush(I);
295  }
296  }
297 
298  buildFrom(cornersClick);
299 }
300 
310 void vpPolygon::init(const std::vector<vpImagePoint> &corners)
311 {
312  _corners = corners;
313 
315  updateArea();
316  updateCenter();
317 
318  precalcValuesPnPoly();
319 }
320 
330 void vpPolygon::init(const std::list<vpImagePoint> &corners)
331 {
332  _corners.insert(_corners.begin(), corners.begin(), corners.end());
333 
335  updateArea();
336  updateCenter();
337 
338  precalcValuesPnPoly();
339 }
340 
352 bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
353  const vpImagePoint &ip4) const
354 {
355  double di1 = ip2.get_i() - ip1.get_i();
356  double dj1 = ip2.get_j() - ip1.get_j();
357 
358  double di2 = ip4.get_i() - ip3.get_i();
359  double dj2 = ip4.get_j() - ip3.get_j();
360 
361  double denominator = di1 * dj2 - dj1 * di2;
362 
363  if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
364  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
365  }
366 
367  double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
368  if (alpha < 0 || alpha >= 1) {
369  return false;
370  }
371 
372  double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
373  if (beta < 0 || beta >= 1) {
374  return false;
375  }
376 
377  return true;
378 }
379 
388 bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
389 {
390  if (_corners.size() < 3) {
391  return false;
392  }
393 
394  bool test = false;
395  switch (method) {
397  vpImagePoint infPoint(100000, 100000); // take a point at 'infinity'
398  vpUniRand generator;
399  infPoint.set_i(infPoint.get_i() + 1000 * generator());
400  infPoint.set_j(infPoint.get_j() + 1000 * generator()); // we add random since it appears that
401  // sometimes infPoint may cause a
402  // degenerated case (so relaunch and
403  // hope that result will be
404  // different).
405 
406  bool oddNbIntersections = false;
407  for (unsigned int i = 0; i < _corners.size(); ++i) {
408  vpImagePoint ip1 = _corners[i];
409  vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
410  bool intersection = false;
411 
412  // If the points are the same we continue without trying to found
413  // an intersection
414  if (ip1 == ip2)
415  continue;
416 
417  try {
418  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
419  }
420  catch (...) {
421  return isInside(ip);
422  }
423 
424  if (intersection) {
425  oddNbIntersections = !oddNbIntersections;
426  }
427  }
428 
429  test = oddNbIntersections;
430  } break;
431 
432  // Reference: http://alienryderflex.com/polygon/
433  case PnPolyRayCasting:
434  default: {
435  bool oddNodes = false;
436  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
437  if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) ||
438  (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) {
439  oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
440  }
441 
442  j = i;
443  }
444 
445  test = oddNodes;
446  } break;
447  }
448 
449  return test;
450 }
451 
452 void vpPolygon::precalcValuesPnPoly()
453 {
454  if (_corners.size() < 3) {
455  return;
456  }
457 
458  m_PnPolyConstants.resize(_corners.size());
459  m_PnPolyMultiples.resize(_corners.size());
460 
461  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
462  if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
463  m_PnPolyConstants[i] = _corners[i].get_u();
464  m_PnPolyMultiples[i] = 0.0;
465  }
466  else {
467  m_PnPolyConstants[i] = _corners[i].get_u() -
468  (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) +
469  (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
470  m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
471  }
472 
473  j = i;
474  }
475 }
476 
487 {
488  if (_corners.size() == 0) {
489  _area = 0;
490  _goodPoly = false;
491  return;
492  }
493  _area = 0;
494 
495  for (unsigned int i = 0; i < _corners.size(); ++i) {
496  unsigned int i_p_1 = (i + 1) % _corners.size();
497  _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
498  }
499 
500  _area /= 2;
501  if (_area < 0) {
502  _area = -_area;
503  }
504 }
505 
518 {
519  if (_corners.size() == 0) {
520  _center = vpImagePoint(0, 0);
521  _goodPoly = false;
522  return;
523  }
524  double i_tmp = 0;
525  double j_tmp = 0;
526 #if 0
527  for (unsigned int i = 0; i<(_corners.size()-1); ++i) {
528  i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
529  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
530 
531  j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
532  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
533  }
534 #else
535  for (unsigned int i = 0; i < _corners.size(); ++i) {
536  unsigned int i_p_1 = (i + 1) % _corners.size();
537  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
538  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
539 
540  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
541  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
542  }
543 #endif
544 
545  if (_area > 0) {
546  _center.set_i(fabs(i_tmp / (6 * _area)));
547  _center.set_j(fabs(j_tmp / (6 * _area)));
548  }
549  else {
550  _center = _corners[0];
551  _goodPoly = false;
552  }
553 }
554 
561 {
562  if (_corners.size() == 0) {
565  _goodPoly = false;
566  return;
567  }
568 
569  std::set<double> setI;
570  std::set<double> setJ;
571  for (unsigned int i = 0; i < _corners.size(); ++i) {
572  setI.insert(_corners[i].get_i());
573  setJ.insert(_corners[i].get_j());
574  }
575  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
576  std::set<double>::const_iterator iterI = setI.end();
577  std::set<double>::const_iterator iterJ = setJ.end();
578  --iterI;
579  --iterJ;
580  vpImagePoint br(*iterI, *iterJ);
581 
582  if (tl == br) {
583  _goodPoly = false;
584  }
585  _bbox.setTopLeft(tl);
586  _bbox.setBottomRight(br);
587 }
588 
597 void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
598 {
599  const unsigned int N = (unsigned int)_corners.size();
600  for (unsigned int i = 0; i < N; ++i) {
601  vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
602  }
603 }
604 
616 bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
617  const PointInPolygonMethod &method)
618 {
619  vpPolygon poly(roi);
620  return poly.isInside(vpImagePoint(i, j), method);
621 }
622 
626 unsigned 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 ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:294
double get_j() const
Definition: vpImagePoint.h:125
void set_i(double ii)
Definition: vpImagePoint.h:283
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:449
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
Definition: vpPolygon.cpp:597
bool _goodPoly
Definition: vpPolygon.h:108
void updateBoundingBox()
Definition: vpPolygon.cpp:560
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:310
void updateArea()
Definition: vpPolygon.cpp:486
void updateCenter()
Definition: vpPolygon.cpp:517
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)
Definition: vpPolygon.cpp:183
virtual ~vpPolygon()
Definition: vpPolygon.cpp:155
unsigned int getSize() const
Definition: vpPolygon.cpp:626
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:100
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:162
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)
Definition: vpPolygon.cpp:252
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:388
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:118