Visual Servoing Platform  version 3.0.0
vpPolygon.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Defines a generic 2D polygon.
32  *
33  * Author:
34  * Amaury Dame
35  * Nicolas Melchior
36  * Romain Tallonneau
37  *
38  *****************************************************************************/
39 
40 
41 
42 #include <visp3/core/vpPolygon.h>
43 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpDisplay.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 #include <visp3/core/vpUniRand.h>
47 #include <set>
48 #include <limits>
56  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
57 {
58  std::vector<vpImagePoint> corners;
59  corners.push_back(vpImagePoint(0,0));
60  corners.push_back(vpImagePoint(1,0));
61  corners.push_back(vpImagePoint(0,1));
62  init(corners);
63 }
64 
72 vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
73  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
74 {
75  if(corners.size() < 3){
76  _goodPoly = false;
77  }
78  init(corners);
79 }
80 
87  : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
88 {
89  _corners = poly._corners;
90  _center = poly._center;
91  _area = poly._area;
92  _goodPoly = poly._goodPoly;
93  _bbox = poly._bbox;
94 }
95 
100 {
101 }
102 
108 vpPolygon &
110 {
111  _corners = poly._corners;
112  _center = poly._center;
113  _area = poly._area;
114  _goodPoly = poly._goodPoly;
115  return *this;
116 };
117 
125 void
126 vpPolygon::buildFrom(const std::vector<vpImagePoint>& corners)
127 {
128  init(corners);
129 }
130 
142 void
143 vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
144 {
145  std::vector<vpImagePoint> ipCorners(corners.size());
146  for(unsigned int i=0; i<corners.size(); ++i){
147  vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
148  }
149  buildFrom(ipCorners);
150 }
151 
158 void
160 {
162  vpImagePoint ip;
163 
164  std::vector<vpImagePoint> cornersClick;
165 
166  while(button == vpMouseButton::button1){
167  bool ret = vpDisplay::getClick(I, ip, button, true);
168  if(ret && button == vpMouseButton::button1){
170  cornersClick.push_back(ip);
171  vpDisplay::flush(I);
172  }
173  }
174 
175  buildFrom(cornersClick);
176 }
177 
178 
187 void
188 vpPolygon::init(const std::vector<vpImagePoint>& corners)
189 {
190  _corners = corners;
191 
193  updateArea();
194  updateCenter();
195 }
196 
197 
198 
210 bool
211 vpPolygon::testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4)
212 {
213  double di1 = ip2.get_i() - ip1.get_i();
214  double dj1 = ip2.get_j() - ip1.get_j();
215 
216  double di2 = ip4.get_i() - ip3.get_i();
217  double dj2 = ip4.get_j() - ip3.get_j();
218 
219  double denominator = di1 * dj2 - dj1 * di2;
220 
221  if(fabs(denominator) < std::numeric_limits<double>::epsilon()){
222  throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
223  }
224 
225  double alpha = - ( ( ip1.get_i() - ip3.get_i() ) * dj2 + di2 * ( ip3.get_j() - ip1.get_j())) / denominator;
226  if(alpha < 0 || alpha >= 1){
227  return false;
228  }
229 
230  double beta = - (di1 * (ip3.get_j() - ip1.get_j() ) + dj1 * (ip1.get_i() - ip3.get_i()) ) / denominator;
231  if(beta < 0 || beta >= 1){
232  return false;
233  }
234 
235  return true;
236 }
237 
245 bool
247 {
248  vpImagePoint infPoint(100000, 100000); // take a point at 'inifinity'
249  vpUniRand generator;
250  infPoint.set_i( infPoint.get_i() + 1000 * generator());
251  infPoint.set_j( infPoint.get_j() + 1000 * generator());// we add random since it appears that sometimes infPoint may cause a degenerated case (so realucnch and hope that result will be different).
252 
253  unsigned int nbinterscetion = 0;
254  for(unsigned int i=0; i<_corners.size(); ++i){
255  vpImagePoint ip1 = _corners[i];
256  vpImagePoint ip2 = _corners[(i+1)%_corners.size()];
257  bool intersection = false;
258 
259  // If the points are the same we continue without trying to found
260  // an intersection
261  if (ip1 == ip2)
262  continue;
263 
264  try{
265  intersection = testIntersectionSegments(ip1, ip2, ip, infPoint );
266  }catch(vpException e){
267  return isInside(ip);
268  }
269 
270  if(intersection){
271  ++nbinterscetion;
272  }
273  }
274 
275  return ((nbinterscetion%2)==1);
276 }
277 
278 
279 
289 void
291 {
292  if(_corners.size() == 0){
293  _area = 0;
294  _goodPoly = false;
295  return;
296  }
297  _area = 0;
298 
299  for(unsigned int i=0; i<_corners.size(); ++i){
300  unsigned int i_p_1 = ( i+1 ) % _corners.size();
301  _area += _corners[i].get_j() * _corners[i_p_1].get_i()
302  - _corners[i_p_1].get_j() * _corners[i].get_i();
303  }
304 
305  _area /= 2;
306  if(_area < 0){
307  _area = - _area;
308  }
309 }
310 
311 
323 void
325 {
326  if(_corners.size() == 0){
327  _center = vpImagePoint(0, 0);
328  _goodPoly = false;
329  return;
330  }
331  double i_tmp = 0;
332  double j_tmp = 0;
333 #if 0
334  for(unsigned int i=0; i<(_corners.size()-1); ++i){
335  i_tmp += (_corners[i].get_i() + _corners[i+1].get_i()) *
336  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
337 
338  j_tmp += (_corners[i].get_j() + _corners[i+1].get_j()) *
339  (_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
340  }
341 #else
342  for(unsigned int i=0; i<_corners.size(); ++i){
343  unsigned int i_p_1 = ( i+1 ) % _corners.size();
344  i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) *
345  (_corners[i_p_1].get_i() * _corners[i].get_j()
346  - _corners[i_p_1].get_j() * _corners[i].get_i());
347 
348  j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) *
349  (_corners[i_p_1].get_i() * _corners[i].get_j()
350  - _corners[i_p_1].get_j() * _corners[i].get_i());
351  }
352 #endif
353 
354  if(_area > 0){
355  _center.set_i(fabs(i_tmp / (6 * _area)));
356  _center.set_j(fabs(j_tmp / (6 * _area)));
357  }else{
358  _center = _corners[0];
359  _goodPoly = false;
360  }
361 }
362 
363 void
365 {
366  if(_corners.size() == 0){
369  _goodPoly = false;
370  return;
371  }
372 
373  std::set<double> setI;
374  std::set<double> setJ;
375  for(unsigned int i=0; i<_corners.size(); ++i){
376  setI.insert(_corners[i].get_i());
377  setJ.insert(_corners[i].get_j());
378  }
379  vpImagePoint tl(*(setI.begin()), *(setJ.begin()));
380  std::set<double>::const_iterator iterI = setI.end();
381  std::set<double>::const_iterator iterJ = setJ.end();
382  --iterI;
383  --iterJ;
384  vpImagePoint br(*iterI, *iterJ);
385 
386  if(tl == br){
387  _goodPoly = false;
388  }
389  _bbox.setTopLeft(tl);
390  _bbox.setBottomRight(br);
391 }
392 
401 void
402 vpPolygon::display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness) const
403 {
404  const unsigned int N = (unsigned int)_corners.size();
405  for(unsigned int i=0; i<N; ++i){
406  vpDisplay::displayLine(I, _corners[i], _corners[(i+1)%N], color, thickness);
407  }
408 }
409 
410 bool
411 vpPolygon::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i_test, const double &j_test, const double &i, const double &j)
412 {
413  double dx = p2.get_j() - p1.get_j();
414  double dy = p2.get_i() - p1.get_i();
415  double ex = j - j_test;
416  double ey = i - i_test;
417 
418  double den = dx * ey - dy * ex;
419  double t = 0, u = 0;
420  //if(den != 0){
421  if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
422  t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
423  u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
424  }
425  else{
426  throw vpException(vpException::divideByZeroError, "denominator null");
427  }
428  return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
429 }
430 
440 bool
441 vpPolygon::isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j)
442 {
443  double i_test = 100000.;
444  double j_test = 100000.;
445  unsigned int nbInter = 0;
446  bool computeAgain = true;
447 
448  if(computeAgain){
449  computeAgain = false;
450  for(unsigned int k=0; k< roi.size(); k++){
451  try{
452  if(vpPolygon::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
453  nbInter++;
454  }
455  }
456  catch(...){
457  computeAgain = true;
458  break;
459  }
460  }
461 
462  if(computeAgain){
463  i_test += 100;
464  j_test -= 100;
465  nbInter = 0;
466  }
467  }
468  return ((nbInter%2) == 1);
469 }
virtual ~vpPolygon()
Definition: vpPolygon.cpp:99
double get_i() const
Definition: vpImagePoint.h:190
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void init(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:188
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpPolygon & operator=(const vpPolygon &poly)
Definition: vpPolygon.cpp:109
vpRect _bbox
Boumding box containing the polygon.
Definition: vpPolygon.h:110
vpImagePoint _center
Center of the polygon. It is automatically computed when the corners are set.
Definition: vpPolygon.h:104
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
double get_j() const
Definition: vpImagePoint.h:201
static const vpColor red
Definition: vpColor.h:163
void updateArea()
Definition: vpPolygon.cpp:290
bool isInside(const vpImagePoint &iP)
Definition: vpPolygon.cpp:246
Defines a generic 2D polygon.
Definition: vpPolygon.h:98
void set_i(const double ii)
Definition: vpImagePoint.h:154
void updateBoundingBox()
Definition: vpPolygon.cpp:364
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:285
void set_j(const double jj)
Definition: vpImagePoint.h:165
std::vector< vpImagePoint > _corners
Collection of image points containing the corners.
Definition: vpPolygon.h:102
void initClick(const vpImage< unsigned char > &I)
Definition: vpPolygon.cpp:159
bool _goodPoly
Flag to indicate whether the polygon is a good polygon (ie. it has more than two corners, ...)
Definition: vpPolygon.h:108
virtual bool getClick(bool blocking=true)=0
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:64
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void updateCenter()
Definition: vpPolygon.cpp:324
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:221
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:126
double _area
Area of the polygon.
Definition: vpPolygon.h:106
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1) const
Definition: vpPolygon.cpp:402