diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h index 53de5aeb49..33a0f66884 100644 --- a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h @@ -57,6 +57,7 @@ BEGIN_VISP_NAMESPACE class vpObjectMask; class vpRBDriftDetector; +class vpRBVisualOdometry; /** * \brief @@ -156,6 +157,12 @@ class VISP_EXPORT vpRBTracker m_mask = mask; } + std::shared_ptr getOdometryMethod() const { return m_odometry; } + void setOdometryMethod(const std::shared_ptr &odometry) + { + m_odometry = odometry; + } + #if defined(VISP_HAVE_NLOHMANN_JSON) void loadConfigurationFile(const std::string &filename); void loadConfiguration(const nlohmann::json &j); @@ -242,6 +249,8 @@ class VISP_EXPORT vpRBTracker std::shared_ptr m_mask; std::shared_ptr m_driftDetector; + std::shared_ptr m_odometry; + // vpRBTrackerFilter m_filter; }; diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp index 3060f17940..b4ab7decab 100644 --- a/modules/tracker/rbt/src/core/vpRBTracker.cpp +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #define VP_DEBUG_RB_TRACKER 1 @@ -57,6 +58,7 @@ vpRBTracker::vpRBTracker() : m_firstIteration(true), m_trackers(0), m_lambda(1.0 m_driftDetector = nullptr; m_mask = nullptr; + m_odometry = nullptr; } void vpRBTracker::getPose(vpHomogeneousMatrix &cMo) const @@ -213,12 +215,20 @@ void vpRBTracker::track(vpRBFeatureTrackerInput &input) m_logger.startTimer(); updateRender(input); m_logger.setRenderTime(m_logger.endTimer()); + if (m_firstIteration) { m_firstIteration = false; m_previousFrame.I = input.I; m_previousFrame.IRGB = input.IRGB; } + if (m_odometry) { + m_odometry->compute(input, m_previousFrame); + vpHomogeneousMatrix cnTc = m_odometry->getCameraMotion(); + m_cMo = cnTc * m_cMo; + updateRender(input); + } + m_logger.startTimer(); if (m_mask) { m_mask->updateMask(input, m_previousFrame, input.mask);