Visual Servoing Platform  version 3.2.1 under development (2019-11-19)
vpMbtFaceDepthNormal.h
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 #ifndef _vpMbtFaceDepthNormal_h_
37 #define _vpMbtFaceDepthNormal_h_
38 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 #ifdef VISP_HAVE_PCL
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #endif
46 
47 #include <visp3/core/vpPlane.h>
48 #include <visp3/mbt/vpMbTracker.h>
49 #include <visp3/mbt/vpMbtDistanceLine.h>
50 
51 #define DEBUG_DISPLAY_DEPTH_NORMAL 0
52 
53 class VISP_EXPORT vpMbtFaceDepthNormal
54 {
55 public:
58  MEAN_CENTROID
59  };
60 
62  ROBUST_FEATURE_ESTIMATION = 0,
63  ROBUST_SVD_PLANE_ESTIMATION = 1,
64 #ifdef VISP_HAVE_PCL
65  PCL_PLANE_ESTIMATION = 2
66 #endif
67  };
68 
72  unsigned int m_clippingFlag;
74  double m_distFarClip;
85 
87  virtual ~vpMbtFaceDepthNormal();
88 
89  void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *const faces, int polygon = -1,
90  std::string name = "");
91 
92 #ifdef VISP_HAVE_PCL
93  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
94  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
95  vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY
96 #if DEBUG_DISPLAY_DEPTH_NORMAL
97  ,
98  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
99 #endif
100  , const vpImage<bool> *mask = NULL
101  );
102 #endif
103  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
104  const std::vector<vpColVector> &point_cloud, vpColVector &desired_features,
105  const unsigned int stepX, const unsigned int stepY
106 #if DEBUG_DISPLAY_DEPTH_NORMAL
107  ,
108  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
109 #endif
110  , const vpImage<bool> *mask = NULL
111  );
112 
113  void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features);
114 
115  void computeVisibility();
116  void computeVisibilityDisplay();
117 
118  void computeNormalVisibility(const double nx, const double ny, const double nz, const vpColVector &centroid_point,
119  vpColVector &face_normal);
120 #ifdef VISP_HAVE_PCL
121  void computeNormalVisibility(const float nx, const float ny, const float nz, const pcl::PointXYZ &centroid_point,
122  pcl::PointXYZ &face_normal);
123 #endif
124  void computeNormalVisibility(const double nx, const double ny, const double nz, const vpHomogeneousMatrix &cMo,
125  const vpCameraParameters &camera, vpColVector &correct_normal, vpPoint &centroid);
126 
127  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
128  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
129  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
130  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
131 
132  void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
133  const double scale = 0.05, const unsigned int thickness = 1);
134  void displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
135  const double scale = 0.05, const unsigned int thickness = 1);
136 
137  std::vector<std::vector<double> > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
138  const double scale = 0.05);
139 
140  std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
141  const vpHomogeneousMatrix &cMo,
142  const vpCameraParameters &cam,
143  const bool displayFullModel = false);
144 
145  inline bool isTracked() const { return m_isTrackedDepthNormalFace; }
146 
147  inline bool isVisible() const { return m_polygon->isvisible; }
148 
149  void setCameraParameters(const vpCameraParameters &camera);
150 
151  inline void setFaceCentroidMethod(const vpFaceCentroidType &method) { m_faceCentroidMethod = method; }
152 
153  inline void setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
154 
155  inline void setPclPlaneEstimationMethod(const int method) { m_pclPlaneEstimationMethod = method; }
156 
157  inline void setPclPlaneEstimationRansacMaxIter(const int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
158 
159  inline void setPclPlaneEstimationRansacThreshold(const double threshold)
160  {
161  m_pclPlaneEstimationRansacThreshold = threshold;
162  }
163 
164  void setScanLineVisibilityTest(const bool v);
165 
166  inline void setTracked(const bool tracked) { m_isTrackedDepthNormalFace = tracked; }
167 
168 private:
169  class PolygonLine
170  {
171  public:
173  vpPoint *m_p1;
175  vpPoint *m_p2;
177  vpMbtPolygon m_poly;
179  vpImagePoint m_imPt1;
181  vpImagePoint m_imPt2;
182 
183  PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {}
184 
185  PolygonLine(const PolygonLine &polyLine)
186  : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
187  {
188  m_p1 = &m_poly.p[0];
189  m_p2 = &m_poly.p[1];
190  }
191 
192  PolygonLine &operator=(PolygonLine other)
193  {
194  swap(*this, other);
195 
196  return *this;
197  }
198 
199  void swap(PolygonLine &first, PolygonLine &second)
200  {
201  using std::swap;
202  swap(first.m_p1, second.m_p1);
203  swap(first.m_p2, second.m_p2);
204  swap(first.m_poly, second.m_poly);
205  swap(first.m_imPt1, second.m_imPt1);
206  swap(first.m_imPt2, second.m_imPt2);
207  }
208  };
209 
210  template <class T> class Mat33
211  {
212  public:
213  std::vector<T> data;
214 
215  Mat33() : data(9) {}
216 
217  inline T operator[](const size_t i) const { return data[i]; }
218 
219  inline T &operator[](const size_t i) { return data[i]; }
220 
221  Mat33 inverse() const
222  {
223  // determinant
224  T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
225  data[2] * (data[3] * data[7] - data[4] * data[6]);
226  T invdet = 1 / det;
227 
228  Mat33<T> minv;
229  minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
230  minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
231  minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
232  minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
233  minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
234  minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
235  minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
236  minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
237  minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
238 
239  return minv;
240  }
241  };
242 
243 protected:
259  std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
270  std::vector<PolygonLine> m_polygonLines;
271 
272 #ifdef VISP_HAVE_PCL
273  bool computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
274  vpColVector &desired_features, vpColVector &desired_normal,
275  vpColVector &centroid_point);
276 #endif
277  void computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
278  const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
279  vpColVector &desired_features, vpColVector &desired_normal,
280  vpColVector &centroid_point);
281  void computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
282  vpColVector &desired_features, vpColVector &desired_normal,
283  vpColVector &centroid_point);
284  void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal,
285  const vpColVector &centroid_point);
286 
287  bool computePolygonCentroid(const std::vector<vpPoint> &points, vpPoint &centroid);
288 
289  void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
290  std::vector<vpImagePoint> &roiPts
291 #if DEBUG_DISPLAY_DEPTH_NORMAL
292  ,
293  std::vector<std::vector<vpImagePoint> > &roiPts_vec
294 #endif
295  );
296 
297  void estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
298  vpColVector &x_estimated, std::vector<double> &weights);
299 
300  void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
301  vpColVector &plane_equation_estimated, vpColVector &centroid);
302 
303  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
304 };
305 #endif
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void setTracked(const bool tracked)
Implementation of an homogeneous matrix and operations on such kind of matrices.
int m_pclPlaneEstimationMethod
PCL plane estimation method.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
int m_pclPlaneEstimationRansacMaxIter
PCL pane estimation max number of iterations.
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void setFaceCentroidMethod(const vpFaceCentroidType &method)
double m_distNearClip
Distance for near clipping.
bool m_useScanLine
Scan line visibility.
vpCameraParameters m_cam
Camera intrinsic parameters.
Class that defines what is a point.
Definition: vpPoint.h:58
bool m_faceActivated
True if the face should be considered by the tracker.
void setPclPlaneEstimationRansacThreshold(const double threshold)
vpMbtPolygon * m_polygon
Polygon defining the face.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
Generic class defining intrinsic camera parameters.
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
double m_distFarClip
Distance for near clipping.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
std::vector< PolygonLine > m_polygonLines
vpFaceCentroidType m_faceCentroidMethod
Method to compute the face centroid for the current features.
void setPclPlaneEstimationMethod(const int method)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setPclPlaneEstimationRansacMaxIter(const int maxIter)
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
Compute the geometric centroid.
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
vpPlane m_planeObject
Plane equation described in the object frame.
bool isvisible
flag to specify whether the face is visible or not
Definition: vpMbtPolygon.h:73
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.