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