Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
vpPolygon3D.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 
44 #include <limits.h>
45 
46 #include <visp3/core/vpConfig.h>
47 
48 #include <visp3/core/vpPolygon.h>
49 #include <visp3/core/vpPolygon3D.h>
50 
51 BEGIN_VISP_NAMESPACE
56  : nbpt(0), nbCornersInsidePrev(0), p(nullptr), polyClipped(), clippingFlag(vpPolygon3D::NO_CLIPPING),
57  distNearClip(0.001), distFarClip(100.)
58 { }
59 
61  : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev), p(nullptr), polyClipped(mbtp.polyClipped),
62  clippingFlag(mbtp.clippingFlag), distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip)
63 {
64  if (p)
65  delete[] p;
66  p = new vpPoint[nbpt];
67  for (unsigned int i = 0; i < nbpt; i++)
68  p[i] = mbtp.p[i];
69 }
70 
72 {
73  nbpt = mbtp.nbpt;
75  polyClipped = mbtp.polyClipped;
78  distFarClip = mbtp.distFarClip;
79 
80  if (p)
81  delete[] p;
82  p = new vpPoint[nbpt];
83  for (unsigned int i = 0; i < nbpt; i++)
84  p[i] = mbtp.p[i];
85 
86  return (*this);
87 }
88 
93 {
94  if (p != nullptr) {
95  delete[] p;
96  p = nullptr;
97  }
98 }
99 
107 vpPoint &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 vpPolygon3D::setNbPoint(unsigned int nb)
121 {
122  nbpt = nb;
123  if (p != nullptr)
124  delete[] p;
125  p = new vpPoint[nb];
126 }
127 
134 void vpPolygon3D::addPoint(unsigned int n, const vpPoint &P)
135 {
136  // if( p!nullptr && n < nbpt )
137  p[n] = P;
138 }
139 
147 {
148  for (unsigned int i = 0; i < nbpt; i++) {
149  p[i].changeFrame(cMo);
150  p[i].projection();
151  }
152 }
153 
162 {
163  polyClipped.clear();
164  std::vector<vpColVector> fovNormals;
165  std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp;
166  std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp2;
167 
168  if (cam.isFovComputed() && clippingFlag > 3)
169  fovNormals = cam.getFovNormals();
170 
171  for (unsigned int i = 0; i < nbpt; i++) {
172  p[i % nbpt].projection();
173  polyClippedTemp.push_back(std::make_pair(p[i % nbpt], vpPolygon3D::NO_CLIPPING));
174  }
175 
177  for (unsigned int i = 1; i < 64; i = i * 2) {
178  if (((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i == 1))) {
179  if (i > vpPolygon3D::FAR_CLIPPING && !cam.isFovComputed()) // To make sure we do not compute FOV
180  // clipping if camera normals are not
181  // computed
182  continue;
183 
184  for (unsigned int j = 0; j < polyClippedTemp.size(); j++) {
185  vpPoint p1Clipped = polyClippedTemp[j].first;
186  vpPoint p2Clipped = polyClippedTemp[(j + 1) % polyClippedTemp.size()].first;
187 
188  unsigned int p2ClippedInfoBefore = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
189  unsigned int p1ClippedInfo = polyClippedTemp[j].second;
190  unsigned int p2ClippedInfo = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
191 
192  bool problem = true;
193 
194  switch (i) {
195  case 1:
196  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
197  p2ClippedInfo, i, distNearClip));
198  break;
199  case 2:
200  problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
201  p2ClippedInfo, i, distFarClip));
202  break;
203  case 4:
204  problem =
205  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
206  p2ClippedInfo, fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
207  break;
208  case 8:
209  problem =
210  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
211  p2ClippedInfo, fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
212  break;
213  case 16:
214  problem =
215  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
216  p2ClippedInfo, fovNormals[2], vpPolygon3D::UP_CLIPPING));
217  break;
218  case 32:
219  problem =
220  !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
221  p2ClippedInfo, fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
222  break;
223  }
224 
225  if (!problem) {
226  p1Clipped.projection();
227  polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
228 
229  if (p2ClippedInfo != p2ClippedInfoBefore) {
230  p2Clipped.projection();
231  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
232  }
233 
234  if (nbpt == 2) {
235  if (p2ClippedInfo == p2ClippedInfoBefore) {
236  p2Clipped.projection();
237  polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
238  }
239  break;
240  }
241  }
242  }
243 
244  polyClippedTemp = polyClippedTemp2;
245  polyClippedTemp2.clear();
246  }
247  }
248  }
249 
250  polyClipped = polyClippedTemp;
251 }
252 
271 bool vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped,
272  vpPoint &p2Clipped, unsigned int &p1ClippedInfo,
273  unsigned int &p2ClippedInfo, const vpColVector &normal,
274  const unsigned int &flag)
275 {
276  vpRowVector p1Vec(3);
277  p1Vec[0] = p1.get_X();
278  p1Vec[1] = p1.get_Y();
279  p1Vec[2] = p1.get_Z();
280  p1Vec.normalize();
281 
282  vpRowVector p2Vec(3);
283  p2Vec[0] = p2.get_X();
284  p2Vec[1] = p2.get_Y();
285  p2Vec[2] = p2.get_Z();
286  p2Vec.normalize();
287 
288  if ((clippingFlag & flag) == flag) {
289  double beta1 = acos(p1Vec * normal);
290  double beta2 = acos(p2Vec * normal);
291 
292  // std::cout << beta1 << " && " << beta2 << std::endl;
293 
294  // if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
295  if (beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
296  return false;
297  else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0) {
298  vpPoint pClipped;
299  double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
300  t = t / (normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) +
301  normal[2] * (p2.get_Z() - p1.get_Z()));
302 
303  pClipped.set_X((p2.get_X() - p1.get_X()) * t + p1.get_X());
304  pClipped.set_Y((p2.get_Y() - p1.get_Y()) * t + p1.get_Y());
305  pClipped.set_Z((p2.get_Z() - p1.get_Z()) * t + p1.get_Z());
306 
307  if (beta1 < M_PI / 2.0) {
308  p1ClippedInfo = p1ClippedInfo | flag;
309  p1Clipped = pClipped;
310  }
311  else {
312  p2ClippedInfo = p2ClippedInfo | flag;
313  p2Clipped = pClipped;
314  }
315  }
316  }
317 
318  return true;
319 }
320 
321 bool vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped, vpPoint &p2Clipped,
322  unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
323  const unsigned int &flag, const double &distance)
324 {
325  // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
326  // to avoid a valgrind "Source and destination overlap in memcpy" error,
327  // we introduce a two temporary points.
328  vpPoint p1Clipped_, p2Clipped_;
329  p1Clipped_ = p1;
330  p2Clipped_ = p2;
331 
332  p1Clipped = p1Clipped_;
333  p2Clipped = p2Clipped_;
334 
335  bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
336  if (flag == vpPolygon3D::FAR_CLIPPING)
337  test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
338 
339  bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
340  if (flag == vpPolygon3D::FAR_CLIPPING)
341  test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
342 
343  bool test3 = (p1Clipped.get_Z() < distance);
344  if (flag == vpPolygon3D::FAR_CLIPPING)
345  test3 = (p1Clipped.get_Z() > distance);
346 
347  if (test1)
348  return false;
349 
350  else if (test2) {
351  vpPoint pClippedNear;
352  double t;
353  t = (p2Clipped.get_Z() - p1Clipped.get_Z());
354  t = (distance - p1Clipped.get_Z()) / t;
355 
356  pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X()) * t + p1Clipped.get_X());
357  pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y()) * t + p1Clipped.get_Y());
358  pClippedNear.set_Z(distance);
359 
360  if (test3) {
361  p1Clipped = pClippedNear;
362  if (flag == vpPolygon3D::FAR_CLIPPING)
363  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
364  else
365  p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
366  }
367  else {
368  p2Clipped = pClippedNear;
369  if (flag == vpPolygon3D::FAR_CLIPPING)
370  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
371  else
372  p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
373  }
374  }
375 
376  return true;
377 }
378 
388 std::vector<vpImagePoint> vpPolygon3D::getRoi(const vpCameraParameters &cam)
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> vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
409 {
410  changeFrame(cMo);
411  return getRoi(cam);
412 }
413 
414 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
423 void vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
424 {
425  for (unsigned int i = 0; i < polyClipped.size(); i++) {
426  points.push_back(polyClipped[i].first);
427  }
428 }
429 #endif
430 
439 void vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint, unsigned int> > &poly) { poly = polyClipped; }
440 
449 void vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
450 {
451  for (unsigned int i = 0; i < polyClipped.size(); i++) {
452  poly.push_back(polyClipped[i].first);
453  }
454 }
455 
465 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
466 {
467  for (unsigned int i = 0; i < polyClipped.size(); i++) {
468  vpImagePoint ip;
469  vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip);
470  // std::cout << "## " << ip.get_j() << " - " << ip.get_i() <<
471  // std::endl;
472  roi.push_back(ip);
473  }
474 }
475 
483 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi,
484  const vpHomogeneousMatrix &cMo)
485 {
486  changeFrame(cMo);
488  getRoiClipped(cam, roi);
489 }
490 
502 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi)
503 {
504  for (unsigned int i = 0; i < polyClipped.size(); i++) {
505  vpImagePoint ip;
506  polyClipped[i].first.projection();
507  vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip);
508  roi.push_back(std::make_pair(ip, polyClipped[i].second));
509  }
510 }
511 
520 void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi,
521  const vpHomogeneousMatrix &cMo)
522 {
523  changeFrame(cMo);
525  getRoiClipped(cam, roi);
526 }
527 
536 {
537  unsigned int nbPolyIn = 0;
538  for (unsigned int i = 0; i < nbpt; i++) {
539  if (p[i].get_Z() > 0) {
540  vpImagePoint ip;
541  vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
542  if ((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
543  nbPolyIn++;
544  }
545  }
546 
547  nbCornersInsidePrev = nbPolyIn;
548 
549  return nbPolyIn;
550 }
551 
552 //###################################
553 // Static functions
554 //###################################
555 
572 void vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut,
573  const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
574  const vpCameraParameters &cam, const double &znear, const double &zfar)
575 {
576  ptOut.clear();
577  vpPolygon3D poly;
578  poly.setNbPoint((unsigned int)ptIn.size());
579  poly.setClipping(clippingFlags);
580 
582  poly.setNearClippingDistance(znear);
583 
584  if ((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
585  poly.setFarClippingDistance(zfar);
586 
587  for (unsigned int i = 0; i < ptIn.size(); i++)
588  poly.addPoint(i, ptIn[i]);
589 
590  poly.changeFrame(cMo);
591  poly.computePolygonClipped(cam);
592  poly.getPolygonClipped(ptOut);
593 }
594 
595 void 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
598  // Windows. To fix it we have to add #undef max
599  double i_min_d = (double)INT_MAX;
600  double i_max_d = 0;
601  double j_min_d = (double)INT_MAX;
602  double j_max_d = 0;
603 
604  for (unsigned int i = 0; i < iroi.size(); i += 1) {
605  if (i_min_d > iroi[i].get_i())
606  i_min_d = iroi[i].get_i();
607 
608  if (iroi[i].get_i() < 0)
609  i_min_d = 1;
610 
611  if ((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
612  i_max_d = iroi[i].get_i();
613 
614  if (j_min_d > iroi[i].get_j())
615  j_min_d = iroi[i].get_j();
616 
617  if (iroi[i].get_j() < 0)
618  j_min_d = 1; // border
619 
620  if ((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
621  j_max_d = iroi[i].get_j();
622  }
623  i_min = static_cast<int>(i_min_d);
624  i_max = static_cast<int>(i_max_d);
625  j_min = static_cast<int>(j_min_d);
626  j_max = static_cast<int>(j_max_d);
627 }
628 
636 bool 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) && (corners[i].get_i() < I.getHeight()) &&
641  (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 }
651 END_VISP_NAMESPACE
Generic class defining intrinsic camera parameters.
bool isFovComputed() const
std::vector< vpColVector > getFovNormals() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
void projection(const vpColVector &_cP, vpColVector &_p) const VP_OVERRIDE
Definition: vpPoint.cpp:247
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:404
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:446
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:448
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:450
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
Definition: vpPoint.cpp:267
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:402
Implements a 3D polygon with render functionalities like clipping.
Definition: vpPolygon3D.h:57
void changeFrame(const vpHomogeneousMatrix &cMo)
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:192
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:74
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:205
virtual ~vpPolygon3D()
Definition: vpPolygon3D.cpp:92
double distNearClip
Distance for near clipping.
Definition: vpPolygon3D.h:85
vpPoint & getPoint(const unsigned int _index)
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)
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:79
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
static bool roiInsideImage(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &corners)
std::vector< vpImagePoint > getRoi(const vpCameraParameters &cam)
virtual void setNbPoint(unsigned int nb)
unsigned int clippingFlag
Clipping flag.
Definition: vpPolygon3D.h:83
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:185
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
unsigned int getNbCornerInsideImage(const vpImage< unsigned char > &I, const vpCameraParameters &cam)
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:81
void addPoint(unsigned int n, const vpPoint &P)
double distFarClip
Distance for near clipping.
Definition: vpPolygon3D.h:87
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
vpPolygon3D & operator=(const vpPolygon3D &mbtp)
Definition: vpPolygon3D.cpp:71
unsigned int nbCornersInsidePrev
Definition: vpPolygon3D.h:77
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:124