Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpMbtDistanceKltPoints.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  * Klt polygon, containing points of interest.
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 #ifndef vpMbtDistanceKltPoints_h
41 #define vpMbtDistanceKltPoints_h
42 
43 #include <visp3/core/vpConfig.h>
44 
45 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
46 
47 #include <map>
48 
49 #include <visp3/core/vpDisplay.h>
50 #include <visp3/core/vpGEMM.h>
51 #include <visp3/core/vpPlane.h>
52 #include <visp3/core/vpPolygon3D.h>
53 #include <visp3/klt/vpKltOpencv.h>
54 #include <visp3/mbt/vpMbHiddenFaces.h>
55 #include <visp3/vision/vpHomography.h>
56 
67 class VISP_EXPORT vpMbtDistanceKltPoints
68 {
69 private:
71  vpMatrix H;
73  vpColVector N;
75  vpColVector N_cur;
78  double invd0;
80  vpColVector cRc0_0n;
82  std::map<int, vpImagePoint> initPoints;
84  std::map<int, vpImagePoint> curPoints;
86  std::map<int, int> curPointsInd;
88  unsigned int nbPointsCur;
90  unsigned int nbPointsInit;
92  unsigned int minNbPoint;
94  bool enoughPoints;
96  double dt;
98  double d0;
100  vpCameraParameters cam;
102  bool isTrackedKltPoints;
103 
104 public:
111 
112 private:
113  double compute_1_over_Z(const double x, const double y);
114  void computeP_mu_t(const double x_in, const double y_in, double &x_out, double &y_out, const vpMatrix &cHc0);
115  bool isTrackedFeature(const int id);
116 
117  // private:
118  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
119  // vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
120  // : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(),
121  // curPoints(), curPointsInd(),
122  // nbPointsCur(0), nbPointsInit(0), minNbPoint(4),
123  // enoughPoints(false), dt(1.), d0(1.), cam(),
124  // isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL),
125  // useScanLine(false)
126  // {
127  // throw vpException(vpException::functionNotImplementedError, "Not
128  // implemented!");
129  // }
130  // vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
131  // throw vpException(vpException::functionNotImplementedError, "Not
132  // implemented!"); return *this;
133  // }
134  //#endif
135 
136 public:
138  virtual ~vpMbtDistanceKltPoints();
139 
140  unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage<bool> *mask = NULL);
141  void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0);
142  void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J);
143 
144  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
145  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
146  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
147  const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
148 
149  void displayPrimitive(const vpImage<unsigned char> &_I);
150  void displayPrimitive(const vpImage<vpRGBa> &_I);
151 
152  std::vector<std::vector<double> > getFeaturesForDisplay();
153 
154  std::vector<std::vector<double> > getModelForDisplay(const vpCameraParameters &cam,
155  const bool displayFullModel = false);
156 
162  inline vpCameraParameters &getCameraParameters() { return cam; }
163 
164  inline vpColVector getCurrentNormal() const { return N_cur; }
165 
166  inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; }
167 
168  inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; }
169 
178  inline unsigned int getInitialNumberPoint() const { return nbPointsInit; }
189  inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; }
190 
191  inline bool hasEnoughPoints() const { return enoughPoints; }
192 
193  void init(const vpKltOpencv &_tracker, const vpImage<bool> *mask = NULL);
194 
200  inline bool isTracked() const { return isTrackedKltPoints; }
201 
202  void removeOutliers(const vpColVector &weight, const double &threshold_outlier);
203 
209  virtual inline void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; }
210 
216  inline void setTracked(const bool &track) { this->isTrackedKltPoints = track; }
217 
218 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
219  void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
220 #else
221  void updateMask(IplImage *mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
222 #endif
223 };
224 
225 #endif
226 
227 #endif // VISP_HAVE_OPENCV
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void setTracked(const bool &track)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
std::map< int, int > & getCurrentPointsInd()
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
Generic class defining intrinsic camera parameters.
unsigned int getInitialNumberPoint() const
std::map< int, vpImagePoint > & getCurrentPoints()
virtual void setCameraParameters(const vpCameraParameters &_cam)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
unsigned int getCurrentNumberPoints() const
vpColVector getCurrentNormal() const
vpCameraParameters & getCameraParameters()
bool useScanLine
Use scanline rendering.