Merge pull request #306 from pritzvac/mutex_state_bugfix

Fix: Segmentation fault in State.h

ov_msckf/src/state/State.h

@@ -63,6 +63,7 @@ class State {
    * @return timestep of clone we will marginalize
    */
   double margtimestep() {
+    std::lock_guard<std::mutex> lock(_mutex_state);
     double time = INFINITY;
     for (const auto &clone_imu : _clones_IMU) {
       if (clone_imu.first < time) {
@@ -78,6 +79,9 @@ class State {
    */
   int max_covariance_size() { return (int)_Cov.rows(); }
 
+  /// Mutex for locking access to the state
+  std::mutex _mutex_state;
+
   /// Current timestamp (should be the last update time!)
   double _timestamp = -1;
 

ov_msckf/src/state/StateHelper.cpp

@@ -618,6 +618,8 @@ void StateHelper::augment_clone(std::shared_ptr<State> state, Eigen::Matrix<doub
 void StateHelper::marginalize_old_clone(std::shared_ptr<State> state) {
   if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) {
     double marginal_time = state->margtimestep();
+    // Lock the mutex to avoid deleting any elements from _clones_IMU while accessing it from other threads
+    std::lock_guard<std::mutex> lock(state->_mutex_state);
     assert(marginal_time != INFINITY);
     StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time));
     // Note that the marginalizer should have already deleted the clone