36 #ifndef VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
37 #define VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
39 #include <visp3/core/vpCameraParameters.h>
40 #include <visp3/core/vpRGBf.h>
41 #include <visp3/core/vpRGBa.h>
43 #include <visp3/rbt/vpRBDriftDetector.h>
49 template <
typename T>
class vpImage;
92 struct vpStored3DSurfaceColorPoint
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;
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));
142 const double proba = 1.0 - erf(dist / sqrt(2));
149 return static_cast<double>(variance.R + variance.G + variance.B);
154 standardDev.R = sqrt(variance.R);
155 standardDev.G = sqrt(variance.G);
156 standardDev.B = sqrt(variance.B);
166 fastProjection(cTo, cam, currX, projCurr, projCurrPx);
167 fastProjection(cprevTo, cam, prevX, projPrev, projPrevPx);
170 inline double squaredDist(
const std::array<double, 3> &p)
const
172 return std::pow(p[0] - X[0], 2) + std::pow(p[1] - X[1], 2) + std::pow(p[2] - X[2], 2);
176 std::array<double, 3> &pC, std::array<double, 2> &proj, std::array<int, 2> &px)
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());
188 void updateColor(
const vpRGBf ¤tColor,
float updateRate)
190 stats.update(currentColor, updateRate);
193 vpRGBa getDisplayColor()
const
195 return vpRGBa(
static_cast<unsigned int>(stats.mean.R),
static_cast<unsigned int>(stats.mean.G),
static_cast<unsigned int>(stats.mean.B));
198 std::array<double, 3> X;
199 ColorStatistics stats;
200 std::array<double, 3> currX, prevX;
201 std::array<double, 2> projCurr, projPrev;
202 std::array<int, 2> projCurrPx, projPrevPx;
217 double getScore() const VP_OVERRIDE;
219 bool hasDiverged() const VP_OVERRIDE;
237 double getMinDistForNew3DPoints()
const {
return m_minDist3DNewPoint; }
241 if (distance <= 0.0) {
244 m_minDist3DNewPoint = distance;
258 if (maxError <= 0.0) {
261 m_maxError3D = maxError;
275 m_depthSigma = sigma;
289 m_initialColorSigma = sigma;
309 if (updateRate < 0.0 || updateRate > 1.f) {
312 m_colorUpdateRate = updateRate;
315 #if defined(VISP_HAVE_NLOHMANN_JSON)
325 double m_colorUpdateRate;
326 double m_initialColorSigma;
329 double m_minDist3DNewPoint;
333 std::vector<vpStored3DSurfaceColorPoint> m_points;
Type * data
Address of the first element of the data array.
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
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.
Algorithm that uses tracks object surface points in order to estimate the probability that tracking i...
void setMinDistForNew3DPoints(double distance)
void setFilteringMax3DError(double maxError)
double getInitialColorStandardDeviation() const
Get the standard deviation that is used to initialize the color distribution when adding a new surfac...
void setDepthStandardDeviation(double sigma)
void setColorUpdateRate(double updateRate)
Set the update rate for the color distribution. It should be between 0 and 1.
void setInitialColorStandardDeviation(double sigma)
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...
vpRBProbabilistic3DDriftDetector()
double getFilteringMax3DError() const
Returns the maximum 3D distance (in meters) above which a tracked surface point is rejected for the d...
Online estimation of a Gaussian color distribution , Where is a diagonal variance matrix .
void init(const vpRGBf &c, const vpRGBf &var)
double probability(const vpRGBf &c)
Computes the probability that the input color was sampled from the estimated distribution.
ColorStatistics()=default
void update(const vpRGBf &c, float weight)
Update the color distribution with a new sample c.