Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpMbDepthNormalTracker.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 VP_MB_DEPTH_NORMAL_TRACKER_H
35 #define VP_MB_DEPTH_NORMAL_TRACKER_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 BEGIN_VISP_NAMESPACE
47 class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker
48 {
49 public:
51  virtual ~vpMbDepthNormalTracker() VP_OVERRIDE;
52 
53  virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
54  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE;
55 
56  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
57  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE;
58 
60  {
61  return m_depthNormalFeatureEstimationMethod;
62  }
63 
64  virtual inline vpColVector getError() const VP_OVERRIDE { return m_error_depthNormal; }
65 
66  virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
67  const vpHomogeneousMatrix &cMo,
68  const vpCameraParameters &cam,
69  bool displayFullModel = false) VP_OVERRIDE;
70 
71  virtual inline vpColVector getRobustWeights() const VP_OVERRIDE { return m_w_depthNormal; }
72 
73  virtual void init(const vpImage<unsigned char> &I) VP_OVERRIDE;
74 
75  virtual void loadConfigFile(const std::string &configFile, bool verbose = true) VP_OVERRIDE;
76 
77  void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo,
78  bool verbose = false);
79 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
80  void reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const std::string &cad_name,
81  const vpHomogeneousMatrix &cMo, bool verbose = false);
82 #endif
83 
84  virtual void resetTracker() VP_OVERRIDE;
85 
86  virtual void setCameraParameters(const vpCameraParameters &camera) VP_OVERRIDE;
87 
88  virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method);
89 
90  virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method);
91 
92  virtual void setDepthNormalPclPlaneEstimationMethod(int method);
93 
94  virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter);
95 
96  virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold);
97 
98  virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY);
99 
100  // virtual void setDepthNormalUseRobust(bool use);
101 
102  virtual void setOgreVisibilityTest(const bool &v) VP_OVERRIDE;
103 
104  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE;
105  virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE;
106 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
107  virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
108 #endif
109 
110  virtual void setScanLineVisibilityTest(const bool &v) VP_OVERRIDE;
111 
112  void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking);
113 
114  virtual void testTracking() VP_OVERRIDE;
115 
116  virtual void track(const vpImage<unsigned char> &) VP_OVERRIDE;
117  virtual void track(const vpImage<vpRGBa> &I_color) VP_OVERRIDE;
118 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
119  virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
120 #endif
121  virtual void track(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
122 
123 protected:
129  std::vector<vpMbtFaceDepthNormal *> m_depthNormalListOfActiveFaces;
131  std::vector<vpColVector> m_depthNormalListOfDesiredFeatures;
133  std::vector<vpMbtFaceDepthNormal *> m_depthNormalFaces;
149  std::vector<std::vector<double> > m_featuresToBeDisplayedDepthNormal;
158 #if DEBUG_DISPLAY_DEPTH_NORMAL
159  vpDisplay *m_debugDisp_depthNormal;
160  vpImage<unsigned char> m_debugImage_depthNormal;
161 #endif
162 
163  void addFace(vpMbtPolygon &polygon, bool alreadyClose);
164 
165  void computeVisibility(unsigned int width, unsigned int height);
166 
167  void computeVVS();
168  virtual void computeVVSInit() VP_OVERRIDE;
169  virtual void computeVVSInteractionMatrixAndResidu() VP_OVERRIDE;
170 
171  virtual std::vector<std::vector<double> > getFeaturesForDisplayDepthNormal();
172 
173  virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace = 0,
174  const std::string &name = "") VP_OVERRIDE;
175 
176  virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
177  const std::string &name = "") VP_OVERRIDE;
178 
179  virtual void initFaceFromCorners(vpMbtPolygon &polygon) VP_OVERRIDE;
180 
181  virtual void initFaceFromLines(vpMbtPolygon &polygon) VP_OVERRIDE;
182 
183 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
184  void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
185 #endif
186  void segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
187  void segmentPointCloud(const vpMatrix &point_cloud, unsigned int width, unsigned int height);
188 };
189 END_VISP_NAMESPACE
190 #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
Class that defines generic functionalities for display.
Definition: vpDisplay.h:178
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix m_L_depthNormal
Interaction matrix.
virtual vpColVector getError() const VP_OVERRIDE
vpRobust m_robust_depthNormal
Robust.
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:107
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:610
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:60
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Definition: vpRGBa.h:65
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84