RBProbabilistic3DDriftDetector¶

class RBProbabilistic3DDriftDetector(self)¶

Bases: RBDriftDetector

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{split}\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} \end{split}\]

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.

Methods

`__init__`
`display`	Displays the information used for drift detection.
`getColorUpdateRate`	Get the rate at which the colors of surface points are updated.
`getDepthStandardDeviation`	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.
`getFilteringMax3DError`	Returns the maximum 3D distance (in meters) above which a tracked surface point is rejected for the drift estimation step.
`getInitialColorStandardDeviation`	Get the standard deviation that is used to initialize the color distribution when adding a new surface point.
`getMinDistForNew3DPoints`	Get the minimum distance criterion (in meters) that is used when trying to add new points to track for the drift detection.
`getScore`	Returns the probability [0, 1] that tracking is successful.
`hasDiverged`	Returns whether the tracking has diverged and should be reinitialized.
`loadJsonConfiguration`
`setColorUpdateRate`	Set the update rate for the color distribution.
`setDepthStandardDeviation`
`setFilteringMax3DError`
`setInitialColorStandardDeviation`
`setMinDistForNew3DPoints`
`update`	Update the algorithm after a new tracking step.

Inherited Methods

Operators

`__doc__`
`__init__`
`__module__`

Attributes

`__annotations__`

__init__(self)¶

display(self, I: visp._visp.core.ImageRGBa) → None¶

Displays the information used for drift detection.

Parameters:

I: visp._visp.core.ImageRGBa¶: the image in which to display the information

getColorUpdateRate(self) → float¶

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.

getDepthStandardDeviation(self) → float¶: 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.

getFilteringMax3DError(self) → float¶

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.

getInitialColorStandardDeviation(self) → float¶: 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.

getMinDistForNew3DPoints(self) → float¶

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

getScore(self) → float¶: Returns the probability [0, 1] that tracking is successful.

hasDiverged(self) → bool¶: Returns whether the tracking has diverged and should be reinitialized. This function should be called after update.

loadJsonConfiguration(self: visp._visp.rbt.RBProbabilistic3DDriftDetector, arg0: nlohmann::basic_json<std::map, std::vector, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool, long, unsigned long, double, std::allocator, nlohmann::adl_serializer, std::vector<unsigned char, std::allocator<unsigned char> > >) → None¶

setColorUpdateRate(self, updateRate: float) → None¶

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

Parameters:

updateRate: float¶: the update rate

setDepthStandardDeviation(self, sigma: float) → None¶

setFilteringMax3DError(self, maxError: float) → None¶

setInitialColorStandardDeviation(self, sigma: float) → None¶

setMinDistForNew3DPoints(self, distance: float) → None¶

update(self, previousFrame: visp._visp.rbt.RBFeatureTrackerInput, frame: visp._visp.rbt.RBFeatureTrackerInput, cTo: visp._visp.core.HomogeneousMatrix, cprevTo: visp._visp.core.HomogeneousMatrix) → None¶

Update the algorithm after a new tracking step.

Parameters:

previousFrame: visp._visp.rbt.RBFeatureTrackerInput¶: The 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
frame: visp._visp.rbt.RBFeatureTrackerInput¶: The 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)
cTo: visp._visp.core.HomogeneousMatrix¶: the newly estimated object pose in the camera frame
cprevTo: visp._visp.core.HomogeneousMatrix¶: the previously estimated object pose in the camera frame