Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpMbtFaceDepthDense.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 dense features for a particular face.
33  *
34 *****************************************************************************/
35 
36 #ifndef _vpMbtFaceDepthDense_h_
37 #define _vpMbtFaceDepthDense_h_
38 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
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_DENSE 0
52 
53 BEGIN_VISP_NAMESPACE
54 class VISP_EXPORT vpMbtFaceDepthDense
55 {
56 public:
58  {
59  NO_FILTERING = 0,
60  DEPTH_OCCUPANCY_RATIO_FILTERING = 1 << 1,
63  MIN_DISTANCE_FILTERING = 1 << 2,
65  MAX_DISTANCE_FILTERING = 1 << 3
67  };
68 
72  unsigned int m_clippingFlag;
74  double m_distFarClip;
85 
87  virtual ~vpMbtFaceDepthDense();
88 
89  void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *const faces, vpUniRand &rand_gen,
90  int polygon = -1, std::string name = "");
91 
92 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
93  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo,
94  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, unsigned int stepX,
95  unsigned int stepY
96 #if DEBUG_DISPLAY_DEPTH_DENSE
97  ,
98  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
99 #endif
100  ,
101  const vpImage<bool> *mask = nullptr);
102 #endif
103  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
104  const std::vector<vpColVector> &point_cloud, unsigned int stepX, unsigned int stepY
105 #if DEBUG_DISPLAY_DEPTH_DENSE
106  ,
107  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
108 #endif
109  ,
110  const vpImage<bool> *mask = nullptr);
111  bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
112  const vpMatrix &point_cloud, unsigned int stepX, unsigned int stepY
113 #if DEBUG_DISPLAY_DEPTH_DENSE
114  ,
115  vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
116 #endif
117  ,
118  const vpImage<bool> *mask = nullptr);
119 
120  void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error);
121 
122  void computeVisibility();
123  void computeVisibilityDisplay();
124 
125  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
126  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
127  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
128  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
129 
130  void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
131  double scale = 0.05, unsigned int thickness = 1);
132  void displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
133  double scale = 0.05, unsigned int thickness = 1);
134 
135  std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
136  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
137  bool displayFullModel = false);
138 
139  inline unsigned int getNbFeatures() const { return (unsigned int)(m_pointCloudFace.size() / 3); }
140 
141  inline bool isTracked() const { return m_isTrackedDepthDenseFace; }
142 
143  inline bool isVisible() const { return m_polygon->isvisible; }
144 
145  void setCameraParameters(const vpCameraParameters &camera);
146 
147  void setScanLineVisibilityTest(bool v);
148 
149  inline void setDepthDenseFilteringMaxDistance(double maxDistance) { m_depthDenseFilteringMaxDist = maxDistance; }
150 
151  inline void setDepthDenseFilteringMethod(int method) { m_depthDenseFilteringMethod = method; }
152 
153  inline void setDepthDenseFilteringMinDistance(double minDistance) { m_depthDenseFilteringMinDist = minDistance; }
154 
155  inline void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
156  {
157  if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
158  std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
159  }
160  else {
161  m_depthDenseFilteringOccupancyRatio = occupancyRatio;
162  }
163  }
164 
165  inline void setTracked(bool tracked) { m_isTrackedDepthDenseFace = 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(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
183 
184  PolygonLine(const PolygonLine &polyLine)
185  : m_p1(nullptr), m_p2(nullptr), 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 protected:
222  std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
227  std::vector<double> m_pointCloudFace;
229  std::vector<PolygonLine> m_polygonLines;
230 
231 protected:
232  void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
233  std::vector<vpImagePoint> &roiPts
234 #if DEBUG_DISPLAY_DEPTH_DENSE
235  ,
236  std::vector<std::vector<vpImagePoint> > &roiPts_vec
237 #endif
238  ,
239  double &distanceToFace);
240 
241  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
242 };
243 END_VISP_NAMESPACE
244 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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:169
double m_depthDenseFilteringMinDist
Minimum distance threshold.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
bool m_isVisible
Visibility flag.
double m_distFarClip
Distance for near clipping.
std::vector< double > m_pointCloudFace
List of depth points inside the face.
vpPlane m_planeObject
Plane equation described in the object frame.
void setDepthDenseFilteringMethod(int method)
unsigned int getNbFeatures() const
void setTracked(bool tracked)
void setDepthDenseFilteringMinDistance(double minDistance)
vpMbtPolygon * m_polygon
Polygon defining the face.
bool m_useScanLine
Scan line visibility.
bool m_isTrackedDepthDenseFace
Flag to define if the face should be tracked or not.
double m_depthDenseFilteringMaxDist
Maximum distance threshold.
void setDepthDenseFilteringMaxDistance(double maxDistance)
std::vector< PolygonLine > m_polygonLines
Polygon lines used for scan-line visibility.
int m_depthDenseFilteringMethod
Method to use to consider or not the face.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
vpCameraParameters m_cam
Camera intrinsic parameters.
double m_depthDenseFilteringOccupancyRatio
Ratio between available depth points and theoretical number of points.
double m_distNearClip
Distance for near clipping.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:60
bool isvisible
flag to specify whether the face is visible or not
Definition: vpMbtPolygon.h:66
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:79
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:127