Visual Servoing Platform version 3.5.0
vpMbtFaceDepthDense.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 * Manage depth dense features for a particular face.
33 *
34 *****************************************************************************/
35
36#include <visp3/core/vpCPUFeatures.h>
37#include <visp3/mbt/vpMbtFaceDepthDense.h>
38
39#ifdef VISP_HAVE_PCL
40#include <pcl/common/point_tests.h>
41#endif
42
43#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
44#include <emmintrin.h>
45#define VISP_HAVE_SSE2 1
46#endif
47
48#define USE_SSE_CODE 1
49#if VISP_HAVE_SSE2 && USE_SSE_CODE
50#define USE_SSE 1
51#else
52#define USE_SSE 0
53#endif
54
56 : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL),
57 m_planeObject(), m_polygon(NULL), m_useScanLine(false),
58 m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), m_depthDenseFilteringMaxDist(3.0),
59 m_depthDenseFilteringMinDist(0.8), m_depthDenseFilteringOccupancyRatio(0.3), m_isTrackedDepthDenseFace(true),
60 m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), m_pointCloudFace(), m_polygonLines()
61{
62}
63
65{
66 for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
67 delete m_listOfFaceLines[i];
68 }
69}
70
85void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, vpMbHiddenFaces<vpMbtPolygon> *const faces, vpUniRand& rand_gen, int polygon,
86 std::string name)
87{
88 // Build a PolygonLine to be able to easily display the lines model
89 PolygonLine polygon_line;
90
91 // Add polygon
92 polygon_line.m_poly.setNbPoint(2);
93 polygon_line.m_poly.addPoint(0, P1);
94 polygon_line.m_poly.addPoint(1, P2);
95
96 polygon_line.m_poly.setClipping(m_clippingFlag);
97 polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
98 polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
99
100 polygon_line.m_p1 = &polygon_line.m_poly.p[0];
101 polygon_line.m_p2 = &polygon_line.m_poly.p[1];
102
103 m_polygonLines.push_back(polygon_line);
104
105 // suppress line already in the model
106 bool already_here = false;
108
109 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
110 ++it) {
111 l = *it;
112 if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
113 already_here = true;
114 l->addPolygon(polygon);
115 l->hiddenface = faces;
117 }
118 }
119
120 if (!already_here) {
121 l = new vpMbtDistanceLine;
122
124 l->buildFrom(P1, P2, rand_gen);
125 l->addPolygon(polygon);
126 l->hiddenface = faces;
128
129 l->setIndex((unsigned int)m_listOfFaceLines.size());
130 l->setName(name);
131
134
137
140
141 m_listOfFaceLines.push_back(l);
142 }
143}
144
145#ifdef VISP_HAVE_PCL
147 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
148 unsigned int stepX, unsigned int stepY
149#if DEBUG_DISPLAY_DEPTH_DENSE
150 ,
151 vpImage<unsigned char> &debugImage,
152 std::vector<std::vector<vpImagePoint> > &roiPts_vec
153#endif
154 , const vpImage<bool> *mask
155)
156{
157 unsigned int width = point_cloud->width, height = point_cloud->height;
158 m_pointCloudFace.clear();
159
160 if (point_cloud->width == 0 || point_cloud->height == 0)
161 return false;
162
163 std::vector<vpImagePoint> roiPts;
164 double distanceToFace;
165 computeROI(cMo, width, height, roiPts
166#if DEBUG_DISPLAY_DEPTH_DENSE
167 ,
168 roiPts_vec
169#endif
170 ,
171 distanceToFace);
172
173 if (roiPts.size() <= 2) {
174#ifndef NDEBUG
175 std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
176#endif
177 return false;
178 }
179
182 return false;
183 }
184
185 vpPolygon polygon_2d(roiPts);
186 vpRect bb = polygon_2d.getBoundingBox();
187
188 unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
189 unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
190 unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
191 unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
192
193 bb.setTop(top);
194 bb.setBottom(bottom);
195 bb.setLeft(left);
196 bb.setRight(right);
197
198 if (bb.getHeight() < 0 || bb.getWidth() < 0) {
199 return false;
200 }
201
202 m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
203
204 bool checkSSE2 = vpCPUFeatures::checkSSE2();
205#if !USE_SSE
206 checkSSE2 = false;
207#else
208 bool push = false;
209 double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
210#endif
211
212 int totalTheoreticalPoints = 0, totalPoints = 0;
213 for (unsigned int i = top; i < bottom; i += stepY) {
214 for (unsigned int j = left; j < right; j += stepX) {
215 if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
216 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
217 m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
218 : polygon_2d.isInside(vpImagePoint(i, j)))) {
219 totalTheoreticalPoints++;
220
221 if (vpMeTracker::inMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) {
222 totalPoints++;
223
224 if (checkSSE2) {
225#if USE_SSE
226 if (!push) {
227 push = true;
228 prev_x = (*point_cloud)(j, i).x;
229 prev_y = (*point_cloud)(j, i).y;
230 prev_z = (*point_cloud)(j, i).z;
231 } else {
232 push = false;
233 m_pointCloudFace.push_back(prev_x);
234 m_pointCloudFace.push_back((*point_cloud)(j, i).x);
235
236 m_pointCloudFace.push_back(prev_y);
237 m_pointCloudFace.push_back((*point_cloud)(j, i).y);
238
239 m_pointCloudFace.push_back(prev_z);
240 m_pointCloudFace.push_back((*point_cloud)(j, i).z);
241 }
242#endif
243 } else {
244 m_pointCloudFace.push_back((*point_cloud)(j, i).x);
245 m_pointCloudFace.push_back((*point_cloud)(j, i).y);
246 m_pointCloudFace.push_back((*point_cloud)(j, i).z);
247 }
248
249#if DEBUG_DISPLAY_DEPTH_DENSE
250 debugImage[i][j] = 255;
251#endif
252 }
253 }
254 }
255 }
256
257#if USE_SSE
258 if (checkSSE2 && push) {
259 m_pointCloudFace.push_back(prev_x);
260 m_pointCloudFace.push_back(prev_y);
261 m_pointCloudFace.push_back(prev_z);
262 }
263#endif
264
266 totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
267 return false;
268 }
269
270 return true;
271}
272#endif
273
275 unsigned int height, const std::vector<vpColVector> &point_cloud,
276 unsigned int stepX, unsigned int stepY
277#if DEBUG_DISPLAY_DEPTH_DENSE
278 ,
279 vpImage<unsigned char> &debugImage,
280 std::vector<std::vector<vpImagePoint> > &roiPts_vec
281#endif
282 , const vpImage<bool> *mask
283)
284{
285 m_pointCloudFace.clear();
286
287 if (width == 0 || height == 0)
288 return 0;
289
290 std::vector<vpImagePoint> roiPts;
291 double distanceToFace;
292 computeROI(cMo, width, height, roiPts
293#if DEBUG_DISPLAY_DEPTH_DENSE
294 ,
295 roiPts_vec
296#endif
297 ,
298 distanceToFace);
299
300 if (roiPts.size() <= 2) {
301#ifndef NDEBUG
302 std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
303#endif
304 return false;
305 }
306
309 return false;
310 }
311
312 vpPolygon polygon_2d(roiPts);
313 vpRect bb = polygon_2d.getBoundingBox();
314
315 unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
316 unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
317 unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
318 unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
319
320 bb.setTop(top);
321 bb.setBottom(bottom);
322 bb.setLeft(left);
323 bb.setRight(right);
324
325 m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
326
327 bool checkSSE2 = vpCPUFeatures::checkSSE2();
328#if !USE_SSE
329 checkSSE2 = false;
330#else
331 bool push = false;
332 double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
333#endif
334
335 int totalTheoreticalPoints = 0, totalPoints = 0;
336 for (unsigned int i = top; i < bottom; i += stepY) {
337 for (unsigned int j = left; j < right; j += stepX) {
338 if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
339 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
340 m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
341 : polygon_2d.isInside(vpImagePoint(i, j)))) {
342 totalTheoreticalPoints++;
343
344 if (vpMeTracker::inMask(mask, i, j) && point_cloud[i * width + j][2] > 0) {
345 totalPoints++;
346
347 if (checkSSE2) {
348#if USE_SSE
349 if (!push) {
350 push = true;
351 prev_x = point_cloud[i * width + j][0];
352 prev_y = point_cloud[i * width + j][1];
353 prev_z = point_cloud[i * width + j][2];
354 } else {
355 push = false;
356 m_pointCloudFace.push_back(prev_x);
357 m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
358
359 m_pointCloudFace.push_back(prev_y);
360 m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
361
362 m_pointCloudFace.push_back(prev_z);
363 m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
364 }
365#endif
366 } else {
367 m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
368 m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
369 m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
370 }
371
372#if DEBUG_DISPLAY_DEPTH_DENSE
373 debugImage[i][j] = 255;
374#endif
375 }
376 }
377 }
378 }
379
380#if USE_SSE
381 if (checkSSE2 && push) {
382 m_pointCloudFace.push_back(prev_x);
383 m_pointCloudFace.push_back(prev_y);
384 m_pointCloudFace.push_back(prev_z);
385 }
386#endif
387
389 totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
390 return false;
391 }
392
393 return true;
394}
395
397
399{
400 // Compute lines visibility, only for display
401 vpMbtDistanceLine *line;
402 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
403 ++it) {
404 line = *it;
405 bool isvisible = false;
406
407 for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
408 ++itindex) {
409 int index = *itindex;
410 if (index == -1) {
411 isvisible = true;
412 } else {
413 if (line->hiddenface->isVisible((unsigned int)index)) {
414 isvisible = true;
415 }
416 }
417 }
418
419 // Si la ligne n'appartient a aucune face elle est tout le temps visible
420 if (line->Lindex_polygon.empty())
421 isvisible = true; // Not sure that this can occur
422
423 if (isvisible) {
424 line->setVisible(true);
425 } else {
426 line->setVisible(false);
427 }
428 }
429}
430
432 vpColVector &error)
433{
434 if (m_pointCloudFace.empty()) {
435 L.resize(0, 0);
436 error.resize(0);
437 return;
438 }
439
440 L.resize(getNbFeatures(), 6, false, false);
441 error.resize(getNbFeatures(), false);
442
443 // Transform the plane equation for the current pose
446
447 double nx = m_planeCamera.getA();
448 double ny = m_planeCamera.getB();
449 double nz = m_planeCamera.getC();
450 double D = m_planeCamera.getD();
451
452 bool checkSSE2 = vpCPUFeatures::checkSSE2();
453#if !USE_SSE
454 checkSSE2 = false;
455#endif
456
457 if (checkSSE2) {
458#if USE_SSE
459 size_t cpt = 0;
460 if (getNbFeatures() >= 2) {
461 double *ptr_point_cloud = &m_pointCloudFace[0];
462 double *ptr_L = L.data;
463 double *ptr_error = error.data;
464
465 const __m128d vnx = _mm_set1_pd(nx);
466 const __m128d vny = _mm_set1_pd(ny);
467 const __m128d vnz = _mm_set1_pd(nz);
468 const __m128d vd = _mm_set1_pd(D);
469
470 double tmp_a1[2], tmp_a2[2], tmp_a3[2];
471
472 for (; cpt <= m_pointCloudFace.size() - 6; cpt += 6, ptr_point_cloud += 6) {
473 const __m128d vx = _mm_loadu_pd(ptr_point_cloud);
474 const __m128d vy = _mm_loadu_pd(ptr_point_cloud + 2);
475 const __m128d vz = _mm_loadu_pd(ptr_point_cloud + 4);
476
477 const __m128d va1 = _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz));
478 const __m128d va2 = _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx));
479 const __m128d va3 = _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy));
480
481 _mm_storeu_pd(tmp_a1, va1);
482 _mm_storeu_pd(tmp_a2, va2);
483 _mm_storeu_pd(tmp_a3, va3);
484
485 *ptr_L = nx;
486 ptr_L++;
487 *ptr_L = ny;
488 ptr_L++;
489 *ptr_L = nz;
490 ptr_L++;
491 *ptr_L = tmp_a1[0];
492 ptr_L++;
493 *ptr_L = tmp_a2[0];
494 ptr_L++;
495 *ptr_L = tmp_a3[0];
496 ptr_L++;
497
498 *ptr_L = nx;
499 ptr_L++;
500 *ptr_L = ny;
501 ptr_L++;
502 *ptr_L = nz;
503 ptr_L++;
504 *ptr_L = tmp_a1[1];
505 ptr_L++;
506 *ptr_L = tmp_a2[1];
507 ptr_L++;
508 *ptr_L = tmp_a3[1];
509 ptr_L++;
510
511 const __m128d verror =
512 _mm_add_pd(_mm_add_pd(vd, _mm_mul_pd(vnx, vx)), _mm_add_pd(_mm_mul_pd(vny, vy), _mm_mul_pd(vnz, vz)));
513 _mm_storeu_pd(ptr_error, verror);
514 ptr_error += 2;
515 }
516 }
517
518 for (; cpt < m_pointCloudFace.size(); cpt += 3) {
519 double x = m_pointCloudFace[cpt];
520 double y = m_pointCloudFace[cpt + 1];
521 double z = m_pointCloudFace[cpt + 2];
522
523 double _a1 = (nz * y) - (ny * z);
524 double _a2 = (nx * z) - (nz * x);
525 double _a3 = (ny * x) - (nx * y);
526
527 // L
528 L[(unsigned int)(cpt / 3)][0] = nx;
529 L[(unsigned int)(cpt / 3)][1] = ny;
530 L[(unsigned int)(cpt / 3)][2] = nz;
531 L[(unsigned int)(cpt / 3)][3] = _a1;
532 L[(unsigned int)(cpt / 3)][4] = _a2;
533 L[(unsigned int)(cpt / 3)][5] = _a3;
534
535 vpColVector normal(3);
536 normal[0] = nx;
537 normal[1] = ny;
538 normal[2] = nz;
539
540 vpColVector pt(3);
541 pt[0] = x;
542 pt[1] = y;
543 pt[2] = z;
544
545 // Error
546 error[(unsigned int)(cpt / 3)] = D + (normal.t() * pt);
547 }
548#endif
549 } else {
550 vpColVector normal(3);
551 normal[0] = nx;
552 normal[1] = ny;
553 normal[2] = nz;
554 vpColVector pt(3);
555
556 unsigned int idx = 0;
557 for (size_t i = 0; i < m_pointCloudFace.size(); i += 3, idx++) {
558 double x = m_pointCloudFace[i];
559 double y = m_pointCloudFace[i + 1];
560 double z = m_pointCloudFace[i + 2];
561
562 double _a1 = (nz * y) - (ny * z);
563 double _a2 = (nx * z) - (nz * x);
564 double _a3 = (ny * x) - (nx * y);
565
566 // L
567 L[idx][0] = nx;
568 L[idx][1] = ny;
569 L[idx][2] = nz;
570 L[idx][3] = _a1;
571 L[idx][4] = _a2;
572 L[idx][5] = _a3;
573
574 pt[0] = x;
575 pt[1] = y;
576 pt[2] = z;
577 // Error
578 error[idx] = D + (normal.t() * pt);
579 }
580 }
581}
582
583void vpMbtFaceDepthDense::computeROI(const vpHomogeneousMatrix &cMo, unsigned int width,
584 unsigned int height, std::vector<vpImagePoint> &roiPts
585#if DEBUG_DISPLAY_DEPTH_DENSE
586 ,
587 std::vector<std::vector<vpImagePoint> > &roiPts_vec
588#endif
589 ,
590 double &distanceToFace)
591{
592 if (m_useScanLine || m_clippingFlag > 2)
593 m_cam.computeFov(width, height);
594
595 if (m_useScanLine) {
596 for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
597 it->m_p1->changeFrame(cMo);
598 it->m_p2->changeFrame(cMo);
599
600 vpImagePoint ip1, ip2;
601
602 it->m_poly.changeFrame(cMo);
603 it->m_poly.computePolygonClipped(m_cam);
604
605 if (it->m_poly.polyClipped.size() == 2 &&
606 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
607 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
608 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
609 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
610 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
611 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
612
613 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
614 m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
615 true);
616
617 vpPoint faceCentroid;
618
619 for (unsigned int i = 0; i < linesLst.size(); i++) {
620 linesLst[i].first.project();
621 linesLst[i].second.project();
622
623 vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
624 vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
625
626 it->m_imPt1 = ip1;
627 it->m_imPt2 = ip2;
628
629 roiPts.push_back(ip1);
630 roiPts.push_back(ip2);
631
632 faceCentroid.set_X(faceCentroid.get_X() + linesLst[i].first.get_X() + linesLst[i].second.get_X());
633 faceCentroid.set_Y(faceCentroid.get_Y() + linesLst[i].first.get_Y() + linesLst[i].second.get_Y());
634 faceCentroid.set_Z(faceCentroid.get_Z() + linesLst[i].first.get_Z() + linesLst[i].second.get_Z());
635
636#if DEBUG_DISPLAY_DEPTH_DENSE
637 std::vector<vpImagePoint> roiPts_;
638 roiPts_.push_back(ip1);
639 roiPts_.push_back(ip2);
640 roiPts_vec.push_back(roiPts_);
641#endif
642 }
643
644 if (linesLst.empty()) {
645 distanceToFace = std::numeric_limits<double>::max();
646 } else {
647 faceCentroid.set_X(faceCentroid.get_X() / (2 * linesLst.size()));
648 faceCentroid.set_Y(faceCentroid.get_Y() / (2 * linesLst.size()));
649 faceCentroid.set_Z(faceCentroid.get_Z() / (2 * linesLst.size()));
650
651 distanceToFace =
652 sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
653 faceCentroid.get_Z() * faceCentroid.get_Z());
654 }
655 }
656 }
657 } else {
658 // Get polygon clipped
659 m_polygon->getRoiClipped(m_cam, roiPts, cMo);
660
661 // Get 3D polygon clipped
662 std::vector<vpPoint> polygonsClipped;
663 m_polygon->getPolygonClipped(polygonsClipped);
664
665 if (polygonsClipped.empty()) {
666 distanceToFace = std::numeric_limits<double>::max();
667 } else {
668 vpPoint faceCentroid;
669
670 for (size_t i = 0; i < polygonsClipped.size(); i++) {
671 faceCentroid.set_X(faceCentroid.get_X() + polygonsClipped[i].get_X());
672 faceCentroid.set_Y(faceCentroid.get_Y() + polygonsClipped[i].get_Y());
673 faceCentroid.set_Z(faceCentroid.get_Z() + polygonsClipped[i].get_Z());
674 }
675
676 faceCentroid.set_X(faceCentroid.get_X() / polygonsClipped.size());
677 faceCentroid.set_Y(faceCentroid.get_Y() / polygonsClipped.size());
678 faceCentroid.set_Z(faceCentroid.get_Z() / polygonsClipped.size());
679
680 distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
681 faceCentroid.get_Z() * faceCentroid.get_Z());
682 }
683
684#if DEBUG_DISPLAY_DEPTH_DENSE
685 roiPts_vec.push_back(roiPts);
686#endif
687 }
688}
689
691 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
692 bool displayFullModel)
693{
694 std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
695
696 for (size_t i = 0; i < models.size(); i++) {
697 vpImagePoint ip1(models[i][1], models[i][2]);
698 vpImagePoint ip2(models[i][3], models[i][4]);
699 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
700 }
701}
702
704 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
705 bool displayFullModel)
706{
707 std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
708
709 for (size_t i = 0; i < models.size(); i++) {
710 vpImagePoint ip1(models[i][1], models[i][2]);
711 vpImagePoint ip2(models[i][3], models[i][4]);
712 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
713 }
714}
715
717 const vpCameraParameters & /*cam*/, const double /*scale*/,
718 const unsigned int /*thickness*/)
719{
720}
721
723 const vpCameraParameters & /*cam*/, const double /*scale*/,
724 const unsigned int /*thickness*/)
725{
726}
727
739std::vector<std::vector<double> > vpMbtFaceDepthDense::getModelForDisplay(unsigned int width, unsigned int height,
740 const vpHomogeneousMatrix &cMo,
741 const vpCameraParameters &cam,
742 bool displayFullModel)
743{
744 std::vector<std::vector<double> > models;
745
746 if ((m_polygon->isVisible() && m_isTrackedDepthDenseFace) || displayFullModel) {
748
749 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
750 ++it) {
751 vpMbtDistanceLine *line = *it;
752 std::vector<std::vector<double> > lineModels = line->getModelForDisplay(width, height, cMo, cam, displayFullModel);
753 models.insert(models.end(), lineModels.begin(), lineModels.end());
754 }
755 }
756
757 return models;
758}
759
769bool vpMbtFaceDepthDense::samePoint(const vpPoint &P1, const vpPoint &P2) const
770{
771 double dx = fabs(P1.get_oX() - P2.get_oX());
772 double dy = fabs(P1.get_oY() - P2.get_oY());
773 double dz = fabs(P1.get_oZ() - P2.get_oZ());
774
775 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
776 dz <= std::numeric_limits<double>::epsilon())
777 return true;
778 else
779 return false;
780}
781
783{
784 m_cam = camera;
785
786 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
787 ++it) {
788 (*it)->setCameraParameters(camera);
789 }
790}
791
793{
794 m_useScanLine = v;
795
796 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
797 ++it) {
798 (*it)->useScanLine = v;
799 }
800}
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
bool isVisible(unsigned int i)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
vpMbScanLine & getMbScanLineRenderer()
Manage the line of a polygon used in the model-based tracker.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
vpMbtPolygon & getPolygon()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
double m_depthDenseFilteringMinDist
Minimum distance threshold.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
bool m_isVisible
Visibility flag.
double m_distFarClip
Distance for near clipping.
std::vector< double > m_pointCloudFace
List of depth points inside the face.
vpPlane m_planeObject
Plane equation described in the object frame.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void setCameraParameters(const vpCameraParameters &camera)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, std::vector< vpImagePoint > &roiPts, double &distanceToFace)
unsigned int getNbFeatures() const
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
void setScanLineVisibilityTest(bool v)
vpMbtPolygon * m_polygon
Polygon defining the face.
bool m_useScanLine
Scan line visibility.
bool m_isTrackedDepthDenseFace
Flag to define if the face should be tracked or not.
double m_depthDenseFilteringMaxDist
Maximum distance threshold.
std::vector< PolygonLine > m_polygonLines
Polygon lines used for scan-line visibility.
int m_depthDenseFilteringMethod
Method to use to consider or not the face.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
vpCameraParameters m_cam
Camera intrinsic parameters.
double m_depthDenseFilteringOccupancyRatio
Ratio between available depth points and theoretical number of points.
double m_distNearClip
Distance for near clipping.
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05, unsigned int thickness=1)
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
int getIndex() const
Definition: vpMbtPolygon.h:101
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
double getD() const
Definition: vpPlane.h:108
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
double getB() const
Definition: vpPlane.h:104
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_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:454
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
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
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
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 setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Defines a generic 2D polygon.
Definition: vpPolygon.h:104
vpRect getBoundingBox() const
Definition: vpPolygon.h:177
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getWidth() const
Definition: vpRect.h:228
void setTop(double pos)
Definition: vpRect.h:358
double getLeft() const
Definition: vpRect.h:174
void setLeft(double pos)
Definition: vpRect.h:322
void setRight(double pos)
Definition: vpRect.h:349
double getRight() const
Definition: vpRect.h:180
double getBottom() const
Definition: vpRect.h:98
double getHeight() const
Definition: vpRect.h:167
void setBottom(double pos)
Definition: vpRect.h:289
double getTop() const
Definition: vpRect.h:193
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
VISP_EXPORT bool checkSSE2()