Visual Servoing Platform  version 3.6.1 under development (2024-04-16)
vpMbtDistanceKltPoints.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #ifndef _vpMbtDistanceKltPoints_h_
37 #define _vpMbtDistanceKltPoints_h_
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
42 
43 #include <map>
44 
45 #include <visp3/core/vpDisplay.h>
46 #include <visp3/core/vpGEMM.h>
47 #include <visp3/core/vpPlane.h>
48 #include <visp3/core/vpPolygon3D.h>
49 #include <visp3/klt/vpKltOpencv.h>
50 #include <visp3/mbt/vpMbHiddenFaces.h>
51 #include <visp3/vision/vpHomography.h>
52 
63 class VISP_EXPORT vpMbtDistanceKltPoints
64 {
65 private:
67  vpMatrix H;
69  vpColVector N;
71  vpColVector N_cur;
74  double invd0;
76  vpColVector cRc0_0n;
78  std::map<int, vpImagePoint> initPoints;
80  std::map<int, vpImagePoint> curPoints;
82  std::map<int, int> curPointsInd;
84  unsigned int nbPointsCur;
86  unsigned int nbPointsInit;
88  unsigned int minNbPoint;
90  bool enoughPoints;
92  double dt;
94  double d0;
98  bool isTrackedKltPoints;
99 
100 public:
107 
108 private:
109  double compute_1_over_Z(double x, double y);
110  void computeP_mu_t(double x_in, double y_in, double &x_out, double &y_out, const vpMatrix &cHc0);
111  bool isTrackedFeature(int id);
112 
113  // private:
114  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
115  // vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
116  // : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(),
117  // curPoints(), curPointsInd(),
118  // nbPointsCur(0), nbPointsInit(0), minNbPoint(4),
119  // enoughPoints(false), dt(1.), d0(1.), cam(),
120  // isTrackedKltPoints(true), polygon(nullptr), hiddenface(nullptr),
121  // useScanLine(false)
122  // {
123  // throw vpException(vpException::functionNotImplementedError, "Not
124  // implemented!");
125  // }
126  // vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
127  // throw vpException(vpException::functionNotImplementedError, "Not
128  // implemented!"); return *this;
129  // }
130  //#endif
131 
132 public:
134  virtual ~vpMbtDistanceKltPoints();
135 
136  unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage<bool> *mask = nullptr);
137  void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0);
138  void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J);
139 
140  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
141  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
142  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
143  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
144 
145  void displayPrimitive(const vpImage<unsigned char> &_I);
146  void displayPrimitive(const vpImage<vpRGBa> &_I);
147 
148  std::vector<std::vector<double> > getFeaturesForDisplay();
149 
150  std::vector<std::vector<double> > getModelForDisplay(const vpCameraParameters &cam, bool displayFullModel = false);
151 
157  inline vpCameraParameters &getCameraParameters() { return cam; }
158 
159  inline vpColVector getCurrentNormal() const { return N_cur; }
160 
161  inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; }
162 
163  inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; }
164 
173  inline unsigned int getInitialNumberPoint() const { return nbPointsInit; }
184  inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; }
185 
186  inline bool hasEnoughPoints() const { return enoughPoints; }
187 
188  void init(const vpKltOpencv &_tracker, const vpImage<bool> *mask = nullptr);
189 
195  inline bool isTracked() const { return isTrackedKltPoints; }
196 
197  void removeOutliers(const vpColVector &weight, const double &threshold_outlier);
198 
204  virtual inline void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; }
205 
211  inline void setTracked(const bool &track) { this->isTrackedKltPoints = track; }
212 
213  void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
214 };
215 
216 #endif
217 
218 #endif // VISP_HAVE_OPENCV
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:168
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:73
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
bool useScanLine
Use scanline rendering.
vpColVector getCurrentNormal() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
virtual void setCameraParameters(const vpCameraParameters &_cam)
unsigned int getCurrentNumberPoints() const
std::map< int, int > & getCurrentPointsInd()
vpCameraParameters & getCameraParameters()
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
std::map< int, vpImagePoint > & getCurrentPoints()
unsigned int getInitialNumberPoint() const
void setTracked(const bool &track)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58