From 33a5c232869d7ea050fe5581ef5999d5d323cf56 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 12 Jul 2016 12:50:26 +0200
Subject: [PATCH] LandmarkBase: Removed constrained_by_list_ already in
NodeConstrained
---
src/landmark_base.cpp | 1 -
src/landmark_base.h | 4 +---
src/landmark_polyline_2D.cpp | 6 +++++-
3 files changed, 6 insertions(+), 5 deletions(-)
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 19abc1a98..bf38f52a1 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 51c6dae56..c0a02aa68 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 6e2c7838e..c3a017d64 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;
--
GitLab