Visual Servoing Platform  version 3.6.1 under development (2025-03-14)
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 
53 BEGIN_VISP_NAMESPACE
64 class VISP_EXPORT vpMbtDistanceKltPoints
65 {
66 private:
68  vpMatrix H;
70  vpColVector N;
72  vpColVector N_cur;
75  double invd0;
77  vpColVector cRc0_0n;
79  std::map<int, vpImagePoint> initPoints;
81  std::map<int, vpImagePoint> curPoints;
83  std::map<int, int> curPointsInd;
85  unsigned int nbPointsCur;
87  unsigned int nbPointsInit;
89  unsigned int minNbPoint;
91  bool enoughPoints;
93  double dt;
95  double d0;
99  bool isTrackedKltPoints;
100 
101 public:
108 
109 private:
110  double compute_1_over_Z(double x, double y);
111  void computeP_mu_t(double x_in, double y_in, double &x_out, double &y_out, const vpMatrix &cHc0);
112  bool isTrackedFeature(int id);
113 
114  // private:
115  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
116  // vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
117  // : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(),
118  // curPoints(), curPointsInd(),
119  // nbPointsCur(0), nbPointsInit(0), minNbPoint(4),
120  // enoughPoints(false), dt(1.), d0(1.), cam(),
121  // isTrackedKltPoints(true), polygon(nullptr), hiddenface(nullptr),
122  // useScanLine(false)
123  // {
124  // throw vpException(vpException::functionNotImplementedError, "Not
125  // implemented!");
126  // }
127  // vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
128  // throw vpException(vpException::functionNotImplementedError, "Not
129  // implemented!"); return *this;
130  // }
131  //#endif
132 
133 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
134  vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &) = delete; // non construction-copyable
135  vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &) = delete; // non copyable
136 #endif
137 
138 public:
140  virtual ~vpMbtDistanceKltPoints();
141 
142  unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage<bool> *mask = nullptr);
143  void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0);
144  void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J);
145 
146  void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
147  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
148  void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
149  const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
150 
151  void displayPrimitive(const vpImage<unsigned char> &_I);
152  void displayPrimitive(const vpImage<vpRGBa> &_I);
153 
154  std::vector<std::vector<double> > getFeaturesForDisplay();
155 
156  std::vector<std::vector<double> > getModelForDisplay(const vpCameraParameters &cam, bool displayFullModel = false);
157 
163  inline vpCameraParameters &getCameraParameters() { return cam; }
164 
165  inline vpColVector getCurrentNormal() const { return N_cur; }
166 
167  inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; }
168 
169  inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; }
170 
179  inline unsigned int getInitialNumberPoint() const { return nbPointsInit; }
190  inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; }
191 
192  inline bool hasEnoughPoints() const { return enoughPoints; }
193 
194  void init(const vpKltOpencv &_tracker, const vpImage<bool> *mask = nullptr);
195 
201  inline bool isTracked() const { return isTrackedKltPoints; }
202 
203  void removeOutliers(const vpColVector &weight, const double &threshold_outlier);
204 
210  virtual inline void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; }
211 
217  inline void setTracked(const bool &track) { this->isTrackedKltPoints = track; }
218 
219  void updateMask(cv::Mat &mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0);
220 };
221 END_VISP_NAMESPACE
222 #endif
223 
224 #endif // VISP_HAVE_OPENCV
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
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:60