Re-code setStatus() with a for loop over state_blk_vec

src/landmark_base.cpp

@@ -15,11 +15,8 @@ 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)
 {
-    //
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = nullptr;
@@ -41,7 +38,6 @@ void LandmarkBase::remove()
         std::cout << "Removing   L" << id() << std::endl;
         LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
 
-
         // remove from upstream
         auto M = map_ptr_.lock();
         if (M)
@@ -66,33 +62,47 @@ void LandmarkBase::setStatus(LandmarkStatus _st)
     // State Blocks
     if (status_ == LANDMARK_FIXED)
     {
-        if (getPPtr()!=nullptr)
-        {
-            getPPtr()->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(getPPtr());
-        }
-        if (getOPtr()!=nullptr)
-        {
-            getOPtr()->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(getOPtr());
-        }
+        for (auto sb : state_block_vec_)
+            if (sb != nullptr)
+            {
+                sb->fix();
+                if (getProblem() != nullptr)
+                    getProblem()->updateStateBlockPtr(sb);
+            }
+//        if (getPPtr()!=nullptr)
+//        {
+//            getPPtr()->fix();
+//            if (getProblem() != nullptr)
+//                getProblem()->updateStateBlockPtr(getPPtr());
+//        }
+//        if (getOPtr()!=nullptr)
+//        {
+//            getOPtr()->fix();
+//            if (getProblem() != nullptr)
+//                getProblem()->updateStateBlockPtr(getOPtr());
+//        }
     }
     else if(status_ == LANDMARK_ESTIMATED)
     {
-        if (getPPtr()!=nullptr)
-        {
-            getPPtr()->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(getPPtr());
-        }
-        if (getOPtr()!=nullptr)
-        {
-            getOPtr()->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(getOPtr());
-        }
+        for (auto sb : state_block_vec_)
+            if (sb != nullptr)
+            {
+                sb->unfix();
+                if (getProblem() != nullptr)
+                    getProblem()->updateStateBlockPtr(sb);
+            }
+//        if (getPPtr()!=nullptr)
+//        {
+//            getPPtr()->unfix();
+//            if (getProblem() != nullptr)
+//                getProblem()->updateStateBlockPtr(getPPtr());
+//        }
+//        if (getOPtr()!=nullptr)
+//        {
+//            getOPtr()->unfix();
+//            if (getProblem() != nullptr)
+//                getProblem()->updateStateBlockPtr(getOPtr());
+//        }
     }
 }
 

src/landmark_base.h

@@ -39,8 +39,6 @@ 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
         Eigen::VectorXs descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
 
     public:
@@ -149,13 +147,11 @@ inline void LandmarkBase::setId(unsigned int _id)
 
 inline void LandmarkBase::fix()
 {
-    //std::cout << "Fixing frame " << nodeId() << std::endl;
     this->setStatus(LANDMARK_FIXED);
 }
 
 inline void LandmarkBase::unfix()
 {
-    //std::cout << "Unfixing frame " << nodeId() << std::endl;
     this->setStatus(LANDMARK_ESTIMATED);
 }