![]() |
Visual Servoing Platform
version 3.6.1 under development (2025-02-17)
|
#include <visp3/rbt/vpRBSilhouetteCCDTracker.h>
Public Member Functions | |
vpRBSilhouetteCCDTracker () | |
virtual | ~vpRBSilhouetteCCDTracker ()=default |
bool | requiresRGB () const VP_OVERRIDE |
bool | requiresDepth () const VP_OVERRIDE |
bool | requiresSilhouetteCandidates () const VP_OVERRIDE |
void | onTrackingIterStart () VP_OVERRIDE |
void | onTrackingIterEnd () VP_OVERRIDE |
double | getVVSTrackerWeight () const VP_OVERRIDE |
void | extractFeatures (const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE |
void | trackFeatures (const vpRBFeatureTrackerInput &, const vpRBFeatureTrackerInput &, const vpHomogeneousMatrix &) VP_OVERRIDE |
void | initVVS (const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE |
void | computeVVSIter (const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE |
void | updateCovariance (const double) VP_OVERRIDE |
void | display (const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpImage< vpRGBa > &IRGB, const vpImage< unsigned char > &depth) const VP_OVERRIDE |
virtual void | loadJsonConfiguration (const nlohmann::json &j) VP_OVERRIDE |
unsigned | getNumFeatures () const |
bool | vvsHasConverged () const |
void | setTrackerWeight (double weight) |
virtual vpMatrix | getLTL () const |
virtual vpColVector | getLTR () const |
const vpColVector & | getWeightedError () const |
Settings | |
void | setCCDParameters (const vpCCDParameters ¶meters) |
vpCCDParameters | getCCDParameters () const |
double | getTemporalSmoothingFactor () const |
void | setTemporalSmoothingFactor (double factor) |
bool | shouldUseMask () const |
void | setShouldUseMask (bool useMask) |
float | getMinimumMaskConfidence () const |
void | setMinimumMaskConfidence (float confidence) |
void | setDisplayType (vpRBSilhouetteCCDDisplayType type) |
Display | |
bool | featuresShouldBeDisplayed () const |
void | setFeaturesShouldBeDisplayed (bool enableDisplay) |
Covariance computation | |
virtual const vpMatrix | getCovariance () const |
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 | |
void | updateCCDPoints (const vpHomogeneousMatrix &cMo) |
void | computeLocalStatistics (const vpImage< vpRGBa > &I, vpCCDStatistics &stats) |
void | computeErrorAndInteractionMatrix () |
double | computeMaskGradient (const vpImage< float > &mask, const vpRBSilhouetteControlPoint &pccd) const |
Protected Attributes | |
vpCCDParameters | m_ccdParameters |
std::vector< vpRBSilhouetteControlPoint > | m_controlPoints |
vpRobust | m_robust |
vpCCDStatistics | m_stats |
vpCCDStatistics | m_prevStats |
vpMatrix | m_sigma |
double | m_vvsConvergenceThreshold |
double | tol |
std::vector< vpColVector > | m_gradients |
std::vector< vpMatrix > | m_hessians |
vpColVector | m_gradient |
vpMatrix | m_hessian |
double | m_temporalSmoothingFac |
bool | m_useMask |
double | m_minMaskConfidence |
vpRBSilhouetteCCDDisplayType | m_displayType |
vpMatrix | m_L |
vpMatrix | m_LTL |
vpColVector | m_LTR |
vpMatrix | m_cov |
vpColVector | m_covWeightDiag |
vpColVector | m_error |
vpColVector | m_weighted_error |
vpColVector | m_weights |
unsigned | m_numFeatures |
double | m_userVvsWeight |
bool | m_vvsConverged |
bool | m_enableDisplay |
Tracking based on the Contracting Curve Density algorithm.
Definition at line 191 of file vpRBSilhouetteCCDTracker.h.
vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker | ( | ) |
Definition at line 144 of file vpRBSilhouetteCCDTracker.cpp.
|
virtualdefault |
|
staticinherited |
Definition at line 76 of file vpRBFeatureTracker.cpp.
References vpArray2D< Type >::getRows(), vpColVector::t(), and vpMatrix::t().
Referenced by vpRBFeatureTracker::updateCovariance().
|
protected |
Definition at line 585 of file vpRBSilhouetteCCDTracker.cpp.
References vpCCDStatistics::cov_vic, vpCCDParameters::covarianceIterDecreaseFactor, vpArray2D< Type >::data, FastMat63< T >::data, vpCCDParameters::delta_h, vpCameraParameters::get_px(), vpRBSilhouetteControlPoint::getCameraParameters(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpCCDParameters::h, vpCCDStatistics::imgPoints, FastMat33< T >::inverse(), vpMatrix::inverseByCholesky(), vpRBSilhouetteControlPoint::isValid(), m_ccdParameters, m_controlPoints, vpRBFeatureTracker::m_error, m_gradient, m_gradients, m_hessian, m_hessians, vpRBFeatureTracker::m_L, vpRBFeatureTracker::m_LTL, vpRBFeatureTracker::m_LTR, m_prevStats, m_robust, m_sigma, m_stats, m_temporalSmoothingFac, vpRBFeatureTracker::m_weighted_error, vpRBFeatureTracker::m_weights, vpCCDStatistics::mean_vic, vpRobust::MEstimator(), FastVec3< T >::multiply(), FastMat63< T >::multiply(), FastMat63< T >::multiplyBTranspose(), vpCCDStatistics::nv, vpColVector::resize(), vpArray2D< Type >::resize(), vpCCDStatistics::vic, vpRBSilhouetteControlPoint::xs, vpRBSilhouetteControlPoint::ys, and vpRBSilhouetteControlPoint::Zs.
Referenced by computeVVSIter().
|
staticinherited |
Definition at line 54 of file vpRBFeatureTracker.cpp.
References vpArray2D< Type >::data, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrixException::incorrectMatrixSizeError, and vpColVector::resize().
Referenced by vpRBDenseDepthTracker::computeVVSIter(), and vpRBSilhouetteMeTracker::computeVVSIter().
|
protected |
Definition at line 345 of file vpRBSilhouetteCCDTracker.cpp.
References vpCCDParameters::alpha, vpRGBa::B, vpCCDStatistics::cov_vic, vpCCDParameters::delta_h, vpException::fatalError, vpRGBa::G, vpCCDParameters::gamma_1, vpCCDParameters::gamma_2, vpCCDParameters::gamma_3, vpCCDParameters::gamma_4, vpImagePoint::get_i(), vpImagePoint::get_j(), vpImagePoint::get_u(), vpImagePoint::get_v(), vpImage< Type >::getHeight(), vpRBSilhouetteControlPoint::getTheta(), vpImage< Type >::getWidth(), vpCCDParameters::h, vpRBSilhouetteControlPoint::icpoint, vpCCDStatistics::imgPoints, vpRBSilhouetteControlPoint::isValid(), vpCCDParameters::kappa, m_ccdParameters, m_controlPoints, vpCCDStatistics::mean_vic, vpCCDStatistics::nv, vpRBSilhouetteControlPoint::nxs, vpRBSilhouetteControlPoint::nys, vpRGBa::R, vpRBSilhouetteControlPoint::setValid(), vpCCDStatistics::vic, and vpRBSilhouetteControlPoint::xs.
Referenced by computeVVSIter(), and initVVS().
|
protected |
Definition at line 180 of file vpRBSilhouetteCCDTracker.cpp.
References vpImagePoint::get_i(), vpImagePoint::get_j(), vpRBSilhouetteControlPoint::getTheta(), vpCCDParameters::h, vpRBSilhouetteControlPoint::icpoint, and m_ccdParameters.
Referenced by extractFeatures().
|
virtual |
Implements vpRBFeatureTracker.
Definition at line 236 of file vpRBSilhouetteCCDTracker.cpp.
References computeErrorAndInteractionMatrix(), computeLocalStatistics(), vpRBFeatureTrackerInput::IRGB, m_controlPoints, m_stats, vpRBFeatureTracker::m_vvsConverged, m_vvsConvergenceThreshold, tol, and updateCCDPoints().
|
virtual |
Implements vpRBFeatureTracker.
Definition at line 260 of file vpRBSilhouetteCCDTracker.cpp.
References vpRGBa::B, vpException::badValue, vpCCDParameters::delta_h, vpDisplay::displayCross(), vpRGBa::G, vpImagePoint::get_i(), vpImagePoint::get_j(), vpColor::green, vpCCDParameters::h, vpRBSilhouetteControlPoint::icpoint, m_ccdParameters, m_controlPoints, m_displayType, vpRBFeatureTracker::m_error, m_stats, vpRBFeatureTracker::m_weights, vpCCDStatistics::nv, vpRGBa::R, and vpColor::red.
|
virtual |
Extract features from the frame data and the current pose estimate.
Implements vpRBFeatureTracker.
Definition at line 147 of file vpRBSilhouetteCCDTracker.cpp.
References vpRBSilhouetteControlPoint::buildSilhouettePoint(), vpRBFeatureTrackerInput::cam, vpRBRenderData::cMo, computeMaskGradient(), vpRBRenderData::depth, vpRBSilhouetteControlPoint::detectSilhouette(), vpImage< Type >::getHeight(), vpImage< Type >::getWidth(), vpCCDParameters::h, vpRBFeatureTrackerInput::hasMask(), vpRBFeatureTrackerInput::I, vpRBSilhouettePoint::i, vpHomogeneousMatrix::inverse(), vpRBSilhouetteControlPoint::isSilhouette(), vpRBSilhouetteControlPoint::isValid(), vpRBSilhouettePoint::j, m_ccdParameters, m_controlPoints, m_minMaskConfidence, m_useMask, vpRBFeatureTrackerInput::mask, vpRBSilhouettePoint::normal, vpRBSilhouettePoint::orientation, vpRBFeatureTrackerInput::renders, vpRBFeatureTrackerInput::silhouettePoints, and vpRBSilhouettePoint::Z.
|
inlineinherited |
Definition at line 145 of file vpRBFeatureTracker.h.
|
inline |
Definition at line 207 of file vpRBSilhouetteCCDTracker.h.
|
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 168 of file vpRBFeatureTracker.h.
|
inlinevirtualinherited |
Get the left-side term of the Gauss-Newton optimization term.
Definition at line 195 of file vpRBFeatureTracker.h.
|
inlinevirtualinherited |
Get the right-side term of the Gauss-Newton optimization term.
Definition at line 200 of file vpRBFeatureTracker.h.
|
inline |
Returns the minimum mask gradient required for a silhouette point to be considered.
This value is between 0 and 1
Definition at line 244 of file vpRBSilhouetteCCDTracker.h.
|
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 76 of file vpRBFeatureTracker.h.
|
inline |
Returns the amount of temporal smoothing applied when computing the tracking error and its jacobian. This factor is used to interpolate with the error computed on the previous frame for the features selected at the current iteration Temporal smoothing may help smooth out the motion and reduce jitter.
Definition at line 215 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
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 from vpRBFeatureTracker.
Definition at line 263 of file vpRBSilhouetteCCDTracker.h.
|
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 206 of file vpRBFeatureTracker.h.
|
virtual |
Implements vpRBFeatureTracker.
Definition at line 205 of file vpRBSilhouetteCCDTracker.cpp.
References computeLocalStatistics(), vpCCDParameters::delta_h, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpCCDParameters::h, vpRBFeatureTrackerInput::IRGB, m_ccdParameters, m_controlPoints, vpRBFeatureTracker::m_cov, m_gradient, m_gradients, m_hessian, m_hessians, vpRBFeatureTracker::m_numFeatures, m_prevStats, m_sigma, m_stats, vpRBFeatureTracker::m_vvsConverged, vpRBFeatureTracker::m_weights, vpCCDParameters::phi_dim, vpCCDStatistics::reinit(), vpColVector::resize(), vpArray2D< Type >::resize(), and tol.
|
inlinevirtual |
Reimplemented from vpRBFeatureTracker.
Definition at line 278 of file vpRBSilhouetteCCDTracker.h.
References vpRBFeatureTracker::loadJsonConfiguration().
|
inlinevirtual |
Method called after the tracking iteration has finished.
Implements vpRBFeatureTracker.
Definition at line 261 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
Method called when starting a tracking iteration.
Implements vpRBFeatureTracker.
Definition at line 260 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
Whether this tracker requires depth image to extract features.
Implements vpRBFeatureTracker.
Definition at line 199 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
Whether this tracker requires RGB image to extract features.
Implements vpRBFeatureTracker.
Definition at line 198 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
Whether this tracker requires Silhouette candidates.
Implements vpRBFeatureTracker.
Definition at line 200 of file vpRBSilhouetteCCDTracker.h.
|
inline |
Definition at line 206 of file vpRBSilhouetteCCDTracker.h.
|
inline |
Definition at line 251 of file vpRBSilhouetteCCDTracker.h.
|
inlineinherited |
Definition at line 146 of file vpRBFeatureTracker.h.
|
inline |
Definition at line 245 of file vpRBSilhouetteCCDTracker.h.
|
inline |
Definition at line 237 of file vpRBSilhouetteCCDTracker.h.
|
inline |
Sets the temporal smoothing factor.
factor | the new temporal smoothing factor. Should be greater than 0 |
Definition at line 223 of file vpRBSilhouetteCCDTracker.h.
References vpException::badValue.
|
inlineinherited |
Definition at line 190 of file vpRBFeatureTracker.h.
|
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 236 of file vpRBSilhouetteCCDTracker.h.
|
inlinevirtual |
Track the features.
Implements vpRBFeatureTracker.
Definition at line 266 of file vpRBSilhouetteCCDTracker.h.
|
protected |
Definition at line 335 of file vpRBSilhouetteCCDTracker.cpp.
References m_controlPoints.
Referenced by computeVVSIter().
|
inlinevirtual |
Update the covariance matrix.
lambda | the visual servoing gain |
Reimplemented from vpRBFeatureTracker.
Definition at line 270 of file vpRBSilhouetteCCDTracker.h.
|
inlineinherited |
Returns whether the tracker is considered as having converged to the desired pose.
Definition at line 182 of file vpRBFeatureTracker.h.
|
protected |
Definition at line 299 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), computeLocalStatistics(), computeMaskGradient(), display(), extractFeatures(), and initVVS().
|
protected |
Definition at line 301 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), computeLocalStatistics(), computeVVSIter(), display(), extractFeatures(), initVVS(), and updateCCDPoints().
|
protectedinherited |
Right side of the Gauss Newton minimization.
Definition at line 225 of file vpRBFeatureTracker.h.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), initVVS(), and vpRBFeatureTracker::updateCovariance().
|
protectedinherited |
Covariance matrix.
Definition at line 226 of file vpRBFeatureTracker.h.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker::updateCovariance().
|
protected |
Definition at line 321 of file vpRBSilhouetteCCDTracker.h.
Referenced by display().
|
protectedinherited |
Whether VVS has converged, should be updated every VVS iteration.
Definition at line 238 of file vpRBFeatureTracker.h.
Referenced by vpRBFeatureTracker::vpRBFeatureTracker().
|
protectedinherited |
Definition at line 228 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), display(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker::updateCovariance().
|
protected |
Definition at line 314 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protected |
Definition at line 312 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protected |
Sum of local gradients.
Definition at line 315 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protected |
Definition at line 313 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protectedinherited |
Definition at line 222 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker::updateCovariance().
|
protectedinherited |
Error jacobian (In VS terms, the interaction matrix)
Definition at line 223 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), and vpRBSilhouetteMeTracker::computeVVSIter().
|
protectedinherited |
Left side of the Gauss newton minimization.
Definition at line 224 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), and vpRBSilhouetteMeTracker::computeVVSIter().
|
protected |
Definition at line 319 of file vpRBSilhouetteCCDTracker.h.
Referenced by extractFeatures().
|
protectedinherited |
Error weights.
Definition at line 233 of file vpRBFeatureTracker.h.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), vpRBSilhouetteMeTracker::extractFeatures(), initVVS(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker::vpRBFeatureTracker().
|
protected |
Definition at line 305 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protected |
Silhouette points where to compute CCD statistics.
Definition at line 302 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix().
|
protected |
Definition at line 307 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), and initVVS().
|
protected |
Definition at line 304 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix(), computeVVSIter(), display(), and initVVS().
|
protected |
Sum of local hessians.
Definition at line 316 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeErrorAndInteractionMatrix().
|
protected |
Smoothing factor used to integrate data from the previous frame.
Definition at line 318 of file vpRBSilhouetteCCDTracker.h.
Referenced by extractFeatures().
|
protectedinherited |
Number of considered features.
Definition at line 234 of file vpRBFeatureTracker.h.
Referenced by vpRBFeatureTracker::vpRBFeatureTracker().
|
protectedinherited |
User-defined weight for this specific type of feature.
Definition at line 236 of file vpRBFeatureTracker.h.
Referenced by vpRBDenseDepthTracker::computeVVSIter(), computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), initVVS(), vpRBSilhouetteMeTracker::initVVS(), and vpRBFeatureTracker::vpRBFeatureTracker().
|
protected |
Definition at line 309 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeVVSIter().
|
protectedinherited |
Raw VS Error vector.
Definition at line 229 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::extractFeatures(), and vpRBSilhouetteMeTracker::initVVS().
|
protectedinherited |
Weighted VS error.
Definition at line 230 of file vpRBFeatureTracker.h.
Referenced by computeErrorAndInteractionMatrix(), vpRBDenseDepthTracker::computeVVSIter(), vpRBSilhouetteMeTracker::computeVVSIter(), vpRBDenseDepthTracker::display(), display(), vpRBDenseDepthTracker::extractFeatures(), initVVS(), and vpRBSilhouetteMeTracker::initVVS().
|
protected |
Definition at line 310 of file vpRBSilhouetteCCDTracker.h.
Referenced by computeVVSIter(), and initVVS().