diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp index 19abc1a98f01bb99e2784e544fbd0ca220c0bf61..bf38f52a159933ba3fc81604985d3fd005672816 100644 --- a/src/landmark_base.cpp +++ b/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_({}) { // } diff --git a/src/landmark_base.h b/src/landmark_base.h index 51c6dae5668c8257e1b9c1bffb4513ce02115fb8..c0a02aa6830e36b8d69e8487f7b7eba304a84fc8 100644 --- a/src/landmark_base.h +++ b/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(); } diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp index 6e2c7838eb4a945d52d389852159b9b574414543..c3a017d6477156d3817196670846a968cf14ca7a 100644 --- a/src/landmark_polyline_2D.cpp +++ b/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;