diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6e2dbbe0a63578c80e3f9716a416ccae852fe46f..62fbdb69c8e112d70e2f6d9540e6f37db0a757a7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -124,6 +124,7 @@ SET(HDRS_CAPTURE
SET(HDRS_COMMON
include/${PROJECT_NAME}/common/factory.h
include/${PROJECT_NAME}/common/node_base.h
+ include/${PROJECT_NAME}/common/node_state_blocks.h
include/${PROJECT_NAME}/common/time_stamp.h
include/${PROJECT_NAME}/common/wolf.h
include/${PROJECT_NAME}/common/params_base.h
@@ -182,7 +183,6 @@ SET(HDRS_MAP
)
SET(HDRS_PROBLEM
include/${PROJECT_NAME}/problem/problem.h
- # include/${PROJECT_NAME}/problem/prior_problem.h
)
SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/factory_processor.h
@@ -218,7 +218,6 @@ SET(HDRS_SOLVER
SET(HDRS_STATE_BLOCK
include/${PROJECT_NAME}/state_block/composite.h
include/${PROJECT_NAME}/state_block/factory_state_block.h
- include/${PROJECT_NAME}/state_block/has_state_blocks.h
include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
include/${PROJECT_NAME}/state_block/local_parametrization_base.h
include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
@@ -265,6 +264,7 @@ SET(SRCS_CAPTURE
)
SET(SRCS_COMMON
src/common/node_base.cpp
+ src/common/node_state_blocks.cpp
src/common/time_stamp.cpp
)
SET(SRCS_FACTOR
@@ -319,7 +319,6 @@ SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
SET(SRCS_STATE_BLOCK
- src/state_block/has_state_blocks.cpp
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 31092a82461e49d01d1f74853955046e9cc73c46..c53e6f98869161efc08b536467c9d72fbe17fd62 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -29,16 +29,15 @@ class FeatureBase;
//Wolf includes
#include "core/common/wolf.h"
-#include "core/common/node_base.h"
+#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
-#include "core/state_block/has_state_blocks.h"
//std includes
namespace wolf{
//class CaptureBase
-class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<CaptureBase>
+class CaptureBase : public NodeStateBlocks, public std::enable_shared_from_this<CaptureBase>
{
friend FeatureBase;
friend FactorBase;
@@ -47,7 +46,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
private:
FrameBaseWPtr frame_ptr_;
FeatureBasePtrList feature_list_;
- FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
static unsigned int capture_id_count_;
@@ -66,8 +64,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
- ~CaptureBase() override;
- virtual void remove(bool viral_remove_empty_parent=false);
+ ~CaptureBase() override = default;
+ void remove(bool viral_remove_parent_without_children=false) override;
+ bool hasChildren() const override;
// Type
virtual bool isMotion() const { return false; }
@@ -89,26 +88,11 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
FeatureBasePtrList getFeatureList();
bool hasFeature(const FeatureBaseConstPtr &_feature) const;
- FactorBaseConstPtrList getFactorList() const;
- FactorBasePtrList getFactorList();
- void getFactorList(FactorBaseConstPtrList& _fac_list) const;
- void getFactorList(FactorBasePtrList& _fac_list);
-
SensorBaseConstPtr getSensor() const;
SensorBasePtr getSensor();
virtual void setSensor(const SensorBasePtr sensor_ptr);
- // constrained by
- private:
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
-
public:
- unsigned int getHits() const;
- FactorBaseConstPtrList getConstrainedByList() const;
- FactorBasePtrList getConstrainedByList();
- bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
-
// State blocks
StateBlockConstPtr getStateBlock(const char& _key) const;
StateBlockPtr getStateBlock(const char& _key);
@@ -119,8 +103,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
StateBlockConstPtr getSensorIntrinsic() const;
StateBlockPtr getSensorIntrinsic();
- bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
-
void fix() override;
void unfix() override;
@@ -235,24 +217,6 @@ inline FeatureBasePtrList CaptureBase::getFeatureList()
return feature_list_;
}
-inline unsigned int CaptureBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBaseConstPtrList CaptureBase::getConstrainedByList() const
-{
- FactorBaseConstPtrList list_const;
- for (auto&& obj_ptr : constrained_by_list_)
- list_const.push_back(obj_ptr);
- return list_const;
-}
-
-inline FactorBasePtrList CaptureBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 31ae60e6c388ac9113db10ea2dfe33718b709cae..febc63cd81cbac43370bc5701d7e92b628724175 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -101,6 +101,7 @@ class NodeBase
std::string getType() const;
std::string getName() const;
bool isRemoving() const;
+ virtual bool hasChildren() const = 0;
void setType(const std::string& _type);
void setName(const std::string& _name);
diff --git a/include/core/common/node_state_blocks.h b/include/core/common/node_state_blocks.h
new file mode 100644
index 0000000000000000000000000000000000000000..43992e3faf4846772280b5e8e801571ae33b3ce2
--- /dev/null
+++ b/include/core/common/node_state_blocks.h
@@ -0,0 +1,502 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "core/common/wolf.h"
+#include "core/common/node_base.h"
+#include "core/state_block/vector_composite.h"
+#include "core/state_block/spec_composite.h"
+
+#include <map>
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(NodeStateBlocks)
+
+class NodeStateBlocks : public NodeBase
+{
+ friend Problem;
+ friend FactorBase;
+
+ public:
+ NodeStateBlocks(const std::string& _category, const std::string& _type);
+ NodeStateBlocks(const std::string& _category, const std::string& _type, const SpecStateComposite& _specs);
+ NodeStateBlocks(const std::string& _category,
+ const std::string& _type,
+ const TypeComposite& _types,
+ const VectorComposite& _vectors);
+ virtual ~NodeStateBlocks();
+
+ StateKeys getKeys() const
+ {
+ return state_block_composite_.getKeys();
+ }
+ SpecStateComposite getSpecs() const;
+ bool has(const char& _sb_key) const
+ {
+ return state_block_composite_.has(_sb_key);
+ }
+ bool has(const std::string& _keys) const
+ {
+ return state_block_composite_.has(_keys);
+ }
+
+ std::map<char, StateBlockConstPtr> getStateBlockMap() const;
+ std::map<char, StateBlockPtr> getStateBlockMap();
+ std::vector<StateBlockConstPtr> getStateBlockVec() const;
+ std::vector<StateBlockPtr> getStateBlockVec();
+
+ // Some typical shortcuts -- not all should be coded here, see notes below.
+ StateBlockConstPtr getP() const;
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getP();
+ StateBlockPtr getO();
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API
+ // in derived classes of this.
+ virtual void fix();
+ virtual void unfix();
+ virtual bool isFixed() const;
+
+ virtual StateBlockPtr addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual unsigned int removeStateBlock(const char& _sb_key);
+ bool hasStateBlock(const StateBlockConstPtr& _sb) const;
+ bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
+ bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
+ StateBlockConstPtr getStateBlock(const char& _sb_key) const;
+ StateBlockPtr getStateBlock(const char& _sb_key);
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template <typename SB, typename... Args>
+ std::shared_ptr<SB> emplaceStateBlock(const char& _sb_key,
+ ProblemPtr _problem,
+ Args&&... _args_of_derived_state_block_constructor);
+
+ // Register/remove state blocks to/from wolf::Problem
+ void setProblem(ProblemPtr _problem) override;
+ virtual void remove(bool viral_remove_parent_without_children = false);
+ bool hasChildren() const override;
+
+ // Factors
+ private:
+ FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ void removeFactor(FactorBasePtr _co_ptr);
+
+ public:
+ FactorBaseConstPtrList getFactorList() const;
+ FactorBasePtrList getFactorList();
+ void getFactorList(FactorBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
+ bool hasFactor(FactorBaseConstPtr _fac) const;
+
+ FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
+ FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
+ FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
+ FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
+
+ // States
+ VectorComposite getState(const StateKeys& _keys = "") const;
+ void setState(const VectorComposite& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state,
+ const StateKeys& _keys,
+ const std::list<SizeEigen>& _sizes,
+ const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify = true);
+ void setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify = true);
+
+ VectorXd getStateVector(const StateKeys& _keys) const;
+ unsigned int getSize(const StateKeys& _keys = "") const;
+ unsigned int getLocalSize(const StateKeys& _keys = "") const;
+
+ // Covariance
+ bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
+
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
+ void perturb(double amplitude = 0.01);
+
+ protected:
+ // transform state
+ void transform(const VectorComposite& _transformation);
+
+ void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+
+ private:
+ Composite<StateBlockPtr> state_block_composite_;
+
+ FactorBasePtrList factor_sensory_list_;
+ std::map<char,FactorBasePtr> factor_prior_map_;
+};
+
+} // namespace wolf
+
+//////////// IMPLEMENTATION /////////////
+
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+inline NodeStateBlocks::NodeStateBlocks(const std::string& _category, const std::string& _type)
+ : NodeBase(_category, _type), state_block_composite_()
+{
+ //
+}
+
+inline NodeStateBlocks::~NodeStateBlocks()
+{
+ //
+}
+
+inline std::map<char, StateBlockConstPtr> NodeStateBlocks::getStateBlockMap() const
+{
+ std::map<char, StateBlockConstPtr> map_const;
+ for (auto&& pair : state_block_composite_) map_const[pair.first] = pair.second;
+ return map_const;
+}
+
+inline std::map<char, StateBlockPtr> NodeStateBlocks::getStateBlockMap()
+{
+ return state_block_composite_;
+}
+
+inline std::vector<StateBlockConstPtr> NodeStateBlocks::getStateBlockVec() const
+{
+ std::vector<StateBlockConstPtr> sbv;
+ for (auto key : getKeys())
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
+
+inline std::vector<StateBlockPtr> NodeStateBlocks::getStateBlockVec()
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto key : getKeys())
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
+
+inline unsigned int NodeStateBlocks::removeStateBlock(const char& _sb_key)
+{
+ return state_block_composite_.erase(_sb_key);
+}
+
+template <typename SB, typename... Args>
+inline std::shared_ptr<SB> NodeStateBlocks::emplaceStateBlock(const char& _sb_key,
+ ProblemPtr _problem,
+ Args&&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_composite_.count(_sb_key) == 0 and
+ "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+
+ addStateBlock(_sb_key, sb, _problem);
+
+ return sb;
+}
+
+inline bool NodeStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
+{
+ assert(has(_sb_key) and "Cannot set a state block out of the state keys! Use addStateBlock instead.");
+ assert(state_block_composite_.count(_sb_key) > 0 and
+ "Cannot set an inexistent state block! Use addStateBlock instead.");
+ state_block_composite_.at(_sb_key) = _sb;
+ return true; // success
+}
+
+inline wolf::StateBlockConstPtr NodeStateBlocks::getStateBlock(const char& _sb_key) const
+{
+ auto it = state_block_composite_.find(_sb_key);
+ if (it != state_block_composite_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr NodeStateBlocks::getStateBlock(const char& _sb_key)
+{
+ auto it = state_block_composite_.find(_sb_key);
+ if (it != state_block_composite_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockConstPtr NodeStateBlocks::getP() const
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockPtr NodeStateBlocks::getP()
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockConstPtr NodeStateBlocks::getO() const
+{
+ return getStateBlock('O');
+}
+
+inline wolf::StateBlockPtr NodeStateBlocks::getO()
+{
+ return getStateBlock('O');
+}
+
+inline void NodeStateBlocks::fix()
+{
+ for (auto pair_key_sbp : state_block_composite_)
+ if (pair_key_sbp.second != nullptr) pair_key_sbp.second->fix();
+}
+
+inline void NodeStateBlocks::unfix()
+{
+ for (auto pair_key_sbp : state_block_composite_)
+ if (pair_key_sbp.second != nullptr) pair_key_sbp.second->unfix();
+}
+
+inline bool NodeStateBlocks::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_composite_)
+ {
+ if (pair_key_sbp.second) fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+inline void NodeStateBlocks::setState(const VectorComposite& _state, const bool _notify)
+{
+ for (const auto& pair_key_vec : _state)
+ {
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+ const auto& sb = getStateBlock(key);
+ if (!sb) WOLF_ERROR("Stateblock key ", key, " not in the keys");
+ assert(sb and "Stateblock key not in the keys");
+
+ sb->setState(vec, _notify);
+ }
+}
+
+inline void NodeStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify)
+{
+ int size = getSize(_keys);
+ assert(_state.size() == size and "In FrameBase::setState wrong state size");
+
+ unsigned int index = 0;
+ for (const char key : _keys)
+ {
+ const auto& sb = getStateBlock(key);
+ assert(sb and "Stateblock key not in the keys");
+
+ sb->setState(_state.segment(index, sb->getSize()),
+ _notify); // do not notify if state block is not estimated by the solver
+ index += sb->getSize();
+ }
+}
+
+inline void NodeStateBlocks::setState(const Eigen::VectorXd& _state,
+ const StateKeys& _keys,
+ const std::list<SizeEigen>& _sizes,
+ const bool _notify)
+{
+ SizeEigen index = 0;
+ auto size_it = _sizes.begin();
+ for (const auto& key : _keys)
+ {
+ const auto& sb = getStateBlock(key);
+ assert(sb and "Stateblock key not in the keys");
+ assert(*size_it == sb->getSize() and "State block size mismatch");
+
+ sb->setState(_state.segment(index, *size_it), _notify);
+ index += *size_it;
+ size_it++;
+ }
+}
+
+inline void NodeStateBlocks::setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify)
+{
+ auto vec_it = _vectors.begin();
+ for (const auto key : _keys)
+ {
+ const auto& sb = getStateBlock(key);
+ assert(sb and "Stateblock key not in the keys");
+ assert(vec_it->size() == sb->getSize() and "State block size mismatch");
+
+ sb->setState(*vec_it, _notify);
+ vec_it++;
+ }
+}
+
+//// _keys can be either stateblock keys of the node or a subset of this keys
+inline VectorXd NodeStateBlocks::getStateVector(const StateKeys& _keys) const
+{
+ VectorXd state(getSize(_keys));
+
+ unsigned int index = 0;
+ for (const char key : _keys)
+ {
+ const auto& sb = getStateBlock(key);
+
+ assert(sb != nullptr and "Requested StateBlock key not in the keys");
+
+ state.segment(index, sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
+ return state;
+}
+
+inline VectorComposite NodeStateBlocks::getState(const StateKeys& _keys) const
+{
+ const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
+
+ VectorComposite state;
+
+ for (const auto key : keys)
+ {
+ auto state_it = state_block_composite_.find(key);
+
+ if (state_it != state_block_composite_.end()) state.emplace(key, state_it->second->getState());
+ }
+
+ return state;
+}
+
+inline unsigned int NodeStateBlocks::getSize(const StateKeys& _keys) const
+{
+ const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
+
+ unsigned int size = 0;
+ for (const char key : keys)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb)
+ {
+ WOLF_ERROR("Stateblock key ", key, " not in the keys");
+ }
+ size += sb->getSize();
+ }
+
+ return size;
+}
+
+inline bool NodeStateBlocks::hasStateBlock(const StateBlockConstPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_composite_.begin(),
+ state_block_composite_.end(),
+ [_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
+
+ return it != state_block_composite_.end();
+}
+
+inline bool NodeStateBlocks::stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const
+{
+ const auto& it = std::find_if(state_block_composite_.begin(),
+ state_block_composite_.end(),
+ [_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
+
+ if (it != state_block_composite_.end())
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = '0'; // '0' = not found
+ return false;
+ }
+}
+
+inline unsigned int NodeStateBlocks::getLocalSize(const StateKeys& _keys) const
+{
+ StateKeys keys;
+ if (_keys == "")
+ keys = getKeys();
+ else
+ keys = _keys;
+
+ unsigned int size = 0;
+ for (const char key : keys)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb)
+ {
+ WOLF_ERROR("Stateblock key ", key, " not in the keys");
+ }
+ size += sb->getLocalSize();
+ }
+
+ return size;
+}
+
+inline FactorBasePtr NodeStateBlocks::addFactor(FactorBasePtr _fac_ptr)
+{
+ factor_list_.push_back(_fac_ptr);
+ return _fac_ptr;
+}
+
+inline void NodeStateBlocks::removeFactor(FactorBasePtr _fac_ptr)
+{
+ factor_list_.remove(_fac_ptr);
+}
+
+
+inline FactorBaseConstPtrList NodeStateBlocks::getFactorList() const
+{
+ FactorBaseConstPtrList list_const;
+ for (auto&& obj_ptr : factor_list_) list_const.push_back(obj_ptr);
+ return list_const;
+}
+
+inline FactorBasePtrList NodeStateBlocks::getFactorList()
+{
+ return factor_list_;
+}
+
+inline void NodeStateBlocks::getFactorList(FactorBaseConstPtrList& _fac_list) const
+{
+ for (auto&& obj_ptr : factor_list_) _fac_list.push_back(obj_ptr);
+}
+
+inline void NodeStateBlocks::getFactorList(FactorBasePtrList& _fac_list)
+{
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
+}
+
+inline bool NodeStateBlocks::hasFactor(FactorBaseConstPtr _factor) const
+{
+ return std::find(factor_list_.begin(),
+ factor_list_.end(),
+ _factor) != factor_list_.end();
+}
+
+inline bool NodeStateBlocks::hasChildren() const
+{
+ return not factor_list_.empty();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 82f3f1ea1869bdbec0235024b98978174305f355..3d67568866ec6e0fb923312e4546a0525dd07a4e 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -226,6 +226,10 @@ struct MatrixSizeCheck
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
+// NodeStateBlocks
+WOLF_PTR_TYPEDEFS(NodeStateBlocks);
+WOLF_LIST_TYPEDEFS(NodeStateBlocks);
+
// Problem
WOLF_PTR_TYPEDEFS(Problem);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index b5529a667335850a5c5dd09f0d64ab843d08d9d5..b4e58bb0862c8bbe2d5de11c0b1ae54ae62eeef9 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -76,15 +76,16 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
- FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
- CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
- FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
- LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+ FeatureBaseWPtr feature_ptr_; ///< Feature from which the factor was created (optional)
+ NodeStateBlocksWPtrList node_state_blocks_list_; ///< NodeStateBlocks pointer list
+ FrameBaseWPtrList frame_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_list_; ///< CaptureBase pointer list
+ LandmarkBaseWPtrList landmark_list_; ///< LandmarkBase pointer list
+ SensorBaseWPtrList sensor_list_; ///< SensorBase pointer list
protected:
unsigned int factor_id_;
@@ -98,20 +99,37 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
void setProblem(ProblemPtr) override final;
public:
+ /** \brief constructor from feature
+ **/
FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
- const FrameBasePtrList& _frame_other_list,
- const CaptureBasePtrList& _capture_other_list,
- const FeatureBasePtrList& _feature_other_list,
- const LandmarkBasePtrList& _landmark_other_list,
+ const FrameBasePtrList& _frame_list,
+ const CaptureBasePtrList& _capture_list,
+ const LandmarkBasePtrList& _landmark_list,
+ const SensorBasePtrList& _sensor_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ /** \brief constructor from measurement and noise square root information upper matrix (cholesky)
+ **/
+ FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
+ const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _measurement_sqrt_information_upper,
+ const FrameBasePtrList& _frame_list,
+ const CaptureBasePtrList& _capture_list,
+ const LandmarkBasePtrList& _landmark_list,
+ const SensorBasePtrList& _sensor_list,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorBase() override = default;
- virtual void remove(bool viral_remove_empty_parent=false);
+ virtual void remove(bool viral_remove_parent_without_children=false);
+ bool hasChildren() const override { return false;};
unsigned int id() const;
@@ -181,20 +199,10 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FeatureBaseConstPtr getFeature() const;
FeatureBasePtr getFeature();
- /** \brief Returns a pointer to its capture
- **/
- CaptureBaseConstPtr getCapture() const;
- CaptureBasePtr getCapture();
-
- /** \brief Returns a pointer to its frame
- **/
- FrameBaseConstPtr getFrame() const;
- FrameBasePtr getFrame();
-
- /** \brief Returns a pointer to its capture's sensor
- **/
- SensorBaseConstPtr getSensor() const;
- SensorBasePtr getSensor();
+ // /** \brief Returns a pointer to its capture's sensor
+ // **/
+ // SensorBaseConstPtr getSensor() const;
+ // SensorBasePtr getSensor();
/** \brief Returns the factor residual size
**/
@@ -212,40 +220,36 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
bool getApplyLossFunction() const;
- /** \brief Returns a pointer to the first frame constrained to
- **/
- FrameBaseConstPtr getFrameOther() const;
- FrameBasePtr getFrameOther();
+ // /** \brief Returns a pointer to the first frame constrained to
+ // **/
+ // FrameBaseConstPtr getFrame() const;
+ // FrameBasePtr getFrame();
- /** \brief Returns a pointer to the first capture constrained to
- **/
- CaptureBaseConstPtr getCaptureOther() const;
- CaptureBasePtr getCaptureOther();
-
- /** \brief Returns a pointer to the first feature constrained to
- **/
- FeatureBaseConstPtr getFeatureOther() const;
- FeatureBasePtr getFeatureOther();
+ // /** \brief Returns a pointer to the first capture constrained to
+ // **/
+ // CaptureBaseConstPtr getCapture() const;
+ // CaptureBasePtr getCapture();
- /** \brief Returns a pointer to the first landmark constrained to
- **/
- LandmarkBaseConstPtr getLandmarkOther() const;
- LandmarkBasePtr getLandmarkOther();
+ // /** \brief Returns a pointer to the first landmark constrained to
+ // **/
+ // LandmarkBaseConstPtr getLandmark() const;
+ // LandmarkBasePtr getLandmark();
// get pointer lists to other nodes
- FrameBaseWPtrList getFrameOtherList() { return frame_other_list_; }
- CaptureBaseWPtrList getCaptureOtherList() { return capture_other_list_; }
- FeatureBaseWPtrList getFeatureOtherList() { return feature_other_list_; }
- LandmarkBaseWPtrList getLandmarkOtherList() { return landmark_other_list_; }
- FrameBaseConstWPtrList getFrameOtherList() const;
- CaptureBaseConstWPtrList getCaptureOtherList() const;
- FeatureBaseConstWPtrList getFeatureOtherList() const;
- LandmarkBaseConstWPtrList getLandmarkOtherList() const;
-
- bool hasFrameOther(const FrameBaseConstPtr& _frm_other) const;
- bool hasCaptureOther(const CaptureBaseConstPtr& _cap_other) const;
- bool hasFeatureOther(const FeatureBaseConstPtr& _ftr_other) const;
- bool hasLandmarkOther(const LandmarkBaseConstPtr& _lmk_other) const;
+ NodeStateBlocksWPtrList getNodeStateBlocksList() { return node_state_blocks_list_; }
+ FrameBaseWPtrList getFrameList() { return frame_list_; }
+ CaptureBaseWPtrList getCaptureList() { return capture_list_; }
+ LandmarkBaseWPtrList getLandmarkList() { return landmark_list_; }
+ SensorBaseWPtrList getSensorList() { return sensor_list_; }
+ FrameBaseConstWPtrList getFrameList() const;
+ CaptureBaseConstWPtrList getCaptureList() const;
+ LandmarkBaseConstWPtrList getLandmarkList() const;
+ SensorBaseConstWPtrList getSensorList() const;
+
+ bool hasFrame(const FrameBaseConstPtr& _frm) const;
+ bool hasCapture(const CaptureBaseConstPtr& _cap) const;
+ bool hasLandmark(const LandmarkBaseConstPtr& _lmk) const;
+ bool hasSensor(const SensorBaseConstPtr& _sen) const;
/**
* @brief getProcessor
@@ -277,12 +281,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
private:
- void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
-
- /**
- * @brief setProcessor
- * @param _processor_ptr
- */
+ void setFeature(const FeatureBasePtr _ft_ptr);
void setProcessor(const ProcessorBasePtr& _processor_ptr);
};
@@ -311,16 +310,6 @@ inline unsigned int FactorBase::id() const
return factor_id_;
}
-inline FeatureBaseConstPtr FactorBase::getFeature() const
-{
- return feature_ptr_.lock();
-}
-
-inline FeatureBasePtr FactorBase::getFeature()
-{
- return feature_ptr_.lock();
-}
-
inline FactorStatus FactorBase::getStatus() const
{
return status_;
@@ -331,69 +320,65 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
-inline FrameBaseConstPtr FactorBase::getFrameOther() const
+inline FeatureBaseConstPtr FactorBase::getFeature() const
{
- if (frame_other_list_.empty()) return nullptr;
- if (frame_other_list_.front().expired()) return nullptr;
-
- return frame_other_list_.front().lock();
+ if (feature_ptr_.expired()) return nullptr;
+ return feature_ptr_.lock();
}
-inline FrameBasePtr FactorBase::getFrameOther()
+inline FeatureBasePtr FactorBase::getFeature()
{
- if (frame_other_list_.empty()) return nullptr;
- if (frame_other_list_.front().expired()) return nullptr;
-
- return frame_other_list_.front().lock();
+ if (feature_ptr_.expired()) return nullptr;
+ return feature_ptr_.lock();
}
-inline CaptureBaseConstPtr FactorBase::getCaptureOther() const
-{
- if (capture_other_list_.empty()) return nullptr;
- if (capture_other_list_.front().expired()) return nullptr;
+// inline FrameBaseConstPtr FactorBase::getFrame() const
+// {
+// if (frame_list_.empty()) return nullptr;
+// if (frame_list_.front().expired()) return nullptr;
- return capture_other_list_.front().lock();
-}
+// return frame_list_.front().lock();
+// }
-inline CaptureBasePtr FactorBase::getCaptureOther()
-{
- if (capture_other_list_.empty()) return nullptr;
- if (capture_other_list_.front().expired()) return nullptr;
+// inline FrameBasePtr FactorBase::getFrame()
+// {
+// if (frame_list_.empty()) return nullptr;
+// if (frame_list_.front().expired()) return nullptr;
- return capture_other_list_.front().lock();
-}
+// return frame_list_.front().lock();
+// }
-inline FeatureBaseConstPtr FactorBase::getFeatureOther() const
-{
- if (feature_other_list_.empty()) return nullptr;
- if (feature_other_list_.front().expired()) return nullptr;
+// inline CaptureBaseConstPtr FactorBase::getCapture() const
+// {
+// if (capture_list_.empty()) return nullptr;
+// if (capture_list_.front().expired()) return nullptr;
- return feature_other_list_.front().lock();
-}
+// return capture_list_.front().lock();
+// }
-inline FeatureBasePtr FactorBase::getFeatureOther()
-{
- if (feature_other_list_.empty()) return nullptr;
- if (feature_other_list_.front().expired()) return nullptr;
+// inline CaptureBasePtr FactorBase::getCapture()
+// {
+// if (capture_list_.empty()) return nullptr;
+// if (capture_list_.front().expired()) return nullptr;
- return feature_other_list_.front().lock();
-}
+// return capture_list_.front().lock();
+// }
-inline LandmarkBaseConstPtr FactorBase::getLandmarkOther() const
-{
- if (landmark_other_list_.empty()) return nullptr;
- if (landmark_other_list_.front().expired()) return nullptr;
+// inline LandmarkBaseConstPtr FactorBase::getLandmark() const
+// {
+// if (landmark_list_.empty()) return nullptr;
+// if (landmark_list_.front().expired()) return nullptr;
- return landmark_other_list_.front().lock();
-}
+// return landmark_list_.front().lock();
+// }
-inline LandmarkBasePtr FactorBase::getLandmarkOther()
-{
- if (landmark_other_list_.empty()) return nullptr;
- if (landmark_other_list_.front().expired()) return nullptr;
+// inline LandmarkBasePtr FactorBase::getLandmark()
+// {
+// if (landmark_list_.empty()) return nullptr;
+// if (landmark_list_.front().expired()) return nullptr;
- return landmark_other_list_.front().lock();
-}
+// return landmark_list_.front().lock();
+// }
inline ProcessorBaseConstPtr FactorBase::getProcessor() const
{
@@ -405,6 +390,11 @@ inline ProcessorBasePtr FactorBase::getProcessor()
return processor_ptr_.lock();
}
+inline void FactorBase::setFeature(const FeatureBasePtr _ft_ptr)
+{
+ feature_ptr_ = _ft_ptr;
+}
+
inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
{
processor_ptr_ = _processor_ptr;
@@ -420,34 +410,34 @@ inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpp
return measurement_sqrt_information_upper_;
}
-inline FrameBaseConstWPtrList FactorBase::getFrameOtherList() const
+inline FrameBaseConstWPtrList FactorBase::getFrameList() const
{
FrameBaseConstWPtrList list_const;
- for (auto&& obj_ptr : frame_other_list_)
+ for (auto&& obj_ptr : frame_list_)
list_const.push_back(obj_ptr);
return list_const;
}
-inline CaptureBaseConstWPtrList FactorBase::getCaptureOtherList() const
+inline CaptureBaseConstWPtrList FactorBase::getCaptureList() const
{
CaptureBaseConstWPtrList list_const;
- for (auto&& obj_ptr : capture_other_list_)
+ for (auto&& obj_ptr : capture_list_)
list_const.push_back(obj_ptr);
return list_const;
}
-inline FeatureBaseConstWPtrList FactorBase::getFeatureOtherList() const
+inline LandmarkBaseConstWPtrList FactorBase::getLandmarkList() const
{
- FeatureBaseConstWPtrList list_const;
- for (auto&& obj_ptr : feature_other_list_)
+ LandmarkBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : landmark_list_)
list_const.push_back(obj_ptr);
return list_const;
}
-inline LandmarkBaseConstWPtrList FactorBase::getLandmarkOtherList() const
+inline SensorBaseConstWPtrList FactorBase::getSensorList() const
{
- LandmarkBaseConstWPtrList list_const;
- for (auto&& obj_ptr : landmark_other_list_)
+ SensorBaseConstWPtrList list_const;
+ for (auto&& obj_ptr : sensor_list_)
list_const.push_back(obj_ptr);
return list_const;
}
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index e1a4a8ee8a68af60bf322cdc75d1ded12a448b33..a90da0e49e8342382836c6abfc9f27afd615e8f7 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -38,14 +38,13 @@ namespace wolf {
//class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
- friend FactorBase;
friend CaptureBase;
friend SensorBase;
+ friend FactorBase;
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
- FactorBasePtrList constrained_by_list_;
static unsigned int feature_id_count_;
@@ -57,7 +56,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
Eigen::VectorXd expectation_; ///< expectation
- void setProblem(ProblemPtr _problem) override final;
public:
@@ -78,9 +76,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
const Eigen::MatrixXd& _meas_uncertainty,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
- ~FeatureBase() override;
- virtual void remove(bool viral_remove_empty_parent=false);
-
+ ~FeatureBase() override = default;
+ virtual void remove(bool viral_remove_parent_without_children=false);
+ bool hasChildren() const override { return false;};
// properties
unsigned int id() const;
@@ -127,11 +125,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void getFactorList(FactorBasePtrList & _fac_list);
bool hasFactor(FactorBaseConstPtr _fac) const;
- unsigned int getHits() const;
- FactorBaseConstPtrList getConstrainedByList() const;
- FactorBasePtrList getConstrainedByList();
- bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
-
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
@@ -156,8 +149,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
void removeFactor(FactorBasePtr _co_ptr);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+
};
}
@@ -176,11 +168,6 @@ std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&...
return ftr;
}
-inline unsigned int FeatureBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
inline FactorBaseConstPtrList FeatureBase::getFactorList() const
{
FactorBaseConstPtrList list_const;
@@ -194,18 +181,6 @@ inline FactorBasePtrList FeatureBase::getFactorList()
return factor_list_;
}
-inline FactorBaseConstPtrList FeatureBase::getConstrainedByList() const
-{
- FactorBaseConstPtrList list_const;
- for (auto&& obj_ptr : constrained_by_list_)
- list_const.push_back(obj_ptr);
- return list_const;
-}
-
-inline FactorBasePtrList FeatureBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
inline unsigned int FeatureBase::id() const
{
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 5ba7ef6757528a9901ceb43b0416ef2ac8f3126c..364a0fb829bbf068b6c9d6902de549486ea5e4d4 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -31,8 +31,7 @@ class StateBlock;
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
-#include "core/common/node_base.h"
-#include "core/state_block/has_state_blocks.h"
+#include "core/common/node_state_blocks.h"
//std includes
@@ -40,7 +39,7 @@ namespace wolf {
//class FrameBase
-class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase>
+class FrameBase : public NodeStateBlocks, public std::enable_shared_from_this<FrameBase>
{
friend CaptureBase;
friend FactorBase;
@@ -48,7 +47,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
- FactorBasePtrList constrained_by_list_;
static unsigned int frame_id_count_;
@@ -90,7 +88,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
const TypeComposite& _frame_types,
const VectorComposite& _frame_vectors);
- ~FrameBase() override;
+ ~FrameBase() override = default;
// Add and remove from WOLF tree ---------------------------------
template<typename classType, typename... T>
@@ -99,7 +97,8 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
void link(TrajectoryBasePtr);
void link(ProblemPtr _prb);
- virtual void remove(bool viral_remove_empty_parent=false);
+ void remove(bool viral_remove_parent_without_children=false) override;
+ bool hasChildren() const override;
// Frame properties -----------------------------------------------
public:
@@ -119,10 +118,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
protected:
void setProblem(ProblemPtr _problem) override final;
- // States
- public:
- bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
-
// Wolf tree access ---------------------------------------------------
public:
@@ -138,15 +133,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBasePtrList getCaptureList();
bool hasCapture(const CaptureBaseConstPtr& _capture) const;
- FactorBaseConstPtrList getFactorList() const;
- FactorBasePtrList getFactorList();
- void getFactorList(FactorBaseConstPtrList& _fac_list) const;
- void getFactorList(FactorBasePtrList& _fac_list);
-
- FactorBaseConstPtrList getConstrainedByList() const;
- FactorBasePtrList getConstrainedByList();
- bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
-
template <class C>
std::shared_ptr<const C> getCaptureOfType() const;
template <class C>
@@ -171,15 +157,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
-
- FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
- FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
- FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
- FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
-
- unsigned int getHits() const;
-
// Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
bool constr_by, //
@@ -203,9 +181,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
void removeCapture(CaptureBasePtr _capt_ptr);
void setTrajectory(TrajectoryBasePtr _trj_ptr);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
- virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
-
};
} // namespace wolf
@@ -275,24 +250,6 @@ inline CaptureBasePtrList FrameBase::getCaptureList()
return capture_list_;
}
-inline unsigned int FrameBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBaseConstPtrList FrameBase::getConstrainedByList() const
-{
- FactorBaseConstPtrList list_const;
- for (auto&& obj_ptr : constrained_by_list_)
- list_const.push_back(obj_ptr);
- return list_const;
-}
-
-inline FactorBasePtrList FrameBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 29d4ba6666acb5fba05c02d69067da4acbfd4ef9..69434090adad469922429cfef23ee1dad62e2473 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -44,6 +44,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
public:
HardwareBase();
~HardwareBase() override;
+ bool hasChildren() const override;
SensorBaseConstPtrList getSensorList() const;
SensorBasePtrList getSensorList();
@@ -93,4 +94,9 @@ inline SensorBasePtrList HardwareBase::getSensorList()
return sensor_list_;
}
+inline bool HardwareBase::hasChildren() const
+{
+ return not sensor_list_.empty();
+}
+
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index ae001390ec1a4a6b0b2bfb84e45040567f5b7b12..4c1a6c956ab49061de22d1b894ece1ae64c45c00 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -29,9 +29,8 @@
//Wolf includes
#include "core/common/wolf.h"
-#include "core/common/node_base.h"
+#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
-#include "core/state_block/has_state_blocks.h"
//std includes
@@ -57,13 +56,12 @@ static LandmarkBasePtr create(const YAML::Node& _node) \
} \
//class LandmarkBase
-class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
+class LandmarkBase : public NodeStateBlocks, public std::enable_shared_from_this<LandmarkBase>
{
friend FactorBase;
private:
MapBaseWPtr map_ptr_;
- FactorBasePtrList constrained_by_list_;
static unsigned int landmark_id_count_;
protected:
@@ -99,24 +97,16 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
LandmarkBase(const YAML::Node& _n);
WOLF_LANDMARK_CREATE(LandmarkBase);
- ~LandmarkBase() override;
- virtual void remove(bool viral_remove_empty_parent=false);
+ ~LandmarkBase() override = default;
+ void remove(bool viral_remove_parent_without_children=false) override;
virtual YAML::Node toYaml() const;
// Properties
unsigned int id() const;
void setId(unsigned int _id);
- // State blocks
- bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
-
public:
- unsigned int getHits() const;
- FactorBaseConstPtrList getConstrainedByList() const;
- FactorBasePtrList getConstrainedByList();
- bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
-
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
@@ -191,22 +181,4 @@ inline void LandmarkBase::setId(unsigned int _id)
landmark_id_count_ = _id;
}
-inline unsigned int LandmarkBase::getHits() const
-{
- return constrained_by_list_.size();
-}
-
-inline FactorBaseConstPtrList LandmarkBase::getConstrainedByList() const
-{
- FactorBaseConstPtrList list_const;
- for (auto&& obj_ptr : constrained_by_list_)
- list_const.push_back(obj_ptr);
- return list_const;
-}
-
-inline FactorBasePtrList LandmarkBase::getConstrainedByList()
-{
- return constrained_by_list_;
-}
-
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 764464c43ef96b01f1bf20ce18bca3f7d29c310a..66303881b9058af3e28ff94ef7ce703ebee4c80f 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -93,7 +93,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
WOLF_MAP_CREATE(MapBase, ParamsMapBase);
- ~MapBase() override;
+ ~MapBase() override = default;
+ bool hasChildren() const override;
protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index fb077e8e9b0bd55924e2998f426da9bb14e07a0c..88ba661e7d43707ef8f13a999c841458f284ca92 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -37,9 +37,9 @@ class Loader;
//wolf includes
#include "core/common/wolf.h"
+#include "core/common/node_state_blocks.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/state_block/has_state_blocks.h"
#include "core/state_block/spec_composite.h"
#include "core/state_block/vector_composite.h"
#include "core/processor/motion_provider.h"
@@ -356,7 +356,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
- bool getCovariance(HasStateBlocksConstPtr _has_states_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
+ bool getCovariance(NodeStateBlocksConstPtr _has_states_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c6773d321645c134fd15e88833b177663e6bda79..f991f99037d6191356224f0937ceee0f853fa619 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -251,6 +251,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
~ProcessorBase() override;
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
+ bool hasChildren() const override {return false;};
unsigned int id() const;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 7e141b32af1f34954994d5aeb0abd0b0fb4d8e52..c49a7f99361665a93034820a0a4172a023fd80d4 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -31,10 +31,9 @@ class StateBlock;
//Wolf includes
#include "core/common/wolf.h"
#include "core/sensor/spec_state_sensor.h"
-#include "core/common/node_base.h"
+#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
#include "core/common/params_base.h"
-#include "core/state_block/has_state_blocks.h"
#include "yaml-schema-cpp/yaml_schema.hpp"
//std includes
@@ -105,7 +104,7 @@ struct ParamsSensorBase: public ParamsBase
};
WOLF_PTR_TYPEDEFS(SensorBase);
-class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
+class SensorBase : public NodeStateBlocks, public std::enable_shared_from_this<SensorBase>
{
friend Problem;
friend ProcessorBase;
@@ -176,15 +175,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- using HasStateBlocks::addStateBlock;
StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
StateBlockPtr getStateBlockDynamic(const char& _key);
StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts);
- bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
-
// Declare a state block as dynamic or static (with _dymanic = false)
void setStateBlockDynamic(const char& _key, bool _dynamic = true);
@@ -214,9 +210,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
FeatureBaseConstPtr getPriorFeature(char) const;
FeatureBasePtr getPriorFeature(char);
- protected:
- virtual void registerNewStateBlocks(ProblemPtr _problem) override;
-
public:
void fixExtrinsics();
@@ -331,7 +324,7 @@ inline CaptureBasePtr SensorBase::getLastCapture()
inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((features_prior_map_.find(_key) == features_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
+ NodeStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
}
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 4c16c1e0c094e9b822a547ae6fca5484ee0f13da..ed99f062652eeab40d4eaedd3fde66556f2127ec 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -48,6 +48,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
public:
TrajectoryBase();
~TrajectoryBase() override;
+ bool hasChildren() const override;
// Frames
SizeEigen size() const;
@@ -92,6 +93,11 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void getFactorList(FactorBasePtrList & _fac_list);
};
+inline bool TrajectoryBase::hasChildren() const
+{
+ return not frame_list_.empty();
+}
+
inline FrameConstPtrMap TrajectoryBase::getFrameMap() const
{
FrameConstPtrMap map_const;
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 9585cc595ee3841d09e42ddc3d57b2c346bc68c7..507e56d1d31a1cb775a9b049459967d985d2db1d 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -37,7 +37,7 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
{
n_frames = _node_input["n_frames"] .as<unsigned int>();
n_fix_first_frames = _node_input["n_fix_first_frames"] .as<unsigned int>();
- viral_remove_empty_parent = _node_input["viral_remove_empty_parent"] .as<bool>();
+ viral_remove_parent_without_children = _node_input["viral_remove_parent_without_children"] .as<bool>();
if (n_frames <= n_fix_first_frames)
throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
}
@@ -46,12 +46,12 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
return ParamsTreeManagerBase::print() + "\n"
+ "n_frames: " + toString(n_frames) + "\n"
+ "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n"
- + "viral_remove_empty_parent: " + toString(viral_remove_empty_parent) + "\n";
+ + "viral_remove_parent_without_children: " + toString(viral_remove_parent_without_children) + "\n";
}
unsigned int n_frames;
unsigned int n_fix_first_frames;
- bool viral_remove_empty_parent;
+ bool viral_remove_parent_without_children;
};
class TreeManagerSlidingWindow : public TreeManagerBase
diff --git a/schema/tree_manager/TreeManagerSlidingWindow.schema b/schema/tree_manager/TreeManagerSlidingWindow.schema
index 6424af1fbc002d85112fb4a96ded5d602a518d86..740dd6b0bf9754d3c8d8551fc63f06cf126744dc 100644
--- a/schema/tree_manager/TreeManagerSlidingWindow.schema
+++ b/schema/tree_manager/TreeManagerSlidingWindow.schema
@@ -15,7 +15,7 @@ n_fix_first_frames:
_type: unsigned int
_doc: Amount of frames fixed at the begining of the sliding window.
-viral_remove_empty_parent:
+viral_remove_parent_without_children:
_mandatory: true
_type: bool
_doc: If the other wolf nodes (landmarks) have to be removed when removing frames. Otherwise, they will remain alive but unconstrained.
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 6919fc4eb6bc3ef5017d38ef661b26f8848198f4..4c370cf56a484937b3a477e5121095643d127b7c 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -34,8 +34,7 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
- NodeBase("CAPTURE", _type),
- HasStateBlocks(),
+ NodeStateBlocks("CAPTURE", _type),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
capture_id_(++capture_id_count_),
@@ -78,48 +77,45 @@ CaptureBase::CaptureBase(const std::string& _type,
}
}
-CaptureBase::~CaptureBase()
-{
-}
-
-void CaptureBase::remove(bool viral_remove_empty_parent)
+void CaptureBase::remove(bool viral_remove_parent_without_children)
{
/* Order of removing:
* Factors are removed first, and afterwards the rest of nodes containing state blocks.
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (!is_removing_)
- {
- is_removing_ = true;
- CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
+ if (is_removing_)
+ return;
- // SensorBase::last_capture_
- if (getSensor() and getSensor()->getLastCapture() == this_C)
- getSensor()->updateLastCapture();
+ is_removing_ = true;
+ CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
- // remove downstream
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
- }
- while (!feature_list_.empty())
- {
- feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
- }
+ // SensorBase::last_capture_
+ if (getSensor() and getSensor()->getLastCapture() == this_C)
+ getSensor()->updateLastCapture();
- // Remove State Blocks
- removeStateBlocks(getProblem());
+ // remove downstream
+ while (!feature_list_.empty())
+ {
+ feature_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
+ }
- // remove from upstream
- FrameBasePtr F = frame_ptr_.lock();
- if (F)
- {
- F->removeCapture(this_C);
- if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
- F->remove(viral_remove_empty_parent); // remove upstream
- }
+ // remove from upstream
+ FrameBasePtr F = frame_ptr_.lock();
+ if (F)
+ {
+ F->removeCapture(this_C);
+ if (viral_remove_parent_without_children and not F->hasChildren())
+ F->remove(viral_remove_parent_without_children); // remove upstream
}
+
+ // Remove State Blocks
+ NodeStateBlocks::remove(viral_remove_parent_without_children);
+}
+
+bool CaptureBase::hasChildren() const
+{
+ return NodeStateBlocks::hasChildren() or not feature_list_.empty();
}
bool CaptureBase::process()
@@ -141,45 +137,6 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
feature_list_.remove(_ft_ptr);
}
-FactorBaseConstPtrList CaptureBase::getFactorList() const
-{
- FactorBaseConstPtrList fac_list;
- getFactorList(fac_list);
- return fac_list;
-}
-
-FactorBasePtrList CaptureBase::getFactorList()
-{
- FactorBasePtrList fac_list;
- getFactorList(fac_list);
- return fac_list;
-}
-
-void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
-{
- for (auto f_ptr : getFeatureList())
- if (not f_ptr->isRemoving())
- f_ptr->getFactorList(_fac_list);
-}
-
-void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
-{
- for (auto f_ptr : getFeatureList())
- if (not f_ptr->isRemoving())
- f_ptr->getFactorList(_fac_list);
-}
-
-FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- return _fac_ptr;
-}
-
-void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.remove(_fac_ptr);
-}
-
bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
{
return std::find(feature_list_.begin(),
@@ -187,24 +144,17 @@ bool CaptureBase::hasFeature(const FeatureBaseConstPtr &_feature) const
_feature) != feature_list_.end();
}
-bool CaptureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
- return std::find(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- _factor) != constrained_by_list_.end();
-}
-
StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
{
if (getSensor() and getSensor()->has(_key))
{
if (getSensor()->isStateBlockDynamic(_key))
- return HasStateBlocks::getStateBlock(_key);
+ return NodeStateBlocks::getStateBlock(_key);
else
return getSensor()->getStateBlock(_key);
}
else // No sensor associated, or sensor without this state block: assume sensor params are here
- return HasStateBlocks::getStateBlock(_key);
+ return NodeStateBlocks::getStateBlock(_key);
}
StateBlockPtr CaptureBase::getStateBlock(const char& _key)
@@ -212,27 +162,22 @@ StateBlockPtr CaptureBase::getStateBlock(const char& _key)
if (getSensor() and getSensor()->has(_key))
{
if (getSensor()->isStateBlockDynamic(_key))
- return HasStateBlocks::getStateBlock(_key);
+ return NodeStateBlocks::getStateBlock(_key);
else
return getSensor()->getStateBlock(_key);
}
else // No sensor associated, or sensor without this state block: assume sensor params are here
- return HasStateBlocks::getStateBlock(_key);
-}
-
-bool CaptureBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
-{
- return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
+ return NodeStateBlocks::getStateBlock(_key);
}
void CaptureBase::fix()
{
- HasStateBlocks::fix();
+ NodeStateBlocks::fix();
}
void CaptureBase::unfix()
{
- HasStateBlocks::unfix();
+ NodeStateBlocks::unfix();
}
void CaptureBase::move(FrameBasePtr _frm_ptr)
@@ -270,9 +215,7 @@ void CaptureBase::unlink()
if (not this->getFrame())
return;
- for (auto ftr : getFeatureList())
- assert(ftr->getFactorList().empty() && " unlinking a capture with factors!");
- assert(getConstrainedByList().empty() && " unlinking a capture constrained by factors!");
+ assert(getFactorList().empty() && " unlinking a capture constrained by factors!");
// unlink from frame
this->getFrame()->removeCapture(shared_from_this());
@@ -286,8 +229,7 @@ void CaptureBase::setProblem(ProblemPtr _problem)
assert(getFrame() && "Cannot set problem: Capture has no Frame!");
- NodeBase::setProblem(_problem);
- registerNewStateBlocks(_problem);
+ NodeStateBlocks::setProblem(_problem);
// update SensorBase::last_capture_
if (getSensor() and
@@ -313,13 +255,13 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
_stream << " -> Sen-";
_stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : getConstrainedByList())
- if (cby)
- _stream << "Fac" << cby->id() << " \t";
- }
+ // if (_constr_by)
+ // {
+ // _stream << " <-- ";
+ // for (auto cby : getConstrainedByList())
+ // if (cby)
+ // _stream << "Fac" << cby->id() << " \t";
+ // }
_stream << std::endl;
if (getStateBlockMap().size() > 0)
@@ -377,37 +319,37 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, st
log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
- // check contrained_by
- for (auto cby : getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (auto Cow : cby->getCaptureOtherList())
- _stream << " Cap" << Cow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this capture
- inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
+ // // check contrained_by
+ // for (auto cby : getConstrainedByList())
+ // {
+ // if (_verbose)
+ // {
+ // _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ // for (auto Cow : cby->getCaptureOtherList())
+ // _stream << " Cap" << Cow.lock()->id();
+ // _stream << std::endl;
+ // }
+
+ // // check constrained_by pointer to this capture
+ // inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
+ // << " not found among constrained-by factors\n";
+ // log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
+
+ // for (auto sb : cby->getStateBlockPtrVector())
+ // {
+ // if (_verbose)
+ // {
+ // _stream << _tabs << " " << "sb @ " << sb.get();
+ // if (sb)
+ // {
+ // auto lp = sb->getLocalParametrization();
+ // if (lp)
+ // _stream << " (lp @ " << lp.get() << ")";
+ // }
+ // _stream << std::endl;
+ // }
+ // }
+ // }
// check frame
auto frm_cap = _cap_ptr->getFrame();
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index d1310a3d2e8658decc19ead49d7b719b343d5ed8..1a5daac0b34e43d489d1fb17eeba10b31ce7ee10 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -109,12 +109,12 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
}
_stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
+ // if (_constr_by)
+ // {
+ // _stream << " <-- ";
+ // for (auto cby : getConstrainedByList())
+ // _stream << "Fac" << cby->id() << " \t";
+ // }
_stream << std::endl;
if (getStateBlockMap().size() > 0)
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d84a3a5ba1ad8438b9fe7888aa0739694b5c3f1e
--- /dev/null
+++ b/src/common/node_state_blocks.cpp
@@ -0,0 +1,273 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/common/node_state_blocks.h"
+#include "core/state_block/factory_state_block.h"
+
+namespace wolf
+{
+
+NodeStateBlocks::NodeStateBlocks(const std::string& _category, const std::string& _type, const SpecStateComposite& _specs) :
+ NodeBase(_category, _type)
+{
+ for (auto spec : _specs)
+ {
+ if (spec.first == 'P' and
+ spec.second.getType() != "P" and
+ spec.second.getType() != "StatePoint2d" and
+ spec.second.getType() != "StatePoint3d" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'P', the state block should be of type 'P', 'StatePoint2d' or 'StatePoint3d'");
+ }
+ if (spec.first == 'O' and
+ spec.second.getType() != "O" and
+ spec.second.getType() != "StateAngle" and
+ spec.second.getType() != "StateQuaternion" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ if (spec.first == 'V' and
+ spec.second.getType() != "V" and
+ spec.second.getType() != "StateVector2d" and
+ spec.second.getType() != "StateVector3d" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ addStateBlock(spec.first, FactoryStateBlock::create(spec.second.getType(), spec.second.getState(), spec.second.isFixed()), nullptr);
+ }
+}
+
+NodeStateBlocks::NodeStateBlocks(const std::string& _category, const std::string& _type, const TypeComposite& _types, const VectorComposite& _vectors) :
+ NodeBase(_category, _type)
+{
+ if (not _types.has(_vectors.getKeys()) or not _vectors.has(_types.getKeys()))
+ {
+ throw std::runtime_error("NodeStateBlocks::NodeStateBlocks: provided type and vector composites don't have the same structure");
+ }
+
+ for (auto type_pair : _types)
+ {
+ auto key = type_pair.first;
+ auto type = type_pair.second;
+ auto vector = _vectors.at(key);
+
+ if (key == 'P' and
+ type != "P" and
+ type != "StatePoint2d" and
+ type != "StatePoint3d" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'P', the state block should be of type 'P', 'StatePoint2d' or 'StatePoint3d'");
+ }
+ if (key == 'O' and
+ type != "O" and
+ type != "StateAngle" and
+ type != "StateQuaternion" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ if (key == 'V' and
+ type != "V" and
+ type != "StateVector2d" and
+ type != "StateVector3d" )
+ {
+ throw std::runtime_error("NodeStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ addStateBlock(key, FactoryStateBlock::create(type, vector, false), nullptr);
+ }
+}
+
+void NodeStateBlocks::remove(bool viral_remove_parent_without_children)
+{
+ for (const char key : getKeys())
+ {
+ auto sbp = getStateBlock(key);
+ if (sbp != nullptr)
+ {
+ if (getProblem())
+ {
+ getProblem()->notifyStateBlock(sbp, REMOVE);
+ }
+ removeStateBlock(key);
+ }
+ }
+}
+
+SpecStateComposite NodeStateBlocks::getSpecs() const
+{
+ SpecStateComposite specs;
+ for (auto && state_pair : state_block_composite_)
+ {
+ specs.emplace(state_pair.first, SpecState(state_pair.second->getType(),
+ state_pair.second->getState(),
+ state_pair.second->isFixed() ? "fix" : "initial_guess"));
+ }
+ return specs;
+}
+
+StateBlockPtr NodeStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
+{
+ assert(state_block_composite_.count(_sb_key) == 0 and
+ "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_composite_.emplace(_sb_key, _sb);
+
+ // conditionally register to problem
+ if(_problem)
+ _problem->notifyStateBlock(_sb, ADD);
+
+ return _sb;
+}
+
+void NodeStateBlocks::setProblem(ProblemPtr _problem)
+{
+ NodeBase::setProblem(_problem);
+
+ // register state blocks
+ if (_problem != nullptr)
+ {
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ _problem->notifyStateBlock(pair_key_sbp.second, ADD);
+ }
+}
+
+bool NodeStateBlocks::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
+{
+ if (not this->has(_keys))
+ {
+ WOLF_WARN("NodeStateBlocks::getCovariance does not have the keys ", _keys, ". Available keys: ", getKeys());
+ return false;
+ }
+
+ bool success(true);
+
+ // resizing
+ SizeEigen sz = getLocalSize();
+ _cov.resize(sz, sz);
+
+ // filling covariance
+ int i = 0, j = 0;
+ for (auto key_i : _keys)
+ {
+ auto sb_i = getStateBlock(key_i);
+ j = 0;
+ for (auto key_j : _keys)
+ {
+ auto sb_j = getStateBlock(key_j);
+ success = success && getProblem()->getCovarianceBlock(sb_i, sb_j, _cov, i, j);
+ j += sb_j->getLocalSize();
+ }
+ i += sb_i->getLocalSize();
+ }
+
+ return success;
+}
+
+FactorBaseConstPtr NodeStateBlocks::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
+{
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBasePtr NodeStateBlocks::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type)
+{
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBaseConstPtr NodeStateBlocks::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
+{
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+FactorBasePtr NodeStateBlocks::getFactorOf(ProcessorBasePtr _processor_ptr)
+{
+ for (auto factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
+ return nullptr;
+}
+
+void NodeStateBlocks::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_composite_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void NodeStateBlocks::transform(const VectorComposite& _transformation)
+{
+ for (auto& pair_key_sb : state_block_composite_)
+ pair_key_sb.second->transform(_transformation);
+}
+
+
+void NodeStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ if (_metric && _state_blocks)
+ {
+ for (auto key : getKeys())
+ {
+ auto sb = getStateBlock(key);
+ if (sb)
+ _stream << _tabs << " " << key
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << " @ " << sb << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est")
+ << ",\t x = ( " << std::setprecision(3) << getStateVector(getKeys()).transpose() << " )"
+ << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (auto key : getKeys())
+ {
+ auto sb = getStateBlock(key);
+ if (sb)
+ _stream << " " << key
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
+ << "] @ " << sb;
+ }
+ _stream << std::endl;
+ }
+}
+
+}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 9610063417bd9a283b01d90068abf51e655e2f68..d4b1079327e100d40c9e88dfe5ae597aa6076c2d 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -30,157 +30,149 @@ unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
- const FrameBasePtrList& _frame_other_list,
- const CaptureBasePtrList& _capture_other_list,
- const FeatureBasePtrList& _feature_other_list,
- const LandmarkBasePtrList& _landmark_other_list,
+ const FrameBasePtrList& _frame_list,
+ const CaptureBasePtrList& _capture_list,
+ const LandmarkBasePtrList& _landmark_list,
+ const SensorBasePtrList& _sensor_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase(_tp,
+ _top,
+ _feature_ptr->getMeasurement(),
+ _feature_ptr->getMeasurementSquareRootInformationUpper(),
+ _frame_list,
+ _capture_list,
+ _landmark_list,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
+{
+}
+
+FactorBase::FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
+ const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _measurement_sqrt_information_upper,
+ const FrameBasePtrList& _frame_list,
+ const CaptureBasePtrList& _capture_list,
+ const LandmarkBasePtrList& _landmark_list,
+ const SensorBasePtrList& _sensor_list,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(), // will be filled in link()
processor_ptr_(_processor_ptr),
- frame_other_list_(),
- capture_other_list_(),
- feature_other_list_(),
- landmark_other_list_(),
+ feature_ptr_(),
+ node_state_blocks_list_(),
+ frame_list_(),
+ capture_list_(),
+ landmark_list_(),
+ sensor_list_(),
factor_id_(++factor_id_count_),
topology_(_top),
status_(_status),
- apply_loss_function_(_apply_loss_function)
+ apply_loss_function_(_apply_loss_function),
+ measurement_(_measurement),
+ measurement_sqrt_information_upper_(_measurement_sqrt_information_upper)
{
- for (auto Fo : _frame_other_list)
- frame_other_list_.push_back(Fo);
- for (auto Co : _capture_other_list)
- capture_other_list_.push_back(Co);
- for (auto fo : _feature_other_list)
- feature_other_list_.push_back(fo);
- for (auto Lo : _landmark_other_list)
- landmark_other_list_.push_back(Lo);
-
- assert(_feature_ptr && "null feature pointer when creating a factor");
- measurement_ = _feature_ptr->getMeasurement();
- measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
+ for (auto Fo : _frame_list)
+ {
+ frame_list_.push_back(Fo);
+ node_state_blocks_list_.push_back(Fo);
+ }
+ for (auto Co : _capture_list)
+ {
+ capture_list_.push_back(Co);
+ node_state_blocks_list_.push_back(Co);
+ }
+ for (auto Lo : _landmark_list)
+ {
+ landmark_list_.push_back(Lo);
+ node_state_blocks_list_.push_back(Lo);
+ }
+ for (auto So : _sensor_list)
+ {
+ sensor_list_.push_back(So);
+ node_state_blocks_list_.push_back(So);
+ }
}
-
-void FactorBase::remove(bool viral_remove_empty_parent)
+void FactorBase::remove(bool viral_remove_parent_without_children)
{
/* Order of removing:
* Factors are removed first, and afterwards the rest of nodes containing state blocks.
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (!is_removing_)
- {
- is_removing_ = true;
- FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
-
- // add factor to be removed from solver
- if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
- getProblem()->notifyFactor(this_fac,REMOVE);
-
- // remove from upstream
- FeatureBasePtr f = feature_ptr_.lock();
- if (f)
- {
- f->removeFactor(this_fac);
- if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
- f->remove(viral_remove_empty_parent); // remove upstream
- }
-
- // remove other: {Frame, Capture, Feature, Landmark}
- for (auto& frm_ow : frame_other_list_)
- {
- auto frm_o = frm_ow.lock();
- if (frm_o)
- {
- frm_o->removeConstrainedBy(this_fac);
- if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove(viral_remove_empty_parent);
- }
- }
+ if (is_removing_)
+ return;
- for (auto& cap_ow : capture_other_list_)
- {
- auto cap_o = cap_ow.lock();
- if (cap_o)
- {
- cap_o->removeConstrainedBy(this_fac);
- if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove(viral_remove_empty_parent);
- }
- }
+ is_removing_ = true;
+ FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
- for (auto& ftr_ow : feature_other_list_)
- {
- auto ftr_o = ftr_ow.lock();
- if (ftr_o)
- {
- ftr_o->removeConstrainedBy(this_fac);
- if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove(viral_remove_empty_parent);
- }
- }
+ // add factor to be removed from solver
+ if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
+ getProblem()->notifyFactor(this_fac,REMOVE);
- for (auto& lmk_ow : landmark_other_list_)
+ // remove from upstream
+ for (auto& node_states_w : node_state_blocks_list_)
+ {
+ auto node = node_states_w.lock();
+ if (node)
{
- auto lmk_o = lmk_ow.lock();
- if (lmk_o)
- {
- lmk_o->removeConstrainedBy(this_fac);
- if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
- lmk_o->remove(viral_remove_empty_parent);
- }
+ node->removeFactor(this_fac);
+ if (viral_remove_parent_without_children and not node->hasChildren())
+ node->remove(viral_remove_parent_without_children);
}
-
- // std::cout << "Removed c" << id() << std::endl;
}
-}
-CaptureBaseConstPtr FactorBase::getCapture() const
-{
- auto ftr = getFeature();
- if (ftr != nullptr and not ftr->isRemoving())
- return ftr->getCapture();
- else return nullptr;
+ // std::cout << "Removed c" << id() << std::endl;
}
-CaptureBasePtr FactorBase::getCapture()
-{
- auto ftr = getFeature();
- if (ftr != nullptr and not ftr->isRemoving())
- return ftr->getCapture();
- else return nullptr;
-}
-
-FrameBaseConstPtr FactorBase::getFrame() const
-{
- auto cpt = getCapture();
- if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
- else return nullptr;
-}
-
-FrameBasePtr FactorBase::getFrame()
-{
- auto cpt = getCapture();
- if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
- else return nullptr;
-}
-
-SensorBaseConstPtr FactorBase::getSensor() const
-{
- auto cpt = getCapture();
- if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
- else return nullptr;
-}
-
-SensorBasePtr FactorBase::getSensor()
-{
- auto cpt = getCapture();
- if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
- else return nullptr;
-}
+// CaptureBaseConstPtr FactorBase::getCapture() const
+// {
+// auto ftr = getFeature();
+// if (ftr != nullptr and not ftr->isRemoving())
+// return ftr->getCapture();
+// else return nullptr;
+// }
+
+// CaptureBasePtr FactorBase::getCapture()
+// {
+// auto ftr = getFeature();
+// if (ftr != nullptr and not ftr->isRemoving())
+// return ftr->getCapture();
+// else return nullptr;
+// }
+
+// FrameBaseConstPtr FactorBase::getFrame() const
+// {
+// auto cpt = getCapture();
+// if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
+// else return nullptr;
+// }
+
+// FrameBasePtr FactorBase::getFrame()
+// {
+// auto cpt = getCapture();
+// if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
+// else return nullptr;
+// }
+
+// SensorBaseConstPtr FactorBase::getSensor() const
+// {
+// auto cpt = getCapture();
+// if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
+// else return nullptr;
+// }
+
+// SensorBasePtr FactorBase::getSensor()
+// {
+// auto cpt = getCapture();
+// if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
+// else return nullptr;
+// }
void FactorBase::setStatus(FactorStatus _status)
{
@@ -196,57 +188,57 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
-bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
+bool FactorBase::hasFrame(const FrameBaseConstPtr &_frm) const
{
- auto frm_it = find_if(frame_other_list_.begin(),
- frame_other_list_.end(),
- [_frm_other](const FrameBaseConstWPtr &frm_ow)
+ auto frm_it = find_if(frame_list_.begin(),
+ frame_list_.end(),
+ [_frm](const FrameBaseConstWPtr &frm_ow)
{
- return frm_ow.lock() == _frm_other;
+ return frm_ow.lock() == _frm;
});
- if (frm_it != frame_other_list_.end())
+ if (frm_it != frame_list_.end())
return true;
return false;
}
-bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
+bool FactorBase::hasCapture(const CaptureBaseConstPtr &_cap) const
{
- auto cap_it = find_if(capture_other_list_.begin(),
- capture_other_list_.end(),
- [_cap_other](const CaptureBaseConstWPtr &cap_ow)
+ auto cap_it = find_if(capture_list_.begin(),
+ capture_list_.end(),
+ [_cap](const CaptureBaseConstWPtr &cap_ow)
{
- return cap_ow.lock() == _cap_other;
+ return cap_ow.lock() == _cap;
});
- if (cap_it != capture_other_list_.end())
+ if (cap_it != capture_list_.end())
return true;
return false;
}
-bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
+bool FactorBase::hasLandmark(const LandmarkBaseConstPtr &_lmk) const
{
- auto ftr_it = find_if(feature_other_list_.begin(),
- feature_other_list_.end(),
- [_ftr_other](const FeatureBaseConstWPtr &ftr_ow)
+ auto lmk_it = find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [_lmk](const LandmarkBaseConstWPtr &lmk_ow)
{
- return ftr_ow.lock() == _ftr_other;
+ return lmk_ow.lock() == _lmk;
});
- if (ftr_it != feature_other_list_.end())
+ if (lmk_it != landmark_list_.end())
return true;
return false;
}
-bool FactorBase::hasLandmarkOther(const LandmarkBaseConstPtr &_lmk_other) const
+bool FactorBase::hasSensor(const SensorBaseConstPtr& _sen) const
{
- auto lmk_it = find_if(landmark_other_list_.begin(),
- landmark_other_list_.end(),
- [_lmk_other](const LandmarkBaseConstWPtr &lmk_ow)
+ auto sen_it = find_if(sensor_list_.begin(),
+ sensor_list_.end(),
+ [_sen](const SensorBaseConstWPtr &sen_ow)
{
- return lmk_ow.lock() == _lmk_other;
+ return sen_ow.lock() == _sen;
});
- if (lmk_it != landmark_other_list_.end())
+ if (sen_it != sensor_list_.end())
return true;
return false;
@@ -272,31 +264,16 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
this->setProblem(_ftr_ptr->getProblem());
- // constrained by
- for (auto& frm_ow : frame_other_list_)
+ // constrained node_state_blocks
+ for (auto& node_w : node_state_blocks_list_)
{
- auto frame_other = frm_ow.lock();
- if(frame_other != nullptr)
+ auto node = node_w.lock();
+ if(node != nullptr)
{
- assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
- frame_other->addConstrainedBy(shared_from_this());
+ assert(node->getProblem() && "Forbidden: linking a factor with a floating NodeStateBlocks.");
+ node->addFactor(shared_from_this());
}
}
- for (auto& cap_ow : capture_other_list_)
- {
- auto capture_other = cap_ow.lock();
- if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
- }
- for (auto& ftr_ow : feature_other_list_)
- {
- auto feature_other = ftr_ow.lock();
- if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
- }
- for (auto& lmk_ow : landmark_other_list_)
- {
- auto landmark_other = lmk_ow.lock();
- if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
- }
}
void FactorBase::setProblem(ProblemPtr _problem)
@@ -311,23 +288,16 @@ void FactorBase::setProblem(ProblemPtr _problem)
void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " -->";
- if ( getFrameOtherList() .empty()
- && getCaptureOtherList() .empty()
- && getFeatureOtherList() .empty()
- && getLandmarkOtherList().empty())
- _stream << " Abs";
-
- for (auto Fow : getFrameOtherList())
+ _stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " --> ";
+ _stream << topology_;
+
+ for (auto Fow : getFrameList())
if (!Fow.expired())
_stream << " Frm" << Fow.lock()->id();
- for (auto Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureList())
if (!Cow.expired())
_stream << " Cap" << Cow.lock()->id();
- for (auto fow : getFeatureOtherList())
- if (!fow.expired())
- _stream << " Ftr" << fow.lock()->id();
- for (auto Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkList())
if (!Low.expired())
_stream << " Lmk" << Low.lock()->id();
_stream << std::endl;
@@ -346,17 +316,8 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose)
_stream << _tabs << "Fac" << id() << " @ " << _fac_ptr.get();
- if ( getFrameOtherList() .empty()
- && getCaptureOtherList() .empty()
- && getFeatureOtherList() .empty()
- && getLandmarkOtherList().empty() ) // case ABSOLUTE:
- {
- if (_verbose)
- _stream << " --> Abs.";
- }
-
// find constrained_by pointer in constrained frame
- for (auto Fow : getFrameOtherList())
+ for (auto Fow : getFrameList())
{
if (!Fow.expired())
{
@@ -364,22 +325,22 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose)
{
_stream << " ( --> Frm" << Fo->id() << " <- ";
- for (auto cby : Fo->getConstrainedByList())
+ for (auto cby : Fo->getFactorList())
_stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained frame
- bool found = Fo->isConstrainedBy(_fac_ptr);
+ bool found = Fo->hasFactor(_fac_ptr);
inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
}
}
- if (_verbose && !getFrameOtherList().empty())
+ if (_verbose and not getFrameList().empty())
_stream << ")";
// find constrained_by pointer in constrained capture
- for (auto Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureList())
{
if (!Cow.expired())
{
@@ -388,45 +349,22 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose)
{
_stream << " ( --> Cap" << Co->id() << " <- ";
- for (auto cby : Co->getConstrainedByList())
+ for (auto cby : Co->getFactorList())
_stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained frame
- bool found = Co->isConstrainedBy(_fac_ptr);
+ bool found = Co->hasFactor(_fac_ptr);
inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
}
}
- if (_verbose && !getCaptureOtherList().empty())
+ if (_verbose and not getCaptureList().empty())
_stream << ")";
- // find constrained_by pointer in constrained feature
- for (auto fow : getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- if (_verbose)
- {
- _stream << " ( --> Ftr" << fo->id() << " <- ";
- for (auto cby : fo->getConstrainedByList())
- _stream << " Fac" << cby->id();
- }
-
- // check constrained_by pointer in constrained feature
- bool found = fo->isConstrainedBy(_fac_ptr);
- inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
- }
- }
- if (_verbose && !getFeatureOtherList().empty())
- _stream << ")";
-
// find constrained_by pointer in constrained landmark
- for (auto Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkList())
{
if (Low.expired())
{
@@ -435,19 +373,19 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
if (_verbose)
{
_stream << " ( --> Lmk" << Lo->id() << " <- ";
- for (auto cby : Lo->getConstrainedByList())
+ for (auto cby : Lo->getFactorList())
_stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained landmark
- bool found = Lo->isConstrainedBy(_fac_ptr);
+ bool found = Lo->hasFactor(_fac_ptr);
inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
}
}
- if (_verbose && !getLandmarkOtherList().empty())
+ if (_verbose and not getLandmarkList().empty())
_stream << ")";
if (_verbose)
_stream << std::endl;
@@ -520,9 +458,8 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
found = found || found_here;
}
-
// find in constrained Frame
- for (auto Fow : getFrameOtherList())
+ for (auto Fow : getFrameList())
{
if (!Fow.expired())
{
@@ -543,7 +480,7 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
// find in constrained Capture
- for (auto Cow : getCaptureOtherList())
+ for (auto Cow : getCaptureList())
{
if (!Cow.expired())
{
@@ -554,34 +491,8 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
}
}
- // find in constrained Feature
- for (auto fow : getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- // find in constrained feature's Frame
- auto foF = fo->getFrame();
- found_here = foF->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOF" << foF->id();
- found = found || found_here;
-
- // find in constrained feature's Capture
- auto foC = fo->getCapture();
- found_here = foC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOC" << foC->id();
- found = found || found_here;
-
- // find in constrained feature's Sensor
- auto foS = fo->getCapture()->getSensor();
- found_here = foS->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " FtrOS" << foS->id();
- found = found || found_here;
- }
- }
-
// find in constrained landmark
- for (auto Low : getLandmarkOtherList())
+ for (auto Low : getLandmarkList())
{
if (!Low.expired())
{
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 777e9188ec11195cc4a6568c1b4cf44f5f7b2bac..2545e46e22ff86c73be987a65d7e656b94c761ec 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -53,41 +53,32 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXd& _measu
// std::cout << "constructed +f" << id() << std::endl;
}
-FeatureBase::~FeatureBase()
-{
-// std::cout << "destructed -f" << id() << std::endl;
-}
-
-void FeatureBase::remove(bool viral_remove_empty_parent)
+void FeatureBase::remove(bool viral_remove_parent_without_children)
{
/* Order of removing:
* Factors are removed first, and afterwards the rest of nodes containing state blocks.
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (!is_removing_)
+ if (is_removing_)
+ return;
+
+ is_removing_ = true;
+ FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
+
+ // remove downstream
+ while (not factor_list_.empty())
+ {
+ factor_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
+ }
+
+ // remove from upstream
+ CaptureBasePtr C = capture_ptr_.lock();
+ if (C)
{
- is_removing_ = true;
- FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
-
- // remove downstream
- while (!factor_list_.empty())
- {
- factor_list_.front()->remove(viral_remove_empty_parent); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
- }
-
- // remove from upstream
- CaptureBasePtr C = capture_ptr_.lock();
- if (C)
- {
- C->removeFeature(this_f); // remove from upstream
- if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
- C->remove(viral_remove_empty_parent); // remove upstream
- }
+ C->removeFeature(this_f); // remove from upstream
+ if (viral_remove_parent_without_children and not C->hasChildren())
+ C->remove(viral_remove_parent_without_children); // remove upstream
}
}
@@ -102,34 +93,6 @@ void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
factor_list_.remove(_co_ptr);
}
-FrameBaseConstPtr FeatureBase::getFrame() const
-{
- return capture_ptr_.lock()->getFrame();
-}
-
-FrameBasePtr FeatureBase::getFrame()
-{
- return capture_ptr_.lock()->getFrame();
-}
-
-FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- return _fac_ptr;
-}
-
-void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.remove(_fac_ptr);
-}
-
-bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
- return std::find(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- _factor) != constrained_by_list_.end();
-}
-
bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
{
return std::find(factor_list_.begin(),
@@ -139,8 +102,8 @@ bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
- // FIXME?
- _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
+ for (auto&& obj_ptr : factor_list_)
+ _fac_list.push_back(obj_ptr);
}
void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
@@ -148,6 +111,16 @@ void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
+FrameBaseConstPtr FeatureBase::getFrame() const
+{
+ return capture_ptr_.lock()->getFrame();
+}
+
+FrameBasePtr FeatureBase::getFrame()
+{
+ return capture_ptr_.lock()->getFrame();
+}
+
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov)
{
WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
@@ -178,13 +151,6 @@ void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
measurement_ = _meas;
}
-void FeatureBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto fac : factor_list_)
- fac->setProblem(_problem);
-}
-
void FeatureBase::link(CaptureBasePtr _cpt_ptr)
{
assert(!is_removing_ && "linking a removed feature");
@@ -203,15 +169,7 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
}
void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "");
- if (_constr_by)
- {
- _stream << " <--\t";
- for (auto cby : getConstrainedByList())
- if (cby)
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
+ _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "") << std::endl;
if (_metric)
_stream << _tabs << " " << "m = ( " << std::setprecision(2) << getMeasurement().transpose()
<< " )" << std::endl;
@@ -221,10 +179,6 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
- if (_depth >= 4)
- for (auto c : getFactorList())
- if (c)
- c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
@@ -248,21 +202,21 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, st
// check contrained_by
- for (auto cby : getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (auto fow : cby->getFeatureOtherList())
- _stream << " Ftr" << fow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this feature
- inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
- }
+ // for (auto cby : getConstrainedByList())
+ // {
+ // if (_verbose)
+ // {
+ // _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ // for (auto fow : cby->getFeatureOtherList())
+ // _stream << " Ftr" << fow.lock()->id();
+ // _stream << std::endl;
+ // }
+
+ // // check constrained_by pointer to this feature
+ // inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
+ // << " not found among constrained-by factors\n";
+ // log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
+ // }
// Check capture
auto cap_ftr = _ftr_ptr->getCapture();
@@ -274,13 +228,13 @@ CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, st
log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
// Check factors
- for(auto fac : getFactorList())
- {
- inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
- << " ---> Fac" << fac->id() << " @ " << fac
- << " -X-> Ftr" << id();
- log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
- }
+ // for(auto fac : getFactorList())
+ // {
+ // inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ // << " ---> Fac" << fac->id() << " @ " << fac
+ // << " -X-> Ftr" << id();
+ // log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
+ // }
return log;
}
@@ -290,7 +244,7 @@ bool FeatureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbos
auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
_log.compose(local_log);
- for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+ // for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 3a27fd90d3ad5cad30cc59e3a7609bdce6abbd95..6ff34d69b1f01651d11dd4993b01de1f4f308266 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -37,8 +37,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(),
+ NodeStateBlocks("FRAME", "FrameBase"),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
@@ -60,8 +59,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
FrameBase::FrameBase(const TimeStamp& _ts,
const SpecStateComposite& _frame_specs) :
- NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(_frame_specs),
+ NodeStateBlocks("FRAME", "FrameBase",_frame_specs),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
@@ -72,51 +70,46 @@ FrameBase::FrameBase(const TimeStamp& _ts,
FrameBase::FrameBase(const TimeStamp& _ts,
const TypeComposite& _frame_types,
const VectorComposite& _frame_vectors) :
- NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(_frame_types, _frame_vectors),
+ NodeStateBlocks("FRAME", "FrameBase",_frame_types, _frame_vectors),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
{
}
-
-FrameBase::~FrameBase()
-{
-}
-
-void FrameBase::remove(bool viral_remove_empty_parent)
+void FrameBase::remove(bool viral_remove_parent_without_children)
{
/* Order of removing:
* Factors are removed first, and afterwards the rest of nodes containing state blocks.
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (!is_removing_)
+ if (is_removing_)
+ return;
+
+ is_removing_ = true;
+ FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
+
+ // remove downstream
+ while (!capture_list_.empty())
{
- is_removing_ = true;
- FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
+ capture_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
+ }
- // remove downstream
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
- }
- while (!capture_list_.empty())
- {
- capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
- }
+ // remove from upstream
+ TrajectoryBasePtr T = trajectory_ptr_.lock();
+ if (T)
+ {
+ T->removeFrame(this_F); // remove from upstream
+ }
- // Remove Frame State Blocks
- removeStateBlocks(getProblem());
+ // Remove Frame State Blocks
+ NodeStateBlocks::remove(viral_remove_parent_without_children);
+}
- // remove from upstream
- TrajectoryBasePtr T = trajectory_ptr_.lock();
- if (T)
- {
- T->removeFrame(this_F); // remove from upstream
- }
- }
+bool FrameBase::hasChildren() const
+{
+ return NodeStateBlocks::hasChildren() or not capture_list_.empty();
}
void FrameBase::setTimeStamp(const TimeStamp& _ts)
@@ -130,11 +123,6 @@ void FrameBase::link(ProblemPtr _prb)
this->link(_prb->getTrajectory());
}
-bool FrameBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
-{
- return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
-}
-
FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
@@ -261,86 +249,6 @@ CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr)
return captures;
}
-FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
-{
- for (auto factor_ptr : getConstrainedByList())
- if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
- return factor_ptr;
-
- for (auto factor_ptr : getFactorList())
- if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
- return factor_ptr;
-
- return nullptr;
-}
-
-FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type)
-{
- for (auto factor_ptr : getConstrainedByList())
- if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
- return factor_ptr;
-
- for (auto factor_ptr : getFactorList())
- if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
- return factor_ptr;
-
- return nullptr;
-}
-
-FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
-{
- for (auto factor_ptr : getConstrainedByList())
- if (factor_ptr->getProcessor() == _processor_ptr)
- return factor_ptr;
-
- for (auto factor_ptr : getFactorList())
- if (factor_ptr->getProcessor() == _processor_ptr)
- return factor_ptr;
-
- return nullptr;
-}
-
-FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr)
-{
- for (auto factor_ptr : getConstrainedByList())
- if (factor_ptr->getProcessor() == _processor_ptr)
- return factor_ptr;
-
- for (auto factor_ptr : getFactorList())
- if (factor_ptr->getProcessor() == _processor_ptr)
- return factor_ptr;
-
- return nullptr;
-}
-
-FactorBaseConstPtrList FrameBase::getFactorList() const
-{
- FactorBaseConstPtrList fac_list;
- getFactorList(fac_list);
- return fac_list;
-}
-
-FactorBasePtrList FrameBase::getFactorList()
-{
- FactorBasePtrList fac_list;
- getFactorList(fac_list);
- return fac_list;
-}
-
-void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
-{
- for (auto c_ptr : getCaptureList())
- if (not c_ptr->isRemoving())
- c_ptr->getFactorList(_fac_list);
-}
-
-void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
-{
- for (auto c_ptr : getCaptureList())
- if (not c_ptr->isRemoving())
- c_ptr->getFactorList(_fac_list);
-}
-
bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
{
return std::find(capture_list_.begin(),
@@ -348,24 +256,6 @@ bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
_capture) != capture_list_.end();
}
-FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- return _fac_ptr;
-}
-
-void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.remove(_fac_ptr);
-}
-
-bool FrameBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
- return std::find(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- _factor) != constrained_by_list_.end();
-}
-
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
@@ -388,8 +278,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr || _problem == this->getProblem())
return;
- NodeBase::setProblem(_problem);
- registerNewStateBlocks(_problem);
+ NodeStateBlocks::setProblem(_problem);
for (auto cap : capture_list_)
cap->setProblem(_problem);
@@ -401,13 +290,13 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
<< " " << getKeys()
<< " ts = " << std::setprecision(3) << getTimeStamp()
<< ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : getConstrainedByList())
- if (cby)
- _stream << "Fac" << cby->id() << " \t";
- }
+ // if (_constr_by)
+ // {
+ // _stream << " <-- ";
+ // for (auto cby : getConstrainedByList())
+ // if (cby)
+ // _stream << "Fac" << cby->id() << " \t";
+ // }
_stream << std::endl;
printState(_metric, _state_blocks, _stream, _tabs);
@@ -463,34 +352,34 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
// check constrained_by
- for (auto cby : getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
- for (auto Fow : cby->getFrameOtherList())
- _stream << " Frm" << Fow.lock()->id() << std::endl;
-
-
- // check constrained_by pointer to this frame
- inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFrameOther(_frm_ptr)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (_verbose) {
- _stream << _tabs << " " << "sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
- }
+ // for (auto cby : getConstrainedByList())
+ // {
+ // if (_verbose)
+ // {
+ // _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ // for (auto Fow : cby->getFrameOtherList())
+ // _stream << " Frm" << Fow.lock()->id() << std::endl;
+
+
+ // // check constrained_by pointer to this frame
+ // inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
+ // << " not found among constrained-by factors\n";
+ // log.assertTrue((cby->hasFrameOther(_frm_ptr)), inconsistency_explanation);
+
+ // for (auto sb : cby->getStateBlockPtrVector())
+ // {
+ // if (_verbose) {
+ // _stream << _tabs << " " << "sb @ " << sb.get();
+ // if (sb) {
+ // auto lp = sb->getLocalParametrization();
+ // if (lp)
+ // _stream << " (lp @ " << lp.get() << ")";
+ // }
+ // _stream << std::endl;
+ // }
+ // }
+ // }
+ // }
// Trajectory
auto trj_ptr = getTrajectory();
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index eaa9373b1efce14ed4e44f5e8b27f6cdf3c5c387..cfbe6ccc9a8996589d9193e97a711a5c40b48456 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -35,8 +35,7 @@ namespace wolf {
unsigned int LandmarkBase::landmark_id_count_ = 0;
LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
- NodeBase("LANDMARK", _type),
- HasStateBlocks(),
+ NodeStateBlocks("LANDMARK", _type),
map_ptr_(),
landmark_id_(++landmark_id_count_)
{
@@ -52,57 +51,39 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
}
LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n) :
- NodeBase("LANDMARK", _type),
- HasStateBlocks(SpecStateComposite(_n["states"])),
+ NodeStateBlocks("LANDMARK", _type),
map_ptr_(),
landmark_id_(_n["id"].as<int>())
{
}
LandmarkBase::LandmarkBase(const YAML::Node& _n) :
- NodeBase("LANDMARK", "LandmarkBase"),
- HasStateBlocks(SpecStateComposite(_n["states"])),
+ NodeStateBlocks("LANDMARK", _n["type"].as<std::string>(), SpecStateComposite(_n["states"])),
map_ptr_(),
landmark_id_(_n["id"].as<int>())
{
}
-LandmarkBase::~LandmarkBase()
-{
- removeStateBlocks(getProblem());
-}
-
-void LandmarkBase::remove(bool viral_remove_empty_parent)
+void LandmarkBase::remove(bool viral_remove_parent_without_children)
{
/* Order of removing:
* Factors are removed first, and afterwards the rest of nodes containing state blocks.
* In case multi-threading, solver can be called while removing.
* This order avoids SolverManager to ignore notifications (efficiency)
*/
- if (!is_removing_)
- {
- is_removing_ = true;
- LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
+ if (not is_removing_)
+ return;
- // remove constrained by
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent);
- }
+ is_removing_ = true;
+ LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
- // Remove State Blocks
- removeStateBlocks(getProblem());
+ // remove from upstream
+ auto M = map_ptr_.lock();
+ if (M)
+ M->removeLandmark(this_L);
- // remove from upstream
- auto M = map_ptr_.lock();
- if (M)
- M->removeLandmark(this_L);
- }
-}
-
-bool LandmarkBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
-{
- return getProblem()->getCovariance(shared_from_this(), _keys,_cov);
+ // Remove State Blocks
+ NodeStateBlocks::remove(viral_remove_parent_without_children);
}
YAML::Node LandmarkBase::toYaml() const
@@ -110,7 +91,7 @@ YAML::Node LandmarkBase::toYaml() const
YAML::Node node;
node["id"] = landmark_id_;
node["type"] = node_type_;
- node["states"] = HasStateBlocks::getSpecs().toYaml();
+ node["states"] = NodeStateBlocks::getSpecs().toYaml();
node["transformable"] = getP()->isTransformable();
return node;
}
@@ -137,38 +118,19 @@ void LandmarkBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr || _problem == this->getProblem())
return;
- NodeBase::setProblem(_problem);
- registerNewStateBlocks(_problem);
-}
-
-FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- return _fac_ptr;
-}
-
-void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.remove(_fac_ptr);
-}
-
-bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
- return std::find(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- _factor) != constrained_by_list_.end();
+ NodeStateBlocks::setProblem(_problem);
}
void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Lmk" << id() << " " << getType();
- if (_constr_by)
- {
- _stream << "\t<-- ";
- for (auto cby : getConstrainedByList())
- if (cby)
- _stream << "Fac" << cby->id() << " \t";
- }
+ // if (_constr_by)
+ // {
+ // _stream << "\t<-- ";
+ // for (auto cby : getConstrainedByList())
+ // if (cby)
+ // _stream << "Fac" << cby->id() << " \t";
+ // }
_stream << std::endl;
printState(_metric, _state_blocks, _stream, _tabs);
@@ -212,39 +174,39 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr,
log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
// check constrained-by factors
- for (auto cby : getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
-
- for (auto Low : cby->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- auto Lo = Low.lock();
- _stream << " Lmk" << Lo->id();
- }
- }
- _stream << std::endl;
- }
-
- inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector()) {
- if (_verbose) {
- _stream << _tabs << " " << "sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
+ // for (auto cby : getConstrainedByList())
+ // {
+ // if (_verbose)
+ // {
+ // _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
+
+ // for (auto Low : cby->getLandmarkOtherList())
+ // {
+ // if (!Low.expired())
+ // {
+ // auto Lo = Low.lock();
+ // _stream << " Lmk" << Lo->id();
+ // }
+ // }
+ // _stream << std::endl;
+ // }
+
+ // inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
+ // << " not found among constrained-by factors\n";
+ // log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
+
+ // for (auto sb : cby->getStateBlockPtrVector()) {
+ // if (_verbose) {
+ // _stream << _tabs << " " << "sb @ " << sb.get();
+ // if (sb) {
+ // auto lp = sb->getLocalParametrization();
+ // if (lp)
+ // _stream << " (lp @ " << lp.get() << ")";
+ // }
+ // _stream << std::endl;
+ // }
+ // }
+ // }
// check map
inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 616b9eaa4404cbd9be34806e20509d9ace328486..c90a7fb3e9c2a81453439914722f37e8377f71d0 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -49,9 +49,9 @@ MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
// std::cout << "constructed M"<< std::endl;
}
-MapBase::~MapBase()
+bool MapBase::hasChildren() const
{
-// std::cout << "destructed -M" << std::endl;
+ return not landmark_list_.empty();
}
LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 953748504650a14f91ec12577a454c368cc5a826..f5f205d073b4ee665eb8ebb20ada136b9552958f 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1008,7 +1008,7 @@ bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _co
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
-bool Problem::getCovariance(HasStateBlocksConstPtr _has_blocks, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
+bool Problem::getCovariance(NodeStateBlocksConstPtr _has_blocks, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
{
if (not _has_blocks)
{
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 483c29fd4a9721a7263ec030093874151b6a5de8..3ecc6ffc9f94682113b575110f82ad92894dc999 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -35,8 +35,7 @@ SensorBase::SensorBase(const std::string& _type,
const int& _dim,
ParamsSensorBasePtr _params,
const SpecStateSensorComposite& _priors) :
- NodeBase("SENSOR", _type, _params->name),
- HasStateBlocks(),
+ NodeStateBlocks("SENSOR", _type),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
params_(_params),
@@ -45,6 +44,8 @@ SensorBase::SensorBase(const std::string& _type,
last_capture_(nullptr),
dim_(_dim)
{
+ setName(_params->name);
+
// Iterate all keys in _priors
for (auto state_pair : _priors)
{
@@ -78,7 +79,7 @@ SensorBase::SensorBase(const std::string& _type,
SensorBase::~SensorBase()
{
// Remove State Blocks
- removeStateBlocks(getProblem());
+ NodeStateBlocks::remove();
}
void SensorBase::fixExtrinsics()
@@ -199,21 +200,6 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
features_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
-{
- if (_problem != nullptr)
- {
- for (auto & pair_key_sbp : getStateBlockMap())
- {
- auto key = pair_key_sbp.first;
- auto sbp = pair_key_sbp.second;
-
- if (sbp and not isStateBlockDynamic(key))
- _problem->notifyStateBlock(sbp, ADD);
- }
- }
-}
-
void SensorBase::setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std)
{
drift_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal();
@@ -365,11 +351,6 @@ StateBlockPtr SensorBase::getIntrinsic()
return getStateBlockDynamic('I');
}
-bool SensorBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
-{
- return getProblem()->getCovariance(shared_from_this(), _keys, _cov);
-}
-
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensor(shared_from_this());
@@ -512,11 +493,9 @@ bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts) c
void SensorBase::setProblem(ProblemPtr _problem)
{
- NodeBase::setProblem(_problem);
+ NodeStateBlocks::setProblem(_problem);
for (auto prc : processor_list_)
prc->setProblem(_problem);
- for(auto ftr_prior : features_prior_map_)
- ftr_prior.second->setProblem(_problem);
}
void SensorBase::link(HardwareBasePtr _hw_ptr)
@@ -528,7 +507,6 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
this->setHardware(_hw_ptr);
this->getHardware()->addSensor(shared_from_this());
this->setProblem(_hw_ptr->getProblem());
- this->registerNewStateBlocks(getProblem());
}
else
{
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index e7cdbb8fbbe5a3d490a3cff1c015b55c0c2fc2f1..4850cb84d944adcb853defa363660c8b594d8a67 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -55,7 +55,7 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
if (remove_first)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
- getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent);
+ getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_parent_without_children);
}
}
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 6ec268ddbc1d353617de5b867dcdd0ff5f39f87a..4023e1e631e1e98935fee6dec2aec2b0d5505612 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -75,7 +75,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
}
// remove frame
- remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent);
+ remove_recent_frame->remove(params_swdr_->viral_remove_parent_without_children);
}
// Call tree manager sliding window
// It will remove oldest frame if tfirst recent frame has been kept
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index 1f24d9b0325f1ab8237ce0003f2a91a0bb3b959f..cc8f5a612c61551b9642ab6c6a4bbe0debd2426c 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -17,7 +17,7 @@ problem:
n_frames_recent: 3
rate_old_frames: 2
n_fix_first_frames: 0
- viral_remove_empty_parent: false
+ viral_remove_parent_without_children: false
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index f69fece0398f4b9f9b4d651828a29abcf26c8ad3..9bea8c5c4fbed78d978368fea3e5ac1776e7c200 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -19,7 +19,7 @@ problem:
n_frames_recent: 3
rate_old_frames: 2
n_fix_first_frames: 2
- viral_remove_empty_parent: true
+ viral_remove_parent_without_children: true
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/tree_manager_sliding_window1.yaml b/test/yaml/tree_manager_sliding_window1.yaml
index 48234b6c892ef024b87a19cefbe7c071ba895f33..a18d33a5a18f69827a48465e769aa38e018a7844 100644
--- a/test/yaml/tree_manager_sliding_window1.yaml
+++ b/test/yaml/tree_manager_sliding_window1.yaml
@@ -2,4 +2,4 @@ type: "TreeManagerSlidingWindow"
plugin: "core"
n_frames: 3
n_fix_first_frames: 2
-viral_remove_empty_parent: true
+viral_remove_parent_without_children: true
diff --git a/test/yaml/tree_manager_sliding_window2.yaml b/test/yaml/tree_manager_sliding_window2.yaml
index 7f074d9e42a8f235b1620e4b6a851b54a3071a8c..bd21b0bdc6ff510c0b8ad34498eb7a9efdb0a488 100644
--- a/test/yaml/tree_manager_sliding_window2.yaml
+++ b/test/yaml/tree_manager_sliding_window2.yaml
@@ -2,4 +2,4 @@ type: "TreeManagerSlidingWindow"
plugin: "core"
n_frames: 3
n_fix_first_frames: 0
-viral_remove_empty_parent: false
\ No newline at end of file
+viral_remove_parent_without_children: false
\ No newline at end of file
diff --git a/test/yaml/tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/tree_manager_sliding_window_dual_rate1.yaml
index d18a4fb29a0259425c7c6ed62aad178dfa156a8f..3a69a9206e0a6d4740bbc0254bfe3fc12c3a1800 100644
--- a/test/yaml/tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/tree_manager_sliding_window_dual_rate1.yaml
@@ -4,4 +4,4 @@ n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
n_fix_first_frames: 2
-viral_remove_empty_parent: true
+viral_remove_parent_without_children: true