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