![]() |
Visual Servoing Platform version 3.7.0
|
#include <vpRBDenseDepthTracker.h>
Classes | |
| struct | vpDepthPoint |
| class | vpDepthPointSet |
Public Types | |
| enum | vpDisplayType { DT_SIMPLE = 0 , DT_WEIGHT = 1 , DT_ERROR = 2 , DT_WEIGHT_AND_ERROR = 3 , DT_INVALID = 4 } |
Public Member Functions | |
| vpRBDenseDepthTracker () | |
| virtual | ~vpRBDenseDepthTracker ()=default |
| bool | requiresRGB () const VP_OVERRIDE |
| bool | requiresDepth () const VP_OVERRIDE |
| bool | requiresSilhouetteCandidates () const VP_OVERRIDE |
| unsigned | getNumFeatures () const |
| bool | vvsHasConverged () const |
| virtual double | getVVSTrackerWeight (double optimizationProgress) const |
| const std::shared_ptr< vpTemporalWeighting > | getTemporalTrackerWeight () const |
| void | setTrackerWeight (double weight) |
| void | setTrackerWeight (const std::shared_ptr< vpTemporalWeighting > &weight) |
| virtual vpMatrix | getLTL () const |
| virtual vpColVector | getLTR () const |
| const vpColVector & | getWeightedError () const |
| void | setComputeJacobianObjectSpace (bool inObjectSpace) |
| bool | hasIgnoredDofs () const |
| void | setEstimatedDofs (const std::array< bool, 6 > &dofs) |
| void | updateOptimizerTerms (const vpHomogeneousMatrix &cMo) |
Settings | |
| unsigned int | getStep () const |
| void | setStep (unsigned int step) |
| unsigned int | getMaxNumFeatures () const |
| void | setMaxNumFeatures (unsigned int num) |
| bool | shouldUseMask () const |
| void | setShouldUseMask (bool useMask) |
| float | getMinimumMaskConfidence () const |
| void | setMinimumMaskConfidence (float confidence) |
| vpDisplayType | getDisplayType () const |
| void | setDisplayType (vpDisplayType type) |
Core Tracking methods | |
| virtual void | reset () |
Display | |
| bool | featuresShouldBeDisplayed () const |
| void | setFeaturesShouldBeDisplayed (bool enableDisplay) |
Covariance computation | |
| virtual const vpMatrix | getCovariance () const |
| virtual void | updateCovariance (const double lambda) |
Static Public Member Functions | |
| static void | computeJTR (const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) |
| static vpMatrix | computeCovarianceMatrix (const vpMatrix &A, const vpColVector &b, const vpMatrix &W) |
Protected Member Functions | |
| vpMatrix | computeoJo () const |
A tracker based on dense depth point-plane alignment.
Tutorials
If you want to have an in-depth presentation of the Render-Based Tracker (RBT), you may have a look at:
Definition at line 76 of file vpRBDenseDepthTracker.h.
| Enumerator | |
|---|---|
| DT_SIMPLE | |
| DT_WEIGHT | |
| DT_ERROR | |
| DT_WEIGHT_AND_ERROR | |
| DT_INVALID | |
Definition at line 80 of file vpRBDenseDepthTracker.h.
|
inline |
Definition at line 89 of file vpRBDenseDepthTracker.h.
References DT_SIMPLE, m_displayType, m_maxFeatures, m_minMaskConfidence, m_step, m_useMask, and vpRBFeatureTracker::vpRBFeatureTracker().
|
virtualdefault |
|
staticinherited |
Definition at line 78 of file vpRBFeatureTracker.cpp.
References vpColVector::t(), and vpMatrix::t().
Referenced by updateCovariance().
|
staticinherited |
Definition at line 56 of file vpRBFeatureTracker.cpp.
References vpArray2D< Type >::data, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrixException::incorrectMatrixSizeError, and vpColVector::resize().
Referenced by updateOptimizerTerms().
|
inlineprotectedinherited |
Definition at line 284 of file vpRBFeatureTracker.h.
References m_estimatedDofs.
Referenced by setEstimatedDofs().
|
virtual |
Implements vpRBFeatureTracker.
Definition at line 186 of file vpRBDenseDepthTracker.cpp.
References vpRBRenderData::boundingBox, vpRBFeatureTrackerInput::cam, vpRBFeatureTrackerInput::depth, vpRect::getArea(), vpRBFeatureTracker::m_cov, vpRBFeatureTracker::m_covWeightDiag, m_depthPointSet, vpRBFeatureTracker::m_error, vpRBFeatureTracker::m_L, vpRBFeatureTracker::m_LTL, vpRBFeatureTracker::m_LTR, m_maxFeatures, vpRBFeatureTracker::m_numFeatures, m_robust, m_step, vpRBFeatureTracker::m_weighted_error, vpRBFeatureTracker::m_weights, vpRBFeatureTrackerInput::renders, vpRobust::TUKEY, vpRBFeatureTracker::updateOptimizerTerms(), vpRBRenderData::zFar, and vpRBRenderData::zNear.
|
virtual |
Implements vpRBFeatureTracker.
Definition at line 212 of file vpRBDenseDepthTracker.cpp.
References vpDisplay::displayPoint(), DT_ERROR, DT_SIMPLE, DT_WEIGHT, DT_WEIGHT_AND_ERROR, m_depthPoints, m_displayType, vpRBFeatureTracker::m_error, vpRBFeatureTracker::m_weights, and vpException::notImplementedError.
|
virtual |
Extract features from the frame data and the current pose estimate.
Implements vpRBFeatureTracker.
Definition at line 56 of file vpRBDenseDepthTracker.cpp.
References vpRBRenderData::boundingBox, vpRBFeatureTrackerInput::cam, vpRBRenderData::cMo, vpRBFeatureTrackerInput::depth, vpRBRenderData::depth, vpHomogeneousMatrix::getTranslationVector(), vpRBFeatureTrackerInput::hasMask(), vpMath::isFinite(), vpRBFeatureTracker::m_cov, vpRBFeatureTracker::m_covWeightDiag, m_depthPoints, m_depthPointSet, vpRBFeatureTracker::m_error, vpRBFeatureTracker::m_L, m_maxFeatures, m_minMaskConfidence, vpRBFeatureTracker::m_numFeatures, m_step, m_useMask, vpRBFeatureTracker::m_weighted_error, vpRBFeatureTracker::m_weights, vpRBFeatureTrackerInput::mask, vpColVector::normalize(), vpRBRenderData::normals, vpRBDenseDepthTracker::vpDepthPoint::objectNormal, vpRBDenseDepthTracker::vpDepthPoint::observation, vpRBDenseDepthTracker::vpDepthPoint::oX, vpRBDenseDepthTracker::vpDepthPoint::pixelPos, vpMath::rad(), vpRBFeatureTrackerInput::renders, and vpUniRand::sampleWithoutReplacement().
|
inlineinherited |
Definition at line 163 of file vpRBFeatureTracker.h.
References m_enableDisplay.
|
inlinevirtualinherited |
Retrieve the 6 x 6 pose covariance matrix, computed from the weights associated to each feature.
The updateCovariance method should have been called before
Definition at line 186 of file vpRBFeatureTracker.h.
References m_cov.
|
inline |
Definition at line 137 of file vpRBDenseDepthTracker.h.
References m_displayType.
|
inlinevirtualinherited |
Get the left-side term of the Gauss-Newton optimization term.
Definition at line 215 of file vpRBFeatureTracker.h.
References m_LTL.
|
inlinevirtualinherited |
Get the right-side term of the Gauss-Newton optimization term.
Definition at line 220 of file vpRBFeatureTracker.h.
References m_LTR.
|
inline |
Definition at line 110 of file vpRBDenseDepthTracker.h.
References m_maxFeatures.
|
inline |
Returns the minimum mask confidence that a pixel linked to depth point should have if it should be kept during tracking.
This value is between 0 and 1
Definition at line 128 of file vpRBDenseDepthTracker.h.
References m_minMaskConfidence.
|
inlineinherited |
Return the type of feature that is used by this tracker.
Get the number of features used to compute the pose update
Definition at line 88 of file vpRBFeatureTracker.h.
References m_numFeatures.
|
inline |
Definition at line 101 of file vpRBDenseDepthTracker.h.
References m_step.
|
inlineinherited |
Definition at line 208 of file vpRBFeatureTracker.h.
References m_weighting.
|
inlinevirtualinherited |
Get the importance of this tracker in the optimization step. The default computation is the following:
, where
is the weight defined by setTrackerWeight, and
is the number of features.
Reimplemented in vpRBSilhouetteCCDTracker.
Definition at line 207 of file vpRBFeatureTracker.h.
References m_numFeatures, and m_weighting.
|
inlineinherited |
Get a weighted version of the error vector. This should not include the userVVSWeight, but may include reweighting to remove outliers, occlusions, etc.
Definition at line 226 of file vpRBFeatureTracker.h.
References m_weighted_error.
|
inlineinherited |
Definition at line 246 of file vpRBFeatureTracker.h.
References m_estimatedDofs.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix().
|
inlinevirtual |
Implements vpRBFeatureTracker.
Definition at line 160 of file vpRBDenseDepthTracker.h.
|
inlinevirtual |
Reimplemented from vpRBFeatureTracker.
Definition at line 315 of file vpRBDenseDepthTracker.h.
References vpRBFeatureTracker::loadJsonConfiguration(), m_displayType, m_maxFeatures, m_minMaskConfidence, m_step, m_useMask, setDisplayType(), setMaxNumFeatures(), setMinimumMaskConfidence(), setShouldUseMask(), and setStep().
| vpRBDenseDepthTracker::NLOHMANN_JSON_SERIALIZE_ENUM | ( | vpRBDenseDepthTracker::vpDisplayType | , |
| { {vpRBDenseDepthTracker::vpDisplayType::DT_INVALID, nullptr}, {vpRBDenseDepthTracker::vpDisplayType::DT_SIMPLE, "simple"}, {vpRBDenseDepthTracker::vpDisplayType::DT_WEIGHT, "weight"}, {vpRBDenseDepthTracker::vpDisplayType::DT_ERROR, "error"}, {vpRBDenseDepthTracker::vpDisplayType::DT_WEIGHT_AND_ERROR, "weightAndError"} } | ) |
References DT_ERROR, DT_INVALID, DT_SIMPLE, DT_WEIGHT, and DT_WEIGHT_AND_ERROR.
|
inlinevirtual |
Method called after the tracking iteration has finished.
Implements vpRBFeatureTracker.
Definition at line 156 of file vpRBDenseDepthTracker.h.
|
inlinevirtual |
Method called when starting a tracking iteration.
Implements vpRBFeatureTracker.
Definition at line 155 of file vpRBDenseDepthTracker.h.
|
inlinevirtual |
Whether this tracker requires depth image to extract features.
Implements vpRBFeatureTracker.
Definition at line 94 of file vpRBDenseDepthTracker.h.
|
inlinevirtual |
Whether this tracker requires RGB image to extract features.
Implements vpRBFeatureTracker.
Definition at line 93 of file vpRBDenseDepthTracker.h.
|
inlinevirtual |
Whether this tracker requires Silhouette candidates.
Implements vpRBFeatureTracker.
Definition at line 95 of file vpRBDenseDepthTracker.h.
|
inlinevirtualinherited |
Resets feature state. Can be called when the object changes or is lost, Ensuring that prior data does not influence the tracking behaviour.
Reimplemented in vpRBSilhouetteCCDTracker.
Definition at line 152 of file vpRBFeatureTracker.h.
|
inlineinherited |
Definition at line 238 of file vpRBFeatureTracker.h.
References m_jacobianInObjectSpace.
|
inline |
Definition at line 139 of file vpRBDenseDepthTracker.h.
References vpException::badValue, DT_INVALID, and m_displayType.
Referenced by loadJsonConfiguration().
|
inlineinherited |
Definition at line 255 of file vpRBFeatureTracker.h.
References computeoJo(), m_estimatedDofs, and m_oJo.
Referenced by loadJsonConfiguration(), and vpRBFeatureTracker().
|
inlineinherited |
Definition at line 164 of file vpRBFeatureTracker.h.
References m_enableDisplay.
Referenced by visp.python.rbt.xfeat.RBXFeatFeatureTracker.RBXFeatFeatureTracker::load_settings().
|
inline |
Definition at line 111 of file vpRBDenseDepthTracker.h.
References m_maxFeatures.
Referenced by loadJsonConfiguration().
|
inline |
Definition at line 129 of file vpRBDenseDepthTracker.h.
References vpException::badValue, and m_minMaskConfidence.
Referenced by loadJsonConfiguration().
|
inline |
Definition at line 121 of file vpRBDenseDepthTracker.h.
References m_useMask.
Referenced by loadJsonConfiguration().
|
inline |
Definition at line 102 of file vpRBDenseDepthTracker.h.
References vpException::badValue, and m_step.
Referenced by loadJsonConfiguration().
|
inlineinherited |
Definition at line 210 of file vpRBFeatureTracker.h.
References m_weighting.
Referenced by visp.python.rbt.xfeat.RBXFeatFeatureTracker.RBXFeatFeatureTracker::load_settings().
|
inlineinherited |
Definition at line 209 of file vpRBFeatureTracker.h.
References m_weighting.
Referenced by visp.python.rbt.xfeat.RBXFeatFeatureTracker.RBXFeatFeatureTracker::load_settings().
|
inline |
Returns whether the tracking algorithm should filter out points that are unlikely to be on the object according to the mask. If the mask is not computed beforehand, then it has no effect.
Definition at line 120 of file vpRBDenseDepthTracker.h.
References m_useMask.
|
inlinevirtual |
Track the features.
Implements vpRBFeatureTracker.
Definition at line 159 of file vpRBDenseDepthTracker.h.
|
virtualinherited |
Update the covariance matrix.
| lambda | the visual servoing gain |
Reimplemented in vpRBSilhouetteCCDTracker.
Definition at line 49 of file vpRBFeatureTracker.cpp.
References computeCovarianceMatrix(), vpMatrix::diag(), m_cov, m_covWeightDiag, m_error, and m_L.
|
inlineinherited |
Definition at line 262 of file vpRBFeatureTracker.h.
References computeJTR(), m_covWeightDiag, m_error, m_jacobianInObjectSpace, m_L, m_LTL, m_LTR, m_oJo, m_weighted_error, and m_weights.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), and vpRBSilhouetteMeTracker::computeVVSIter().
|
inlineinherited |
Returns whether the tracker is considered as having converged to the desired pose.
Definition at line 200 of file vpRBFeatureTracker.h.
References m_vvsConverged.
|
protectedinherited |
Right side of the Gauss Newton minimization.
Definition at line 301 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::changeScale(), vpRBDenseDepthTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), getCovariance(), vpRBSilhouetteCCDTracker::initVVS(), updateCovariance(), and vpRBSilhouetteCCDTracker::updateCovariance().
|
protectedinherited |
Covariance matrix.
Definition at line 302 of file vpRBFeatureTracker.h.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::initVVS(), updateCovariance(), and updateOptimizerTerms().
|
protected |
Definition at line 334 of file vpRBDenseDepthTracker.h.
Referenced by display(), and extractFeatures().
|
protected |
Definition at line 335 of file vpRBDenseDepthTracker.h.
Referenced by computeVVSIter(), and extractFeatures().
|
protected |
Definition at line 341 of file vpRBDenseDepthTracker.h.
Referenced by display(), getDisplayType(), loadJsonConfiguration(), setDisplayType(), and vpRBDenseDepthTracker().
|
protectedinherited |
Whether VVS has converged, should be updated every VVS iteration.
Definition at line 314 of file vpRBFeatureTracker.h.
Referenced by featuresShouldBeDisplayed(), loadJsonConfiguration(), setFeaturesShouldBeDisplayed(), and vpRBFeatureTracker().
|
protectedinherited |
Definition at line 304 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::display(), vpRBSilhouetteCCDTracker::display(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteCCDTracker::getVVSTrackerWeight(), vpRBSilhouetteMeTracker::initVVS(), vpRBSilhouetteCCDTracker::reset(), updateCovariance(), and updateOptimizerTerms().
|
protectedinherited |
Whether the tracked features should be displayed.
Definition at line 315 of file vpRBFeatureTracker.h.
Referenced by computeoJo(), hasIgnoredDofs(), and setEstimatedDofs().
|
protectedinherited |
Matrix representation of the estimated DOFS.
Definition at line 317 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), setComputeJacobianObjectSpace(), updateOptimizerTerms(), and vpRBFeatureTracker().
|
protectedinherited |
Definition at line 298 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::initVVS(), updateCovariance(), and updateOptimizerTerms().
|
protectedinherited |
Error jacobian (In VS terms, the interaction matrix).
Definition at line 299 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), getLTL(), and updateOptimizerTerms().
|
protectedinherited |
Left side of the Gauss newton minimization.
Definition at line 300 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), getLTR(), and updateOptimizerTerms().
|
protected |
Definition at line 338 of file vpRBDenseDepthTracker.h.
Referenced by computeVVSIter(), extractFeatures(), getMaxNumFeatures(), loadJsonConfiguration(), setMaxNumFeatures(), and vpRBDenseDepthTracker().
|
protected |
Definition at line 340 of file vpRBDenseDepthTracker.h.
Referenced by extractFeatures(), getMinimumMaskConfidence(), loadJsonConfiguration(), setMinimumMaskConfidence(), and vpRBDenseDepthTracker().
|
protectedinherited |
Error weights.
Definition at line 309 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::changeScale(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteCCDTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::extractFeatures(), getNumFeatures(), getVVSTrackerWeight(), vpRBSilhouetteCCDTracker::initVVS(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker().
|
protectedinherited |
Definition at line 316 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), setEstimatedDofs(), and updateOptimizerTerms().
|
protected |
Definition at line 336 of file vpRBDenseDepthTracker.h.
Referenced by computeVVSIter().
|
protected |
Definition at line 337 of file vpRBDenseDepthTracker.h.
Referenced by computeVVSIter(), extractFeatures(), getStep(), loadJsonConfiguration(), setStep(), and vpRBDenseDepthTracker().
|
protected |
Definition at line 339 of file vpRBDenseDepthTracker.h.
Referenced by extractFeatures(), loadJsonConfiguration(), setShouldUseMask(), shouldUseMask(), and vpRBDenseDepthTracker().
|
protectedinherited |
User-defined weight for this specific type of feature.
Definition at line 312 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBSilhouetteCCDTracker::initVVS(), vpRBSilhouetteMeTracker::initVVS(), vpRBSilhouetteCCDTracker::reset(), vpRBFeatureTracker(), and vvsHasConverged().
|
protectedinherited |
Raw VS Error vector.
Definition at line 305 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), getWeightedError(), vpRBSilhouetteMeTracker::initVVS(), and updateOptimizerTerms().
|
protectedinherited |
Number of considered features.
Definition at line 310 of file vpRBFeatureTracker.h.
Referenced by getTemporalTrackerWeight(), getVVSTrackerWeight(), vpRBSilhouetteCCDTracker::getVVSTrackerWeight(), loadJsonConfiguration(), setTrackerWeight(), setTrackerWeight(), and vpRBFeatureTracker().
|
protectedinherited |
Weighted VS error.
Definition at line 306 of file vpRBFeatureTracker.h.
Referenced by vpRBSilhouetteCCDTracker::changeScale(), vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::display(), vpRBSilhouetteCCDTracker::display(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteCCDTracker::initVVS(), vpRBSilhouetteMeTracker::initVVS(), and updateOptimizerTerms().