Visual Servoing Platform  version 3.4.0
vpPolygon.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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  *
39  *****************************************************************************/
40 
41 #include <limits>
42 #include <set>
43 #include <visp3/core/vpDisplay.h>
44 #include <visp3/core/vpException.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 #include <visp3/core/vpPolygon.h>
47 #include <visp3/core/vpUniRand.h>
52  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
53 {
54 }
55 
64 vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
65  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
66 {
67  if (corners.size() < 3) {
68  _goodPoly = false;
69  }
70  init(corners);
71 }
72 
81 vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
82  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
83 {
84  if (corners.size() < 3) {
85  _goodPoly = false;
86  }
87  init(corners);
88 }
89 
96  : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
97  m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
98 {
99 }
100 
105 
112 {
113  _corners = poly._corners;
114  _center = poly._center;
115  _area = poly._area;
116  _goodPoly = poly._goodPoly;
117  _bbox = poly._bbox;
118  m_PnPolyConstants = poly.m_PnPolyConstants;
119  m_PnPolyMultiples = poly.m_PnPolyMultiples;
120  return *this;
121 }
122 
131 void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners) { init(corners); }
132 
141 void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners) { init(corners); }
142 
155 void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam)
156 {
157  std::vector<vpImagePoint> ipCorners(corners.size());
158  for (unsigned int i = 0; i < corners.size(); ++i) {
159  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
160  }
161  buildFrom(ipCorners);
162 }
163 
173 void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
174  unsigned int thickness)
175 {
177  vpImagePoint ip;
178 
179  std::vector<vpImagePoint> cornersClick;
180 
181  while (button == vpMouseButton::button1) {
182  bool ret = vpDisplay::getClick(I, ip, button, true);
183  if (ret && button == vpMouseButton::button1) {
184  vpDisplay::displayCross(I, ip, size, color, thickness);
185  cornersClick.push_back(ip);
186  vpDisplay::flush(I);
187  }
188  }
189 
190  buildFrom(cornersClick);
191 }
192 
203 void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
204 {
206  vpImagePoint ip;
207 
208  std::vector<vpImagePoint> cornersClick;
209 
210  while (button == vpMouseButton::button1) {
211  bool ret = vpDisplay::getClick(I, ip, button, true);
212  if (ret && button == vpMouseButton::button1) {
213  vpDisplay::displayCross(I, ip, size, color, thickness);
214  cornersClick.push_back(ip);
215  vpDisplay::flush(I);
216  }
217  }
218 
219  buildFrom(cornersClick);
220 }
221 
231 void vpPolygon::init(const std::vector<vpImagePoint> &corners)
232 {
233  _corners = corners;
234 
236  updateArea();
237  updateCenter();
238 
239  precalcValuesPnPoly();
240 }
241 
251 void vpPolygon::init(const std::list<vpImagePoint> &corners)
252 {
253  _corners.insert(_corners.begin(), corners.begin(), corners.end());
254 
256  updateArea();
257  updateCenter();
258 
259  precalcValuesPnPoly();
260 }
261 
273 bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
274  const vpImagePoint &ip4) const
275 {
276  double di1 = ip2.get_i() - ip1.get_i();
277  double dj1 = ip2.get_j() - ip1.get_j();
278 
279  double di2 = ip4.get_i() - ip3.get_i();
280  double dj2 = ip4.get_j() - ip3.get_j();
281 
282  double denominator = di1 * dj2 - dj1 * di2;
283 
284  if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
285  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
286  }
287 
288  double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
289  if (alpha < 0 || alpha >= 1) {
290  return false;
291  }
292 
293  double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
294  if (beta < 0 || beta >= 1) {
295  return false;
296  }
297 
298  return true;
299 }
300 
309 bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
310 {
311  if (_corners.size() < 3) {
312  return false;
313  }
314 
315  bool test = false;
316  switch (method) {
318  vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
319  vpUniRand generator;
320  infPoint.set_i(infPoint.get_i() + 1000 * generator());
321  infPoint.set_j(infPoint.get_j() + 1000 * generator()); // we add random since it appears that
322  // sometimes infPoint may cause a
323  // degenerated case (so realucnch and
324  // hope that result will be
325  // different).
326 
327  bool oddNbIntersections = false;
328  for (unsigned int i = 0; i < _corners.size(); ++i) {
329  vpImagePoint ip1 = _corners[i];
330  vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
331  bool intersection = false;
332 
333  // If the points are the same we continue without trying to found
334  // an intersection
335  if (ip1 == ip2)
336  continue;
337 
338  try {
339  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
340  } catch (...) {
341  return isInside(ip);
342  }
343 
344  if (intersection) {
345  oddNbIntersections = !oddNbIntersections;
346  }
347  }
348 
349  test = oddNbIntersections;
350  } break;
351 
352  // Reference: http://alienryderflex.com/polygon/
353  case PnPolyRayCasting:
354  default: {
355  bool oddNodes = false;
356  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
357  if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) ||
358  (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) {
359  oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
360  }
361 
362  j = i;
363  }
364 
365  test = oddNodes;
366  } break;
367  }
368 
369  return test;
370 }
371 
372 void vpPolygon::precalcValuesPnPoly()
373 {
374  if (_corners.size() < 3) {
375  return;
376  }
377 
378  m_PnPolyConstants.resize(_corners.size());
379  m_PnPolyMultiples.resize(_corners.size());
380 
381  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
382  if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
383  m_PnPolyConstants[i] = _corners[i].get_u();
384  m_PnPolyMultiples[i] = 0.0;
385  } else {
386  m_PnPolyConstants[i] = _corners[i].get_u() -
387  (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) +
388  (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
389  m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
390  }
391 
392  j = i;
393  }
394 }
395 
406 {
407  if (_corners.size() == 0) {
408  _area = 0;
409  _goodPoly = false;
410  return;
411  }
412  _area = 0;
413 
414  for (unsigned int i = 0; i < _corners.size(); ++i) {
415  unsigned int i_p_1 = (i + 1) % _corners.size();
416  _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
417  }
418 
419  _area /= 2;
420  if (_area < 0) {
421  _area = -_area;
422  }
423 }
424 
437 {
438  if (_corners.size() == 0) {
439  _center = vpImagePoint(0, 0);
440  _goodPoly = false;
441  return;
442  }
443  double i_tmp = 0;
444  double j_tmp = 0;
445 #if 0
446  for(unsigned int i=0; i<(_corners.size()-1); ++i){
447  i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
448  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
449 
450  j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
451  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
452  }
453 #else
454  for (unsigned int i = 0; i < _corners.size(); ++i) {
455  unsigned int i_p_1 = (i + 1) % _corners.size();
456  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
457  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
458 
459  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
460  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
461  }
462 #endif
463 
464  if (_area > 0) {
465  _center.set_i(fabs(i_tmp / (6 * _area)));
466  _center.set_j(fabs(j_tmp / (6 * _area)));
467  } else {
468  _center = _corners[0];
469  _goodPoly = false;
470  }
471 }
472 
479 {
480  if (_corners.size() == 0) {
483  _goodPoly = false;
484  return;
485  }
486 
487  std::set<double> setI;
488  std::set<double> setJ;
489  for (unsigned int i = 0; i < _corners.size(); ++i) {
490  setI.insert(_corners[i].get_i());
491  setJ.insert(_corners[i].get_j());
492  }
493  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
494  std::set<double>::const_iterator iterI = setI.end();
495  std::set<double>::const_iterator iterJ = setJ.end();
496  --iterI;
497  --iterJ;
498  vpImagePoint br(*iterI, *iterJ);
499 
500  if (tl == br) {
501  _goodPoly = false;
502  }
503  _bbox.setTopLeft(tl);
504  _bbox.setBottomRight(br);
505 }
506 
515 void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
516 {
517  const unsigned int N = (unsigned int)_corners.size();
518  for (unsigned int i = 0; i < N; ++i) {
519  vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
520  }
521 }
522 
534 bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
535  const PointInPolygonMethod &method)
536 {
537  vpPolygon poly(roi);
538  return poly.isInside(vpImagePoint(i, j), method);
539 }
540 
544 unsigned int vpPolygon::getSize() const { return ((unsigned int)_corners.size()); }
virtual ~vpPolygon()
Definition: vpPolygon.cpp:104
double get_v() const
Definition: vpImagePoint.h:273
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
double get_i() const
Definition: vpImagePoint.h:203
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:293
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:231
double get_u() const
Definition: vpImagePoint.h:262
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:111
vpRect _bbox
Bounding box containing the polygon.
Definition: vpPolygon.h:117
vpImagePoint _center
Definition: vpPolygon.h:110
static void flush(const vpImage< unsigned char > &I)
double get_j() const
Definition: vpImagePoint.h:214
void updateArea()
Definition: vpPolygon.cpp:405
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309
unsigned int getSize() const
Definition: vpPolygon.cpp:544
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
void set_i(double ii)
Definition: vpImagePoint.h:166
void updateBoundingBox()
Definition: vpPolygon.cpp:478
PointInPolygonMethod
Definition: vpPolygon.h:120
void initClick(const vpImage< unsigned char > &I, unsigned int size=5, const vpColor &color=vpColor::red, unsigned int thickness=1)
Definition: vpPolygon.cpp:173
Generic class defining intrinsic camera parameters.
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:367
void set_j(double jj)
Definition: vpImagePoint.h:177
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:107
bool _goodPoly
Definition: vpPolygon.h:115
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:100
void updateCenter()
Definition: vpPolygon.cpp:436
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:297
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:131
double _area
Area of the polygon.
Definition: vpPolygon.h:112
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:515