Remove status_ from Landmark

src/examples/test_simple_AHP.cpp

@@ -155,7 +155,7 @@ int main(int argc, char** argv)
     // Landmark--------------------
     LandmarkAHPPtr lmk_1 = std::make_shared<LandmarkAHP>(lmk_hmg_c, kf_1, camera, desc);
     problem->addLandmark(lmk_1);
-    lmk_1->setStatus(LANDMARK_FIXED);
+    lmk_1->fix();
     std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
 
     // Constraints------------------

src/landmark_base.cpp

@@ -14,8 +14,9 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
             map_ptr_(),
             state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
             is_removing_(false),
-            landmark_id_(++landmark_id_count_),
-            status_(LANDMARK_ESTIMATED)
+            landmark_id_(++landmark_id_count_)
+//,
+//            status_(LANDMARK_ESTIMATED)
 {
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
@@ -52,32 +53,32 @@ void LandmarkBase::remove()
 }
 
 
-void LandmarkBase::setStatus(LandmarkStatus _st)
-{
-    status_ = _st;
-
-    // State Blocks
-    if (status_ == LANDMARK_FIXED)
-    {
-        for (auto sb : state_block_vec_)
-            if (sb != nullptr)
-            {
-                sb->fix();
-                if (getProblem() != nullptr)
-                    getProblem()->updateStateBlockPtr(sb);
-            }
-    }
-    else if(status_ == LANDMARK_ESTIMATED)
-    {
-        for (auto sb : state_block_vec_)
-            if (sb != nullptr)
-            {
-                sb->unfix();
-                if (getProblem() != nullptr)
-                    getProblem()->updateStateBlockPtr(sb);
-            }
-    }
-}
+//void LandmarkBase::setStatus(LandmarkStatus _st)
+//{
+//    status_ = _st;
+//
+//    // State Blocks
+//    if (status_ == LANDMARK_FIXED)
+//    {
+//        for (auto sb : state_block_vec_)
+//            if (sb != nullptr)
+//            {
+//                sb->fix();
+//                if (getProblem() != nullptr)
+//                    getProblem()->updateStateBlockPtr(sb);
+//            }
+//    }
+//    else if(status_ == LANDMARK_ESTIMATED)
+//    {
+//        for (auto sb : state_block_vec_)
+//            if (sb != nullptr)
+//            {
+//                sb->unfix();
+//                if (getProblem() != nullptr)
+//                    getProblem()->updateStateBlockPtr(sb);
+//            }
+//    }
+//}
 
 void LandmarkBase::registerNewStateBlocks()
 {

src/landmark_base.h

@@ -19,12 +19,12 @@ class StateBlock;
 
 namespace wolf {
 
-typedef enum
-{
-//    LANDMARK_CANDIDATE = 1,   ///< A landmark, just created. Association with it allowed, but not yet establish an actual constraint for the solver
-    LANDMARK_ESTIMATED,   ///< A landmark being estimated. Association with it allowed, establishing actual constraints for the solver where both vehicle and landmark states are being estimated
-    LANDMARK_FIXED,       ///< A landmark estimated. Association with it allowed, establishing actual constraints for the solver, but its value remains static, no longer optimized
-} LandmarkStatus;
+//typedef enum
+//{
+////    LANDMARK_CANDIDATE = 1,   ///< A landmark, just created. Association with it allowed, but not yet establish an actual constraint for the solver
+//    LANDMARK_ESTIMATED,   ///< A landmark being estimated. Association with it allowed, establishing actual constraints for the solver where both vehicle and landmark states are being estimated
+//    LANDMARK_FIXED,       ///< A landmark estimated. Association with it allowed, establishing actual constraints for the solver, but its value remains static, no longer optimized
+//} LandmarkStatus;
 
 
 
@@ -42,7 +42,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
     protected:
         unsigned int landmark_id_; ///< landmark unique id
-        LandmarkStatus status_; ///< status of the 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)
         Eigen::VectorXs descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
 
@@ -66,9 +66,10 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         void setId(unsigned int _id);
 
         // Fix / unfix
-        void setStatus(LandmarkStatus _st);
+//        void setStatus(LandmarkStatus _st);
         void fix();
         void unfix();
+        bool isFixed() const;
 
         // State blocks
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
@@ -151,14 +152,47 @@ inline void LandmarkBase::setId(unsigned int _id)
 
 inline void LandmarkBase::fix()
 {
-    this->setStatus(LANDMARK_FIXED);
+    for( auto sbp : state_block_vec_)
+        if (sbp != nullptr)
+        {
+            sbp->fix();
+            if (getProblem() != nullptr)
+                getProblem()->updateStateBlockPtr(sbp);
+        }
 }
 
 inline void LandmarkBase::unfix()
 {
-    this->setStatus(LANDMARK_ESTIMATED);
+    for( auto sbp : state_block_vec_)
+        if (sbp != nullptr)
+        {
+            sbp->unfix();
+            if (getProblem() != nullptr)
+                getProblem()->updateStateBlockPtr(sbp);
+        }
 }
 
+inline bool LandmarkBase::isFixed() const
+{
+    bool fixed = true;
+    for (auto sb : getStateBlockVec())
+    {
+        if (sb)
+            fixed &= sb->isFixed();
+    }
+    return fixed;
+}
+
+//inline void LandmarkBase::fix()
+//{
+//    this->setStatus(LANDMARK_FIXED);
+//}
+//
+//inline void LandmarkBase::unfix()
+//{
+//    this->setStatus(LANDMARK_ESTIMATED);
+//}
+
 inline ConstraintBasePtr LandmarkBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
 {
     constrained_by_list_.push_back(_ctr_ptr);