Visual Servoing Platform  version 3.2.1 under development (2019-10-18) under development (2019-10-18)
vpPolygon3D.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  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
39 #include <limits.h>
40 
41 #include <visp3/core/vpConfig.h>
47 #include <visp3/core/vpPolygon.h>
48 #include <visp3/core/vpPolygon3D.h>
49 
54  : nbpt(0), nbCornersInsidePrev(0), p(NULL), polyClipped(), clippingFlag(vpPolygon3D::NO_CLIPPING),
55  distNearClip(0.001), distFarClip(100.)
56 {
57 }
58 
62 {
63  if (p)
64  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)
80  delete[] p;
81  p = new vpPoint[nbpt];
82  for (unsigned int i = 0; i < nbpt; i++)
83  p[i] = mbtp.p[i];
84 
85  return (*this);
86 }
87 
92 {
93  if (p != NULL) {
94  delete[] p;
95  p = NULL;
96  }
97 }
98 
106 vpPoint &vpPolygon3D::getPoint(const unsigned int _index)
107 {
108  if (_index >= nbpt) {
109  throw vpException(vpException::dimensionError, "index out of range");
110  }
111  return p[_index];
112 }
113 
119 void vpPolygon3D::setNbPoint(const unsigned int nb)
120 {
121  nbpt = nb;
122  if (p != NULL)
123  delete[] p;
124  p = new vpPoint[nb];
125 }
126 
133 void vpPolygon3D::addPoint(const unsigned int n, const vpPoint &P)
134 {
135  // if( p!NULL && n < nbpt )
136  p[n] = P;
137 }
138 
146 {
147  for (unsigned int i = 0; i < nbpt; i++) {
148  p[i].changeFrame(cMo);
149  p[i].projection();
150  }
151 }
152 
161 {
162  polyClipped.clear();
163  std::vector<vpColVector> fovNormals;
164  std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp;
165  std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp2;
166 
167  if (cam.isFovComputed() && clippingFlag > 3)
168  fovNormals = cam.getFovNormals();
169 
170  for (unsigned int i = 0; i < nbpt; i++) {
171  p[i % nbpt].projection();
172  polyClippedTemp.push_back(std::make_pair(p[i % nbpt], vpPolygon3D::NO_CLIPPING));
173  }
174 
176  for (unsigned int i = 1; i < 64; i = i * 2) {
177  if (((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i == 1))) {
178  if (i > vpPolygon3D::FAR_CLIPPING && !cam.isFovComputed()) // To make sure we do not compute FOV
179  // clipping if camera normals are not
180  // computed
181  continue;
182 
183  for (unsigned int j = 0; j < polyClippedTemp.size(); j++) {
184  vpPoint p1Clipped = polyClippedTemp[j].first;
185  vpPoint p2Clipped = polyClippedTemp[(j + 1) % polyClippedTemp.size()].first;
186 
187  unsigned int p2ClippedInfoBefore = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
188  unsigned int p1ClippedInfo = polyClippedTemp[j].second;
189  unsigned int p2ClippedInfo = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
190 
191  bool problem = true;
192 
193  switch (i) {
194  case 1:
195  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
196  p2ClippedInfo, i, distNearClip));
197  break;
198  case 2:
199  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
200  p2ClippedInfo, i, distFarClip));
201  break;
202  case 4:
203  problem =
204  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
205  p2ClippedInfo, fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
206  break;
207  case 8:
208  problem =
209  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
210  p2ClippedInfo, fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
211  break;
212  case 16:
213  problem =
214  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
215  p2ClippedInfo, fovNormals[2], vpPolygon3D::UP_CLIPPING));
216  break;
217  case 32:
218  problem =
219  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
220  p2ClippedInfo, fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
221  break;
222  }
223 
224  if (!problem) {
225  p1Clipped.projection();
226  polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
227 
228  if (p2ClippedInfo != p2ClippedInfoBefore) {
229  p2Clipped.projection();
230  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
231  }
232 
233  if (nbpt == 2) {
234  if (p2ClippedInfo == p2ClippedInfoBefore) {
235  p2Clipped.projection();
236  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
237  }
238  break;
239  }
240  }
241  }
242 
243  polyClippedTemp = polyClippedTemp2;
244  polyClippedTemp2.clear();
245  }
246  }
247  }
248 
249  polyClipped = polyClippedTemp;
250 }
251 
270 bool vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped,
271  vpPoint &p2Clipped, unsigned int &p1ClippedInfo,
272  unsigned int &p2ClippedInfo, const vpColVector &normal,
273  const unsigned int &flag)
274 {
275  vpRowVector p1Vec(3);
276  p1Vec[0] = p1.get_X();
277  p1Vec[1] = p1.get_Y();
278  p1Vec[2] = p1.get_Z();
279  p1Vec.normalize();
280 
281  vpRowVector p2Vec(3);
282  p2Vec[0] = p2.get_X();
283  p2Vec[1] = p2.get_Y();
284  p2Vec[2] = p2.get_Z();
285  p2Vec.normalize();
286 
287  if ((clippingFlag & flag) == flag) {
288  double beta1 = acos(p1Vec * normal);
289  double beta2 = acos(p2Vec * normal);
290 
291  // std::cout << beta1 << " && " << beta2 << std::endl;
292 
293  // if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
294  if (beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
295  return false;
296  else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0) {
297  vpPoint pClipped;
298  double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
299  t = t / (normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) +
300  normal[2] * (p2.get_Z() - p1.get_Z()));
301 
302  pClipped.set_X((p2.get_X() - p1.get_X()) * t + p1.get_X());
303  pClipped.set_Y((p2.get_Y() - p1.get_Y()) * t + p1.get_Y());
304  pClipped.set_Z((p2.get_Z() - p1.get_Z()) * t + p1.get_Z());
305 
306  if (beta1 < M_PI / 2.0) {
307  p1ClippedInfo = p1ClippedInfo | flag;
308  p1Clipped = pClipped;
309  } else {
310  p2ClippedInfo = p2ClippedInfo | flag;
311  p2Clipped = pClipped;
312  }
313  }
314  }
315 
316  return true;
317 }
318 
319 bool vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2, 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  bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
334  if (flag == vpPolygon3D::FAR_CLIPPING)
335  test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
336 
337  bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
338  if (flag == vpPolygon3D::FAR_CLIPPING)
339  test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
340 
341  bool test3 = (p1Clipped.get_Z() < distance);
342  if (flag == vpPolygon3D::FAR_CLIPPING)
343  test3 = (p1Clipped.get_Z() > distance);
344 
345  if (test1)
346  return false;
347 
348  else if (test2) {
349  vpPoint pClippedNear;
350  double t;
351  t = (p2Clipped.get_Z() - p1Clipped.get_Z());
352  t = (distance - p1Clipped.get_Z()) / t;
353 
354  pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X()) * t + p1Clipped.get_X());
355  pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y()) * t + p1Clipped.get_Y());
356  pClippedNear.set_Z(distance);
357 
358  if (test3) {
359  p1Clipped = pClippedNear;
360  if (flag == vpPolygon3D::FAR_CLIPPING)
361  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
362  else
363  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
364  } else {
365  p2Clipped = pClippedNear;
366  if (flag == vpPolygon3D::FAR_CLIPPING)
367  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
368  else
369  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
370  }
371  }
372 
373  return true;
374 }
375 
385 std::vector<vpImagePoint> vpPolygon3D::getRoi(const vpCameraParameters &cam)
386 {
387  std::vector<vpImagePoint> roi;
388  for (unsigned int i = 0; i < nbpt; i++) {
389  vpImagePoint ip;
390  vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
391  roi.push_back(ip);
392  }
393 
394  return roi;
395 }
396 
405 std::vector<vpImagePoint> vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
406 {
407  changeFrame(cMo);
408  return getRoi(cam);
409 }
410 
411 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
412 
420 void vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
421 {
422  for (unsigned int i = 0; i < polyClipped.size(); i++) {
423  points.push_back(polyClipped[i].first);
424  }
425 }
426 #endif
427 
436 void vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint, unsigned int> > &poly) { poly = polyClipped; }
437 
446 void vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
447 {
448  for (unsigned int i = 0; i < polyClipped.size(); i++) {
449  poly.push_back(polyClipped[i].first);
450  }
451 }
452 
462 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
463 {
464  for (unsigned int i = 0; i < polyClipped.size(); i++) {
465  vpImagePoint ip;
466  vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip);
467  // std::cout << "## " << ip.get_j() << " - " << ip.get_i() <<
468  // std::endl;
469  roi.push_back(ip);
470  }
471 }
472 
480 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi,
481  const vpHomogeneousMatrix &cMo)
482 {
483  changeFrame(cMo);
485  getRoiClipped(cam, roi);
486 }
487 
499 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi)
500 {
501  for (unsigned int i = 0; i < polyClipped.size(); i++) {
502  vpImagePoint ip;
503  polyClipped[i].first.projection();
504  vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip);
505  roi.push_back(std::make_pair(ip, polyClipped[i].second));
506  }
507 }
508 
517 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi,
518  const vpHomogeneousMatrix &cMo)
519 {
520  changeFrame(cMo);
522  getRoiClipped(cam, roi);
523 }
524 
533 {
534  unsigned int nbPolyIn = 0;
535  for (unsigned int i = 0; i < nbpt; i++) {
536  if (p[i].get_Z() > 0) {
537  vpImagePoint ip;
538  vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
539  if ((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
540  nbPolyIn++;
541  }
542  }
543 
544  nbCornersInsidePrev = nbPolyIn;
545 
546  return nbPolyIn;
547 }
548 
549 //###################################
550 // Static functions
551 //###################################
552 
569 void vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut,
570  const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
571  const vpCameraParameters &cam, const double &znear, const double &zfar)
572 {
573  ptOut.clear();
574  vpPolygon3D poly;
575  poly.setNbPoint((unsigned int)ptIn.size());
576  poly.setClipping(clippingFlags);
577 
578  if ((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
579  poly.setNearClippingDistance(znear);
580 
581  if ((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
582  poly.setFarClippingDistance(zfar);
583 
584  for (unsigned int i = 0; i < ptIn.size(); i++)
585  poly.addPoint(i, ptIn[i]);
586 
587  poly.changeFrame(cMo);
588  poly.computePolygonClipped(cam);
589  poly.getPolygonClipped(ptOut);
590 }
591 
592 void vpPolygon3D::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int &i_min, int &i_max, int &j_min, int &j_max)
593 {
594  // i_min_d = std::numeric_limits<double>::max(); // create an error under
595  // Windows. To fix it we have to add #undef max
596  double i_min_d = (double)INT_MAX;
597  double i_max_d = 0;
598  double j_min_d = (double)INT_MAX;
599  double j_max_d = 0;
600 
601  for (unsigned int i = 0; i < iroi.size(); i += 1) {
602  if (i_min_d > iroi[i].get_i())
603  i_min_d = iroi[i].get_i();
604 
605  if (iroi[i].get_i() < 0)
606  i_min_d = 1;
607 
608  if ((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
609  i_max_d = iroi[i].get_i();
610 
611  if (j_min_d > iroi[i].get_j())
612  j_min_d = iroi[i].get_j();
613 
614  if (iroi[i].get_j() < 0)
615  j_min_d = 1; // border
616 
617  if ((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
618  j_max_d = iroi[i].get_j();
619  }
620  i_min = static_cast<int>(i_min_d);
621  i_max = static_cast<int>(i_max_d);
622  j_min = static_cast<int>(j_min_d);
623  j_max = static_cast<int>(j_max_d);
624 }
625 
633 bool vpPolygon3D::roiInsideImage(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &corners)
634 {
635  double nbPolyIn = 0;
636  for (unsigned int i = 0; i < corners.size(); ++i) {
637  if ((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) && (corners[i].get_i() < I.getHeight()) &&
638  (corners[i].get_j() < I.getWidth())) {
639  nbPolyIn++;
640  }
641  }
642 
643  if (nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
644  return false;
645 
646  return true;
647 }
double get_i() const
Definition: vpImagePoint.h:204
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void projection(const vpColVector &_cP, vpColVector &_p)
Definition: vpPoint.cpp:216
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
vpRowVector & normalize()
virtual void setNbPoint(const unsigned int nb)
unsigned int nbCornersInsidePrev
Definition: vpPolygon3D.h:79
static bool roiInsideImage(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &corners)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
error that can be emited by ViSP classes.
Definition: vpException.h:71
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:454
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
bool isFovComputed() const
Class that defines what is a point.
Definition: vpPoint.h:58
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:83
void changeFrame(const vpHomogeneousMatrix &cMo)
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:458
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
double get_j() const
Definition: vpImagePoint.h:215
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:456
std::vector< vpColVector > getFovNormals() const
vpPolygon3D & operator=(const vpPolygon3D &mbtp)
Definition: vpPolygon3D.cpp:70
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:413
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:76
unsigned int getHeight() const
Definition: vpImage.h:186
std::vector< vpImagePoint > getRoi(const vpCameraParameters &cam)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
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:91
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:417
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:233
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:415
unsigned int getWidth() const
Definition: vpImage.h:244