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 * Klt cylinder, containing points of interest. 33 * 34 * Authors: 35 * Aurelien Yol 36 * 37 *****************************************************************************/ 38 39 #ifndef vpMbtDistanceKltCylinder_h 40 #define vpMbtDistanceKltCylinder_h 41 42 #include <visp3/core/vpConfig.h> 43 44 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) 45 46 #include <map> 47 48 #include <visp3/core/vpCircle.h> 49 #include <visp3/core/vpCylinder.h> 50 #include <visp3/core/vpDisplay.h> 51 #include <visp3/core/vpGEMM.h> 52 #include <visp3/core/vpPlane.h> 53 #include <visp3/core/vpPolygon3D.h> 54 #include <visp3/klt/vpKltOpencv.h> 55 #include <visp3/mbt/vpMbHiddenFaces.h> 56 #include <visp3/vision/vpHomography.h> 57 58 /*! 59 \class vpMbtDistanceKltCylinder 60 61 \brief Implementation of a polygon of the model containing points of 62 interest. It is used by the model-based tracker KLT, and hybrid. 63 64 \warning This class is only available if OpenCV is installed, and used. 65 66 \ingroup group_mbt_features 67 */ 68 class VISP_EXPORT vpMbtDistanceKltCylinder 69 { 70 private: 71 //! Pose at initialisation 72 vpHomogeneousMatrix c0Mo; 73 //! First extremity of the cylinder (used for display) 74 vpPoint p1Ext; 75 //! Second extremity of the cylinder (used for display) 76 vpPoint p2Ext; 77 //! Cylinder 78 vpCylinder cylinder; 79 //! The upper circle limiting the cylinder (used for display) 80 vpCircle circle1; 81 //! The lower circle limiting the cylinder (used for display) 82 vpCircle circle2; 83 //! Initial points and their ID 84 std::map<int, vpImagePoint> initPoints; 85 //! Initial points and their ID 86 std::map<int, vpPoint> initPoints3D; 87 //! Current points and their ID 88 std::map<int, vpImagePoint> curPoints; 89 //! Current points ID and their indexes 90 std::map<int, int> curPointsInd; 91 //! number of points detected 92 unsigned int nbPointsCur; 93 //! initial number of points 94 unsigned int nbPointsInit; 95 //! Minimal number of points to be tracked 96 unsigned int minNbPoint; 97 //! Boolean to know if there is enough point to be tracked 98 bool enoughPoints; 99 //! Camera parameters 100 vpCameraParameters cam; 101 //! Boolean to specify if the klt points have to be tracked or not 102 bool isTrackedKltCylinder; 103 104 public: 105 //! Pointer to the polygon that define a face 106 std::vector<int> listIndicesCylinderBBox; 107 //! Pointer to the list of faces 108 vpMbHiddenFaces<vpMbtPolygon> *hiddenface; 109 //! Use scanline rendering 110 bool useScanLine; 111 112 private: 113 double computeZ(const double &x, const double &y); 114 bool isTrackedFeature(int id); 115 116 // private: 117 //#ifndef DOXYGEN_SHOULD_SKIP_THIS 118 // vpMbtDistanceKltCylinder(const vpMbtDistanceKltCylinder &) 119 // : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(), 120 // initPoints(), initPoints3D(), curPoints(), curPointsInd(), 121 // nbPointsCur(0), nbPointsInit(0), minNbPoint(4), 122 // enoughPoints(false), cam(), isTrackedKltCylinder(true), 123 // listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false) 124 // { 125 // throw vpException(vpException::functionNotImplementedError, "Not 126 // implemented!"); 127 // } 128 // vpMbtDistanceKltCylinder &operator=(const vpMbtDistanceKltCylinder &){ 129 // throw vpException(vpException::functionNotImplementedError, "Not 130 // implemented!"); return *this; 131 // } 132 //#endif 133 134 public: 135 vpMbtDistanceKltCylinder(); 136 virtual ~vpMbtDistanceKltCylinder(); 137 138 void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r); 139 140 unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker); 141 void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J); 142 143 void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, 144 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false); 145 void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, 146 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false); 147 148 void displayPrimitive(const vpImage<unsigned char> &_I); 149 void displayPrimitive(const vpImage<vpRGBa> &_I); 150 151 /*! 152 Get the camera parameters of the face. 153 154 \return cam : the camera parameters of the face. 155 */ getCameraParameters()156 inline vpCameraParameters &getCameraParameters() { return cam; } 157 getCurrentPoints()158 inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; } 159 getCurrentPointsInd()160 inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; } 161 getCylinder()162 inline vpCylinder getCylinder() const { return cylinder; } 163 164 /*! 165 Get the number of point that was belonging to the face at the 166 initialisation 167 168 \return the number of initial point. 169 170 \sa getCurrentNumberPoints() 171 */ getInitialNumberPoint()172 inline unsigned int getInitialNumberPoint() const { return nbPointsInit; } 173 /*! 174 Get the number of points detected in the last image. 175 176 \warning To have the real number of points, the function 177 computeNbDetectedCurrent() must be called first. 178 179 \return the number of points detected in the current image. 180 181 \sa getInitialNumberPoint() 182 */ getCurrentNumberPoints()183 inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; } 184 185 std::vector<std::vector<double> > getFeaturesForDisplay(); 186 187 std::vector<std::vector<double> > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam); 188 hasEnoughPoints()189 inline bool hasEnoughPoints() const { return enoughPoints; } 190 191 /*! 192 Return if the klt cylinder is used for tracking. 193 194 \return True if it is used, False otherwise. 195 */ isTracked()196 inline bool isTracked() const { return isTrackedKltCylinder; } 197 198 void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo); 199 200 void removeOutliers(const vpColVector &weight, const double &threshold_outlier); 201 202 /*! 203 Set the camera parameters 204 205 \param _cam : the new camera parameters 206 */ setCameraParameters(const vpCameraParameters & _cam)207 virtual inline void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; } 208 209 /*! 210 Set if the klt cylinder has to be considered during tracking phase. 211 212 \param track : True if is has to be tracked, False otherwise. 213 */ setTracked(const bool & track)214 inline void setTracked(const bool &track) { this->isTrackedKltCylinder = track; } 215 216 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) 217 void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0); 218 #else 219 void updateMask(IplImage *mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0); 220 #endif 221 }; 222 223 #endif 224 225 #endif // VISP_HAVE_OPENCV 226