Fix state blocks of landmark

src/landmark_base.cpp

@@ -15,9 +15,9 @@ LandmarkBase::LandmarkBase(const LandmarkType & _tp, const std::string& _type, S
             state_block_vec_(4), // allow for 4 state blocks by default. Should be enough in all applications.
             landmark_id_(++landmark_id_count_),
             type_id_(_tp),
-            status_(LANDMARK_CANDIDATE),
-			p_ptr_(_p_ptr),
-			o_ptr_(_o_ptr)
+            status_(LANDMARK_CANDIDATE)//,
+//			p_ptr_(_p_ptr),
+//			o_ptr_(_o_ptr)
 {
     //
     state_block_vec_[0] = _p_ptr;
@@ -66,32 +66,32 @@ void LandmarkBase::setStatus(LandmarkStatus _st)
     // State Blocks
     if (status_ == LANDMARK_FIXED)
     {
-        if (p_ptr_!=nullptr)
+        if (getPPtr()!=nullptr)
         {
-            p_ptr_->fix();
+            getPPtr()->fix();
             if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(p_ptr_);
+                getProblem()->updateStateBlockPtr(getPPtr());
         }
-        if (o_ptr_!=nullptr)
+        if (getOPtr()!=nullptr)
         {
-            o_ptr_->fix();
+            getOPtr()->fix();
             if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(o_ptr_);
+                getProblem()->updateStateBlockPtr(getOPtr());
         }
     }
     else if(status_ == LANDMARK_ESTIMATED)
     {
-        if (p_ptr_!=nullptr)
+        if (getPPtr()!=nullptr)
         {
-            p_ptr_->unfix();
+            getPPtr()->unfix();
             if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(p_ptr_);
+                getProblem()->updateStateBlockPtr(getPPtr());
         }
-        if (o_ptr_!=nullptr)
+        if (getOPtr()!=nullptr)
         {
-            o_ptr_->unfix();
+            getOPtr()->unfix();
             if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(o_ptr_);
+                getProblem()->updateStateBlockPtr(getOPtr());
         }
     }
 }
@@ -128,15 +128,15 @@ YAML::Node LandmarkBase::saveToYaml() const
     YAML::Node node;
     node["id"] = landmark_id_;
     node["type"] = node_type_;
-    if (p_ptr_ != nullptr)
+    if (getPPtr() != nullptr)
     {
-        node["position"] = p_ptr_->getVector();
-        node["position fixed"] = p_ptr_->isFixed();
+        node["position"] = getPPtr()->getVector();
+        node["position fixed"] = getPPtr()->isFixed();
     }
-    if (o_ptr_ != nullptr)
+    if (getOPtr() != nullptr)
     {
-        node["orientation"] = o_ptr_->getVector();
-        node["orientation fixed"] = p_ptr_->isFixed();
+        node["orientation"] = getOPtr()->getVector();
+        node["orientation fixed"] = getOPtr()->isFixed();
     }
     return node;
 }

src/landmark_base.h

@@ -39,8 +39,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         LandmarkType type_id_;     ///< type of landmark. (types defined at wolf.h)
         LandmarkStatus status_; ///< status of the landmark. (types defined at wolf.h)
         TimeStamp stamp_;       ///< stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
-        StateBlockPtr p_ptr_;     ///< Position state block pointer
-        StateBlockPtr o_ptr_;     ///< Orientation state block pointer
+//        StateBlockPtr p_ptr_;     ///< Position state block pointer
+//        StateBlockPtr o_ptr_;     ///< Orientation state block pointer
         Eigen::VectorXs descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
 
     public:
@@ -207,22 +207,22 @@ inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_pt
 
 inline StateBlockPtr LandmarkBase::getPPtr() const
 {
-    return p_ptr_;
+    return getStateBlockPtr(0);
 }
 
 inline StateBlockPtr LandmarkBase::getOPtr() const
 {
-    return o_ptr_;
+    return getStateBlockPtr(1);
 }
 
 inline void LandmarkBase::setPPtr(const StateBlockPtr _st_ptr)
 {
-    p_ptr_ = _st_ptr;
+    setStateBlockPtr(0, _st_ptr);
 }
 
 inline void LandmarkBase::setOPtr(const StateBlockPtr _st_ptr)
 {
-    o_ptr_ = _st_ptr;
+    setStateBlockPtr(1, _st_ptr);
 }
 
 inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)