Visual Servoing Platform version 3.5.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
64vpPolygon::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
81vpPolygon::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
131void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners) { init(corners); }
132
141void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners) { init(corners); }
142
155void 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
173void 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);
187 }
188 }
189
190 buildFrom(cornersClick);
191}
192
203void 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);
216 }
217 }
218
219 buildFrom(cornersClick);
220}
221
231void vpPolygon::init(const std::vector<vpImagePoint> &corners)
232{
233 _corners = corners;
234
236 updateArea();
237 updateCenter();
238
239 precalcValuesPnPoly();
240}
241
251void 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
273bool 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
309bool 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
372void 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);
505}
506
515void 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
534bool 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
544unsigned 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
@ 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:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_u() const
Definition: vpImagePoint.h:262
double get_i() const
Definition: vpImagePoint.h:203
double get_v() const
Definition: vpImagePoint.h:273
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Defines a generic 2D polygon.
Definition: vpPolygon.h:104
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:515
bool _goodPoly
Definition: vpPolygon.h:115
void updateBoundingBox()
Definition: vpPolygon.cpp:478
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:231
void updateArea()
Definition: vpPolygon.cpp:405
void updateCenter()
Definition: vpPolygon.cpp:436
vpImagePoint _center
Definition: vpPolygon.h:110
vpRect _bbox
Bounding box containing the polygon.
Definition: vpPolygon.h:117
PointInPolygonMethod
Definition: vpPolygon.h:120
@ PnPolyRayCasting
Definition: vpPolygon.h:122
@ PnPolySegmentIntersection
Definition: vpPolygon.h:121
virtual ~vpPolygon()
Definition: vpPolygon.cpp:104
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:131
unsigned int getSize() const
Definition: vpPolygon.cpp:544
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:107
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:111
double _area
Area of the polygon.
Definition: vpPolygon.h:112
void initClick(const vpImage< unsigned char > &I, unsigned int size=5, const vpColor &color=vpColor::red, unsigned int thickness=1)
Definition: vpPolygon.cpp:173
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309
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