diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index e33351c3b3bf416baed99b4afa2601cdd923f67e..853ffc3c104dd9bb4e518b939e31a50ab1f219d4 100644
--- a/include/core/landmark/landmark_base.h
+++ b/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)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5f245efc3e416c82d796f8fcd45a84a3d5444773..20e32da0a4e7d5d03a4475cb7ed37e54a7865311 100644
--- a/src/landmark/landmark_base.cpp
+++ b/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();