diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h index 4cb09d158c..9914ceedc9 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h @@ -77,8 +77,46 @@ class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker bool requiresSilhouetteCandidates() const VP_OVERRIDE { return false; } /** - * @brief Method called when starting a tracking iteration + * \name Settings + * @{ + */ + unsigned int getStep() const { return m_step; } + void setStep(unsigned int step) + { + if (step == 0) { + throw vpException(vpException::badValue, "Step should be greater than 0"); + } + m_step = step; + } + + /** + * \brief 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 + */ + bool shouldUseMask() const { return m_useMask; } + void setShouldUseMask(bool useMask) { m_useMask = useMask; } + + /** + * \brief 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 + */ + float getMinimumMaskConfidence() const { return m_minMaskConfidence; } + void setMinimumMaskConfidence(float confidence) + { + if (confidence > 1.f || confidence < 0.f) { + throw vpException(vpException::badValue, "Mask confidence should be between 0 and 1"); + } + m_minMaskConfidence = confidence; + } + + /** + * \name Settings + * @} + */ + + /** + * @brief Method called when starting a tracking iteration */ void onTrackingIterStart() VP_OVERRIDE { } void onTrackingIterEnd() VP_OVERRIDE { } diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index d072b7378d..16399a3802 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -188,14 +188,16 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker vpRBSilhouetteCCDTracker(); - bool requiresRGB() const VP_OVERRIDE { return true; } - bool requiresDepth() const VP_OVERRIDE { return false; } - bool requiresSilhouetteCandidates() const VP_OVERRIDE { return true; } + /** + * \name Settings + * @{ + */ void setCCDParameters(const vpCCDParameters ¶meters) { m_ccdParameters = parameters; } + vpCCDParameters getCCDParameters() const { return m_ccdParameters; } //void computeMask(const vpImage &render, vpCCDStatistics &stats); /** @@ -212,11 +214,11 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker * @param factor the new temporal smoothing factor. Should be greater than 0 */ void setTemporalSmoothingFactor(double factor) { m_temporalSmoothingFac = factor; } - /** - * \brief Method called when starting a tracking iteration - * + * @} */ + + void onTrackingIterStart() VP_OVERRIDE { } void onTrackingIterEnd() VP_OVERRIDE { } @@ -265,7 +267,6 @@ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker double m_vvsConvergenceThreshold; double tol; vpColVector error_ccd; - vpColVector weighted_error_ccd; std::vector m_gradients; std::vector m_hessians; double m_temporalSmoothingFac; //! Smoothing factor used to integrate data from the previous frame. diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h index b620b89709..ff3dd64f00 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h @@ -74,51 +74,123 @@ class VISP_EXPORT vpRBTracker ~vpRBTracker() = default; + /** + * \name Information retrieval + * @{ + */ void getPose(vpHomogeneousMatrix &cMo) const; void setPose(const vpHomogeneousMatrix &cMo); + vpObjectCentricRenderer &getRenderer(); + const vpRBFeatureTrackerInput &getMostRecentFrame() const { return m_currentFrame; } + const vpRBTrackerLogger &getLogger() const { return m_logger; } - vpCameraParameters getCameraParameters() const; - void setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w); - void setSilhouetteExtractionParameters(const vpSilhouettePointsExtractionSettings &settings); - - void reset(); + /** + * @} + */ + /** + * \name Settings + * @{ + */ + void addTracker(std::shared_ptr tracker); void loadObjectModel(const std::string &file); - void track(const vpImage &I); - void track(const vpImage &I, const vpImage &IRGB); - void track(const vpImage &I, const vpImage &IRGB, const vpImage &depth); + vpCameraParameters getCameraParameters() const; + void setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w); - void addTracker(std::shared_ptr tracker); + vpSilhouettePointsExtractionSettings getSilhouetteExtractionParameters() const + { + return m_depthSilhouetteSettings; + } - void displayMask(vpImage &Imask) const; - void display(const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type); + void setSilhouetteExtractionParameters(const vpSilhouettePointsExtractionSettings &settings); - vpObjectCentricRenderer &getRenderer(); - const vpRBFeatureTrackerInput &getMostRecentFrame() const { return m_currentFrame; } + double getOptimizationGain() const { return m_lambda; } + void setOptimizationGain(double lambda) + { + if (lambda < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater to zero"); + } + m_lambda = lambda; + } + unsigned int getMaxOptimizationIters() const { return m_vvsIterations; } + void setMaxOptimizationIters(unsigned int iters) { m_vvsIterations = iters; } - const std::shared_ptr getDriftDetector() const { return m_driftDetector; } + double getOptimizationInitialMu() const { return m_muInit; } + void setOptimizationInitialMu(double mu) + { + if (mu < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater to zero"); + } + m_muInit = mu; + } -#ifdef VISP_HAVE_MODULE_GUI - void initClick(const vpImage &I, const std::string &initFile, bool displayHelp); + double getOptimizationMuIterFactor() const { return m_muIterFactor; } + void setOptimizationMuIterFactor(double factor) + { + if (factor < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater to zero"); + } + m_muIterFactor = factor; + } -#endif + std::shared_ptr getDriftDetector() const { return m_driftDetector; } + void setDriftDetector(const std::shared_ptr &detector) + { + m_driftDetector = detector; + } - //vpRBTrackerFilter &getFilter() { return m_filter; } + std::shared_ptr getObjectSegmentationMethod() const { return m_mask; } + void setObjectSegmentationMethod(const std::shared_ptr &mask) + { + m_mask = mask; + } #if defined(VISP_HAVE_NLOHMANN_JSON) void loadConfigurationFile(const std::string &filename); void loadConfiguration(const nlohmann::json &j); #endif + /** + * @} + */ + + void reset(); + + /** + * \name Tracking + * @{ + */ + void track(const vpImage &I); + void track(const vpImage &I, const vpImage &IRGB); + void track(const vpImage &I, const vpImage &IRGB, const vpImage &depth); + /** + * @} + */ + + /** + * \name Display + * @{ + */ + void displayMask(vpImage &Imask) const; + void display(const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type); + /** + * @} + */ + +#ifdef VISP_HAVE_MODULE_GUI + void initClick(const vpImage &I, const std::string &initFile, bool displayHelp); +#endif + protected: void track(vpRBFeatureTrackerInput &input); void updateRender(vpRBFeatureTrackerInput &frame); - - std::vector extractSilhouettePoints(const vpImage &Inorm, const vpImage &Idepth, - const vpImage &Ior, const vpImage &Ivalid, const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp); + std::vector extractSilhouettePoints( + const vpImage &Inorm, const vpImage &Idepth, + const vpImage &Ior, const vpImage &Ivalid, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp); vpMatrix getCovariance() { @@ -137,18 +209,16 @@ class VISP_EXPORT vpRBTracker } } - bool m_firstIteration; //! Whether this is the first iteration std::vector> m_trackers; //! List of trackers - vpHomogeneousMatrix m_cMo; - vpHomogeneousMatrix m_cMoPrev; - vpCameraParameters m_cam; - vpRBFeatureTrackerInput m_currentFrame; vpRBFeatureTrackerInput m_previousFrame; + vpHomogeneousMatrix m_cMo; + vpHomogeneousMatrix m_cMoPrev; + vpCameraParameters m_cam; double m_lambda; //! VVS gain unsigned m_vvsIterations; //! Max number of VVS iterations @@ -156,23 +226,17 @@ class VISP_EXPORT vpRBTracker double m_muIterFactor; //! Factor with which to multiply mu at every iteration during VVS. vpSilhouettePointsExtractionSettings m_depthSilhouetteSettings; - vpPanda3DRenderParameters m_rendererSettings; vpObjectCentricRenderer m_renderer; - //vpRenderer m_renderer; unsigned m_imageHeight, m_imageWidth; //! Color and render image dimensions vpRBTrackerLogger m_logger; std::shared_ptr m_mask; - std::shared_ptr m_driftDetector; // vpRBTrackerFilter m_filter; - - vpRBFeatureTrackerInput m_tempRenders; - }; END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index 9f080f8860..db981e9fe9 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -69,9 +69,8 @@ vpRBTracker::vpRBTracker() : m_firstIteration(true), m_trackers(0), m_lambda(1.0 //m_renderer.addSubRenderer(std::make_shared(false)); m_renderer.setRenderParameters(m_rendererSettings); - //m_renderer.initFramework(); - m_driftDetector = nullptr; + m_driftDetector = nullptr; } void vpRBTracker::getPose(vpHomogeneousMatrix &cMo) const @@ -103,7 +102,6 @@ void vpRBTracker::setSilhouetteExtractionParameters(const vpSilhouettePointsExtr m_depthSilhouetteSettings = settings; } - void vpRBTracker::reset() { m_firstIteration = true; @@ -202,8 +200,6 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) } m_logger.setSilhouetteTime(m_logger.endTimer()); - - for (std::shared_ptr &tracker : m_trackers) { tracker->onTrackingIterStart(); } @@ -377,8 +373,6 @@ void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame) #endif { m_renderer.getRenderer()->getRender(frame.renders.silhouetteCanny, frame.renders.isSilhouette, frame.renders.boundingBox, m_imageHeight, m_imageWidth); - // m_renderer.placeRenderInto(m_tempRenders.renders.silhouetteCanny, frame.renders.silhouetteCanny, vpRGBf(0.f)); - // m_renderer.placeRenderInto(m_tempRenders.renders.isSilhouette, frame.renders.isSilhouette, (unsigned char)(0)); } // #pragma omp section // { diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp index 1551aedb2e..9ad0386f72 100644 --- a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -85,6 +85,8 @@ template class FastMat33 + + vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1) { }