diff --git a/doc/mainpage.dox.in b/doc/mainpage.dox.in index 87ecb769f8..c68d1f5959 100644 --- a/doc/mainpage.dox.in +++ b/doc/mainpage.dox.in @@ -526,6 +526,69 @@ in different ways. This will motivate us to continue the efforts. \defgroup group_mbt_xml_parser XML parsers XML parsers dedicated to model-based trackers. */ + + +/******************************************* + * Module rbt + *******************************************/ +/*! + \ingroup module_tracker + \defgroup module_rbt RBT: Render-Based Tracker module + Render-Based Tracker module +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_core Core Render-Based Tracking functionalities + + This group contains the core classes that make Render-Based tracking work. The main interface for Render-Based tracking is vpRBTracker. +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_trackers Trackable features + + These classes represent features that can be tracked by the render-based tracker. All trackable features should inherit from vpRBFeatureTracker + +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_mask Object segmentation + + These classes allow to perform object segmentaiton from rendering information. This segmentation may be used downstream by feature trackers to filter features. + */ + +/*! + \ingroup module_rbt + \defgroup group_rbt_drift Drift and divergence detection + + These classes allow to detect tracking drift. +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_rendering Rendering + + This group contains additional rendering utilities. +*/ + +/*! + \ingroup module_mbt + \defgroup group_mbt_features Features + Model-based trackers features. +*/ +/*! + \ingroup module_mbt + \defgroup group_mbt_faces Faces management + Faces management including visibility. +*/ +/*! + \ingroup module_mbt + \defgroup group_mbt_xml_parser XML parsers + XML parsers dedicated to model-based trackers. +*/ + /******************************************* * Module robot *******************************************/ diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h index 02ce70db04..22a6609d37 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h @@ -923,15 +923,7 @@ inline void to_json(nlohmann::json &j, const vpMbGenericTracker::TrackerWrapper //KLT tracker settings #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) if (t.m_trackerType & vpMbGenericTracker::KLT_TRACKER) { - nlohmann::json klt = nlohmann::json { - {"maxFeatures", t.tracker.getMaxFeatures()}, - {"windowSize", t.tracker.getWindowSize()}, - {"quality", t.tracker.getQuality()}, - {"minDistance", t.tracker.getMinDistance()}, - {"harris", t.tracker.getHarrisFreeParameter()}, - {"blockSize", t.tracker.getBlockSize()}, - {"pyramidLevels", t.tracker.getPyramidLevels()} - }; + nlohmann::json klt = t.tracker; klt["maskBorder"] = t.maskBorder; j["klt"] = klt; } @@ -1034,14 +1026,7 @@ inline void from_json(const nlohmann::json &j, vpMbGenericTracker::TrackerWrappe #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) if (t.m_trackerType & vpMbGenericTracker::KLT_TRACKER) { const nlohmann::json klt = j.at("klt"); - auto &ktrack = t.tracker; - ktrack.setMaxFeatures(klt.value("maxFeatures", 10000)); - ktrack.setWindowSize(klt.value("windowSize", 5)); - ktrack.setQuality(klt.value("quality", 0.01)); - ktrack.setMinDistance(klt.value("minDistance", 5)); - ktrack.setHarrisFreeParameter(klt.value("harris", 0.01)); - ktrack.setBlockSize(klt.value("blockSize", 3)); - ktrack.setPyramidLevels(klt.value("pyramidLevels", 3)); + t.tracker = klt; t.setMaskBorder(klt.value("maskBorder", t.maskBorder)); t.faces.getMbScanLineRenderer().setMaskBorder(t.maskBorder); } diff --git a/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h index 98374620a7..540189ad1a 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h @@ -51,6 +51,11 @@ class vpRBFeatureTrackerInput; +/** + * \brief A color histogram based segmentation algorithm. + * + * \ingroup group_rbt_mask + */ class VISP_EXPORT vpColorHistogramMask : public vpObjectMask { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h index b5b4962d4f..5be25703e7 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h @@ -45,6 +45,11 @@ #include #include +/*! + + \brief Single object focused renderer + \ingroup group_rbt_rendering +*/ class VISP_EXPORT vpObjectCentricRenderer : public vpPanda3DRendererSet { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h index e97c4bcfab..8ef0592dbf 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h @@ -48,6 +48,11 @@ class vpRBFeatureTrackerInput; #include #endif +/** + * \brief + * + * \ingroup group_rbt_mask + */ class VISP_EXPORT vpObjectMask { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h index a718afa2fe..b7c3ecc5c2 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h @@ -41,6 +41,12 @@ #include #include +/** + * \brief A factory that can be used to create Object segmentation algorithms from JSON data. + * + * \ingroup group_rbt_mask + * + */ class VISP_EXPORT vpObjectMaskFactory : public vpDynamicFactory { private: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h b/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h index 7bbfde383a..5471683c22 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h @@ -46,10 +46,8 @@ /** * - * \ingroup group_ar_renderer_panda3d_filters - * \brief Class that implements a gaussian filter on a grayscale image. - * The grayscale image should be contained in the blue channel of the image. - * + * \ingroup group_rbt_rendering + * \brief */ class VISP_EXPORT vpPanda3DDepthGaussianBlur : public vpPanda3DPostProcessFilter { @@ -63,7 +61,7 @@ class VISP_EXPORT vpPanda3DDepthGaussianBlur : public vpPanda3DPostProcessFilter }; /** - * \ingroup group_ar_renderer_panda3d_filters + * \ingroup group_rbt_rendering * \brief Implementation of canny filtering, using Sobel kernels. * * The results of the canny are filtered based on a threshold value (defined between 0 and 255), checking whether there is enough gradient information. diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h index 927d6f21bc..6786d08d0e 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h @@ -61,7 +61,9 @@ /** - * @brief A base class for all features that can be used and tracker in the vpRenderBasedTracker + * @brief A tracker based on dense depth point-plane alignement + * + * \ingroup group_rbt_trackers * */ class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h index 1d08fa789e..bf8ddaac36 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h @@ -57,6 +57,7 @@ template class vpImage; * - Call vpRBDriftDetector::update to update the drift detection parameters. * - use vpRBDriftDetector::hasDiverged to detect the drift, or vpRBDriftDetector::getScore to use the estimated tracking reliability. * + * \ingroup group_rbt_drift */ class VISP_EXPORT vpRBDriftDetector { diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h index b522655cf3..7d25f49bfb 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h @@ -41,6 +41,11 @@ #include #include +/** + * \brief A factory that can be used to instanciate drift detection algorithms from JSON data. + * + * \ingroup group_rbt_drift + */ class VISP_EXPORT vpRBDriftDetectorFactory : public vpDynamicFactory { private: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h index 4bf4d13968..49a9762df1 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h @@ -61,7 +61,9 @@ enum vpRBFeatureDisplayType /** - * @brief A base class for all features that can be used and tracked in the vpRBTracker + * \brief A base class for all features that can be used and tracked in the vpRBTracker + * + * \ingroup group_rbt_trackers */ class VISP_EXPORT vpRBFeatureTracker { @@ -70,13 +72,13 @@ class VISP_EXPORT vpRBFeatureTracker vpRBFeatureTracker(); /** - * @brief Return the type of feature that is used by this tracker + * \brief Return the type of feature that is used by this tracker * - * @return vpRBFeatureType + * \return vpRBFeatureType */ /** - * @brief Get the number of features used to compute the pose update + * \brief Get the number of features used to compute the pose update * */ unsigned getNumFeatures() const { return m_numFeatures; } @@ -87,21 +89,21 @@ class VISP_EXPORT vpRBFeatureTracker */ /** - * @brief Whether this tracker requires RGB image to extract features + * \brief Whether this tracker requires RGB image to extract features * - * @return true if the tracker requires an RGB image - * @return false otherwise + * \return true if the tracker requires an RGB image + * \return false otherwise */ virtual bool requiresRGB() const = 0; /** - * @brief Whether this tracker requires depth image to extract features + * \brief Whether this tracker requires depth image to extract features * */ virtual bool requiresDepth() const = 0; /** - * @brief Whether this tracker requires Silhouette candidates + * \brief Whether this tracker requires Silhouette candidates */ virtual bool requiresSilhouetteCandidates() const = 0; /** @@ -113,26 +115,27 @@ class VISP_EXPORT vpRBFeatureTracker * \name Core Tracking methods * @{ */ + /** - * @brief Method called when starting a tracking iteration + * \brief Method called when starting a tracking iteration * */ virtual void onTrackingIterStart() = 0; /** - * @brief Method called after the tracking iteration has finished + * \brief Method called after the tracking iteration has finished * */ virtual void onTrackingIterEnd() = 0; /** - * @brief Extract features from the frame data and the current pose estimate + * \brief Extract features from the frame data and the current pose estimate * */ virtual void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0; /** - * @brief Track the features + * \brief Track the features */ virtual void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0; @@ -156,9 +159,9 @@ class VISP_EXPORT vpRBFeatureTracker */ const vpMatrix &getCovariance() const { return m_cov; } /** - * @brief Update the covariance matrix + * \brief Update the covariance matrix * - * @param lambda the visual servoing gain + * \param lambda the visual servoing gain */ virtual void updateCovariance(const double lambda); /** @@ -171,7 +174,7 @@ class VISP_EXPORT vpRBFeatureTracker bool vvsHasConverged() const { return m_vvsConverged; } /** - * @brief Get the importance of this tracker in the optimization step. + * \brief Get the importance of this tracker in the optimization step. * The default computation is the following: * \f$ \sqrt{w / N} \f$, where \f$ w\f$ is the weight defined by setTrackerWeight, and \f$ N \f$ is the number of features. */ @@ -179,17 +182,17 @@ class VISP_EXPORT vpRBFeatureTracker void setTrackerWeight(double weight) { m_userVvsWeight = weight; } /** - * @brief Get the leftside term of the Gauss-Newton optimization term + * \brief Get the leftside term of the Gauss-Newton optimization term */ const vpMatrix &getLTL() const { return m_LTL; } /** - * @brief Get the rightside term of the Gauss-Newton optimization term + * \brief Get the rightside term of the Gauss-Newton optimization term */ const vpColVector &getLTR() const { return m_LTR; } /** - * @brief Get a weighted version of the error vector. + * \brief Get a weighted version of the error vector. * This should not include the userVVSWeight, but may include reweighting to remove outliers, occlusions, etc. */ const vpColVector &getWeightedError() const { return m_weighted_error; } diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h index 653b286e06..59cb4aa8d3 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h @@ -41,6 +41,11 @@ #include #include +/** + * \brief A factory to instantiate feature trackers from JSON data + * + * \ingroup group_rbt_trackers + */ class VISP_EXPORT vpRBFeatureTrackerFactory : public vpDynamicFactory { private: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h index 6be4dbb8a0..87bce7bd23 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h @@ -44,6 +44,11 @@ #include +/** + * \brief Render data storage + * + * \ingroup group_rbt_core + */ struct VISP_EXPORT vpRBRenderData { vpImage normals; //! Image containing the per-pixel normal vector (RGB, in object space) @@ -94,6 +99,12 @@ struct VISP_EXPORT vpRBRenderData }; +/** + * \brief All the data related to a single tracking frame. + * This contains both the input data (from a real camera/outside source) and renders from Panda. + * + * \ingroup group_rbt_core + */ class VISP_EXPORT vpRBFeatureTrackerInput { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h b/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h index cf2e5643ba..d339bfa824 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h @@ -12,6 +12,11 @@ template class vpImage; +/** + * \brief A set of utilities to perform initialization. + * + * \group core + */ class VISP_EXPORT vpRBInitializationHelper { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h index d614fe21a8..7e4b7234e3 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h @@ -52,6 +52,11 @@ #include +/** + * \brief KLT-Based features + * + * \ingroup group_rbt_trackers + */ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker { public: @@ -78,34 +83,102 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth, const vpRBFeatureDisplayType type) const VP_OVERRIDE; + /** + * \name Settings + * @{ + */ + + /** + * \brief Get the minimum acceptable number of points that should be tracked. If KLT tracking has less than this number of points + * The KLT tracking will be fully reinitialized. + */ + unsigned int getMinimumNumberOfPoints() const { return m_numPointsReinit; } + void setMinimumNumberOfPoints(unsigned int points) { m_numPointsReinit = points; } + + /** + * \brief Get the minimum distance that a candidate point should have to every other tracked point if it should be added. + * + * During tracking, KLT points are frequently sampled. This settings used to ensure that multiple klt points do not track the same 3D points + */ + double getMinimumDistanceNewPoints() const { return m_newPointsDistanceThreshold; } + void setMinimumDistanceNewPoints(double distance) { m_newPointsDistanceThreshold = distance; } + + /** + * \brief Return the number of pixels in the image border where points should not be tracked. + * Points that are near image borders are likely to be lost in the future. + */ + unsigned int getFilteringBorderSize() const { return m_border; } + void setFilteringBorderSize(unsigned int border) { m_border = border; } + + /** + * \brief Get the maximum reprojection error, in pixels, for a point to be considered as outlier. + * This reprojection error is computed between the tracked klt position in the image and the reprojection of the associated 3D point. + * If a point goes above this threshold, it is removed from tracking + * + * \return double + */ + double getFilteringMaxReprojectionError() const { return m_maxErrorOutliersPixels; } + void setFilteringMaxReprojectionError(double maxError) { m_maxErrorOutliersPixels = maxError; } + + /** + * \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 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; + } + + /** + * \brief Get the underlying KLT tracker. Use this to read its settings. + */ + const vpKltOpencv &getKltTracker() const { return m_klt; } + /** + * \brief Get the underlying KLT tracker. Use this to modify its settings. + * + * \warning Only modify its tracking settings, not its state. + */ + vpKltOpencv &getKltTracker() { return m_klt; } + #if defined(VISP_HAVE_NLOHMANN_JSON) virtual void loadJsonConfiguration(const nlohmann::json &j) { vpRBFeatureTracker::loadJsonConfiguration(j); - m_klt.setMaxFeatures(j.value("maxFeatures", 10000)); - m_klt.setWindowSize(j.value("windowSize", 5)); - m_klt.setQuality(j.value("quality", 0.01)); - m_klt.setMinDistance(j.value("minDistance", 5)); - m_klt.setHarrisFreeParameter(j.value("harris", 0.01)); - m_klt.setBlockSize(j.value("blockSize", 3)); - m_klt.setPyramidLevels(j.value("pyramidLevels", 3)); - - m_numPointsReinit = j.value("minimumNumPoints", m_numPointsReinit); - m_newPointsDistanceThreshold = j.value("newPointsMinPixelDistance", m_newPointsDistanceThreshold); - m_maxErrorOutliersPixels = j.value("maxReprojectionErrorPixels", m_maxErrorOutliersPixels); + m_klt = j; + setMinimumNumberOfPoints(j.value("minimumNumPoints", m_numPointsReinit)); + setMinimumDistanceNewPoints(j.value("newPointsMinPixelDistance", m_newPointsDistanceThreshold)); + setFilteringMaxReprojectionError(j.value("maxReprojectionErrorPixels", m_maxErrorOutliersPixels)); + setShouldUseMask(j.value("useMask", m_useMask)); + setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence)); } #endif + /** + * @} + * + */ struct vpTrackedKltPoint { public: - vpHomogeneousMatrix cTo0; - vpPoint oX; - vpColVector normal; - vpImagePoint currentPos; + vpHomogeneousMatrix cTo0; //! Initial pose of the object in the camera frame, acquired when the tracked point was first constructed + vpPoint oX; //! Tracked 3D point + vpColVector normal; //! Surface normal at this point, in the object frame + vpImagePoint currentPos; //! Current image coordinates, in normalized image coordinates inline double rotationDifferenceToInitial(const vpHomogeneousMatrix &oMc) { @@ -189,6 +262,9 @@ class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker std::map m_points; + bool m_useMask; + float m_minMaskConfidence; + }; #endif #endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h b/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h index 959ade9b25..31d2216de4 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h @@ -53,6 +53,8 @@ template class vpImage; /** + * \ingroup group_rbt_drift + * * \brief Algorithm that uses tracks object surface points in order to estimate the probability that tracking is successful. * * Given a set of surface points \f$ \mathbf{X}_0, ..., \mathbf{X}_N, \f$, each point \f$\mathbf{X}_i\f$ being associated to: @@ -232,8 +234,6 @@ class VISP_EXPORT vpRBProbabilistic3DDriftDetector : public vpRBDriftDetector * @{ */ - - /** * \brief Get the minimum distance criterion (in meters) that is used * when trying to add new points to track for the drift detection. diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h index 03afb312b9..a36945a3ab 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -172,10 +172,14 @@ class VISP_EXPORT vpCCDStatistics imgPoints.resize(resolution, 2 * 3 * normalPointsNumber, false, false); weight.resize(resolution, 2 * normalPointsNumber, false, false); } - }; +/** + * \brief Tracking based on the Contracting Curve Density algorithm. + * + * \ingroup group_rbt_trackers + */ class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h index 9921fc4540..bdb26cd8c7 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h @@ -50,6 +50,11 @@ #include #include +/*! + \brief Trackable silhouette point representation + + \ingroup group_rbt_core +*/ class VISP_EXPORT vpRBSilhouetteControlPoint { private: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h index 1e6034d14d..a5b1cd92f7 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h @@ -41,6 +41,11 @@ #include #include +/** + * \brief Moving edge feature tracking from depth-extracted object contours + * + * \ingroup group_rbt_trackers + */ class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h index a7fd44c01e..90eb143d44 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h @@ -40,6 +40,10 @@ #include #include +/*! + \brief Silhouette point simple candidate representation. + \ingroup group_rbt_core +*/ class VISP_EXPORT vpRBSilhouettePoint { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h index 6e3d99ffdd..e95307f4b8 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h @@ -53,6 +53,11 @@ class vpRBSilhouettePoint; + +/*! + \brief Silhouette point extraction settings + \ingroup group_rbt_core +*/ class VISP_EXPORT vpSilhouettePointsExtractionSettings { diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h index 4d85767464..4c8663494c 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h @@ -58,6 +58,11 @@ class vpRBDriftDetector; #include #endif +/** + * \brief + * + * \ingroup group_rbt_core + */ class VISP_EXPORT vpRBTracker { public: diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h index c81309f8f9..a5738a9920 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h @@ -45,6 +45,11 @@ #include #endif +/*! + \brief Information storage for render based tracking process. + + \ingroup group_rbt_core +*/ class VISP_EXPORT vpRBTrackerLogger { public: diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp index fe21735fe2..b520269636 100644 --- a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -52,8 +52,9 @@ const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc) if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { return; } + float Z = frame.renders.depth[uv][uu]; - if (Z <= 0.f || (frame.hasDepth() && frame.depth[uv][uu] > 0.f && fabs(frame.depth[uv][uu] - Z) > 5e-3)) { + if (Z <= 0.f || (frame.hasDepth() && frame.depth[uv][uu] > 0.f && fabs(frame.depth[uv][uu] - Z) > 1e-1)) { return; } vpRBKltTracker::vpTrackedKltPoint p; @@ -73,12 +74,11 @@ const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc) vpRBKltTracker::vpRBKltTracker() : vpRBFeatureTracker(), m_numPointsReinit(20), m_newPointsDistanceThreshold(5.0), m_border(5), - m_maxErrorOutliersPixels(10.0) + m_maxErrorOutliersPixels(10.0), m_useMask(false), m_minMaskConfidence(0.0) { } - void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &cMo) { m_Iprev = m_I; @@ -197,24 +197,36 @@ void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const void vpRBKltTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &cMo) { - const unsigned int nbKltFeatures = static_cast(m_klt.getNbFeatures()); + unsigned int nbKltFeatures = static_cast(m_klt.getNbFeatures()); if (nbKltFeatures > 0) { m_klt.track(m_I); } std::map newPoints; const vpHomogeneousMatrix oMc = cMo.inverse(); + bool testMask = m_useMask && frame.hasMask(); + nbKltFeatures = static_cast(m_klt.getNbFeatures()); + for (unsigned int i = 0; i < nbKltFeatures; ++i) { long id = 0; float u = 0.f, v = 0.f; double x = 0.0, y = 0.0; m_klt.getFeature(i, id, u, v); unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); + // Filter points that are too close to image borders and cannot be reliably tracked if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { continue; } + float Z = frame.renders.depth[uv][uu]; + if (Z <= 0.f) { + continue; + } + + if (testMask && frame.mask[uv][uu] < m_minMaskConfidence) { + continue; + } - if (frame.mask.getSize() > 0 && frame.mask[uv][uu] > 0.5 && m_points.find(id) != m_points.end()) { + if (m_points.find(id) != m_points.end()) { vpTrackedKltPoint &p = m_points[id]; if (p.rotationDifferenceToInitial(oMc) > vpMath::rad(45.0) && p.normalDotProd(cMo) < cos(vpMath::rad(70))) { continue; diff --git a/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp b/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp index 3b79f0e741..5af637e813 100644 --- a/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp +++ b/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp @@ -227,8 +227,6 @@ void vpPanda3DDepthCannyFilter::getRender(vpImage &I, vpImage(std::max(0.0, bb.getTop())); const unsigned left = static_cast(std::max(0.0, bb.getLeft())); - const unsigned bottom = static_cast(std::min(static_cast(h), bb.getBottom())); - const unsigned right = static_cast(std::min(static_cast(w), bb.getRight())); const unsigned numComponents = m_texture->get_num_components(); const unsigned rowIncrement = m_renderParameters.getImageWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.json b/tutorial/tracking/render-based/data/sequence1/dragon.json index 3a0fcd0ec8..3602503ad7 100644 --- a/tutorial/tracking/render-based/data/sequence1/dragon.json +++ b/tutorial/tracking/render-based/data/sequence1/dragon.json @@ -51,6 +51,14 @@ "useMask": true, "minMaskConfidence": 0.7 }, + { + "type": "silhouetteColor", + "weight": 0.01, + "ccd": { + "h": 8, + "delta_h": 1 + } + }, { "type": "klt", @@ -66,7 +74,7 @@ "maxFeatures": 500, "minDistance": 5.0, "pyramidLevels": 3, - "quality": 0.05, + "quality": 0.01, "windowSize": 5 } ]