Visual Servoing Platform  version 3.6.1 under development (2025-03-13)
Classes | Public Member Functions | List of all members
vpRBProbabilistic3DDriftDetector Class Reference
tracker: ViSP trackers » RBT: Render-Based Tracker module » Drift and divergence detection

#include <visp3/rbt/vpRBProbabilistic3DDriftDetector.h>

+ Inheritance diagram for vpRBProbabilistic3DDriftDetector:

Public Member Functions

 vpRBProbabilistic3DDriftDetector ()
 
void update (const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) VP_OVERRIDE
 
double getScore () const VP_OVERRIDE
 
bool hasDiverged () const VP_OVERRIDE
 
void display (const vpImage< vpRGBa > &I) VP_OVERRIDE
 
Settings
double getMinDistForNew3DPoints () const
 
void setMinDistForNew3DPoints (double distance)
 
double getFilteringMax3DError () const
 
void setFilteringMax3DError (double maxError)
 
double getDepthStandardDeviation () const
 
void setDepthStandardDeviation (double sigma)
 
double getInitialColorStandardDeviation () const
 
void setInitialColorStandardDeviation (double sigma)
 
double getColorUpdateRate () const
 
void setColorUpdateRate (double updateRate)
 
void loadJsonConfiguration (const nlohmann::json &) VP_OVERRIDE
 

Detailed Description

Algorithm that uses tracks object surface points in order to estimate the probability that tracking is successful.

Given a set of surface points $ \mathbf{X}_0, ..., \mathbf{X}_N$, each point $\mathbf{X}_i$ being associated to:

  • a color distribution $\mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})$,
  • its distance to the camera being $Z_i$,
  • its projection in the current color and depth images $\mathbf{I_c}, \mathbf{I_Z}$ having coordinates $u_i, v_i$.
  • its visibility $V(\mathbf{X_i})$, which is 1 if $u_i, v_i$ lie in the image, $Z_i$ is close to the rendered depth value and the normal at the surface marks the point as visible from the camera's point of view.

We compute the probability that tracking is successful for a given pose $^{c}\mathbf{T}_o$ as:

\[ p(^{c}\mathbf{T}_o) = \frac{1}{\sum_{i=0}^N w_i \cdot V(\mathbf{X_i})}\sum_{i=0}^N w_i \cdot V(\mathbf{X_i}) \cdot p(\mathbf{X_i}) \]

with

\[ \begin{aligned} p(\mathbf{X_i}) &= p(\mathbf{I_c}(u_i, v_i)|\mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})) \cdot p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) \\ p(\mathbf{I_c}(u_i, v_i) | \mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})) &= erfc(\frac{1}{\sqrt{2}}\lVert \frac{\mathbf{I_c}(u_i, v_i) - \mathbf{\bar c_i}}{diag(\mathbf{\Sigma_{c_i}})} \rVert_2) \\ p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) &= erfc(\frac{1}{\sigma_Z \sqrt{2}}\mathbf{I_Z}(u_i, v_i) - Z_i) \end{aligned} \]

if the depth is unavailable, then we set $p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) = 1$

Here, the color distribution is estimated online for each point separately using exponential moving average/variance techniques. For each point the update step is computed as $p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2))\cdot \alpha$ where $\alpha$ is a fixed parameter. Larger values will lead to faster update rates and may be more beneficial for non lambertian materials.

For the depth, $\sigma_Z$ is a fixed parameter to be tweaked by the user.

Every time update() is called, the set of points $ \mathbf{X}_0, ..., \mathbf{X}_N, $ may grow larger. If a new candidate point is visible and is far enough from points already in the set, it is added to it.

Definition at line 88 of file vpRBProbabilistic3DDriftDetector.h.

Constructor & Destructor Documentation

◆ vpRBProbabilistic3DDriftDetector()

vpRBProbabilistic3DDriftDetector::vpRBProbabilistic3DDriftDetector ( )
inline

Definition at line 208 of file vpRBProbabilistic3DDriftDetector.h.

Member Function Documentation

◆ display()

void vpRBProbabilistic3DDriftDetector::display ( const vpImage< vpRGBa > &  I)
virtual

Displays the information used for drift detection.

Parameters
Ithe image in which to display the information

Implements vpRBDriftDetector.

Definition at line 218 of file vpRBProbabilistic3DDriftDetector.cpp.

◆ getColorUpdateRate()

double vpRBProbabilistic3DDriftDetector::getColorUpdateRate ( ) const
inline

Get the rate at which the colors of surface points are updated.

Note that if depth is available, this component is further multiplied by the probability of depth being correct for a given point.

A high value will lead to a fast update rate (short term memory), while a lower one will update slower. A slower update may lead to a more stable tracking score. A higher value may be better suited to non isotropic materials.

Definition at line 300 of file vpRBProbabilistic3DDriftDetector.h.

◆ getDepthStandardDeviation()

double vpRBProbabilistic3DDriftDetector::getDepthStandardDeviation ( ) const
inline

Get the standard deviation that is used when computing the probability that the observed depth Z is the correct depth given the rendered depth at the same pixel.

Definition at line 269 of file vpRBProbabilistic3DDriftDetector.h.

◆ getFilteringMax3DError()

double vpRBProbabilistic3DDriftDetector::getFilteringMax3DError ( ) const
inline

Returns the maximum 3D distance (in meters) above which a tracked surface point is rejected for the drift estimation step.

The surface point's distance to the camera is compared to rendered depth. If the difference between the two is too great, it is rejected.

This is mainly used to handle self occlusions.

