Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpMbDepthNormalTracker.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Model-based tracker using depth normal features.
32  */
33 
34 #ifndef _vpMbDepthNormalTracker_h_
35 #define _vpMbDepthNormalTracker_h_
36 
37 #include <visp3/core/vpConfig.h>
38 #include <visp3/core/vpPlane.h>
39 #include <visp3/mbt/vpMbTracker.h>
40 #include <visp3/mbt/vpMbtFaceDepthNormal.h>
41 
42 #if DEBUG_DISPLAY_DEPTH_NORMAL
43 #include <visp3/core/vpDisplay.h>
44 #endif
45 
46 class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker
47 {
48 public:
50  virtual ~vpMbDepthNormalTracker() vp_override;
51 
52  virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
53  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) vp_override;
54 
55  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
56  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) vp_override;
57 
59  {
60  return m_depthNormalFeatureEstimationMethod;
61  }
62 
63  virtual inline vpColVector getError() const vp_override { return m_error_depthNormal; }
64 
65  virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
66  const vpHomogeneousMatrix &cMo,
67  const vpCameraParameters &cam,
68  bool displayFullModel = false) vp_override;
69 
70  virtual inline vpColVector getRobustWeights() const vp_override { return m_w_depthNormal; }
71 
72  virtual void init(const vpImage<unsigned char> &I) vp_override;
73 
74  virtual void loadConfigFile(const std::string &configFile, bool verbose = true) vp_override;
75 
76  void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo,
77  bool verbose = false);
78 #if defined(VISP_HAVE_PCL)
79  void reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const std::string &cad_name,
80  const vpHomogeneousMatrix &cMo, bool verbose = false);
81 #endif
82 
83  virtual void resetTracker() vp_override;
84 
85  virtual void setCameraParameters(const vpCameraParameters &camera) vp_override;
86 
87  virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method);
88 
89  virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method);
90 
91  virtual void setDepthNormalPclPlaneEstimationMethod(int method);
92 
93  virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter);
94 
95  virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold);
96 
97  virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY);
98 
99  // virtual void setDepthNormalUseRobust(bool use);
100 
101  virtual void setOgreVisibilityTest(const bool &v) vp_override;
102 
103  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) vp_override;
104  virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo) vp_override;
105 #if defined(VISP_HAVE_PCL)
106  virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
107 #endif
108 
109  virtual void setScanLineVisibilityTest(const bool &v) vp_override;
110 
111  void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking);
112 
113  virtual void testTracking() vp_override;
114 
115  virtual void track(const vpImage<unsigned char> &) vp_override;
116  virtual void track(const vpImage<vpRGBa> &I_color) vp_override;
117 #if defined(VISP_HAVE_PCL)
118  virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
119 #endif
120  virtual void track(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
121 
122 protected:
128  std::vector<vpMbtFaceDepthNormal *> m_depthNormalListOfActiveFaces;
130  std::vector<vpColVector> m_depthNormalListOfDesiredFeatures;
132  std::vector<vpMbtFaceDepthNormal *> m_depthNormalFaces;
148  std::vector<std::vector<double> > m_featuresToBeDisplayedDepthNormal;
157 #if DEBUG_DISPLAY_DEPTH_NORMAL
158  vpDisplay *m_debugDisp_depthNormal;
159  vpImage<unsigned char> m_debugImage_depthNormal;
160 #endif
161 
162  void addFace(vpMbtPolygon &polygon, bool alreadyClose);
163 
164  void computeVisibility(unsigned int width, unsigned int height);
165 
166  void computeVVS();
167  virtual void computeVVSInit() vp_override;
168  virtual void computeVVSInteractionMatrixAndResidu() vp_override;
169 
170  virtual std::vector<std::vector<double> > getFeaturesForDisplayDepthNormal();
171 
172  virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace = 0,
173  const std::string &name = "") vp_override;
174 
175  virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
176  const std::string &name = "") vp_override;
177 
178  virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override;
179 
180  virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override;
181 
182 #ifdef VISP_HAVE_PCL
183  void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
184 #endif
185  void segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
186  void segmentPointCloud(const vpMatrix &point_cloud, unsigned int width, unsigned int height);
187 };
188 #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
Class that defines generic functionalities for display.
Definition: vpDisplay.h:173
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
virtual vpColVector getError() const vp_override
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
vpColVector m_w_depthNormal
Robust weights.
vpColVector m_error_depthNormal
(s - s*)
vpColVector m_weightedError_depthNormal
Weighted error.
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthFeatureEstimationMethod() const
std::vector< std::vector< double > > m_featuresToBeDisplayedDepthNormal
Display features.
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:105
virtual void track(const vpImage< unsigned char > &I)=0
virtual void resetTracker()=0
virtual void init(const vpImage< unsigned char > &I)=0
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)=0
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:608
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)=0
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)=0
virtual void computeVVSInit()=0
virtual void testTracking()=0
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
Definition: vpRGBa.h:61
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83