Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
vpRBProbabilistic3DDriftDetector.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  */
30 
36 #ifndef VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
37 #define VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
38 
39 #include <visp3/core/vpCameraParameters.h>
40 #include <visp3/core/vpRGBf.h>
41 #include <visp3/core/vpRGBa.h>
42 
43 #include <visp3/rbt/vpRBDriftDetector.h>
44 
45 #include <array>
46 
47 BEGIN_VISP_NAMESPACE
48 
49 template <typename T> class vpImage;
50 
89 {
90 private:
91 
92  struct vpStored3DSurfaceColorPoint
93  {
94 
103  {
104  ColorStatistics() = default;
105 
106  void init(const vpRGBf &c, const vpRGBf &var)
107  {
108  mean = c;
109  variance = var;
110  computeStddev();
111  }
112 
119  void update(const vpRGBf &c, float weight)
120  {
121  const vpRGBf diff(c.R - mean.R, c.G - mean.G, c.B - mean.B);
122  vpRGBf diffSqr(std::pow(diff.R, 2), std::pow(diff.G, 2), std::pow(diff.B, 2));
123  mean = mean + weight * diff;
124  variance = variance + weight * diffSqr;
125  computeStddev();
126  }
127 
134  double probability(const vpRGBf &c)
135  {
136 
137  const double dist = sqrt(
138  std::pow((c.R - mean.R) / (standardDev.R), 2) +
139  std::pow((c.G - mean.G) / (standardDev.G), 2) +
140  std::pow((c.B - mean.B) / (standardDev.B), 2));
141 
142  const double proba = 1.0 - erf(dist / sqrt(2));
143 
144  return proba;
145  }
146 
147  double trace()
148  {
149  return static_cast<double>(variance.R + variance.G + variance.B);
150  }
151 
153  {
154  standardDev.R = sqrt(variance.R);
155  standardDev.G = sqrt(variance.G);
156  standardDev.B = sqrt(variance.B);
157  }
158 
162  };
163 
164  inline void update(const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo, const vpCameraParameters &cam)
165  {
166  fastProjection(cTo, cam, currX, projCurr, projCurrPx);
167  fastProjection(cprevTo, cam, prevX, projPrev, projPrevPx);
168  }
169 
170  inline double squaredDist(const std::array<double, 3> &p) const
171  {
172  return std::pow(p[0] - X[0], 2) + std::pow(p[1] - X[1], 2) + std::pow(p[2] - X[2], 2);
173  }
174 
175  inline void fastProjection(const vpHomogeneousMatrix &cTo, const vpCameraParameters &cam,
176  std::array<double, 3> &pC, std::array<double, 2> &proj, std::array<int, 2> &px)
177  {
178  const double *T = cTo.data;
179  pC[0] = (T[0] * X[0] + T[1] * X[1] + T[2] * X[2] + T[3]);
180  pC[1] = (T[4] * X[0] + T[5] * X[1] + T[6] * X[2] + T[7]);
181  pC[2] = (T[8] * X[0] + T[9] * X[1] + T[10] * X[2] + T[11]);
182  proj[0] = pC[0] / pC[2];
183  proj[1] = pC[1] / pC[2];
184  px[0] = static_cast<int>((proj[0] * cam.get_px()) + cam.get_u0());
185  px[1] = static_cast<int>((proj[1] * cam.get_py()) + cam.get_v0());
186  }
187 
188  void updateColor(const vpRGBf &currentColor, float updateRate)
189  {
190  stats.update(currentColor, updateRate);
191  }
192 
193  vpRGBa getDisplayColor() const
194  {
195  return vpRGBa(static_cast<unsigned int>(stats.mean.R), static_cast<unsigned int>(stats.mean.G), static_cast<unsigned int>(stats.mean.B));
196  }
197 
198  std::array<double, 3> X; // Point position in object frame
199  ColorStatistics stats;
200  std::array<double, 3> currX, prevX;
201  std::array<double, 2> projCurr, projPrev; // Projection in camera normalized coordinates of the point for the current and previous camera poses.
202  std::array<int, 2> projCurrPx, projPrevPx; // Projection in pixels of the point for the current and previous camera poses.
203  bool visible; // Whether the point is visible
204  };
205 
206 public:
207 
208  vpRBProbabilistic3DDriftDetector() : m_colorUpdateRate(0.2), m_initialColorSigma(25.0), m_depthSigma(0.04), m_maxError3D(0.001), m_minDist3DNewPoint(0.003)
209  { }
210 
211  void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) VP_OVERRIDE;
212 
217  double getScore() const VP_OVERRIDE;
218 
219  bool hasDiverged() const VP_OVERRIDE;
220 
221  void display(const vpImage<vpRGBa> &I) VP_OVERRIDE;
222 
237  double getMinDistForNew3DPoints() const { return m_minDist3DNewPoint; }
238 
239  void setMinDistForNew3DPoints(double distance)
240  {
241  if (distance <= 0.0) {
242  throw vpException(vpException::badValue, "Distance criterion for candidate rejection should be greater than 0.");
243  }
244  m_minDist3DNewPoint = distance;
245  }
246 
254  double getFilteringMax3DError() const { return m_maxError3D; }
255 
256  void setFilteringMax3DError(double maxError)
257  {
258  if (maxError <= 0.0) {
259  throw vpException(vpException::badValue, "Maximum 3D error for rejection should be greater than 0.");
260  }
261  m_maxError3D = maxError;
262  }
263 
269  double getDepthStandardDeviation() const { return m_depthSigma; }
270  void setDepthStandardDeviation(double sigma)
271  {
272  if (sigma < 0.0) {
273  throw vpException(vpException::badValue, "Depth standard deviation should be greater than 0");
274  }
275  m_depthSigma = sigma;
276  }
277 
283  double getInitialColorStandardDeviation() const { return m_depthSigma; }
285  {
286  if (sigma < 0.0) {
287  throw vpException(vpException::badValue, "Initial color standard deviation should be greater than 0");
288  }
289  m_initialColorSigma = sigma;
290  }
291 
300  double getColorUpdateRate() const { return m_colorUpdateRate; }
301 
307  void setColorUpdateRate(double updateRate)
308  {
309  if (updateRate < 0.0 || updateRate > 1.f) {
310  throw vpException(vpException::badValue, "Color update rate should be between 0 and 1");
311  }
312  m_colorUpdateRate = updateRate;
313  }
314 
315 #if defined(VISP_HAVE_NLOHMANN_JSON)
316  void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE;
317 #endif
318 
324 private:
325  double m_colorUpdateRate;
326  double m_initialColorSigma;
327  double m_depthSigma;
328  double m_maxError3D;
329  double m_minDist3DNewPoint;
330 
331  double m_score;
332 
333  std::vector<vpStored3DSurfaceColorPoint> m_points;
334 };
335 
336 END_VISP_NAMESPACE
337 
338 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition: vpImage.h:131
Base interface for algorithms that should detect tracking drift for the render-based tracker.
virtual double getScore() const =0
Get the estimated tracking reliability. A high score should mean that the tracking is reliable....
virtual void loadJsonConfiguration(const nlohmann::json &)=0
virtual void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo)=0
Update the algorithm after a new tracking step.
All the data related to a single tracking frame. This contains both the input data (from a real camer...
Algorithm that uses tracks object surface points in order to estimate the probability that tracking i...
double getInitialColorStandardDeviation() const
Get the standard deviation that is used to initialize the color distribution when adding a new surfac...
void setColorUpdateRate(double updateRate)
Set the update rate for the color distribution. It should be between 0 and 1.
double getColorUpdateRate() const
Get the rate at which the colors of surface points are updated.
double getDepthStandardDeviation() const
Get the standard deviation that is used when computing the probability that the observed depth Z is t...
double getFilteringMax3DError() const
Returns the maximum 3D distance (in meters) above which a tracked surface point is rejected for the d...
Definition: vpRGBa.h:70
Definition: vpRGBf.h:64
float B
Blue component.
Definition: vpRGBf.h:157
float G
Green component.
Definition: vpRGBf.h:156
float R
Red component.
Definition: vpRGBf.h:155
Online estimation of a Gaussian color distribution , Where is a diagonal variance matrix .
double probability(const vpRGBf &c)
Computes the probability that the input color was sampled from the estimated distribution.
void update(const vpRGBf &c, float weight)
Update the color distribution with a new sample c.