Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
vpMbtFaceDepthNormal.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  * Manage depth normal features for a particular face.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/mbt/vpMbtFaceDepthNormal.h>
38 #include <visp3/mbt/vpMbtTukeyEstimator.h>
39 
40 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
41 #include <pcl/common/centroid.h>
42 #include <pcl/filters/extract_indices.h>
43 #include <pcl/segmentation/sac_segmentation.h>
44 #endif
45 
46 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
47 #include <emmintrin.h>
48 #define VISP_HAVE_SSE2 1
49 #endif
50 
51 #define USE_SSE_CODE 1
52 #if VISP_HAVE_SSE2 && USE_SSE_CODE
53 #define USE_SSE 1
54 #else
55 #define USE_SSE 0
56 #endif
57 
59  : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(nullptr),
60  m_planeObject(), m_polygon(nullptr), m_useScanLine(false), m_faceActivated(false),
61  m_faceCentroidMethod(GEOMETRIC_CENTROID), m_faceDesiredCentroid(), m_faceDesiredNormal(),
62  m_featureEstimationMethod(ROBUST_FEATURE_ESTIMATION), m_isTrackedDepthNormalFace(true), m_isVisible(false),
63  m_listOfFaceLines(), m_planeCamera(),
64  m_pclPlaneEstimationMethod(2), // SAC_MSAC, see pcl/sample_consensus/method_types.h
65  m_pclPlaneEstimationRansacMaxIter(200), m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines()
66 { }
67 
69 {
70  for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
71  delete m_listOfFaceLines[i];
72  }
73 }
74 
90  vpUniRand &rand_gen, int polygon, std::string name)
91 {
92  // Build a PolygonLine to be able to easily display the lines model
93  PolygonLine polygon_line;
94 
95  // Add polygon
96  polygon_line.m_poly.setNbPoint(2);
97  polygon_line.m_poly.addPoint(0, P1);
98  polygon_line.m_poly.addPoint(1, P2);
99 
100  polygon_line.m_poly.setClipping(m_clippingFlag);
101  polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
102  polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
103 
104  polygon_line.m_p1 = &polygon_line.m_poly.p[0];
105  polygon_line.m_p2 = &polygon_line.m_poly.p[1];
106 
107  m_polygonLines.push_back(polygon_line);
108 
109  // suppress line already in the model
110  bool already_here = false;
112 
113  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
114  ++it) {
115  l = *it;
116  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
117  already_here = true;
118  l->addPolygon(polygon);
119  l->hiddenface = faces;
121  }
122  }
123 
124  if (!already_here) {
125  l = new vpMbtDistanceLine;
126 
128  l->buildFrom(P1, P2, rand_gen);
129  l->addPolygon(polygon);
130  l->hiddenface = faces;
132 
133  l->setIndex((unsigned int)m_listOfFaceLines.size());
134  l->setName(name);
135 
138 
141 
144 
145  m_listOfFaceLines.push_back(l);
146  }
147 }
148 
149 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
150 bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width,
151  unsigned int height,
152  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
153  vpColVector &desired_features, unsigned int stepX, unsigned int stepY
154 #if DEBUG_DISPLAY_DEPTH_NORMAL
155  ,
156  vpImage<unsigned char> &debugImage,
157  std::vector<std::vector<vpImagePoint> > &roiPts_vec
158 #endif
159  ,
160  const vpImage<bool> *mask)
161 {
162  m_faceActivated = false;
163 
164  if (width == 0 || height == 0)
165  return false;
166 
167  std::vector<vpImagePoint> roiPts;
168  vpColVector desired_normal(3);
169 
170  computeROI(cMo, width, height, roiPts
171 #if DEBUG_DISPLAY_DEPTH_NORMAL
172  ,
173  roiPts_vec
174 #endif
175  );
176 
177  if (roiPts.size() <= 2) {
178 #ifndef NDEBUG
179  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
180 #endif
181  return false;
182  }
183 
184  vpPolygon polygon_2d(roiPts);
185  vpRect bb = polygon_2d.getBoundingBox();
186 
187  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
188  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
189  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
190  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
191 
192  bb.setTop(top);
193  bb.setBottom(bottom);
194  bb.setLeft(left);
195  bb.setRight(right);
196 
197  // Keep only 3D points inside the projected polygon face
198  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face(new pcl::PointCloud<pcl::PointXYZ>);
199  std::vector<double> point_cloud_face_vec, point_cloud_face_custom;
200 
202  point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
203  point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
204  }
206  point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
207  }
208  else if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
209  point_cloud_face->reserve((size_t)(bb.getWidth() * bb.getHeight()));
210  }
211 
213 #if !USE_SSE
214  checkSSE2 = false;
215 #else
216  bool push = false;
217  double prev_x, prev_y, prev_z;
218 #endif
219 
220  double x = 0.0, y = 0.0;
221  for (unsigned int i = top; i < bottom; i += stepY) {
222  for (unsigned int j = left; j < right; j += stepX) {
223  if (vpMeTracker::inRoiMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0 &&
224  (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
225  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
226  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
227  : polygon_2d.isInside(vpImagePoint(i, j)))) {
228 
229  if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
230  point_cloud_face->push_back((*point_cloud)(j, i));
231  }
234  point_cloud_face_vec.push_back((*point_cloud)(j, i).x);
235  point_cloud_face_vec.push_back((*point_cloud)(j, i).y);
236  point_cloud_face_vec.push_back((*point_cloud)(j, i).z);
237 
239  // Add point for custom method for plane equation estimation
241 
242  if (checkSSE2) {
243 #if USE_SSE
244  if (!push) {
245  push = true;
246  prev_x = x;
247  prev_y = y;
248  prev_z = (*point_cloud)(j, i).z;
249  }
250  else {
251  push = false;
252  point_cloud_face_custom.push_back(prev_x);
253  point_cloud_face_custom.push_back(x);
254 
255  point_cloud_face_custom.push_back(prev_y);
256  point_cloud_face_custom.push_back(y);
257 
258  point_cloud_face_custom.push_back(prev_z);
259  point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
260  }
261 #endif
262  }
263  else {
264  point_cloud_face_custom.push_back(x);
265  point_cloud_face_custom.push_back(y);
266  point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
267  }
268  }
269  }
270 
271 #if DEBUG_DISPLAY_DEPTH_NORMAL
272  debugImage[i][j] = 255;
273 #endif
274  }
275  }
276  }
277 
278 #if USE_SSE
279  if (checkSSE2 && push) {
280  point_cloud_face_custom.push_back(prev_x);
281  point_cloud_face_custom.push_back(prev_y);
282  point_cloud_face_custom.push_back(prev_z);
283  }
284 #endif
285 
286  if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) {
287  return false;
288  }
289 
290  // Face centroid computed by the different methods
291  vpColVector centroid_point(3);
292 
293  if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
294  if (!computeDesiredFeaturesPCL(point_cloud_face, desired_features, desired_normal, centroid_point)) {
295  return false;
296  }
297  }
299  computeDesiredFeaturesSVD(point_cloud_face_vec, cMo, desired_features, desired_normal, centroid_point);
300  }
302  computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face_vec, cMo, desired_features,
303  desired_normal, centroid_point);
304  }
305  else {
306  throw vpException(vpException::badValue, "Unknown feature estimation method!");
307  }
308 
309  computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
310 
311  m_faceActivated = true;
312 
313  return true;
314 }
315 #endif
316 
318  unsigned int height, const std::vector<vpColVector> &point_cloud,
319  vpColVector &desired_features, unsigned int stepX, unsigned int stepY
320 #if DEBUG_DISPLAY_DEPTH_NORMAL
321  ,
322  vpImage<unsigned char> &debugImage,
323  std::vector<std::vector<vpImagePoint> > &roiPts_vec
324 #endif
325  ,
326  const vpImage<bool> *mask)
327 {
328  m_faceActivated = false;
329 
330  if (width == 0 || height == 0)
331  return false;
332 
333  std::vector<vpImagePoint> roiPts;
334  vpColVector desired_normal(3);
335 
336  computeROI(cMo, width, height, roiPts
337 #if DEBUG_DISPLAY_DEPTH_NORMAL
338  ,
339  roiPts_vec
340 #endif
341  );
342 
343  if (roiPts.size() <= 2) {
344 #ifndef NDEBUG
345  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
346 #endif
347  return false;
348  }
349 
350  vpPolygon polygon_2d(roiPts);
351  vpRect bb = polygon_2d.getBoundingBox();
352 
353  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
354  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
355  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
356  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
357 
358  bb.setTop(top);
359  bb.setBottom(bottom);
360  bb.setLeft(left);
361  bb.setRight(right);
362 
363  // Keep only 3D points inside the projected polygon face
364  std::vector<double> point_cloud_face, point_cloud_face_custom;
365 
366  point_cloud_face.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
368  point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
369  }
370 
371  bool checkSSE2 = vpCPUFeatures::checkSSE2();
372 #if !USE_SSE
373  checkSSE2 = false;
374 #else
375  bool push = false;
376  double prev_x, prev_y, prev_z;
377 #endif
378 
379  double x = 0.0, y = 0.0;
380  for (unsigned int i = top; i < bottom; i += stepY) {
381  for (unsigned int j = left; j < right; j += stepX) {
382  if (vpMeTracker::inRoiMask(mask, i, j) && point_cloud[i * width + j][2] > 0 &&
383  (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
384  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
385  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
386  : polygon_2d.isInside(vpImagePoint(i, j)))) {
387 // Add point
388  point_cloud_face.push_back(point_cloud[i * width + j][0]);
389  point_cloud_face.push_back(point_cloud[i * width + j][1]);
390  point_cloud_face.push_back(point_cloud[i * width + j][2]);
391 
393  // Add point for custom method for plane equation estimation
395 
396  if (checkSSE2) {
397 #if USE_SSE
398  if (!push) {
399  push = true;
400  prev_x = x;
401  prev_y = y;
402  prev_z = point_cloud[i * width + j][2];
403  }
404  else {
405  push = false;
406  point_cloud_face_custom.push_back(prev_x);
407  point_cloud_face_custom.push_back(x);
408 
409  point_cloud_face_custom.push_back(prev_y);
410  point_cloud_face_custom.push_back(y);
411 
412  point_cloud_face_custom.push_back(prev_z);
413  point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
414  }
415 #endif
416  }
417  else {
418  point_cloud_face_custom.push_back(x);
419  point_cloud_face_custom.push_back(y);
420  point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
421  }
422  }
423 
424 #if DEBUG_DISPLAY_DEPTH_NORMAL
425  debugImage[i][j] = 255;
426 #endif
427  }
428  }
429  }
430 
431 #if USE_SSE
432  if (checkSSE2 && push) {
433  point_cloud_face_custom.push_back(prev_x);
434  point_cloud_face_custom.push_back(prev_y);
435  point_cloud_face_custom.push_back(prev_z);
436  }
437 #endif
438 
439  if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
440  return false;
441  }
442 
443  // Face centroid computed by the different methods
444  vpColVector centroid_point(3);
445 
446 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
447  if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
448  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(new pcl::PointCloud<pcl::PointXYZ>);
449  point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
450 
451  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
452  point_cloud_face_pcl->push_back(
453  pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
454  }
455 
456  computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point);
457  }
458  else
459 #endif
461  computeDesiredFeaturesSVD(point_cloud_face, cMo, desired_features, desired_normal, centroid_point);
462  }
464  computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face, cMo, desired_features,
465  desired_normal, centroid_point);
466  }
467  else {
468  throw vpException(vpException::badValue, "Unknown feature estimation method!");
469  }
470 
471  computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
472 
473  m_faceActivated = true;
474 
475  return true;
476 }
477 
479  unsigned int height, const vpMatrix &point_cloud,
480  vpColVector &desired_features, unsigned int stepX, unsigned int stepY
481 #if DEBUG_DISPLAY_DEPTH_NORMAL
482  ,
483  vpImage<unsigned char> &debugImage,
484  std::vector<std::vector<vpImagePoint> > &roiPts_vec
485 #endif
486  ,
487  const vpImage<bool> *mask)
488 {
489  m_faceActivated = false;
490 
491  if (width == 0 || height == 0)
492  return false;
493 
494  std::vector<vpImagePoint> roiPts;
495  vpColVector desired_normal(3);
496 
497  computeROI(cMo, width, height, roiPts
498 #if DEBUG_DISPLAY_DEPTH_NORMAL
499  ,
500  roiPts_vec
501 #endif
502  );
503 
504  if (roiPts.size() <= 2) {
505 #ifndef NDEBUG
506  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
507 #endif
508  return false;
509  }
510 
511  vpPolygon polygon_2d(roiPts);
512  vpRect bb = polygon_2d.getBoundingBox();
513 
514  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
515  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
516  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
517  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
518 
519  bb.setTop(top);
520  bb.setBottom(bottom);
521  bb.setLeft(left);
522  bb.setRight(right);
523 
524  // Keep only 3D points inside the projected polygon face
525  std::vector<double> point_cloud_face, point_cloud_face_custom;
526 
527  point_cloud_face.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
529  point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
530  }
531 
532  bool checkSSE2 = vpCPUFeatures::checkSSE2();
533 #if !USE_SSE
534  checkSSE2 = false;
535 #else
536  bool push = false;
537  double prev_x, prev_y, prev_z;
538 #endif
539 
540  double x = 0.0, y = 0.0;
541  for (unsigned int i = top; i < bottom; i += stepY) {
542  for (unsigned int j = left; j < right; j += stepX) {
543  if (vpMeTracker::inRoiMask(mask, i, j) && point_cloud[i * width + j][2] > 0 &&
544  (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
545  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
546  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
547  : polygon_2d.isInside(vpImagePoint(i, j)))) {
548 // Add point
549  point_cloud_face.push_back(point_cloud[i * width + j][0]);
550  point_cloud_face.push_back(point_cloud[i * width + j][1]);
551  point_cloud_face.push_back(point_cloud[i * width + j][2]);
552 
554  // Add point for custom method for plane equation estimation
556 
557  if (checkSSE2) {
558 #if USE_SSE
559  if (!push) {
560  push = true;
561  prev_x = x;
562  prev_y = y;
563  prev_z = point_cloud[i * width + j][2];
564  }
565  else {
566  push = false;
567  point_cloud_face_custom.push_back(prev_x);
568  point_cloud_face_custom.push_back(x);
569 
570  point_cloud_face_custom.push_back(prev_y);
571  point_cloud_face_custom.push_back(y);
572 
573  point_cloud_face_custom.push_back(prev_z);
574  point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
575  }
576 #endif
577  }
578  else {
579  point_cloud_face_custom.push_back(x);
580  point_cloud_face_custom.push_back(y);
581  point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
582  }
583  }
584 
585 #if DEBUG_DISPLAY_DEPTH_NORMAL
586  debugImage[i][j] = 255;
587 #endif
588  }
589  }
590  }
591 
592 #if USE_SSE
593  if (checkSSE2 && push) {
594  point_cloud_face_custom.push_back(prev_x);
595  point_cloud_face_custom.push_back(prev_y);
596  point_cloud_face_custom.push_back(prev_z);
597  }
598 #endif
599 
600  if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
601  return false;
602  }
603 
604  // Face centroid computed by the different methods
605  vpColVector centroid_point(3);
606 
607 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
608  if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
609  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(new pcl::PointCloud<pcl::PointXYZ>);
610  point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
611 
612  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
613  point_cloud_face_pcl->push_back(
614  pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
615  }
616 
617  computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point);
618  }
619  else
620 #endif
622  computeDesiredFeaturesSVD(point_cloud_face, cMo, desired_features, desired_normal, centroid_point);
623  }
625  computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face, cMo, desired_features,
626  desired_normal, centroid_point);
627  }
628  else {
629  throw vpException(vpException::badValue, "Unknown feature estimation method!");
630  }
631 
632  computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
633 
634  m_faceActivated = true;
635 
636  return true;
637 }
638 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
639 bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
640  vpColVector &desired_features, vpColVector &desired_normal,
641  vpColVector &centroid_point)
642 {
643  try {
644  // Compute plane equation for this subset of point cloud
645  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
646  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
647  // Create the segmentation object
648  pcl::SACSegmentation<pcl::PointXYZ> seg;
649  // Optional
650  seg.setOptimizeCoefficients(true);
651  // Mandatory
652  seg.setModelType(pcl::SACMODEL_PLANE);
653  seg.setMethodType(m_pclPlaneEstimationMethod);
654  seg.setDistanceThreshold(m_pclPlaneEstimationRansacThreshold);
655  seg.setMaxIterations(m_pclPlaneEstimationRansacMaxIter);
656 
657  seg.setInputCloud(point_cloud_face);
658  seg.segment(*inliers, *coefficients);
659 
660  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_extracted(new pcl::PointCloud<pcl::PointXYZ>);
661  // Create the filtering object
662  pcl::ExtractIndices<pcl::PointXYZ> extract;
663 
664  // Extract the inliers
665  extract.setInputCloud(point_cloud_face);
666  extract.setIndices(inliers);
667  extract.setNegative(false);
668  extract.filter(*point_cloud_face_extracted);
669 
670 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
671  pcl::PointXYZ centroid_point_pcl;
672  if (pcl::computeCentroid(*point_cloud_face_extracted, centroid_point_pcl)) {
673  pcl::PointXYZ face_normal;
674  computeNormalVisibility(coefficients->values[0], coefficients->values[1], coefficients->values[2],
675  centroid_point_pcl, face_normal);
676 
677  desired_features.resize(3, false);
678  desired_features[0] = -coefficients->values[0] / coefficients->values[3];
679  desired_features[1] = -coefficients->values[1] / coefficients->values[3];
680  desired_features[2] = -coefficients->values[2] / coefficients->values[3];
681 
682  desired_normal[0] = face_normal.x;
683  desired_normal[1] = face_normal.y;
684  desired_normal[2] = face_normal.z;
685 
686  centroid_point[0] = centroid_point_pcl.x;
687  centroid_point[1] = centroid_point_pcl.y;
688  centroid_point[2] = centroid_point_pcl.z;
689  }
690  else {
691  std::cerr << "Cannot compute centroid!" << std::endl;
692  return false;
693  }
694 #else
695  std::cerr << "Cannot compute centroid using PCL " << PCL_VERSION_PRETTY << "!" << std::endl;
696  return false;
697 #endif
698  }
699  catch (const pcl::PCLException &e) {
700  std::cerr << "Catch a PCL exception: " << e.what() << std::endl;
701  throw;
702  }
703 
704  return true;
705 }
706 #endif
707 
708 void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
709  const std::vector<double> &point_cloud_face,
710  const vpHomogeneousMatrix &cMo,
711  vpColVector &desired_features,
712  vpColVector &desired_normal,
713  vpColVector &centroid_point)
714 {
715  std::vector<double> weights;
716  double den = 0.0;
717  estimateFeatures(point_cloud_face_custom, cMo, desired_features, weights);
718 
719  // Compute face centroid
720  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
721  centroid_point[0] += weights[i] * point_cloud_face[3 * i];
722  centroid_point[1] += weights[i] * point_cloud_face[3 * i + 1];
723  centroid_point[2] += weights[i] * point_cloud_face[3 * i + 2];
724 
725  den += weights[i];
726  }
727 
728  centroid_point[0] /= den;
729  centroid_point[1] /= den;
730  centroid_point[2] /= den;
731 
732  computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
733  desired_normal);
734 }
735 
736 void vpMbtFaceDepthNormal::computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face,
737  const vpHomogeneousMatrix &cMo, vpColVector &desired_features,
738  vpColVector &desired_normal, vpColVector &centroid_point)
739 {
740  vpColVector plane_equation_SVD;
741  estimatePlaneEquationSVD(point_cloud_face, cMo, plane_equation_SVD, centroid_point);
742 
743  desired_features.resize(3, false);
744  desired_features[0] = -plane_equation_SVD[0] / plane_equation_SVD[3];
745  desired_features[1] = -plane_equation_SVD[1] / plane_equation_SVD[3];
746  desired_features[2] = -plane_equation_SVD[2] / plane_equation_SVD[3];
747 
748  computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
749  desired_normal);
750 }
751 
753  const vpColVector &desired_normal,
754  const vpColVector &centroid_point)
755 {
756  // Compute desired centroid in the object frame
757  vpColVector centroid_cam(4);
758  centroid_cam[0] = centroid_point[0];
759  centroid_cam[1] = centroid_point[1];
760  centroid_cam[2] = centroid_point[2];
761  centroid_cam[3] = 1;
762 
763  vpColVector centroid_obj = cMo.inverse() * centroid_cam;
764  m_faceDesiredCentroid.setWorldCoordinates(centroid_obj[0], centroid_obj[1], centroid_obj[2]);
765 
766  // Compute desired face normal in the object frame
767  vpColVector face_normal_cam(4);
768  face_normal_cam[0] = desired_normal[0];
769  face_normal_cam[1] = desired_normal[1];
770  face_normal_cam[2] = desired_normal[2];
771  face_normal_cam[3] = 1;
772 
773  vpColVector face_normal_obj = cMo.inverse() * face_normal_cam;
774  m_faceDesiredNormal.setWorldCoordinates(face_normal_obj[0], face_normal_obj[1], face_normal_obj[2]);
775 }
776 
777 bool vpMbtFaceDepthNormal::computePolygonCentroid(const std::vector<vpPoint> &points_, vpPoint &centroid)
778 {
779  if (points_.empty()) {
780  return false;
781  }
782 
783  if (points_.size() < 2) {
784  centroid = points_[0];
785  return true;
786  }
787 
788  std::vector<vpPoint> points = points_;
789  points.push_back(points_.front());
790 
791  double A1 = 0.0, A2 = 0.0, c_x1 = 0.0, c_x2 = 0.0, c_y = 0.0, c_z = 0.0;
792 
793  for (size_t i = 0; i < points.size() - 1; i++) {
794  // projection onto xy plane
795  c_x1 += (points[i].get_X() + points[i + 1].get_X()) *
796  (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
797  c_y += (points[i].get_Y() + points[i + 1].get_Y()) *
798  (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
799  A1 += points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y();
800 
801  // projection onto xz plane
802  c_x2 += (points[i].get_X() + points[i + 1].get_X()) *
803  (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
804  c_z += (points[i].get_Z() + points[i + 1].get_Z()) *
805  (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
806  A2 += points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z();
807  }
808 
809  c_x1 /= 3.0 * A1;
810  c_y /= 3.0 * A1;
811  c_x2 /= 3.0 * A2;
812  c_z /= 3.0 * A2;
813 
814  if (A1 > A2) {
815  centroid.set_X(c_x1);
816  }
817  else {
818  centroid.set_X(c_x2);
819  }
820 
821  centroid.set_Y(c_y);
822  centroid.set_Z(c_z);
823 
824  return true;
825 }
826 
827 void vpMbtFaceDepthNormal::computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
828  std::vector<vpImagePoint> &roiPts
829 #if DEBUG_DISPLAY_DEPTH_NORMAL
830  ,
831  std::vector<std::vector<vpImagePoint> > &roiPts_vec
832 #endif
833 )
834 {
835  if (m_useScanLine || m_clippingFlag > 2)
836  m_cam.computeFov(width, height);
837 
838  if (m_useScanLine) {
839  for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
840  it->m_p1->changeFrame(cMo);
841  it->m_p2->changeFrame(cMo);
842 
843  vpImagePoint ip1, ip2;
844 
845  it->m_poly.changeFrame(cMo);
846  it->m_poly.computePolygonClipped(m_cam);
847 
848  if (it->m_poly.polyClipped.size() == 2 &&
849  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
850  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
851  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
852  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
853  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
854  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
855 
856  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
857  m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
858  true);
859 
860  for (unsigned int i = 0; i < linesLst.size(); i++) {
861  linesLst[i].first.project();
862  linesLst[i].second.project();
863 
864  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
865  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
866 
867  it->m_imPt1 = ip1;
868  it->m_imPt2 = ip2;
869 
870  roiPts.push_back(ip1);
871  roiPts.push_back(ip2);
872 
873 #if DEBUG_DISPLAY_DEPTH_NORMAL
874  std::vector<vpImagePoint> roiPts_;
875  roiPts_.push_back(ip1);
876  roiPts_.push_back(ip2);
877  roiPts_vec.push_back(roiPts_);
878 #endif
879  }
880  }
881  }
882  }
883  else {
884  // Get polygon clipped
885  m_polygon->getRoiClipped(m_cam, roiPts, cMo);
886 
887 #if DEBUG_DISPLAY_DEPTH_NORMAL
888  roiPts_vec.push_back(roiPts);
889 #endif
890  }
891 }
892 
894 
896 {
897  // Compute lines visibility, only for display
898  vpMbtDistanceLine *line;
899  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
900  ++it) {
901  line = *it;
902  bool isvisible = false;
903 
904  for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
905  ++itindex) {
906  int index = *itindex;
907  if (index == -1) {
908  isvisible = true;
909  }
910  else {
911  if (line->hiddenface->isVisible((unsigned int)index)) {
912  isvisible = true;
913  }
914  }
915  }
916 
917  // Si la ligne n'appartient a aucune face elle est tout le temps visible
918  if (line->Lindex_polygon.empty())
919  isvisible = true; // Not sure that this can occur
920 
921  if (isvisible) {
922  line->setVisible(true);
923  }
924  else {
925  line->setVisible(false);
926  }
927  }
928 }
929 
930 void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double nz, const vpHomogeneousMatrix &cMo,
931  const vpCameraParameters &camera, vpColVector &correct_normal,
932  vpPoint &centroid)
933 {
934  vpColVector faceNormal(3);
935  faceNormal[0] = nx;
936  faceNormal[1] = ny;
937  faceNormal[2] = nz;
938  faceNormal.normalize();
939 
940  // Get polygon clipped
941  std::vector<vpImagePoint> roiPts;
942  m_polygon->getRoiClipped(camera, roiPts, cMo);
943 
944  std::vector<vpPoint> polyPts;
945  m_polygon->getPolygonClipped(polyPts);
946 
947  vpColVector e4(3);
949  computePolygonCentroid(polyPts, centroid);
950  centroid.project();
951 
952  e4[0] = -centroid.get_X();
953  e4[1] = -centroid.get_Y();
954  e4[2] = -centroid.get_Z();
955  e4.normalize();
956  }
957  else {
958  double centroid_x = 0.0;
959  double centroid_y = 0.0;
960  double centroid_z = 0.0;
961 
962  for (size_t i = 0; i < polyPts.size(); i++) {
963  centroid_x += polyPts[i].get_X();
964  centroid_y += polyPts[i].get_Y();
965  centroid_z += polyPts[i].get_Z();
966  }
967 
968  centroid_x /= polyPts.size();
969  centroid_y /= polyPts.size();
970  centroid_z /= polyPts.size();
971 
972  e4[0] = -centroid_x;
973  e4[1] = -centroid_y;
974  e4[2] = -centroid_z;
975  e4.normalize();
976 
977  centroid.set_X(centroid_x);
978  centroid.set_Y(centroid_y);
979  centroid.set_Z(centroid_z);
980  }
981 
982  correct_normal.resize(3, false);
983  double angle = acos(vpColVector::dotProd(e4, faceNormal));
984  if (angle < M_PI_2) {
985  correct_normal = faceNormal;
986  }
987  else {
988  correct_normal[0] = -faceNormal[0];
989  correct_normal[1] = -faceNormal[1];
990  correct_normal[2] = -faceNormal[2];
991  }
992 }
993 
994 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
995 void vpMbtFaceDepthNormal::computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ &centroid_point,
996  pcl::PointXYZ &face_normal)
997 {
998  vpColVector faceNormal(3);
999  faceNormal[0] = nx;
1000  faceNormal[1] = ny;
1001  faceNormal[2] = nz;
1002  faceNormal.normalize();
1003 
1004  vpColVector e4(3);
1005  e4[0] = -centroid_point.x;
1006  e4[1] = -centroid_point.y;
1007  e4[2] = -centroid_point.z;
1008  e4.normalize();
1009 
1010  double angle = acos(vpColVector::dotProd(e4, faceNormal));
1011  if (angle < M_PI_2) {
1012  face_normal = pcl::PointXYZ(faceNormal[0], faceNormal[1], faceNormal[2]);
1013  }
1014  else {
1015  face_normal = pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]);
1016  }
1017 }
1018 #endif
1019 
1020 void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
1021  vpColVector &face_normal)
1022 {
1023  face_normal.resize(3, false);
1024  face_normal[0] = nx;
1025  face_normal[1] = ny;
1026  face_normal[2] = nz;
1027  face_normal.normalize();
1028 
1029  vpColVector e4 = -centroid_point;
1030  e4.normalize();
1031 
1032  double angle = acos(vpColVector::dotProd(e4, face_normal));
1033  if (angle >= M_PI_2) {
1034  face_normal[0] = -face_normal[0];
1035  face_normal[1] = -face_normal[1];
1036  face_normal[2] = -face_normal[2];
1037  }
1038 }
1039 
1041 {
1042  L.resize(3, 6, false, false);
1043 
1044  // Transform the plane equation for the current pose
1047 
1048  double ux = m_planeCamera.getA();
1049  double uy = m_planeCamera.getB();
1050  double uz = m_planeCamera.getC();
1051  double D = m_planeCamera.getD();
1052  double D2 = D * D;
1053 
1054  // Features
1055  features.resize(3, false);
1056  features[0] = -ux / D;
1057  features[1] = -uy / D;
1058  features[2] = -uz / D;
1059 
1060  // L_A
1061  L[0][0] = ux * ux / D2;
1062  L[0][1] = ux * uy / D2;
1063  L[0][2] = ux * uz / D2;
1064  L[0][3] = 0.0;
1065  L[0][4] = uz / D;
1066  L[0][5] = -uy / D;
1067 
1068  // L_B
1069  L[1][0] = ux * uy / D2;
1070  L[1][1] = uy * uy / D2;
1071  L[1][2] = uy * uz / D2;
1072  L[1][3] = -uz / D;
1073  L[1][4] = 0.0;
1074  L[1][5] = ux / D;
1075 
1076  // L_C
1077  L[2][0] = ux * uz / D2;
1078  L[2][1] = uy * uz / D2;
1079  L[2][2] = uz * uz / D2;
1080  L[2][3] = uy / D;
1081  L[2][4] = -ux / D;
1082  L[2][5] = 0.0;
1083 }
1084 
1086  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1087  bool displayFullModel)
1088 {
1089  std::vector<std::vector<double> > models =
1090  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1091 
1092  for (size_t i = 0; i < models.size(); i++) {
1093  vpImagePoint ip1(models[i][1], models[i][2]);
1094  vpImagePoint ip2(models[i][3], models[i][4]);
1095  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1096  }
1097 }
1098 
1100  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1101  bool displayFullModel)
1102 {
1103  std::vector<std::vector<double> > models =
1104  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1105 
1106  for (size_t i = 0; i < models.size(); i++) {
1107  vpImagePoint ip1(models[i][1], models[i][2]);
1108  vpImagePoint ip2(models[i][3], models[i][4]);
1109  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1110  }
1111 }
1112 
1114  const vpCameraParameters &cam, double scale, unsigned int thickness)
1115 {
1117  // Desired feature
1118  vpPoint pt_centroid = m_faceDesiredCentroid;
1119  pt_centroid.changeFrame(cMo);
1120  pt_centroid.project();
1121 
1122  vpImagePoint im_centroid;
1123  vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1124 
1125  vpPoint pt_normal = m_faceDesiredNormal;
1126  pt_normal.changeFrame(cMo);
1127  pt_normal.project();
1128 
1129  vpPoint pt_extremity;
1130  pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
1131  pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
1132  pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
1133  pt_extremity.project();
1134 
1135  vpImagePoint im_extremity;
1136  vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1137 
1138  vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness);
1139 
1140  // Current feature
1141  // Transform the plane equation for the current pose
1144 
1145  double ux = m_planeCamera.getA();
1146  double uy = m_planeCamera.getB();
1147  double uz = m_planeCamera.getC();
1148 
1149  vpColVector correct_normal;
1150  vpCameraParameters cam_copy = cam;
1151  cam_copy.computeFov(I.getWidth(), I.getHeight());
1152  computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid);
1153 
1154  pt_centroid.project();
1155  vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1156 
1157  pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
1158  pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
1159  pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
1160  pt_extremity.project();
1161 
1162  vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1163 
1164  vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
1165  }
1166 }
1167 
1169  const vpCameraParameters &cam, double scale, unsigned int thickness)
1170 {
1172  // Desired feature
1173  vpPoint pt_centroid = m_faceDesiredCentroid;
1174  pt_centroid.changeFrame(cMo);
1175  pt_centroid.project();
1176 
1177  vpImagePoint im_centroid;
1178  vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1179 
1180  vpPoint pt_normal = m_faceDesiredNormal;
1181  pt_normal.changeFrame(cMo);
1182  pt_normal.project();
1183 
1184  vpPoint pt_extremity;
1185  pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
1186  pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
1187  pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
1188  pt_extremity.project();
1189 
1190  vpImagePoint im_extremity;
1191  vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1192 
1193  vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness);
1194 
1195  // Current feature
1196  // Transform the plane equation for the current pose
1199 
1200  double ux = m_planeCamera.getA();
1201  double uy = m_planeCamera.getB();
1202  double uz = m_planeCamera.getC();
1203 
1204  vpColVector correct_normal;
1205  vpCameraParameters cam_copy = cam;
1206  cam_copy.computeFov(I.getWidth(), I.getHeight());
1207  computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid);
1208 
1209  pt_centroid.project();
1210  vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1211 
1212  pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
1213  pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
1214  pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
1215  pt_extremity.project();
1216 
1217  vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1218 
1219  vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
1220  }
1221 }
1222 
1223 void vpMbtFaceDepthNormal::estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
1224  vpColVector &x_estimated, std::vector<double> &w)
1225 {
1226  vpMbtTukeyEstimator<double> tukey_robust;
1227  std::vector<double> residues(point_cloud_face.size() / 3);
1228 
1229  w.resize(point_cloud_face.size() / 3, 1.0);
1230 
1231  unsigned int max_iter = 30, iter = 0;
1232  double error = 0.0, prev_error = -1.0;
1233  double A = 0.0, B = 0.0, C = 0.0;
1234 
1235  Mat33<double> ATA_3x3;
1236 
1237  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1238 #if !USE_SSE
1239  checkSSE2 = false;
1240 #endif
1241 
1242  if (checkSSE2) {
1243 #if USE_SSE
1244  while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1245  if (iter == 0) {
1246  // Transform the plane equation for the current pose
1249 
1250  double ux = m_planeCamera.getA();
1251  double uy = m_planeCamera.getB();
1252  double uz = m_planeCamera.getC();
1253  double D = m_planeCamera.getD();
1254 
1255  // Features
1256  A = -ux / D;
1257  B = -uy / D;
1258  C = -uz / D;
1259 
1260  size_t cpt = 0;
1261  if (point_cloud_face.size() / 3 >= 2) {
1262  const double *ptr_point_cloud = &point_cloud_face[0];
1263  const __m128d vA = _mm_set1_pd(A);
1264  const __m128d vB = _mm_set1_pd(B);
1265  const __m128d vC = _mm_set1_pd(C);
1266  const __m128d vones = _mm_set1_pd(1.0);
1267 
1268  double *ptr_residues = &residues[0];
1269 
1270  for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1271  const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1272  const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1273  const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1274  const __m128d vinvZi = _mm_div_pd(vones, vZi);
1275 
1276  const __m128d tmp =
1277  _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1278  _mm_storeu_pd(ptr_residues, tmp);
1279  }
1280  }
1281 
1282  for (; cpt < point_cloud_face.size(); cpt += 3) {
1283  double xi = point_cloud_face[cpt];
1284  double yi = point_cloud_face[cpt + 1];
1285  double Zi = point_cloud_face[cpt + 2];
1286 
1287  residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
1288  }
1289  }
1290 
1291  tukey_robust.MEstimator(residues, w, 1e-2);
1292 
1293  __m128d vsum_wi2_xi2 = _mm_setzero_pd();
1294  __m128d vsum_wi2_yi2 = _mm_setzero_pd();
1295  __m128d vsum_wi2 = _mm_setzero_pd();
1296  __m128d vsum_wi2_xi_yi = _mm_setzero_pd();
1297  __m128d vsum_wi2_xi = _mm_setzero_pd();
1298  __m128d vsum_wi2_yi = _mm_setzero_pd();
1299 
1300  __m128d vsum_wi2_xi_Zi = _mm_setzero_pd();
1301  __m128d vsum_wi2_yi_Zi = _mm_setzero_pd();
1302  __m128d vsum_wi2_Zi = _mm_setzero_pd();
1303 
1304  // Estimate A, B, C
1305  size_t cpt = 0;
1306  if (point_cloud_face.size() / 3 >= 2) {
1307  const double *ptr_point_cloud = &point_cloud_face[0];
1308  double *ptr_w = &w[0];
1309 
1310  const __m128d vones = _mm_set1_pd(1.0);
1311 
1312  for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_w += 2) {
1313  const __m128d vwi2 = _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w));
1314 
1315  const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1316  const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1317  const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1318  const __m128d vinvZi = _mm_div_pd(vones, vZi);
1319 
1320  vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi)));
1321  vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi)));
1322  vsum_wi2 = _mm_add_pd(vsum_wi2, vwi2);
1323  vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi)));
1324  vsum_wi2_xi = _mm_add_pd(vsum_wi2_xi, _mm_mul_pd(vwi2, vxi));
1325  vsum_wi2_yi = _mm_add_pd(vsum_wi2_yi, _mm_mul_pd(vwi2, vyi));
1326 
1327  const __m128d vwi2_invZi = _mm_mul_pd(vwi2, vinvZi);
1328  vsum_wi2_xi_Zi = _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi));
1329  vsum_wi2_yi_Zi = _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi));
1330  vsum_wi2_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi);
1331  }
1332  }
1333 
1334  double vtmp[2];
1335  _mm_storeu_pd(vtmp, vsum_wi2_xi2);
1336  double sum_wi2_xi2 = vtmp[0] + vtmp[1];
1337 
1338  _mm_storeu_pd(vtmp, vsum_wi2_yi2);
1339  double sum_wi2_yi2 = vtmp[0] + vtmp[1];
1340 
1341  _mm_storeu_pd(vtmp, vsum_wi2);
1342  double sum_wi2 = vtmp[0] + vtmp[1];
1343 
1344  _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
1345  double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
1346 
1347  _mm_storeu_pd(vtmp, vsum_wi2_xi);
1348  double sum_wi2_xi = vtmp[0] + vtmp[1];
1349 
1350  _mm_storeu_pd(vtmp, vsum_wi2_yi);
1351  double sum_wi2_yi = vtmp[0] + vtmp[1];
1352 
1353  _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
1354  double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
1355 
1356  _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
1357  double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
1358 
1359  _mm_storeu_pd(vtmp, vsum_wi2_Zi);
1360  double sum_wi2_Zi = vtmp[0] + vtmp[1];
1361 
1362  for (; cpt < point_cloud_face.size(); cpt += 3) {
1363  double wi2 = w[cpt / 3] * w[cpt / 3];
1364 
1365  double xi = point_cloud_face[cpt];
1366  double yi = point_cloud_face[cpt + 1];
1367  double Zi = point_cloud_face[cpt + 2];
1368  double invZi = 1.0 / Zi;
1369 
1370  sum_wi2_xi2 += wi2 * xi * xi;
1371  sum_wi2_yi2 += wi2 * yi * yi;
1372  sum_wi2 += wi2;
1373  sum_wi2_xi_yi += wi2 * xi * yi;
1374  sum_wi2_xi += wi2 * xi;
1375  sum_wi2_yi += wi2 * yi;
1376 
1377  sum_wi2_xi_Zi += wi2 * xi * invZi;
1378  sum_wi2_yi_Zi += wi2 * yi * invZi;
1379  sum_wi2_Zi += wi2 * invZi;
1380  }
1381 
1382  ATA_3x3[0] = sum_wi2_xi2;
1383  ATA_3x3[1] = sum_wi2_xi_yi;
1384  ATA_3x3[2] = sum_wi2_xi;
1385  ATA_3x3[3] = sum_wi2_xi_yi;
1386  ATA_3x3[4] = sum_wi2_yi2;
1387  ATA_3x3[5] = sum_wi2_yi;
1388  ATA_3x3[6] = sum_wi2_xi;
1389  ATA_3x3[7] = sum_wi2_yi;
1390  ATA_3x3[8] = sum_wi2;
1391 
1392  Mat33<double> minv = ATA_3x3.inverse();
1393 
1394  A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1395  B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1396  C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1397 
1398  cpt = 0;
1399 
1400  // Compute error
1401  prev_error = error;
1402  error = 0.0;
1403 
1404  __m128d verror = _mm_set1_pd(0.0);
1405  if (point_cloud_face.size() / 3 >= 2) {
1406  const double *ptr_point_cloud = &point_cloud_face[0];
1407  const __m128d vA = _mm_set1_pd(A);
1408  const __m128d vB = _mm_set1_pd(B);
1409  const __m128d vC = _mm_set1_pd(C);
1410  const __m128d vones = _mm_set1_pd(1.0);
1411 
1412  double *ptr_residues = &residues[0];
1413 
1414  for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1415  const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1416  const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1417  const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1418  const __m128d vinvZi = _mm_div_pd(vones, vZi);
1419 
1420  const __m128d tmp = _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1421  verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp));
1422 
1423  _mm_storeu_pd(ptr_residues, tmp);
1424  }
1425  }
1426 
1427  _mm_storeu_pd(vtmp, verror);
1428  error = vtmp[0] + vtmp[1];
1429 
1430  for (size_t idx = cpt; idx < point_cloud_face.size(); idx += 3) {
1431  double xi = point_cloud_face[idx];
1432  double yi = point_cloud_face[idx + 1];
1433  double Zi = point_cloud_face[idx + 2];
1434 
1435  error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1436  residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
1437  }
1438 
1439  error /= point_cloud_face.size() / 3;
1440 
1441  iter++;
1442  } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
1443 #endif
1444  }
1445  else {
1446  while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1447  if (iter == 0) {
1448  // Transform the plane equation for the current pose
1451 
1452  double ux = m_planeCamera.getA();
1453  double uy = m_planeCamera.getB();
1454  double uz = m_planeCamera.getC();
1455  double D = m_planeCamera.getD();
1456 
1457  // Features
1458  A = -ux / D;
1459  B = -uy / D;
1460  C = -uz / D;
1461 
1462  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1463  double xi = point_cloud_face[3 * i];
1464  double yi = point_cloud_face[3 * i + 1];
1465  double Zi = point_cloud_face[3 * i + 2];
1466 
1467  residues[i] = (A * xi + B * yi + C - 1 / Zi);
1468  }
1469  }
1470 
1471  tukey_robust.MEstimator(residues, w, 1e-2);
1472 
1473  // Estimate A, B, C
1474  double sum_wi2_xi2 = 0.0, sum_wi2_yi2 = 0.0, sum_wi2 = 0.0;
1475  double sum_wi2_xi_yi = 0.0, sum_wi2_xi = 0.0, sum_wi2_yi = 0.0;
1476 
1477  double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
1478 
1479  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1480  double wi2 = w[i] * w[i];
1481 
1482  double xi = point_cloud_face[3 * i];
1483  double yi = point_cloud_face[3 * i + 1];
1484  double Zi = point_cloud_face[3 * i + 2];
1485  double invZi = 1 / Zi;
1486 
1487  sum_wi2_xi2 += wi2 * xi * xi;
1488  sum_wi2_yi2 += wi2 * yi * yi;
1489  sum_wi2 += wi2;
1490  sum_wi2_xi_yi += wi2 * xi * yi;
1491  sum_wi2_xi += wi2 * xi;
1492  sum_wi2_yi += wi2 * yi;
1493 
1494  sum_wi2_xi_Zi += wi2 * xi * invZi;
1495  sum_wi2_yi_Zi += wi2 * yi * invZi;
1496  sum_wi2_Zi += wi2 * invZi;
1497  }
1498 
1499  ATA_3x3[0] = sum_wi2_xi2;
1500  ATA_3x3[1] = sum_wi2_xi_yi;
1501  ATA_3x3[2] = sum_wi2_xi;
1502  ATA_3x3[3] = sum_wi2_xi_yi;
1503  ATA_3x3[4] = sum_wi2_yi2;
1504  ATA_3x3[5] = sum_wi2_yi;
1505  ATA_3x3[6] = sum_wi2_xi;
1506  ATA_3x3[7] = sum_wi2_yi;
1507  ATA_3x3[8] = sum_wi2;
1508 
1509  Mat33<double> minv = ATA_3x3.inverse();
1510 
1511  A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1512  B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1513  C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1514 
1515  prev_error = error;
1516  error = 0.0;
1517 
1518  // Compute error
1519  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1520  double xi = point_cloud_face[3 * i];
1521  double yi = point_cloud_face[3 * i + 1];
1522  double Zi = point_cloud_face[3 * i + 2];
1523 
1524  error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1525  residues[i] = (A * xi + B * yi + C - 1 / Zi);
1526  }
1527 
1528  error /= point_cloud_face.size() / 3;
1529 
1530  iter++;
1531  } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
1532  }
1533 
1534  x_estimated.resize(3, false);
1535  x_estimated[0] = A;
1536  x_estimated[1] = B;
1537  x_estimated[2] = C;
1538 }
1539 
1540 void vpMbtFaceDepthNormal::estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face,
1541  const vpHomogeneousMatrix &cMo,
1542  vpColVector &plane_equation_estimated, vpColVector &centroid)
1543 {
1544  unsigned int max_iter = 10;
1545  double prev_error = 1e3;
1546  double error = 1e3 - 1;
1547 
1548  std::vector<double> weights(point_cloud_face.size() / 3, 1.0);
1549  std::vector<double> residues(point_cloud_face.size() / 3);
1550  vpMatrix M((unsigned int)(point_cloud_face.size() / 3), 3);
1551  vpMbtTukeyEstimator<double> tukey;
1552  vpColVector normal;
1553 
1554  for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
1555  if (iter != 0) {
1556  tukey.MEstimator(residues, weights, 1e-4);
1557  }
1558  else {
1559  // Transform the plane equation for the current pose
1562 
1563  double A = m_planeCamera.getA();
1564  double B = m_planeCamera.getB();
1565  double C = m_planeCamera.getC();
1566  double D = m_planeCamera.getD();
1567 
1568  // Compute distance point to estimated plane
1569  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1570  residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1571  C * point_cloud_face[3 * i + 2] + D) /
1572  sqrt(A * A + B * B + C * C);
1573  }
1574 
1575  tukey.MEstimator(residues, weights, 1e-4);
1576  plane_equation_estimated.resize(4, false);
1577  }
1578 
1579  // Compute centroid
1580  double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
1581  double total_w = 0.0;
1582 
1583  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1584  centroid_x += weights[i] * point_cloud_face[3 * i];
1585  centroid_y += weights[i] * point_cloud_face[3 * i + 1];
1586  centroid_z += weights[i] * point_cloud_face[3 * i + 2];
1587  total_w += weights[i];
1588  }
1589 
1590  centroid_x /= total_w;
1591  centroid_y /= total_w;
1592  centroid_z /= total_w;
1593 
1594  // Minimization
1595  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1596  M[(unsigned int)i][0] = weights[i] * (point_cloud_face[3 * i] - centroid_x);
1597  M[(unsigned int)i][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
1598  M[(unsigned int)i][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
1599  }
1600 
1601  vpMatrix J = M.t() * M;
1602 
1603  vpColVector W;
1604  vpMatrix V;
1605  J.svd(W, V);
1606 
1607  double smallestSv = W[0];
1608  unsigned int indexSmallestSv = 0;
1609  for (unsigned int i = 1; i < W.size(); i++) {
1610  if (W[i] < smallestSv) {
1611  smallestSv = W[i];
1612  indexSmallestSv = i;
1613  }
1614  }
1615 
1616  normal = V.getCol(indexSmallestSv);
1617 
1618  // Compute plane equation
1619  double A = normal[0], B = normal[1], C = normal[2];
1620  double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
1621 
1622  // Update plane equation
1623  plane_equation_estimated[0] = A;
1624  plane_equation_estimated[1] = B;
1625  plane_equation_estimated[2] = C;
1626  plane_equation_estimated[3] = D;
1627 
1628  // Compute error points to estimated plane
1629  prev_error = error;
1630  error = 0.0;
1631  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1632  residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1633  C * point_cloud_face[3 * i + 2] + D) /
1634  sqrt(A * A + B * B + C * C);
1635  error += weights[i] * residues[i];
1636  }
1637  error /= total_w;
1638  }
1639 
1640  // Update final weights
1641  tukey.MEstimator(residues, weights, 1e-4);
1642 
1643  // Update final centroid
1644  centroid.resize(3, false);
1645  double total_w = 0.0;
1646 
1647  for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1648  centroid[0] += weights[i] * point_cloud_face[3 * i];
1649  centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
1650  centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
1651  total_w += weights[i];
1652  }
1653 
1654  centroid[0] /= total_w;
1655  centroid[1] /= total_w;
1656  centroid[2] /= total_w;
1657 
1658  // Compute final plane equation
1659  double A = normal[0], B = normal[1], C = normal[2];
1660  double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
1661 
1662  // Update final plane equation
1663  plane_equation_estimated[0] = A;
1664  plane_equation_estimated[1] = B;
1665  plane_equation_estimated[2] = C;
1666  plane_equation_estimated[3] = D;
1667 }
1668 
1674 std::vector<std::vector<double> >
1676 {
1677  std::vector<std::vector<double> > features;
1678 
1680  // Desired feature
1681  vpPoint pt_centroid = m_faceDesiredCentroid;
1682  pt_centroid.changeFrame(cMo);
1683  pt_centroid.project();
1684 
1685  vpImagePoint im_centroid;
1686  vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1687 
1688  vpPoint pt_normal = m_faceDesiredNormal;
1689  pt_normal.changeFrame(cMo);
1690  pt_normal.project();
1691 
1692  vpPoint pt_extremity;
1693  pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
1694  pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
1695  pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
1696  pt_extremity.project();
1697 
1698  vpImagePoint im_extremity;
1699  vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1700 
1701  {
1702 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
1703  std::vector<double> params = { 2, //desired normal
1704  im_centroid.get_i(),
1705  im_centroid.get_j(),
1706  im_extremity.get_i(),
1707  im_extremity.get_j() };
1708 #else
1709  std::vector<double> params;
1710  params.push_back(2); //desired normal
1711  params.push_back(im_centroid.get_i());
1712  params.push_back(im_centroid.get_j());
1713  params.push_back(im_extremity.get_i());
1714  params.push_back(im_extremity.get_j());
1715 #endif
1716  features.push_back(params);
1717  }
1718 
1719  // Current feature
1720  // Transform the plane equation for the current pose
1723 
1724  double ux = m_planeCamera.getA();
1725  double uy = m_planeCamera.getB();
1726  double uz = m_planeCamera.getC();
1727 
1728  vpColVector correct_normal;
1729  computeNormalVisibility(ux, uy, uz, cMo, cam, correct_normal, pt_centroid);
1730 
1731  pt_centroid.project();
1732  vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1733 
1734  pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
1735  pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
1736  pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
1737  pt_extremity.project();
1738 
1739  vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1740 
1741  {
1742 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
1743  std::vector<double> params = { 3, //normal at current pose
1744  im_centroid.get_i(),
1745  im_centroid.get_j(),
1746  im_extremity.get_i(),
1747  im_extremity.get_j() };
1748 #else
1749  std::vector<double> params;
1750  params.push_back(3); //normal at current pose
1751  params.push_back(im_centroid.get_i());
1752  params.push_back(im_centroid.get_j());
1753  params.push_back(im_extremity.get_i());
1754  params.push_back(im_extremity.get_j());
1755 #endif
1756  features.push_back(params);
1757  }
1758  }
1759 
1760  return features;
1761 }
1762 
1774 std::vector<std::vector<double> > vpMbtFaceDepthNormal::getModelForDisplay(unsigned int width, unsigned int height,
1775  const vpHomogeneousMatrix &cMo,
1776  const vpCameraParameters &cam,
1777  bool displayFullModel)
1778 {
1779  std::vector<std::vector<double> > models;
1780 
1781  if ((m_polygon->isVisible() && m_isTrackedDepthNormalFace) || displayFullModel) {
1783 
1784  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1785  ++it) {
1786  vpMbtDistanceLine *line = *it;
1787  std::vector<std::vector<double> > lineModels =
1788  line->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1789  models.insert(models.end(), lineModels.begin(), lineModels.end());
1790  }
1791  }
1792 
1793  return models;
1794 }
1795 
1805 bool vpMbtFaceDepthNormal::samePoint(const vpPoint &P1, const vpPoint &P2) const
1806 {
1807  double dx = fabs(P1.get_oX() - P2.get_oX());
1808  double dy = fabs(P1.get_oY() - P2.get_oY());
1809  double dz = fabs(P1.get_oZ() - P2.get_oZ());
1810 
1811  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
1812  dz <= std::numeric_limits<double>::epsilon())
1813  return true;
1814  else
1815  return false;
1816 }
1817 
1819 {
1820  m_cam = camera;
1821 
1822  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1823  ++it) {
1824  (*it)->setCameraParameters(camera);
1825  }
1826 }
1827 
1829 {
1830  m_useScanLine = v;
1831 
1832  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1833  ++it) {
1834  (*it)->useScanLine = v;
1835  }
1836 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
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:163
static double dotProd(const vpColVector &a, const vpColVector &b)
vpColVector & normalize()
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor blue
Definition: vpColor.h:217
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
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:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double sqr(double x)
Definition: vpMath.h:201
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2132
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4975
vpMbScanLine & getMbScanLineRenderer()
bool isVisible(unsigned int i)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
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)
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)
vpMbtPolygon & getPolygon()
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.
int m_pclPlaneEstimationMethod
PCL plane estimation method.
double m_distNearClip
Distance for near clipping.
void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
void setCameraParameters(const vpCameraParameters &camera)
void computeDesiredFeaturesSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &desired_features, vpColVector &desired_normal, vpColVector &centroid_point)
double m_distFarClip
Distance for near clipping.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const std::vector< vpColVector > &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05, unsigned int thickness=1)
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05)
bool computePolygonCentroid(const std::vector< vpPoint > &points, vpPoint &centroid)
void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, std::vector< vpImagePoint > &roiPts)
void estimatePlaneEquationSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &plane_equation_estimated, vpColVector &centroid)
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, const vpColVector &centroid_point)
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpCameraParameters m_cam
Camera intrinsic parameters.
vpPlane m_planeObject
Plane equation described in the object frame.
bool m_faceActivated
True if the face should be considered by the tracker.
@ GEOMETRIC_CENTROID
Compute the geometric centroid.
vpFaceCentroidType m_faceCentroidMethod
Method to compute the face centroid for the current features.
int m_pclPlaneEstimationRansacMaxIter
PCL pane estimation max number of iterations.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
void computeDesiredFeaturesRobustFeatures(const std::vector< double > &point_cloud_face_custom, const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &desired_features, vpColVector &desired_normal, vpColVector &centroid_point)
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
void estimateFeatures(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &x_estimated, std::vector< double > &weights)
std::vector< PolygonLine > m_polygonLines
void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point, vpColVector &face_normal)
bool m_useScanLine
Scan line visibility.
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:91
static bool inRoiMask(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)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
double getD() const
Definition: vpPlane.h:106
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:454
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:465
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:447
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:458
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:486
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:463
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:488
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:449
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:490
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:456
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:445
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpPoint.cpp:242
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:110
Implements a 3D polygon with render functionalities like clipping.
Definition: vpPolygon3D.h:55
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:189
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:202
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:182
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:98
vpRect getBoundingBox() const
Definition: vpPolygon.h:166
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:448
Defines a rectangle in the plane.
Definition: vpRect.h:76
double getWidth() const
Definition: vpRect.h:224
void setTop(double pos)
Definition: vpRect.h:354
double getLeft() const
Definition: vpRect.h:170
void setLeft(double pos)
Definition: vpRect.h:318
void setRight(double pos)
Definition: vpRect.h:345
double getRight() const
Definition: vpRect.h:176
double getBottom() const
Definition: vpRect.h:94
double getHeight() const
Definition: vpRect.h:163
void setBottom(double pos)
Definition: vpRect.h:285
double getTop() const
Definition: vpRect.h:189
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:123
VISP_EXPORT bool checkSSE2()