![]() |
Visual Servoing Platform
version 3.6.1 under development (2025-02-17)
|
#include <visp3/rbt/vpRBTracker.h>
Public Member Functions | |
vpRBTracker () | |
~vpRBTracker ()=default | |
void | reset () |
Information retrieval | |
void | getPose (vpHomogeneousMatrix &cMo) const |
void | setPose (const vpHomogeneousMatrix &cMo) |
vpObjectCentricRenderer & | getRenderer () |
const vpRBFeatureTrackerInput & | getMostRecentFrame () const |
const vpRBTrackerLogger & | getLogger () const |
vpMatrix | getCovariance () const |
Settings | |
void | addTracker (std::shared_ptr< vpRBFeatureTracker > tracker) |
void | setupRenderer (const std::string &file) |
std::string | getModelPath () const |
void | setModelPath (const std::string &path) |
std::vector< std::shared_ptr< vpRBFeatureTracker > > | getFeatureTrackers () |
vpCameraParameters | getCameraParameters () const |
void | setCameraParameters (const vpCameraParameters &cam, unsigned h, unsigned w) |
unsigned int | getImageWidth () const |
unsigned int | getImageHeight () const |
vpSilhouettePointsExtractionSettings | getSilhouetteExtractionParameters () const |
void | setSilhouetteExtractionParameters (const vpSilhouettePointsExtractionSettings &settings) |
double | getOptimizationGain () const |
void | setOptimizationGain (double lambda) |
unsigned int | getMaxOptimizationIters () const |
void | setMaxOptimizationIters (unsigned int iters) |
double | getOptimizationInitialMu () const |
void | setOptimizationInitialMu (double mu) |
double | getOptimizationMuIterFactor () const |
void | setOptimizationMuIterFactor (double factor) |
std::shared_ptr< vpRBDriftDetector > | getDriftDetector () const |
void | setDriftDetector (const std::shared_ptr< vpRBDriftDetector > &detector) |
std::shared_ptr< vpObjectMask > | getObjectSegmentationMethod () const |
void | setObjectSegmentationMethod (const std::shared_ptr< vpObjectMask > &mask) |
std::shared_ptr< vpRBVisualOdometry > | getOdometryMethod () const |
void | setOdometryMethod (const std::shared_ptr< vpRBVisualOdometry > &odometry) |
bool | getVerbose () |
void | setVerbose (bool verbose) |
void | loadConfigurationFile (const std::string &filename) |
void | loadConfiguration (const nlohmann::json &j) |
Tracking | |
void | startTracking () |
void | track (const vpImage< unsigned char > &I) |
void | track (const vpImage< unsigned char > &I, const vpImage< vpRGBa > &IRGB) |
void | track (const vpImage< unsigned char > &I, const vpImage< vpRGBa > &IRGB, const vpImage< float > &depth) |
Display | |
void | displayMask (vpImage< unsigned char > &Imask) const |
void | display (const vpImage< unsigned char > &I, const vpImage< vpRGBa > &IRGB, const vpImage< unsigned char > &depth) |
Protected Member Functions | |
void | track (vpRBFeatureTrackerInput &input) |
void | updateRender (vpRBFeatureTrackerInput &frame) |
std::vector< vpRBSilhouettePoint > | extractSilhouettePoints (const vpImage< vpRGBf > &Inorm, const vpImage< float > &Idepth, const vpImage< vpRGBf > &Ior, const vpImage< unsigned char > &Ivalid, const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp) |
template<typename T > | |
void | checkDimensionsOrThrow (const vpImage< T > &I, const std::string &imgType) const |
Protected Attributes | |
bool | m_firstIteration |
std::vector< std::shared_ptr< vpRBFeatureTracker > > | m_trackers |
vpRBFeatureTrackerInput | m_currentFrame |
vpRBFeatureTrackerInput | m_previousFrame |
std::string | m_modelPath |
vpHomogeneousMatrix | m_cMo |
vpHomogeneousMatrix | m_cMoPrev |
vpCameraParameters | m_cam |
double | m_lambda |
unsigned | m_vvsIterations |
double | m_muInit |
double | m_muIterFactor |
vpSilhouettePointsExtractionSettings | m_depthSilhouetteSettings |
vpPanda3DRenderParameters | m_rendererSettings |
vpObjectCentricRenderer | m_renderer |
unsigned | m_imageHeight |
unsigned | m_imageWidth |
vpRBTrackerLogger | m_logger |
bool | m_verbose |
std::shared_ptr< vpObjectMask > | m_mask |
std::shared_ptr< vpRBDriftDetector > | m_driftDetector |
std::shared_ptr< vpRBVisualOdometry > | m_odometry |
Definition at line 67 of file vpRBTracker.h.
BEGIN_VISP_NAMESPACE vpRBTracker::vpRBTracker | ( | ) |
Definition at line 52 of file vpRBTracker.cpp.
References m_driftDetector, m_mask, m_odometry, m_renderer, m_rendererSettings, vpPanda3DRenderParameters::setClippingDistance(), and vpObjectCentricRenderer::setRenderParameters().
|
default |
void vpRBTracker::addTracker | ( | std::shared_ptr< vpRBFeatureTracker > | tracker | ) |
Definition at line 543 of file vpRBTracker.cpp.
References vpException::badValue, and m_trackers.
|
inlineprotected |
Definition at line 234 of file vpRBTracker.h.
References vpException::dimensionError, vpImage< Type >::getCols(), and vpImage< Type >::getRows().
Referenced by track().
void vpRBTracker::display | ( | const vpImage< unsigned char > & | I, |
const vpImage< vpRGBa > & | IRGB, | ||
const vpImage< unsigned char > & | depth | ||
) |
Definition at line 558 of file vpRBTracker.cpp.
References vpRBFeatureTrackerInput::cam, vpImage< Type >::getSize(), m_currentFrame, m_driftDetector, m_trackers, vpRBRenderData::normals, and vpRBFeatureTrackerInput::renders.
void vpRBTracker::displayMask | ( | vpImage< unsigned char > & | Imask | ) | const |
Definition at line 551 of file vpRBTracker.cpp.
References m_currentFrame, m_mask, and vpRBFeatureTrackerInput::mask.
|
protected |
Definition at line 480 of file vpRBTracker.cpp.
References vpException::badValue, vpSilhouettePointsExtractionSettings::getSilhouetteCandidates(), m_depthSilhouetteSettings, m_previousFrame, and vpRBFeatureTrackerInput::silhouettePoints.
Referenced by track().
vpCameraParameters vpRBTracker::getCameraParameters | ( | ) | const |
Definition at line 99 of file vpRBTracker.cpp.
References m_cam.
vpMatrix vpRBTracker::getCovariance | ( | ) | const |
Definition at line 76 of file vpRBTracker.cpp.
References vpException::dimensionError, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), m_lambda, m_trackers, and vpMatrix::pseudoInverse().
|
inline |
Definition at line 150 of file vpRBTracker.h.
|
inline |
Definition at line 100 of file vpRBTracker.h.
|
inline |
Definition at line 106 of file vpRBTracker.h.
|
inline |
Definition at line 105 of file vpRBTracker.h.
|
inline |
Definition at line 83 of file vpRBTracker.h.
|
inline |
Definition at line 123 of file vpRBTracker.h.
|
inline |
Definition at line 97 of file vpRBTracker.h.
|
inline |
Definition at line 82 of file vpRBTracker.h.
|
inline |
Definition at line 156 of file vpRBTracker.h.
|
inline |
Definition at line 162 of file vpRBTracker.h.
|
inline |
Definition at line 115 of file vpRBTracker.h.
|
inline |
Definition at line 132 of file vpRBTracker.h.
|
inline |
Definition at line 141 of file vpRBTracker.h.
void vpRBTracker::getPose | ( | vpHomogeneousMatrix & | cMo | ) | const |
Definition at line 64 of file vpRBTracker.cpp.
References m_cMo.
vpObjectCentricRenderer & vpRBTracker::getRenderer | ( | ) |
Definition at line 576 of file vpRBTracker.cpp.
References m_renderer.
|
inline |
Definition at line 108 of file vpRBTracker.h.
|
inline |
Get verbosity mode.
Definition at line 172 of file vpRBTracker.h.
void vpRBTracker::loadConfiguration | ( | const nlohmann::json & | j | ) |
Definition at line 603 of file vpRBTracker.cpp.
References vpException::badValue, vpDynamicFactory< T >::buildFromJson(), vpObjectMaskFactory::getFactory(), vpRBDriftDetectorFactory::getFactory(), vpRBFeatureTrackerFactory::getFactory(), m_cam, m_depthSilhouetteSettings, m_driftDetector, m_firstIteration, m_imageHeight, m_imageWidth, m_lambda, m_mask, m_muInit, m_muIterFactor, m_trackers, m_verbose, m_vvsIterations, setCameraParameters(), setMaxOptimizationIters(), setModelPath(), setOptimizationGain(), setOptimizationInitialMu(), and setOptimizationMuIterFactor().
Referenced by loadConfigurationFile().
void vpRBTracker::loadConfigurationFile | ( | const std::string & | filename | ) |
Definition at line 582 of file vpRBTracker.cpp.
References vpException::ioError, and loadConfiguration().
void vpRBTracker::reset | ( | ) |
Definition at line 126 of file vpRBTracker.cpp.
References m_firstIteration.
void vpRBTracker::setCameraParameters | ( | const vpCameraParameters & | cam, |
unsigned | h, | ||
unsigned | w | ||
) |
Definition at line 101 of file vpRBTracker.cpp.
References vpException::badValue, vpCameraParameters::get_projModel(), m_cam, m_imageHeight, m_imageWidth, m_renderer, m_rendererSettings, vpPanda3DRenderParameters::setCameraIntrinsics(), vpPanda3DRenderParameters::setImageResolution(), and vpObjectCentricRenderer::setRenderParameters().
Referenced by loadConfiguration().
|
inline |
Definition at line 151 of file vpRBTracker.h.
|
inline |
Definition at line 124 of file vpRBTracker.h.
References vpException::badValue.
Referenced by loadConfiguration().
void vpRBTracker::setModelPath | ( | const std::string & | path | ) |
Definition at line 131 of file vpRBTracker.cpp.
References m_modelPath.
Referenced by loadConfiguration().
|
inline |
Definition at line 157 of file vpRBTracker.h.
|
inline |
Definition at line 163 of file vpRBTracker.h.
|
inline |
Definition at line 116 of file vpRBTracker.h.
References vpException::badValue.
Referenced by loadConfiguration().
|
inline |
Definition at line 133 of file vpRBTracker.h.
References vpException::badValue.
Referenced by loadConfiguration().
|
inline |
Definition at line 142 of file vpRBTracker.h.
References vpException::badValue.
Referenced by loadConfiguration().
void vpRBTracker::setPose | ( | const vpHomogeneousMatrix & | cMo | ) |
Definition at line 69 of file vpRBTracker.cpp.
References vpHomogeneousMatrix::inverse(), m_cMo, m_cMoPrev, m_renderer, and vpPanda3DRendererSet::setCameraPose().
void vpRBTracker::setSilhouetteExtractionParameters | ( | const vpSilhouettePointsExtractionSettings & | settings | ) |
Definition at line 121 of file vpRBTracker.cpp.
References m_depthSilhouetteSettings.
void vpRBTracker::setupRenderer | ( | const std::string & | file | ) |
Definition at line 136 of file vpRBTracker.cpp.
References vpPanda3DRendererSet::addLight(), vpPanda3DRendererSet::addNodeToScene(), vpPanda3DRendererSet::addSubRenderer(), vpException::badValue, vpIoTools::checkFilename(), vpPanda3DRendererSet::initFramework(), vpPanda3DBaseRenderer::loadObject(), m_renderer, m_trackers, and vpObjectCentricRenderer::setFocusedObject().
Referenced by startTracking().
|
inline |
Enable/disable verbosity mode.
verbose | : When true verbose mode is enabled. When false verbosity is disabled. |
Definition at line 181 of file vpRBTracker.h.
void vpRBTracker::startTracking | ( | ) |
Definition at line 193 of file vpRBTracker.cpp.
References m_modelPath, and setupRenderer().
void vpRBTracker::track | ( | const vpImage< unsigned char > & | I | ) |
Definition at line 163 of file vpRBTracker.cpp.
References vpException::badValue, vpRBFeatureTrackerInput::cam, checkDimensionsOrThrow(), vpRBFeatureTrackerInput::I, m_cam, and m_trackers.
Referenced by track().
Definition at line 177 of file vpRBTracker.cpp.
References vpException::badValue, vpRBFeatureTrackerInput::cam, checkDimensionsOrThrow(), vpRBFeatureTrackerInput::I, vpRBFeatureTrackerInput::IRGB, m_cam, m_trackers, and track().
void vpRBTracker::track | ( | const vpImage< unsigned char > & | I, |
const vpImage< vpRGBa > & | IRGB, | ||
const vpImage< float > & | depth | ||
) |
Definition at line 198 of file vpRBTracker.cpp.
References vpRBFeatureTrackerInput::cam, checkDimensionsOrThrow(), vpRBFeatureTrackerInput::depth, vpRBFeatureTrackerInput::I, vpRBFeatureTrackerInput::IRGB, m_cam, and track().
|
protected |
Check if all trackers have converged
Definition at line 211 of file vpRBTracker.cpp.
References vpRBTrackerLogger::addTrackerVVSTime(), vpException::badValue, vpRBFeatureTrackerInput::cam, vpRBRenderData::depth, vpExponentialMap::direct(), vpRBTrackerLogger::endTimer(), extractSilhouettePoints(), vpMatrix::eye(), vpArray2D< Type >::getRows(), vpImage< Type >::getSize(), vpRBFeatureTrackerInput::I, vpHomogeneousMatrix::inverse(), vpRBFeatureTrackerInput::IRGB, vpRBRenderData::isSilhouette, m_cMo, m_cMoPrev, m_currentFrame, m_driftDetector, m_firstIteration, m_lambda, m_logger, m_mask, m_muInit, m_muIterFactor, m_odometry, m_previousFrame, m_trackers, m_verbose, m_vvsIterations, vpRBFeatureTrackerInput::mask, vpRBRenderData::normals, vpRBFeatureTrackerInput::renders, vpRBTrackerLogger::reset(), vpRBTrackerLogger::setDriftDetectionTime(), vpRBTrackerLogger::setInitVVSTime(), vpRBTrackerLogger::setMaskTime(), vpRBTrackerLogger::setOdometryTime(), vpRBTrackerLogger::setRenderTime(), vpRBTrackerLogger::setTrackerFeatureExtractionTime(), vpRBTrackerLogger::setTrackerFeatureTrackingTime(), vpRBTrackerLogger::setTrackerIterStartTime(), vpRBRenderData::silhouetteCanny, vpRBFeatureTrackerInput::silhouettePoints, vpRBTrackerLogger::startTimer(), and updateRender().
|
protected |
Definition at line 408 of file vpRBTracker.cpp.
References vpRBRenderData::boundingBox, vpRBRenderData::cMo, vpObjectCentricRenderer::computeClipping(), vpRBRenderData::depth, vpObjectCentricRenderer::getBoundingBox(), vpPanda3DRendererSet::getRenderer(), vpSilhouettePointsExtractionSettings::getThreshold(), vpHomogeneousMatrix::inverse(), vpRBRenderData::isSilhouette, m_cMo, m_depthSilhouetteSettings, m_imageHeight, m_imageWidth, m_renderer, m_rendererSettings, vpRBRenderData::normals, vpPanda3DBaseRenderer::renderFrame(), vpRBFeatureTrackerInput::renders, vpImage< Type >::resize(), vpPanda3DRendererSet::setCameraPose(), vpPanda3DRenderParameters::setClippingDistance(), vpObjectCentricRenderer::setRenderParameters(), vpRBRenderData::silhouetteCanny, vpSilhouettePointsExtractionSettings::thresholdIsRelative(), vpRBRenderData::zFar, and vpRBRenderData::zNear.
Referenced by track().
|
protected |
Definition at line 255 of file vpRBTracker.h.
Referenced by getCameraParameters(), loadConfiguration(), setCameraParameters(), and track().
|
protected |
Definition at line 253 of file vpRBTracker.h.
Referenced by getPose(), setPose(), track(), and updateRender().
|
protected |
Definition at line 254 of file vpRBTracker.h.
|
protected |
List of trackers.
Definition at line 249 of file vpRBTracker.h.
Referenced by display(), displayMask(), and track().
|
protected |
Factor with which to multiply mu at every iteration during VVS.
Definition at line 262 of file vpRBTracker.h.
Referenced by extractSilhouettePoints(), loadConfiguration(), setSilhouetteExtractionParameters(), and updateRender().
|
protected |
Definition at line 272 of file vpRBTracker.h.
Referenced by display(), loadConfiguration(), track(), and vpRBTracker().
|
protected |
Definition at line 245 of file vpRBTracker.h.
Referenced by loadConfiguration(), reset(), and track().
|
protected |
Definition at line 266 of file vpRBTracker.h.
Referenced by loadConfiguration(), setCameraParameters(), and updateRender().
|
protected |
Definition at line 266 of file vpRBTracker.h.
Referenced by loadConfiguration(), setCameraParameters(), and updateRender().
|
protected |
Definition at line 257 of file vpRBTracker.h.
Referenced by getCovariance(), loadConfiguration(), and track().
|
protected |
Color and render image dimensions.
Definition at line 268 of file vpRBTracker.h.
Referenced by track().
|
protected |
Definition at line 271 of file vpRBTracker.h.
Referenced by displayMask(), loadConfiguration(), track(), and vpRBTracker().
|
protected |
Definition at line 252 of file vpRBTracker.h.
Referenced by setModelPath(), and startTracking().
|
protected |
Max number of VVS iterations.
Definition at line 259 of file vpRBTracker.h.
Referenced by loadConfiguration(), and track().
|
protected |
Initial mu value for Levenberg-Marquardt.
Definition at line 260 of file vpRBTracker.h.
Referenced by loadConfiguration(), and track().
|
protected |
Definition at line 273 of file vpRBTracker.h.
Referenced by track(), and vpRBTracker().
|
protected |
Definition at line 250 of file vpRBTracker.h.
Referenced by extractSilhouettePoints(), and track().
|
protected |
Definition at line 264 of file vpRBTracker.h.
Referenced by getRenderer(), setCameraParameters(), setPose(), setupRenderer(), updateRender(), and vpRBTracker().
|
protected |
Definition at line 263 of file vpRBTracker.h.
Referenced by setCameraParameters(), updateRender(), and vpRBTracker().
|
protected |
Whether this is the first iteration.
Definition at line 247 of file vpRBTracker.h.
Referenced by addTracker(), display(), getCovariance(), loadConfiguration(), setupRenderer(), and track().
|
protected |
Definition at line 269 of file vpRBTracker.h.
Referenced by loadConfiguration(), and track().
|
protected |
VVS gain.
Definition at line 258 of file vpRBTracker.h.
Referenced by loadConfiguration(), and track().