Visual Servoing Platform  version 3.0.0
vpPolygon3D.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  * Make the complete tracking of an object by using its CAD model
32  *
33  * Authors:
34  * Aurelien Yol
35  *
36  *****************************************************************************/
37 
38 #include <limits.h>
39 
40 #include <visp3/core/vpConfig.h>
46 #include <visp3/core/vpPolygon3D.h>
47 #include <visp3/core/vpPolygon.h>
48 
53  : nbpt(0), nbCornersInsidePrev(0),
54  p(NULL), polyClipped(), clippingFlag(vpPolygon3D::NO_CLIPPING),
55  distNearClip(0.001), distFarClip(100.)
56 {
57 }
58 
60  : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev),
61  p(NULL), polyClipped(mbtp.polyClipped), clippingFlag(mbtp.clippingFlag),
62  distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip)
63 {
64  if (p) delete [] p;
65  p = new vpPoint [nbpt];
66  for(unsigned int i = 0; i < nbpt; i++)
67  p[i] = mbtp.p[i];
68 }
69 
71 {
72  nbpt = mbtp.nbpt;
74  polyClipped = mbtp.polyClipped;
77  distFarClip = mbtp.distFarClip;
78 
79  if (p) delete [] p;
80  p = new vpPoint [nbpt];
81  for(unsigned int i = 0; i < nbpt; i++)
82  p[i] = mbtp.p[i];
83 
84  return (*this);
85 }
86 
91 {
92  if (p !=NULL)
93  {
94  delete[] p;
95  p = NULL;
96  }
97 }
98 
106 vpPoint &
107 vpPolygon3D::getPoint(const unsigned int _index)
108 {
109  if(_index >= nbpt){
110  throw vpException(vpException::dimensionError, "index out of range");
111  }
112  return p[_index];
113 }
114 
120 void
121 vpPolygon3D::setNbPoint(const unsigned int nb)
122 {
123  nbpt = nb ;
124  if (p != NULL)
125  delete[] p;
126  p = new vpPoint[nb] ;
127 }
128 
135 void
136 vpPolygon3D::addPoint(const unsigned int n, const vpPoint &P)
137 {
138  //if( p!NULL && n < nbpt )
139  p[n] = P ;
140 }
141 
147 void
149 {
150  for (unsigned int i = 0 ; i < nbpt ; i++)
151  {
152  p[i].changeFrame(cMo) ;
153  p[i].projection() ;
154  }
155 }
156 
164 void
166 {
167  polyClipped.clear();
168  std::vector<vpColVector> fovNormals;
169  std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp;
170  std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp2;
171 
172  if(cam.isFovComputed() && clippingFlag > 3)
173  fovNormals = cam.getFovNormals();
174 
175  for(unsigned int i = 0 ; i < nbpt ; i++){
176  p[i%nbpt].projection();
177  polyClippedTemp.push_back(std::make_pair(p[i%nbpt],vpPolygon3D::NO_CLIPPING));
178  }
179 
181  for(unsigned int i = 1 ; i < 64 ; i=i*2)
182  {
183  if(((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i==1)))
184  {
185  for(unsigned int j = 0 ; j < polyClippedTemp.size() ; j++)
186  {
187  vpPoint p1Clipped = polyClippedTemp[j].first;
188  vpPoint p2Clipped = polyClippedTemp[(j+1)%polyClippedTemp.size()].first;
189 
190  unsigned int p2ClippedInfoBefore = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
191  unsigned int p1ClippedInfo = polyClippedTemp[j].second;
192  unsigned int p2ClippedInfo = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
193 
194  bool problem = true;
195 
196  switch(i){
197  case 1:
198  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
199  i, distNearClip));
200  break;
201  case 2:
202  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
203  i, distFarClip));
204  break;
205  case 4:
206  problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
207  fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
208  break;
209  case 8:
210  problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
211  fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
212  break;
213  case 16:
214  problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
215  fovNormals[2], vpPolygon3D::UP_CLIPPING));
216  break;
217  case 32:
218  problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
219  fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
220  break;
221  }
222 
223  if(!problem)
224  {
225  p1Clipped.projection();
226  polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
227 
228  if(p2ClippedInfo != p2ClippedInfoBefore)
229  {
230  p2Clipped.projection();
231  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
232  }
233 
234  if(nbpt == 2){
235  if(p2ClippedInfo == p2ClippedInfoBefore)
236  {
237  p2Clipped.projection();
238  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
239  }
240  break;
241  }
242  }
243  }
244 
245  polyClippedTemp = polyClippedTemp2;
246  polyClippedTemp2.clear();
247  }
248  }
249 
250  polyClipped = polyClippedTemp;
251 }
252 
271 bool
272 vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
273  vpPoint &p1Clipped, vpPoint &p2Clipped,
274  unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
275  const vpColVector &normal, const unsigned int &flag)
276 {
277  vpRowVector p1Vec(3);
278  p1Vec[0] = p1.get_X(); p1Vec[1] = p1.get_Y(); p1Vec[2] = p1.get_Z();
279  p1Vec = p1Vec.normalize();
280 
281  vpRowVector p2Vec(3);
282  p2Vec[0] = p2.get_X(); p2Vec[1] = p2.get_Y(); p2Vec[2] = p2.get_Z();
283  p2Vec = p2Vec.normalize();
284 
285  if((clippingFlag & flag) == flag){
286  double beta1 = acos( p1Vec * normal );
287  double beta2 = acos( p2Vec * normal );
288 
289 // std::cout << beta1 << " && " << beta2 << std::endl;
290 
291  // if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
292  if(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
293  return false;
294  else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0){
295  vpPoint pClipped;
296  double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
297  t = t / ( normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) + normal[2] * (p2.get_Z() - p1.get_Z()) );
298 
299  pClipped.set_X((p2.get_X() - p1.get_X())*t + p1.get_X());
300  pClipped.set_Y((p2.get_Y() - p1.get_Y())*t + p1.get_Y());
301  pClipped.set_Z((p2.get_Z() - p1.get_Z())*t + p1.get_Z());
302 
303  if(beta1 < M_PI / 2.0){
304  p1ClippedInfo = p1ClippedInfo | flag;
305  p1Clipped = pClipped;
306  }
307  else{
308  p2ClippedInfo = p2ClippedInfo | flag;
309  p2Clipped = pClipped;
310  }
311  }
312  }
313 
314  return true;
315 }
316 
317 bool
318 vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
319  vpPoint &p1Clipped, vpPoint &p2Clipped,
320  unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
321  const unsigned int &flag, const double &distance)
322 {
323  // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
324  // to avoid a valgrind "Source and destination overlap in memcpy" error,
325  // we introduce a two temporary points.
326  vpPoint p1Clipped_, p2Clipped_;
327  p1Clipped_ = p1;
328  p2Clipped_ = p2;
329 
330  p1Clipped = p1Clipped_;
331  p2Clipped = p2Clipped_;
332 
333 
334  bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
335  if(flag == vpPolygon3D::FAR_CLIPPING)
336  test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
337 
338  bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
339  if(flag == vpPolygon3D::FAR_CLIPPING)
340  test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
341 
342  bool test3 = (p1Clipped.get_Z() < distance);
343  if(flag == vpPolygon3D::FAR_CLIPPING)
344  test3 = (p1Clipped.get_Z() > distance);
345 
346  if(test1)
347  return false;
348 
349  else if(test2){
350  vpPoint pClippedNear;
351  double t;
352  t = (p2Clipped.get_Z() - p1Clipped.get_Z());
353  t = (distance - p1Clipped.get_Z()) / t;
354 
355  pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X())*t + p1Clipped.get_X());
356  pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y())*t + p1Clipped.get_Y());
357  pClippedNear.set_Z(distance);
358 
359  if(test3){
360  p1Clipped = pClippedNear;
361  if(flag == vpPolygon3D::FAR_CLIPPING)
362  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
363  else
364  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
365  }
366  else{
367  p2Clipped = pClippedNear;
368  if(flag == vpPolygon3D::FAR_CLIPPING)
369  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
370  else
371  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
372  }
373  }
374 
375  return true;
376 }
377 
387 std::vector<vpImagePoint>
389 {
390  std::vector<vpImagePoint> roi;
391  for (unsigned int i = 0; i < nbpt; i ++){
392  vpImagePoint ip;
393  vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
394  roi.push_back(ip);
395  }
396 
397  return roi;
398 }
399 
408 std::vector<vpImagePoint>
410 {
411  changeFrame(cMo);
412  return getRoi(cam);
413 }
414 
415 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
416 
423 void
424 vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
425 {
426  for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
427  points.push_back(polyClipped[i].first);
428  }
429 }
430 #endif
431 
439 void
440 vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint,unsigned int> > &poly)
441 {
442  poly = polyClipped;
443 }
444 
452 void
453 vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
454 {
455  for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
456  poly.push_back(polyClipped[i].first);
457  }
458 }
459 
468 void
469 vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
470 {
471  for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
472  vpImagePoint ip;
473  vpMeterPixelConversion::convertPoint(cam,polyClipped[i].first.get_x(),polyClipped[i].first.get_y(),ip);
474 // std::cout << "## " << ip.get_j() << " - " << ip.get_i() << std::endl;
475  roi.push_back(ip);
476  }
477 }
478 
486 void
487 vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi, const vpHomogeneousMatrix &cMo)
488 {
489  changeFrame(cMo);
491  getRoiClipped(cam, roi);
492 }
493 
502 void
503 vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi)
504 {
505  for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
506  vpImagePoint ip;
507  polyClipped[i].first.projection();
508  vpMeterPixelConversion::convertPoint(cam,polyClipped[i].first.get_x(),polyClipped[i].first.get_y(),ip);
509  roi.push_back(std::make_pair(ip, polyClipped[i].second));
510  }
511 }
512 
520 void
521 vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo)
522 {
523  changeFrame(cMo);
525  getRoiClipped(cam, roi);
526 }
527 
528 
529 
536 unsigned int
538 {
539  unsigned int nbPolyIn = 0;
540  for (unsigned int i = 0; i < nbpt; i ++){
541  if(p[i].get_Z() > 0){
542  vpImagePoint ip;
543  vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
544  if((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
545  nbPolyIn++;
546  }
547  }
548 
549  nbCornersInsidePrev = nbPolyIn;
550 
551  return nbPolyIn;
552 }
553 
554 //###################################
555 // Static functions
556 //###################################
557 
571 void
572 vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
573  const vpCameraParameters &cam, const double &znear, const double &zfar)
574 {
575  ptOut.clear();
576  vpPolygon3D poly;
577  poly.setNbPoint((unsigned int)ptIn.size());
578  poly.setClipping(clippingFlags);
579 
580  if((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
581  poly.setNearClippingDistance(znear);
582 
583  if((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
584  poly.setFarClippingDistance(zfar);
585 
586  for(unsigned int i = 0; i < ptIn.size(); i++)
587  poly.addPoint(i,ptIn[i]);
588 
589  poly.changeFrame(cMo);
590  poly.computePolygonClipped(cam);
591  poly.getPolygonClipped(ptOut);
592 }
593 
594 void
595 vpPolygon3D::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int & i_min, int &i_max, int &j_min, int &j_max)
596 {
597  // i_min_d = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
598  double i_min_d = (double) INT_MAX;
599  double i_max_d = 0;
600  double j_min_d = (double) INT_MAX;
601  double j_max_d = 0;
602 
603  for (unsigned int i = 0; i < iroi.size(); i += 1){
604  if(i_min_d > iroi[i].get_i())
605  i_min_d = iroi[i].get_i();
606 
607  if(iroi[i].get_i() < 0)
608  i_min_d = 1;
609 
610  if((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
611  i_max_d = iroi[i].get_i();
612 
613  if(j_min_d > iroi[i].get_j())
614  j_min_d = iroi[i].get_j();
615 
616  if(iroi[i].get_j() < 0)
617  j_min_d = 1;//border
618 
619  if((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
620  j_max_d = iroi[i].get_j();
621  }
622  i_min = static_cast<int> (i_min_d);
623  i_max = static_cast<int> (i_max_d);
624  j_min = static_cast<int> (j_min_d);
625  j_max = static_cast<int> (j_max_d);
626 }
627 
635 bool
636 vpPolygon3D::roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners)
637 {
638  double nbPolyIn = 0;
639  for(unsigned int i=0; i<corners.size(); ++i){
640  if((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) &&
641  (corners[i].get_i() < I.getHeight()) && (corners[i].get_j() < I.getWidth())){
642  nbPolyIn++;
643  }
644  }
645 
646  if(nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
647  return false;
648 
649  return true;
650 }
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void projection(const vpColVector &_cP, vpColVector &_p)
Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP...
Definition: vpPoint.cpp:229
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
virtual void setNbPoint(const unsigned int nb)
unsigned int nbCornersInsidePrev
Number of corners inside the image during the last call to getNbCornerInsideImage.
Definition: vpPolygon3D.h:79
static bool roiInsideImage(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &corners)
double get_i() const
Definition: vpImagePoint.h:190
unsigned int getWidth() const
Definition: vpImage.h:161
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:70
bool isFovComputed() const
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:196
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 ...
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:478
double get_j() const
Definition: vpImagePoint.h:201
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
Class that defines what is a point.
Definition: vpPoint.h:59
void changeFrame(const vpHomogeneousMatrix &cMo)
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:482
vpPoint & getPoint(const unsigned int _index)
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
double distFarClip
Distance for near clipping.
Definition: vpPolygon3D.h:89
double distNearClip
Distance for near clipping.
Definition: vpPolygon3D.h:87
void addPoint(const unsigned int n, const vpPoint &P)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Generic class defining intrinsic camera parameters.
unsigned int getNbCornerInsideImage(const vpImage< unsigned char > &I, const vpCameraParameters &cam)
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:480
vpPolygon3D & operator=(const vpPolygon3D &mbtp)
Definition: vpPolygon3D.cpp:70
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:83
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:440
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:189
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:77
std::vector< vpImagePoint > getRoi(const vpCameraParameters &cam)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
std::vector< vpColVector > getFovNormals() const
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:205
unsigned int getHeight() const
Definition: vpImage.h:152
static void getClippedPolygon(const std::vector< vpPoint > &ptIn, std::vector< vpPoint > &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags, const vpCameraParameters &cam=vpCameraParameters(), const double &znear=0.001, const double &zfar=100)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int clippingFlag
Clipping flag.
Definition: vpPolygon3D.h:85
virtual ~vpPolygon3D()
Definition: vpPolygon3D.cpp:90
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:247
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:438