From 3b23ce6a23c3cefef01ca0aac518e9aa1fc6a379 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 3 Nov 2016 21:04:50 +0100
Subject: [PATCH] Re-code setStatus() with a for loop over state_blk_vec
---
src/landmark_base.cpp | 68 +++++++++++++++++++++++++------------------
src/landmark_base.h | 4 ---
2 files changed, 39 insertions(+), 33 deletions(-)
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index e80ac3e99..42266a575 100644
--- a/src/landmark_base.cpp
+++ b/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());
+// }
}
}
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 43cd76cc0..12e127656 100644
--- a/src/landmark_base.h
+++ b/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);
}
--
GitLab