From e65badd39910fae1809a27530297fe2041e37ee9 Mon Sep 17 00:00:00 2001
From: artivis <deray.jeremie@gmail.com>
Date: Tue, 29 May 2018 14:52:30 +0200
Subject: [PATCH] mutex StateBlock state vector

---
 src/state_block.h | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/src/state_block.h b/src/state_block.h
index 780b0b8bf..9fe7c0074 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -14,6 +14,7 @@ class LocalParametrizationBase;
 
 //std includes
 #include <iostream>
+#include <mutex>
 
 
 namespace wolf {
@@ -50,7 +51,10 @@ public:
 
         bool fixed_; ///< Key to indicate whether the state is fixed or not
 
+        std::atomic<int> state_size_; ///< State vector size
         Eigen::VectorXs state_; ///< State vector storing the state values
+        mutable std::mutex mut_state_; ///< State vector mutex
+
         LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
 
     public:
@@ -132,6 +136,7 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP
 //        notifications_{Notification::ADD},
         node_ptr_(), // nullptr
         fixed_(_fixed),
+        state_size_(_state.size()),
         state_(_state),
         local_param_ptr_(_local_param_ptr)
 {
@@ -142,6 +147,7 @@ inline StateBlock::StateBlock(const unsigned int _size, bool _fixed, LocalParame
 //        notifications_{Notification::ADD},
         node_ptr_(), // nullptr
         fixed_(_fixed),
+        state_size_(_size),
         state_(Eigen::VectorXs::Zero(_size)),
         local_param_ptr_(_local_param_ptr)
 {
@@ -156,26 +162,33 @@ inline StateBlock::~StateBlock()
 
 inline Eigen::VectorXs StateBlock::getState() const
 {
+    std::lock_guard<std::mutex> lock(mut_state_);
     return state_;
 }
 
 inline void StateBlock::setState(const Eigen::VectorXs& _state)
 {
     assert(_state.size() == state_.size());
-    state_ = _state;
+
+    {
+      std::lock_guard<std::mutex> lock(mut_state_);
+      state_ = _state;
+      state_size_ = state_.size();
+    }
+
     addNotification(Notification::STATE_UPDATE);
 }
 
 inline unsigned int StateBlock::getSize() const
 {
-    return state_.size();
+    return state_size_.load();
 }
 
 inline Size StateBlock::getLocalSize() const
 {
     if(local_param_ptr_)
         return local_param_ptr_->getLocalSize();
-    return state_.size();
+    return getSize();
 }
 
 inline bool StateBlock::isFixed() const
-- 
GitLab