hotfix state block vector size assert fail

include/core/landmark/landmark_base.h

@@ -168,8 +168,11 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
 
 inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
 {
-    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-    return state_block_vec_[_i];
+    //    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
+    if (_i < state_block_vec_.size())
+        return state_block_vec_[_i];
+    else
+        return nullptr;
 }
 
 inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)

src/landmark/landmark_base.cpp

@@ -156,7 +156,7 @@ YAML::Node LandmarkBase::saveToYaml() const
         node["position"] = getP()->getState();
         node["position fixed"] = getP()->isFixed();
     }
-    if (getO() != nullptr)
+    if (state_block_vec_.size() > 1 && getO() != nullptr)
     {
         node["orientation"] = getO()->getState();
         node["orientation fixed"] = getO()->isFixed();