From e7ef8bb1874bbdd730f8bbcf44848b716f368932 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Tue, 18 Oct 2022 15:10:56 +0200
Subject: [PATCH] Fix odometry getter for multithread

---
 include/core/processor/motion_provider.h | 11 +++++------
 src/factor/factor_base.cpp               | 14 ++++++++------
 src/problem/problem.cpp                  |  2 +-
 src/processor/motion_provider.cpp        | 10 ++++++++++
 src/processor/processor_motion.cpp       |  4 +++-
 5 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index d7bd26d00..c26021425 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -86,8 +86,12 @@ class MotionProvider
 
     protected:
         StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
-        VectorComposite odometry_;
         ParamsMotionProviderPtr params_motion_provider_;
+
+    private:
+        VectorComposite odometry_;
+        mutable std::mutex mut_odometry_;
+
 };
 
 inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
@@ -97,11 +101,6 @@ inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMo
     //
 }
 
-inline wolf::VectorComposite MotionProvider::getOdometry ( ) const
-{
-    return odometry_;
-}
-
 }
 
 /////  IMPLEMENTATION ///////
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 9ac1675b8..961006341 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -141,42 +141,44 @@ void FactorBase::remove(bool viral_remove_empty_parent)
 CaptureBaseConstPtr FactorBase::getCapture() const
 {
     auto ftr = getFeature();
-    if (ftr != nullptr) return ftr->getCapture();
+    if (ftr != nullptr and not ftr->isRemoving()) 
+        return ftr->getCapture();
     else return nullptr;
 }
 
 CaptureBasePtr FactorBase::getCapture()
 {
     auto ftr = getFeature();
-    if (ftr != nullptr) return ftr->getCapture();
+    if (ftr != nullptr and not ftr->isRemoving()) 
+        return ftr->getCapture();
     else return nullptr;
 }
 
 FrameBaseConstPtr FactorBase::getFrame() const
 {
     auto cpt = getCapture();
-    if(cpt != nullptr) return cpt->getFrame();
+    if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
     else return nullptr;
 }
 
 FrameBasePtr FactorBase::getFrame()
 {
     auto cpt = getCapture();
-    if(cpt != nullptr) return cpt->getFrame();
+    if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
     else return nullptr;
 }
 
 SensorBaseConstPtr FactorBase::getSensor() const
 {
     auto cpt = getCapture();
-    if(cpt != nullptr) return cpt->getSensor();
+    if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
     else return nullptr;
 }
 
 SensorBasePtr FactorBase::getSensor()
 {
     auto cpt = getCapture();
-    if(cpt != nullptr) return cpt->getSensor();
+    if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
     else return nullptr;
 }
 
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index fa168d5d5..e95ce8253 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -587,7 +587,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
         }
     }
 
-    // check for empty blocks and fill them with the the prior, or with zeros in the worst case
+    // check for empty blocks and fill them with the prior, or with zeros in the worst case
 
     VectorComposite state_last;
 
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index e2723db61..d3a7db555 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -37,8 +37,18 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
 
 void MotionProvider::setOdometry(const VectorComposite& _odom)
 {
+    std::lock_guard<std::mutex> lock(mut_odometry_);
+
     assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
 
     for (auto key : state_structure_)
         odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
 }
+
+wolf::VectorComposite MotionProvider::getOdometry ( ) const
+{
+    std::lock_guard<std::mutex> lock(mut_odometry_);
+
+    return odometry_;
+}
+
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 3c014ca6a..b1ed04c2b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -809,7 +809,9 @@ void ProcessorMotion::integrateOneStep()
                              jacobian_calib_);
 
     // integrate odometry
-    statePlusDelta(odometry_, delta_, dt_, odometry_);
+    VectorComposite odom;
+    statePlusDelta(getOdometry(), delta_, dt_, odom);
+    setOdometry(odom);
 }
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
-- 
GitLab