Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
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, vpUniRand &rand_gen,
90  int polygon = -1, std::string name = "");
91 
92 #ifdef VISP_HAVE_PCL
93  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
94  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
95  vpColVector &desired_features, unsigned int stepX, 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  ,
101  const vpImage<bool> *mask = NULL);
102 #endif
103  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
104  const std::vector<vpColVector> &point_cloud, vpColVector &desired_features,
105  unsigned int stepX, 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  ,
111  const vpImage<bool> *mask = NULL);
112 
113  void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features);
114 
115  void computeVisibility();
116  void computeVisibilityDisplay();
117 
118  void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
119  vpColVector &face_normal);
120 #ifdef VISP_HAVE_PCL
121  void computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ &centroid_point,
122  pcl::PointXYZ &face_normal);
123 #endif
124  void computeNormalVisibility(double nx, double ny, 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, unsigned int thickness = 1, bool displayFullModel = false);
129  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
130  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
131 
132  void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
133  double scale = 0.05, unsigned int thickness = 1);
134  void displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
135  double scale = 0.05, unsigned int thickness = 1);
136 
137  std::vector<std::vector<double> > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
138  double scale = 0.05);
139 
140  std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
141  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
142  bool displayFullModel = false);
143 
144  inline bool isTracked() const { return m_isTrackedDepthNormalFace; }
145 
146  inline bool isVisible() const { return m_polygon->isvisible; }
147 
148  void setCameraParameters(const vpCameraParameters &camera);
149 
150  inline void setFaceCentroidMethod(const vpFaceCentroidType &method) { m_faceCentroidMethod = method; }
151 
152  inline void setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
153 
154  inline void setPclPlaneEstimationMethod(int method) { m_pclPlaneEstimationMethod = method; }
155 
156  inline void setPclPlaneEstimationRansacMaxIter(int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
157 
158  inline void setPclPlaneEstimationRansacThreshold(double threshold)
159  {
160  m_pclPlaneEstimationRansacThreshold = threshold;
161  }
162 
163  void setScanLineVisibilityTest(bool v);
164 
165  inline void setTracked(bool tracked) { m_isTrackedDepthNormalFace = tracked; }
166 
167 private:
168  class PolygonLine
169  {
170  public:
172  vpPoint *m_p1;
174  vpPoint *m_p2;
176  vpMbtPolygon m_poly;
178  vpImagePoint m_imPt1;
180  vpImagePoint m_imPt2;
181 
182  PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {}
183 
184  PolygonLine(const PolygonLine &polyLine)
185  : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
186  {
187  m_p1 = &m_poly.p[0];
188  m_p2 = &m_poly.p[1];
189  }
190 
191  PolygonLine &operator=(PolygonLine other)
192  {
193  swap(*this, other);
194 
195  return *this;
196  }
197 
198  void swap(PolygonLine &first, PolygonLine &second)
199  {
200  using std::swap;
201  swap(first.m_p1, second.m_p1);
202  swap(first.m_p2, second.m_p2);
203  swap(first.m_poly, second.m_poly);
204  swap(first.m_imPt1, second.m_imPt1);
205  swap(first.m_imPt2, second.m_imPt2);
206  }
207  };
208 
209  template <class T> class Mat33
210  {
211  public:
212  std::vector<T> data;
213 
214  Mat33() : data(9) {}
215 
216  inline T operator[](const size_t i) const { return data[i]; }
217 
218  inline T &operator[](const size_t i) { return data[i]; }
219 
220  Mat33 inverse() const
221  {
222  // determinant
223  T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
224  data[2] * (data[3] * data[7] - data[4] * data[6]);
225  T invdet = 1 / det;
226 
227  Mat33<T> minv;
228  minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
229  minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
230  minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
231  minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
232  minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
233  minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
234  minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
235  minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
236  minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
237 
238  return minv;
239  }
240  };
241 
242 protected:
258  std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
269  std::vector<PolygonLine> m_polygonLines;
270 
271 #ifdef VISP_HAVE_PCL
272  bool computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
273  vpColVector &desired_features, vpColVector &desired_normal,
274  vpColVector &centroid_point);
275 #endif
276  void computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
277  const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
278  vpColVector &desired_features, vpColVector &desired_normal,
279  vpColVector &centroid_point);
280  void computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
281  vpColVector &desired_features, vpColVector &desired_normal,
282  vpColVector &centroid_point);
283  void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal,
284  const vpColVector &centroid_point);
285 
286  bool computePolygonCentroid(const std::vector<vpPoint> &points, vpPoint &centroid);
287 
288  void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
289  std::vector<vpImagePoint> &roiPts
290 #if DEBUG_DISPLAY_DEPTH_NORMAL
291  ,
292  std::vector<std::vector<vpImagePoint> > &roiPts_vec
293 #endif
294  );
295 
296  void estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
297  vpColVector &x_estimated, std::vector<double> &weights);
298 
299  void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
300  vpColVector &plane_equation_estimated, vpColVector &centroid);
301 
302  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
303 };
304 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.
int m_pclPlaneEstimationMethod
PCL plane estimation method.
double m_distNearClip
Distance for near clipping.
double m_distFarClip
Distance for near clipping.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void setPclPlaneEstimationMethod(int method)
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
void setPclPlaneEstimationRansacThreshold(double threshold)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
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 setTracked(bool tracked)
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
std::vector< PolygonLine > m_polygonLines
void setFaceCentroidMethod(const vpFaceCentroidType &method)
void setPclPlaneEstimationRansacMaxIter(int maxIter)
bool m_useScanLine
Scan line visibility.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
bool isvisible
flag to specify whether the face is visible or not
Definition: vpMbtPolygon.h:73
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101