Add VO pointer and call to RBT main class

lagadic · Oct 8, 2024 · 5aef239 · 5aef239
1 parent 45cf1b6
commit 5aef239
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git 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<vpRBVisualOdometry> getOdometryMethod() const { return m_odometry; }
+  void setOdometryMethod(const std::shared_ptr<vpRBVisualOdometry> &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<vpObjectMask> m_mask;
   std::shared_ptr<vpRBDriftDetector> m_driftDetector;
+  std::shared_ptr<vpRBVisualOdometry> m_odometry;
+
 
   // vpRBTrackerFilter m_filter;
 };

diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp
@@ -44,6 +44,7 @@
 #include <visp3/rbt/vpRBFeatureTrackerFactory.h>
 #include <visp3/rbt/vpRBDriftDetectorFactory.h>
 #include <visp3/rbt/vpObjectMaskFactory.h>
+#include <visp3/rbt/vpRBVisualOdometry.h>
 #include <visp3/rbt/vpRBInitializationHelper.h>
 
 #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);