Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpMbDepthNormalTracker.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  * Model-based tracker using depth normal features.
33  *
34  *****************************************************************************/
35 
36 #ifndef _vpMbDepthNormalTracker_h_
37 #define _vpMbDepthNormalTracker_h_
38 
39 #include <visp3/core/vpConfig.h>
40 #include <visp3/core/vpPlane.h>
41 #include <visp3/mbt/vpMbTracker.h>
42 #include <visp3/mbt/vpMbtFaceDepthNormal.h>
43 
44 #if DEBUG_DISPLAY_DEPTH_NORMAL
45 #include <visp3/core/vpDisplay.h>
46 #endif
47 
48 class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker
49 {
50 public:
52  virtual ~vpMbDepthNormalTracker();
53 
54  virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
55  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
56 
57  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
58  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
59 
61  {
62  return m_depthNormalFeatureEstimationMethod;
63  }
64 
65  virtual inline vpColVector getError() const { return m_error_depthNormal; }
66 
67  virtual inline vpColVector getRobustWeights() const { return m_w_depthNormal; }
68 
69  virtual void init(const vpImage<unsigned char> &I);
70 
71  virtual void loadConfigFile(const std::string &configFile);
72 
73  void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
74  const bool verbose = false);
75 #if defined(VISP_HAVE_PCL)
76  void reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const std::string &cad_name,
77  const vpHomogeneousMatrix &cMo_, const bool verbose = false);
78 #endif
79 
80  virtual void resetTracker();
81 
82  virtual void setCameraParameters(const vpCameraParameters &camera);
83 
84  virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method);
85 
86  virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method);
87 
88  virtual void setDepthNormalPclPlaneEstimationMethod(const int method);
89 
90  virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter);
91 
92  virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold);
93 
94  virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY);
95 
96  // virtual void setDepthNormalUseRobust(const bool use);
97 
98  virtual void setOgreVisibilityTest(const bool &v);
99 
100  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
101 #if defined(VISP_HAVE_PCL)
102  virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
103 #endif
104 
105  virtual void setScanLineVisibilityTest(const bool &v);
106 
107  void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking);
108 
109  virtual void testTracking();
110 
111  virtual void track(const vpImage<unsigned char> &);
112 #if defined(VISP_HAVE_PCL)
113  virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
114 #endif
115  virtual void track(const std::vector<vpColVector> &point_cloud, const unsigned int width, const unsigned int height);
116 
117 protected:
125  std::vector<vpMbtFaceDepthNormal *> m_depthNormalListOfActiveFaces;
127  std::vector<vpColVector> m_depthNormalListOfDesiredFeatures;
129  std::vector<vpMbtFaceDepthNormal *> m_depthNormalFaces;
152 #if DEBUG_DISPLAY_DEPTH_NORMAL
153  vpDisplay *m_debugDisp_depthNormal;
154  vpImage<unsigned char> m_debugImage_depthNormal;
155 #endif
156 
157  void addFace(vpMbtPolygon &polygon, const bool alreadyClose);
158 
159  void computeVisibility(const unsigned int width, const unsigned int height);
160 
161  void computeVVS();
162  virtual void computeVVSInit();
164 
165  virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
166  const int idFace = 0, const std::string &name = "");
167 
168  virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
169  const std::string &name = "");
170 
171  virtual void initFaceFromCorners(vpMbtPolygon &polygon);
172 
173  virtual void initFaceFromLines(vpMbtPolygon &polygon);
174 
175 #ifdef VISP_HAVE_PCL
176  void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
177 #endif
178  void segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
179  const unsigned int height);
180 };
181 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:171
virtual void track(const vpImage< unsigned char > &I)=0
virtual void computeVVSInteractionMatrixAndResidu()=0
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
virtual void computeVVSInit()=0
vpColVector m_w_depthNormal
Robust weights.
Implementation of an homogeneous matrix and operations on such kind of matrices.
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
virtual void loadConfigFile(const std::string &configFile)
vpColVector m_error_depthNormal
(s - s*)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")=0
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
virtual vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthFeatureEstimationMethod() const
virtual void resetTracker()=0
virtual vpColVector getError() const
Class that defines what is a point.
Definition: vpPoint.h:58
virtual void init(const vpImage< unsigned char > &I)=0
vpColVector m_weightedError_depthNormal
Weighted error.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)=0
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
virtual vpColVector getRobustWeights() const
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
virtual void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbTracker.h:473
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0
Generic class defining intrinsic camera parameters.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:110
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
vpImage< unsigned char > m_depthNormalI_dummyVisibility
Dummy image used to compute the visibility.
virtual void setOgreVisibilityTest(const bool &v)
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
virtual void initFaceFromLines(vpMbtPolygon &polygon)=0
virtual void testTracking()=0
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)=0
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:587
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")=0