Definition at line 254 of file vpRBProbabilistic3DDriftDetector.h.

◆ getInitialColorStandardDeviation()

double vpRBProbabilistic3DDriftDetector::getInitialColorStandardDeviation ( ) const
inline

Get the standard deviation that is used to initialize the color distribution when adding a new surface point. This standard deviation is applied on all color components.

Definition at line 283 of file vpRBProbabilistic3DDriftDetector.h.

◆ getMinDistForNew3DPoints()

double vpRBProbabilistic3DDriftDetector::getMinDistForNew3DPoints ( ) const
inline

Get the minimum distance criterion (in meters) that is used when trying to add new points to track for the drift detection.

A candidate surface point is compared to all the currently tracked surface point and if any of these points is below the minimum distance, the candidate is rejected.

Returns
the minimum distance, in meters

Definition at line 237 of file vpRBProbabilistic3DDriftDetector.h.

◆ getScore()

double vpRBProbabilistic3DDriftDetector::getScore ( ) const
virtual

Returns the probability [0, 1] that tracking is successful.

Implements vpRBDriftDetector.

Definition at line 232 of file vpRBProbabilistic3DDriftDetector.cpp.

◆ hasDiverged()

bool vpRBProbabilistic3DDriftDetector::hasDiverged ( ) const
virtual

Returns whether the tracking has diverged and should be reinitialized. This function should be called after update.

Implements vpRBDriftDetector.

Definition at line 237 of file vpRBProbabilistic3DDriftDetector.cpp.

◆ loadJsonConfiguration()

void vpRBProbabilistic3DDriftDetector::loadJsonConfiguration ( const nlohmann::json &  j)
virtual

Implements vpRBDriftDetector.

Definition at line 243 of file vpRBProbabilistic3DDriftDetector.cpp.

References setColorUpdateRate(), setDepthStandardDeviation(), setFilteringMax3DError(), setInitialColorStandardDeviation(), and setMinDistForNew3DPoints().

◆ setColorUpdateRate()

void vpRBProbabilistic3DDriftDetector::setColorUpdateRate ( double  updateRate)
inline

Set the update rate for the color distribution. It should be between 0 and 1.

Parameters
updateRatethe update rate

Definition at line 307 of file vpRBProbabilistic3DDriftDetector.h.

References vpException::badValue.

Referenced by loadJsonConfiguration().

◆ setDepthStandardDeviation()

void vpRBProbabilistic3DDriftDetector::setDepthStandardDeviation ( double  sigma)
inline

Definition at line 270 of file vpRBProbabilistic3DDriftDetector.h.

References vpException::badValue.

Referenced by loadJsonConfiguration().

◆ setFilteringMax3DError()

void vpRBProbabilistic3DDriftDetector::setFilteringMax3DError ( double  maxError)
inline

Definition at line 256 of file vpRBProbabilistic3DDriftDetector.h.

References vpException::badValue.

Referenced by loadJsonConfiguration().

◆ setInitialColorStandardDeviation()

void vpRBProbabilistic3DDriftDetector::setInitialColorStandardDeviation ( double  sigma)
inline

Definition at line 284 of file vpRBProbabilistic3DDriftDetector.h.

References vpException::badValue.

Referenced by loadJsonConfiguration().

◆ setMinDistForNew3DPoints()

void vpRBProbabilistic3DDriftDetector::setMinDistForNew3DPoints ( double  distance)
inline

Definition at line 239 of file vpRBProbabilistic3DDriftDetector.h.

References vpException::badValue.

Referenced by loadJsonConfiguration().

◆ update()

BEGIN_VISP_NAMESPACE void vpRBProbabilistic3DDriftDetector::update ( const vpRBFeatureTrackerInput &  previousFrame,
const vpRBFeatureTrackerInput &  frame,
const vpHomogeneousMatrix &  cTo,
const vpHomogeneousMatrix &  cprevTo 
)
virtual

Update the algorithm after a new tracking step.

Parameters
previousFrameThe previous frame data: contains the input images at t-1 (linked to cprevTo) and the renders at t-2. May be empty for the first iteration
frameThe current frame data: contains the input images at time t (linked to the newly estimated cTo) and the renders at t-1 (linked to cprevTo)
cTothe newly estimated object pose in the camera frame
cprevTothe previously estimated object pose in the camera frame

Implements vpRBDriftDetector.

Definition at line 45 of file vpRBProbabilistic3DDriftDetector.cpp.

References vpRGBa::B, vpRGBf::B, vpException::badValue, vpRBRenderData::boundingBox, vpRBFeatureTrackerInput::cam, vpRBRenderData::cMo, vpPixelMeterConversion::convertPoint(), vpRBRenderData::depth, vpRBFeatureTrackerInput::depth, vpColVector::dotProd(), vpRGBa::G, vpRGBf::G, vpRect::getBottom(), vpImage< Type >::getHeight(), vpRect::getLeft(), vpRect::getRight(), vpRect::getTop(), vpHomogeneousMatrix::getTranslationVector(), vpImage< Type >::getWidth(), vpRBFeatureTrackerInput::hasDepth(), vpRBSilhouettePoint::i, vpHomogeneousMatrix::inverse(), vpRBFeatureTrackerInput::IRGB, vpRBSilhouettePoint::j, vpColVector::normalize(), vpRBRenderData::normals, vpRGBa::R, vpRGBf::R, vpMath::rad(), vpRBFeatureTrackerInput::renders, vpRBFeatureTrackerInput::silhouettePoints, and vpMath::sqr().