Visual Servoing Platform  version 3.2.1 under development (2019-09-15) under development (2019-09-15)
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>
55  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
56 {
57  std::vector<vpImagePoint> corners;
58  corners.push_back(vpImagePoint(0, 0));
59  corners.push_back(vpImagePoint(1, 0));
60  corners.push_back(vpImagePoint(0, 1));
61  init(corners);
62 }
63 
72 vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
73  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
74 {
75  if (corners.size() < 3) {
76  _goodPoly = false;
77  }
78  init(corners);
79 }
80 
89 vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
90  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
91 {
92  if (corners.size() < 3) {
93  _goodPoly = false;
94  }
95  init(corners);
96 }
97 
104  : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
105  m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples)
106 {
107 }
108 
113 
120 {
121  _corners = poly._corners;
122  _center = poly._center;
123  _area = poly._area;
124  _goodPoly = poly._goodPoly;
125  _bbox = poly._bbox;
126  m_PnPolyConstants = poly.m_PnPolyConstants;
127  m_PnPolyMultiples = poly.m_PnPolyMultiples;
128  return *this;
129 }
130 
139 void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners) { init(corners); }
140 
149 void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners) { init(corners); }
150 
163 void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam)
164 {
165  std::vector<vpImagePoint> ipCorners(corners.size());
166  for (unsigned int i = 0; i < corners.size(); ++i) {
167  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
168  }
169  buildFrom(ipCorners);
170 }
171 
181 void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
182  unsigned int thickness)
183 {
185  vpImagePoint ip;
186 
187  std::vector<vpImagePoint> cornersClick;
188 
189  while (button == vpMouseButton::button1) {
190  bool ret = vpDisplay::getClick(I, ip, button, true);
191  if (ret && button == vpMouseButton::button1) {
192  vpDisplay::displayCross(I, ip, size, color, thickness);
193  cornersClick.push_back(ip);
194  vpDisplay::flush(I);
195  }
196  }
197 
198  buildFrom(cornersClick);
199 }
200 
211 void vpPolygon::initClick(const vpImage<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
212 {
214  vpImagePoint ip;
215 
216  std::vector<vpImagePoint> cornersClick;
217 
218  while (button == vpMouseButton::button1) {
219  bool ret = vpDisplay::getClick(I, ip, button, true);
220  if (ret && button == vpMouseButton::button1) {
221  vpDisplay::displayCross(I, ip, size, color, thickness);
222  cornersClick.push_back(ip);
223  vpDisplay::flush(I);
224  }
225  }
226 
227  buildFrom(cornersClick);
228 }
229 
239 void vpPolygon::init(const std::vector<vpImagePoint> &corners)
240 {
241  _corners = corners;
242 
244  updateArea();
245  updateCenter();
246 
247  precalcValuesPnPoly();
248 }
249 
259 void vpPolygon::init(const std::list<vpImagePoint> &corners)
260 {
261  _corners.insert(_corners.begin(), corners.begin(), corners.end());
262 
264  updateArea();
265  updateCenter();
266 
267  precalcValuesPnPoly();
268 }
269 
281 bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
282  const vpImagePoint &ip4) const
283 {
284  double di1 = ip2.get_i() - ip1.get_i();
285  double dj1 = ip2.get_j() - ip1.get_j();
286 
287  double di2 = ip4.get_i() - ip3.get_i();
288  double dj2 = ip4.get_j() - ip3.get_j();
289 
290  double denominator = di1 * dj2 - dj1 * di2;
291 
292  if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
293  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
294  }
295 
296  double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
297  if (alpha < 0 || alpha >= 1) {
298  return false;
299  }
300 
301  double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
302  if (beta < 0 || beta >= 1) {
303  return false;
304  }
305 
306  return true;
307 }
308 
317 bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const
318 {
319  if (_corners.size() < 3) {
320  return false;
321  }
322 
323  bool test = false;
324  switch (method) {
326  vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
327  vpUniRand generator;
328  infPoint.set_i(infPoint.get_i() + 1000 * generator());
329  infPoint.set_j(infPoint.get_j() + 1000 * generator()); // we add random since it appears that
330  // sometimes infPoint may cause a
331  // degenerated case (so realucnch and
332  // hope that result will be
333  // different).
334 
335  bool oddNbIntersections = false;
336  for (unsigned int i = 0; i < _corners.size(); ++i) {
337  vpImagePoint ip1 = _corners[i];
338  vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
339  bool intersection = false;
340 
341  // If the points are the same we continue without trying to found
342  // an intersection
343  if (ip1 == ip2)
344  continue;
345 
346  try {
347  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
348  } catch (...) {
349  return isInside(ip);
350  }
351 
352  if (intersection) {
353  oddNbIntersections = !oddNbIntersections;
354  }
355  }
356 
357  test = oddNbIntersections;
358  } break;
359 
360  // Reference: http://alienryderflex.com/polygon/
361  case PnPolyRayCasting:
362  default: {
363  bool oddNodes = false;
364  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
365  if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) ||
366  (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) {
367  oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u());
368  }
369 
370  j = i;
371  }
372 
373  test = oddNodes;
374  } break;
375  }
376 
377  return test;
378 }
379 
380 void vpPolygon::precalcValuesPnPoly()
381 {
382  if (_corners.size() < 3) {
383  return;
384  }
385 
386  m_PnPolyConstants.resize(_corners.size());
387  m_PnPolyMultiples.resize(_corners.size());
388 
389  for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) {
390  if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits<double>::epsilon())) {
391  m_PnPolyConstants[i] = _corners[i].get_u();
392  m_PnPolyMultiples[i] = 0.0;
393  } else {
394  m_PnPolyConstants[i] = _corners[i].get_u() -
395  (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) +
396  (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
397  m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v());
398  }
399 
400  j = i;
401  }
402 }
403 
414 {
415  if (_corners.size() == 0) {
416  _area = 0;
417  _goodPoly = false;
418  return;
419  }
420  _area = 0;
421 
422  for (unsigned int i = 0; i < _corners.size(); ++i) {
423  unsigned int i_p_1 = (i + 1) % _corners.size();
424  _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
425  }
426 
427  _area /= 2;
428  if (_area < 0) {
429  _area = -_area;
430  }
431 }
432 
445 {
446  if (_corners.size() == 0) {
447  _center = vpImagePoint(0, 0);
448  _goodPoly = false;
449  return;
450  }
451  double i_tmp = 0;
452  double j_tmp = 0;
453 #if 0
454  for(unsigned int i=0; i<(_corners.size()-1); ++i){
455  i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
456  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
457 
458  j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
459  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
460  }
461 #else
462  for (unsigned int i = 0; i < _corners.size(); ++i) {
463  unsigned int i_p_1 = (i + 1) % _corners.size();
464  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
465  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
466 
467  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
468  (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i());
469  }
470 #endif
471 
472  if (_area > 0) {
473  _center.set_i(fabs(i_tmp / (6 * _area)));
474  _center.set_j(fabs(j_tmp / (6 * _area)));
475  } else {
476  _center = _corners[0];
477  _goodPoly = false;
478  }
479 }
480 
487 {
488  if (_corners.size() == 0) {
491  _goodPoly = false;
492  return;
493  }
494 
495  std::set<double> setI;
496  std::set<double> setJ;
497  for (unsigned int i = 0; i < _corners.size(); ++i) {
498  setI.insert(_corners[i].get_i());
499  setJ.insert(_corners[i].get_j());
500  }
501  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
502  std::set<double>::const_iterator iterI = setI.end();
503  std::set<double>::const_iterator iterJ = setJ.end();
504  --iterI;
505  --iterJ;
506  vpImagePoint br(*iterI, *iterJ);
507 
508  if (tl == br) {
509  _goodPoly = false;
510  }
511  _bbox.setTopLeft(tl);
512  _bbox.setBottomRight(br);
513 }
514 
523 void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
524 {
525  const unsigned int N = (unsigned int)_corners.size();
526  for (unsigned int i = 0; i < N; ++i) {
527  vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
528  }
529 }
530 
542 bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
543  const PointInPolygonMethod &method)
544 {
545  vpPolygon poly(roi);
546  return poly.isInside(vpImagePoint(i, j), method);
547 }
548 
552 unsigned int vpPolygon::getSize() const { return ((unsigned int)_corners.size()); }
virtual ~vpPolygon()
Definition: vpPolygon.cpp:112
double get_i() const
Definition: vpImagePoint.h:204
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:239
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:119
unsigned int getSize() const
Definition: vpPolygon.cpp:552
vpRect _bbox
Bounding box containing the polygon.
Definition: vpPolygon.h:117
vpImagePoint _center
Definition: vpPolygon.h:110
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:523
static void flush(const vpImage< unsigned char > &I)
void updateArea()
Definition: vpPolygon.cpp:413
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
void set_i(const double ii)
Definition: vpImagePoint.h:167
double get_u() const
Definition: vpImagePoint.h:263
void updateBoundingBox()
Definition: vpPolygon.cpp:486
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:181
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:317
double get_j() const
Definition: vpImagePoint.h:215
Generic class defining intrinsic camera parameters.
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:309
void set_j(const double jj)
Definition: vpImagePoint.h:178
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:88
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:65
void updateCenter()
Definition: vpPolygon.cpp:444
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:240
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:139
double get_v() const
Definition: vpImagePoint.h:274
double _area
Area of the polygon.
Definition: vpPolygon.h:112