LandmarkBase: Removed constrained_by_list_ already in NodeConstrained

src/landmark_base.cpp

@@ -16,7 +16,6 @@ LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBlock* _p_ptr, StateBl
             status_(LANDMARK_CANDIDATE),
 			p_ptr_(_p_ptr),
 			o_ptr_(_o_ptr)
-//			constrained_by_list_({})
 {
     //
 }

src/landmark_base.h

@@ -34,8 +34,6 @@ class LandmarkBase : public NodeConstrained<MapBase, NodeTerminus>
         StateBlock* p_ptr_;     ///< Position state unit pointer
         StateBlock* o_ptr_;     ///< Orientation state unit pointer
         Eigen::VectorXs descriptor_;    //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
-        ConstraintBaseList constrained_by_list_; ///< List of constraints linked to this landmark
-
 
     public:
 
@@ -138,7 +136,7 @@ inline void LandmarkBase::unfix()
 inline void LandmarkBase::removeConstrainedBy(ConstraintBase* _ctr_ptr)
 {
     NodeConstrained::removeConstrainedBy(_ctr_ptr);
-    if (constrained_by_list_.empty())
+    if (getConstrainedByListPtr()->empty())
         this->destruct();
 }
 

src/landmark_polyline_2D.cpp

@@ -164,22 +164,26 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
     assert((_back ? !last_defined_: !first_defined_) && "defining an already defined extreme");
     assert(state->hasLocalParametrization() && "not defined extreme without local parameterization");
 
+    //std::cout << "Defining extreme --> Removing and adding state blocks and constraints" << std::endl;
+
     // remove and add state block without local parameterization
     if (getProblem() != nullptr)
     	getProblem()->removeStateBlockPtr(state);
+
     state->removeLocalParametrization();
 
     if (getProblem() != nullptr)
     	getProblem()->addStateBlockPtr(state);
 
     // remove and add all constraints to the point
-    for (auto ctr_ptr : constrained_by_list_)
+    for (auto ctr_ptr : *getConstrainedByListPtr())
         for (auto st_ptr : ctr_ptr->getStatePtrVector())
             if (st_ptr == state && getProblem() != nullptr)
             {
                 getProblem()->removeConstraintPtr(ctr_ptr);
                 getProblem()->addConstraintPtr(ctr_ptr);
             }
+
     // update boolean
     if (_back)
         last_defined_ = true;