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 dense features.
33  *
34  *****************************************************************************/
35 
36 #ifndef _vpMbDepthDenseTracker_h_
37 #define _vpMbDepthDenseTracker_h_
38 
39 #include <visp3/core/vpPlane.h>
40 #include <visp3/mbt/vpMbTracker.h>
41 #include <visp3/mbt/vpMbtFaceDepthDense.h>
42 #include <visp3/mbt/vpMbtTukeyEstimator.h>
43 
44 #if DEBUG_DISPLAY_DEPTH_DENSE
45 #include <visp3/core/vpDisplay.h>
46 #endif
47 
48 class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker
49 {
50 public:
51   vpMbDepthDenseTracker();
52   virtual ~vpMbDepthDenseTracker();
53 
54   virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
55                        const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
56 
57   virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
58                        const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
59 
getError()60   virtual inline vpColVector getError() const { return m_error_depthDense; }
61 
62   virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
63                                                                const vpHomogeneousMatrix &cMo,
64                                                                const vpCameraParameters &cam,
65                                                                bool displayFullModel=false);
66 
getRobustWeights()67   virtual inline vpColVector getRobustWeights() const { return m_w_depthDense; }
68 
69   virtual void init(const vpImage<unsigned char> &I);
70 
71   virtual void loadConfigFile(const std::string &configFile, bool verbose=true);
72 
73   void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo,
74                    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, bool verbose = false);
78 #endif
79 
80   virtual void resetTracker();
81 
82   virtual void setCameraParameters(const vpCameraParameters &camera);
83 
84   virtual void setDepthDenseFilteringMaxDistance(double maxDistance);
85   virtual void setDepthDenseFilteringMethod(int method);
86   virtual void setDepthDenseFilteringMinDistance(double minDistance);
87   virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio);
88 
setDepthDenseSamplingStep(unsigned int stepX,unsigned int stepY)89   inline void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
90   {
91     if (stepX == 0 || stepY == 0) {
92       std::cerr << "stepX and stepY must be greater than zero!" << std::endl;
93       return;
94     }
95 
96     m_depthDenseSamplingStepX = stepX;
97     m_depthDenseSamplingStepY = stepY;
98   }
99 
100   virtual void setOgreVisibilityTest(const bool &v);
101 
102   virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
103   virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo);
104 #ifdef VISP_HAVE_PCL
105   virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
106 #endif
107 
108   virtual void setScanLineVisibilityTest(const bool &v);
109 
110   void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking);
111 
112   virtual void testTracking();
113 
114   virtual void track(const vpImage<unsigned char> &);
115   virtual void track(const vpImage<vpRGBa> &);
116 #ifdef VISP_HAVE_PCL
117   virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
118 #endif
119   virtual void track(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height);
120 
121 protected:
122   //! Set of faces describing the object used only for display with scan line.
123   vpMbHiddenFaces<vpMbtPolygon> m_depthDenseHiddenFacesDisplay;
124   //! List of current active (visible and features extracted) faces
125   std::vector<vpMbtFaceDepthDense *> m_depthDenseListOfActiveFaces;
126   //! Nb features
127   unsigned int m_denseDepthNbFeatures;
128   //! List of faces
129   std::vector<vpMbtFaceDepthDense *> m_depthDenseFaces;
130   //! Sampling step in x-direction
131   unsigned int m_depthDenseSamplingStepX;
132   //! Sampling step in y-direction
133   unsigned int m_depthDenseSamplingStepY;
134   //! (s - s*)
135   vpColVector m_error_depthDense;
136   //! Interaction matrix
137   vpMatrix m_L_depthDense;
138   //! Tukey M-Estimator
139   vpMbtTukeyEstimator<double> m_robust_depthDense;
140   //! Robust weights
141   vpColVector m_w_depthDense;
142   //! Weighted error
143   vpColVector m_weightedError_depthDense;
144 #if DEBUG_DISPLAY_DEPTH_DENSE
145   vpDisplay *m_debugDisp_depthDense;
146   vpImage<unsigned char> m_debugImage_depthDense;
147 #endif
148 
149   void addFace(vpMbtPolygon &polygon, bool alreadyClose);
150 
151   void computeVisibility(unsigned int width, unsigned int height);
152 
153   void computeVVS();
154   virtual void computeVVSInit();
155   virtual void computeVVSInteractionMatrixAndResidu();
156   virtual void computeVVSWeights();
157   using vpMbTracker::computeVVSWeights;
158 
159   virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
160                           int idFace = 0, const std::string &name = "");
161 
162   virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
163                             const std::string &name = "");
164 
165   virtual void initFaceFromCorners(vpMbtPolygon &polygon);
166 
167   virtual void initFaceFromLines(vpMbtPolygon &polygon);
168 
169 #ifdef VISP_HAVE_PCL
170   void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
171 #endif
172   void segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width,
173                          unsigned int height);
174 };
175 #endif
176