Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpMbtFaceDepthNormal.h
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 #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:
57  {
59  MEAN_CENTROID
60  };
61 
63  {
64  ROBUST_FEATURE_ESTIMATION = 0,
65  ROBUST_SVD_PLANE_ESTIMATION = 1,
66 #ifdef VISP_HAVE_PCL
67  PCL_PLANE_ESTIMATION = 2
68 #endif
69  };
70 
74  unsigned int m_clippingFlag;
76  double m_distFarClip;
87 
89  virtual ~vpMbtFaceDepthNormal();
90 
91  void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *const faces, vpUniRand &rand_gen,
92  int polygon = -1, std::string name = "");
93 
94 #ifdef VISP_HAVE_PCL
95  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
96  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
97  vpColVector &desired_features, unsigned int stepX, unsigned int stepY
98 #if DEBUG_DISPLAY_DEPTH_NORMAL
99  ,
100  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
101 #endif
102  ,
103  const vpImage<bool> *mask = nullptr);
104 #endif
105  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
106  const std::vector<vpColVector> &point_cloud, vpColVector &desired_features,
107  unsigned int stepX, unsigned int stepY
108 #if DEBUG_DISPLAY_DEPTH_NORMAL
109  ,
110  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
111 #endif
112  ,
113  const vpImage<bool> *mask = nullptr);
114  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
115  const vpMatrix &point_cloud, vpColVector &desired_features,
116  unsigned int stepX, unsigned int stepY
117 #if DEBUG_DISPLAY_DEPTH_NORMAL
118  ,
119  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
120 #endif
121  ,
122  const vpImage<bool> *mask = nullptr);
123 
124  void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features);
125 
126  void computeVisibility();
127  void computeVisibilityDisplay();
128 
129  void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
130  vpColVector &face_normal);
131 #ifdef VISP_HAVE_PCL
132  void computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ &centroid_point,
133  pcl::PointXYZ &face_normal);
134 #endif
135  void computeNormalVisibility(double nx, double ny, double nz, const vpHomogeneousMatrix &cMo,
136  const vpCameraParameters &camera, vpColVector &correct_normal, vpPoint &centroid);
137 
138  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
139  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
140  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
141  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
142 
143  void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
144  double scale = 0.05, unsigned int thickness = 1);
145  void displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
146  double scale = 0.05, unsigned int thickness = 1);
147 
148  std::vector<std::vector<double> > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
149  double scale = 0.05);
150 
151  std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
152  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
153  bool displayFullModel = false);
154 
155  inline bool isTracked() const { return m_isTrackedDepthNormalFace; }
156 
157  inline bool isVisible() const { return m_polygon->isvisible; }
158 
159  void setCameraParameters(const vpCameraParameters &camera);
160 
161  inline void setFaceCentroidMethod(const vpFaceCentroidType &method) { m_faceCentroidMethod = method; }
162 
163  inline void setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
164 
165  inline void setPclPlaneEstimationMethod(int method) { m_pclPlaneEstimationMethod = method; }
166 
167  inline void setPclPlaneEstimationRansacMaxIter(int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
168 
169  inline void setPclPlaneEstimationRansacThreshold(double threshold)
170  {
171  m_pclPlaneEstimationRansacThreshold = threshold;
172  }
173 
174  void setScanLineVisibilityTest(bool v);
175 
176  inline void setTracked(bool tracked) { m_isTrackedDepthNormalFace = tracked; }
177 
178 private:
179  class PolygonLine
180  {
181  public:
183  vpPoint *m_p1;
185  vpPoint *m_p2;
187  vpMbtPolygon m_poly;
189  vpImagePoint m_imPt1;
191  vpImagePoint m_imPt2;
192 
193  PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
194 
195  PolygonLine(const PolygonLine &polyLine)
196  : m_p1(nullptr), m_p2(nullptr), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
197  {
198  m_p1 = &m_poly.p[0];
199  m_p2 = &m_poly.p[1];
200  }
201 
202  PolygonLine &operator=(PolygonLine other)
203  {
204  swap(*this, other);
205 
206  return *this;
207  }
208 
209  void swap(PolygonLine &first, PolygonLine &second)
210  {
211  using std::swap;
212  swap(first.m_p1, second.m_p1);
213  swap(first.m_p2, second.m_p2);
214  swap(first.m_poly, second.m_poly);
215  swap(first.m_imPt1, second.m_imPt1);
216  swap(first.m_imPt2, second.m_imPt2);
217  }
218  };
219 
220  template <class T> class Mat33
221  {
222  public:
223  std::vector<T> data;
224 
225  Mat33() : data(9) { }
226 
227  inline T operator[](const size_t i) const { return data[i]; }
228 
229  inline T &operator[](const size_t i) { return data[i]; }
230 
231  Mat33 inverse() const
232  {
233  // determinant
234  T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
235  data[2] * (data[3] * data[7] - data[4] * data[6]);
236  T invdet = 1 / det;
237 
238  Mat33<T> minv;
239  minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
240  minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
241  minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
242  minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
243  minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
244  minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
245  minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
246  minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
247  minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
248 
249  return minv;
250  }
251  };
252 
253 protected:
269  std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
280  std::vector<PolygonLine> m_polygonLines;
281 
282 #ifdef VISP_HAVE_PCL
283  bool computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
284  vpColVector &desired_features, vpColVector &desired_normal,
285  vpColVector &centroid_point);
286 #endif
287  void computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
288  const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
289  vpColVector &desired_features, vpColVector &desired_normal,
290  vpColVector &centroid_point);
291  void computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
292  vpColVector &desired_features, vpColVector &desired_normal,
293  vpColVector &centroid_point);
294  void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal,
295  const vpColVector &centroid_point);
296 
297  bool computePolygonCentroid(const std::vector<vpPoint> &points, vpPoint &centroid);
298 
299  void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
300  std::vector<vpImagePoint> &roiPts
301 #if DEBUG_DISPLAY_DEPTH_NORMAL
302  ,
303  std::vector<std::vector<vpImagePoint> > &roiPts_vec
304 #endif
305  );
306 
307  void estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
308  vpColVector &x_estimated, std::vector<double> &weights);
309 
310  void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
311  vpColVector &plane_equation_estimated, vpColVector &centroid);
312 
313  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
314 };
315 
316 #ifdef VISP_HAVE_NLOHMANN_JSON
317 #include<nlohmann/json.hpp>
318 #ifdef VISP_HAVE_PCL
319 NLOHMANN_JSON_SERIALIZE_ENUM(vpMbtFaceDepthNormal::vpFeatureEstimationType, {
323 });
324 #else
325 NLOHMANN_JSON_SERIALIZE_ENUM(vpMbtFaceDepthNormal::vpFeatureEstimationType, {
328 });
329 #endif
330 #endif
331 
332 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
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:82
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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:58
bool isvisible
flag to specify whether the face is visible or not
Definition: vpMbtPolygon.h:64
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:123