Visual Servoing Platform version 3.5.0
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
60 : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev), p(NULL), polyClipped(mbtp.polyClipped),
61 clippingFlag(mbtp.clippingFlag), distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip)
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;
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
106vpPoint &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
119void vpPolygon3D::setNbPoint(unsigned int nb)
120{
121 nbpt = nb;
122 if (p != NULL)
123 delete[] p;
124 p = new vpPoint[nb];
125}
126
133void vpPolygon3D::addPoint(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
270bool 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
319bool 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
385std::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
405std::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
420void 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
436void vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint, unsigned int> > &poly) { poly = polyClipped; }
437
446void 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
462void 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
480void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi,
481 const vpHomogeneousMatrix &cMo)
482{
483 changeFrame(cMo);
485 getRoiClipped(cam, roi);
486}
487
499void 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
517void 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
569void 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
579 poly.setNearClippingDistance(znear);
580
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
592void 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
633bool 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}
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:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:88
double get_j() const
Definition: vpImagePoint.h:214
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
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:82
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:454
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition: vpPoint.cpp:222
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:495
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:456
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:452
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:60
void changeFrame(const vpHomogeneousMatrix &cMo)
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:76
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
virtual ~vpPolygon3D()
Definition: vpPolygon3D.cpp:91
double distNearClip
Distance for near clipping.
Definition: vpPolygon3D.h:87
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:81
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:85
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
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:83
void addPoint(unsigned int n, const vpPoint &P)
double distFarClip
Distance for near clipping.
Definition: vpPolygon3D.h:89
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
vpPolygon3D & operator=(const vpPolygon3D &mbtp)
Definition: vpPolygon3D.cpp:70
unsigned int nbCornersInsidePrev
Definition: vpPolygon3D.h:79
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:116