From 3b2d8e1f2227277c7a3f92dba4066d3ddff14d79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Tue, 4 Jun 2019 16:22:35 +0200 Subject: [PATCH] Removed unlink() and private add/remove/set upper and lower and constrainedby nodes --- include/core/capture/capture_base.h | 6 ++- include/core/factor/factor_base.h | 28 ++++------ include/core/feature/feature_base.h | 2 +- include/core/frame/frame_base.h | 13 +---- include/core/hardware/hardware_base.h | 2 +- include/core/landmark/landmark_base.h | 3 +- include/core/map/map_base.h | 2 +- include/core/processor/processor_base.h | 2 +- include/core/sensor/sensor_base.h | 2 +- include/core/trajectory/trajectory_base.h | 2 +- src/capture/capture_base.cpp | 16 +----- src/factor/factor_base.cpp | 64 +---------------------- src/feature/feature_base.cpp | 17 +----- src/frame/frame_base.cpp | 63 +--------------------- src/landmark/landmark_base.cpp | 16 +----- src/processor/processor_base.cpp | 7 +-- src/sensor/sensor_base.cpp | 7 +-- test/gtest_frame_base.cpp | 4 -- 18 files changed, 31 insertions(+), 225 deletions(-) diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 7045ff513..f0670bfa6 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -60,8 +60,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void setTimeStampToNow(); FrameBasePtr getFrame() const; + private: void setFrame(const FrameBasePtr _frm_ptr); + public: virtual void setProblem(ProblemPtr _problem) final; FeatureBasePtrList& getFeatureList(); @@ -71,7 +73,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture virtual void setSensor(const SensorBasePtr sensor_ptr); // constrained by - protected: + private: virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); virtual void removeConstrainedBy(FactorBasePtr _fac_ptr); public: @@ -102,13 +104,13 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture virtual Eigen::VectorXs getCalibration() const; void setCalibration(const Eigen::VectorXs& _calib); void link(FrameBasePtr); - void unlink(); template<typename classType, typename... T> static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all); protected: SizeEigen computeCalibSize() const; + private: FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr); void removeFeature(FeatureBasePtr _ft_ptr); diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 42089e0e5..c039c3194 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -58,19 +58,19 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa /** \brief Constructor of category FAC_ABSOLUTE **/ FactorBase(const std::string& _tp, - bool _apply_loss_function = false, - FactorStatus _status = FAC_ACTIVE); + bool _apply_loss_function = false, + FactorStatus _status = FAC_ACTIVE); - /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK) + /** \brief Constructor valid for all categories (FRAME, CAPTURE; FEATURE, LANDMARK) **/ FactorBase(const std::string& _tp, - const FrameBasePtr& _frame_other_ptr, - const CaptureBasePtr& _capture_other_ptr, - const FeatureBasePtr& _feature_other_ptr, - const LandmarkBasePtr& _landmark_other_ptr, - const ProcessorBasePtr& _processor_ptr = nullptr, - bool _apply_loss_function = false, - FactorStatus _status = FAC_ACTIVE); + const FrameBasePtr& _frame_other_ptr, + const CaptureBasePtr& _capture_other_ptr, + const FeatureBasePtr& _feature_other_ptr, + const LandmarkBasePtr& _landmark_other_ptr, + const ProcessorBasePtr& _processor_ptr = nullptr, + bool _apply_loss_function = false, + FactorStatus _status = FAC_ACTIVE); virtual ~FactorBase() = default; @@ -154,11 +154,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa **/ LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); } -// void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; } -// void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; } -// void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; } -// void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; } - public: /** * @brief getProcessor @@ -173,12 +168,11 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa void setProcessor(const ProcessorBasePtr& _processor_ptr); void link(FeatureBasePtr ftr); - void unlink(); virtual void setProblem(ProblemPtr) final; template<typename classType, typename... T> static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all); - protected: + private: void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} }; diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index b1cd6c7c5..2d3506fc9 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -95,7 +95,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature void getFactorList(FactorBasePtrList & _fac_list); void link(CaptureBasePtr cap_ptr); - void unlink(); template<typename classType, typename... T> static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all); @@ -104,6 +103,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const; + private: void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} FactorBasePtr addFactor(FactorBasePtr _co_ptr); void removeFactor(FactorBasePtr _co_ptr); diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index b307a10e2..8d2513805 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -147,11 +147,10 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; void link(TrajectoryBasePtr); - void unlink(); template<typename classType, typename... T> static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all); - protected: + private: CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr); void removeCapture(CaptureBasePtr _capt_ptr); @@ -159,16 +158,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); virtual void removeConstrainedBy(FactorBasePtr _fac_ptr); -// public: -// static FrameBasePtr create_PO_2D (const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(3)); -// static FrameBasePtr create_PO_3D (const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(7)); -// static FrameBasePtr create_POV_3D(const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10)); }; } // namespace wolf diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h index 07c116e81..99a417954 100644 --- a/include/core/hardware/hardware_base.h +++ b/include/core/hardware/hardware_base.h @@ -27,7 +27,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa SensorBasePtrList& getSensorList(); - protected: + private: virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr); }; diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 07ac0ce11..57d294acf 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -92,7 +92,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma MapBasePtr getMap(); void link(MapBasePtr); - void unlink(); template<typename classType, typename... T> static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all); @@ -102,7 +101,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma */ static LandmarkBasePtr create(const YAML::Node& _node); - protected: + private: void setMap(const MapBasePtr _map_ptr); virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h index 136f42f80..d4d5f4fb7 100644 --- a/include/core/map/map_base.h +++ b/include/core/map/map_base.h @@ -28,7 +28,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> MapBase(); ~MapBase(); - protected: + private: virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr); virtual void removeLandmark(LandmarkBasePtr _landmark_ptr); diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 7c171a11f..72912c21d 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -204,7 +204,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce SensorBasePtr getSensor(); const SensorBasePtr getSensor() const; - protected: + private: void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} public: diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 277abfc1e..e5333b510 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -98,7 +98,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa virtual void setProblem(ProblemPtr _problem) final; HardwareBasePtr getHardware(); - protected: + private: void setHardware(const HardwareBasePtr _hw_ptr); ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr); void removeProcessor(ProcessorBasePtr _proc_ptr); diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 5d2216379..c41bf0983 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -48,7 +48,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj void sortFrame(FrameBasePtr _frm_ptr); void updateLastFrames(); - protected: + private: FrameBasePtr addFrame(FrameBasePtr _frame_ptr); void removeFrame(FrameBasePtr _frame_ptr); diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index db2c85222..db12b5847 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -291,8 +291,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib) void CaptureBase::link(FrameBasePtr _frm_ptr) { - if (this->getFrame() && this->getFrame() != _frm_ptr) - this->unlink(); + assert(this->getFrame() == nullptr && "linking an already linked capture"); if(_frm_ptr) { @@ -318,18 +317,5 @@ void CaptureBase::setProblem(ProblemPtr _problem) ft->setProblem(_problem); } -void CaptureBase::unlink() -{ - if(this->getFrame()) - { - this->getFrame()->removeCapture(shared_from_this()); - this->setFrame(nullptr); - } - else - { - WOLF_WARN("Unlinking an unlinked capture"); - } -} - } // namespace wolf diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 11df542fb..7f769587f 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -138,8 +138,7 @@ void FactorBase::setStatus(FactorStatus _status) void FactorBase::link(FeatureBasePtr _ftr_ptr) { - if (this->getFeature() && this->getFeature() != _ftr_ptr) - unlink(); + assert(this->getFeature() == nullptr && "linking an already linked factor"); if(_ftr_ptr) { @@ -175,65 +174,4 @@ void FactorBase::setProblem(ProblemPtr _problem) this->getProblem()->notifyFactor(shared_from_this(),ADD); } -void FactorBase::unlink() -{ - if(this->getFeature()) - { - this->getFeature()->removeFactor(shared_from_this()); - this->setFeature(nullptr); - // remove factor from the solver - if (this->getProblem() != nullptr && this->getStatus() == FAC_ACTIVE) - this->getProblem()->notifyFactor(shared_from_this(),REMOVE); - } - else - { - WOLF_WARN("Unlinking an unlinked factor"); - } -// auto frame_other = this->frame_other_ptr_.lock(); -// if(frame_other != nullptr) frame_other->removeConstrainedBy(shared_from_this()); -// auto capture_other = this->capture_other_ptr_.lock(); -// if(capture_other != nullptr) capture_other->removeConstrainedBy(shared_from_this()); -// auto feature_other = this->feature_other_ptr_.lock(); -// if(feature_other != nullptr) feature_other->removeConstrainedBy(shared_from_this()); -// auto landmark_other = this->landmark_other_ptr_.lock(); -// if(landmark_other != nullptr) landmark_other->removeConstrainedBy(shared_from_this()); -} - -//void FactorBase::setFrameOther(FrameBasePtr _frm_o) -//{ -// if (_frm_o) -// { -// frame_other_ptr_ = _frm_o; -// frame_other_ptr_->addConstrainedBy(shared_from_this()); -// } -//} -// -//void FactorBase::setCaptureOther(CaptureBasePtr _cap_o) -//{ -// if (_cap_o) -// { -// capture_other_ptr_ = _cap_o; -// frame_other_ptr_->addConstrainedBy(shared_from_this()); -// } -//} -// -//void FactorBase::setFeatureOther(FeatureBasePtr _ftr_o) -//{ -// if (_ftr_o) -// { -// feature_other_ptr_ = _ftr_o; -// frame_other_ptr_->addConstrainedBy(shared_from_this()); -// } -//} -// -//void FactorBase::setLandmarkOther(LandmarkBasePtr _lmk_o) -//{ -// if (_lmk_o) -// { -// landmark_other_ptr_ = _lmk_o; -// frame_other_ptr_->addConstrainedBy(shared_from_this()); -// } -//} - - } // namespace wolf diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index d6596861e..2929eb17f 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -157,8 +157,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con void FeatureBase::link(CaptureBasePtr _cpt_ptr) { - if(this->getCapture() && this->getCapture() != _cpt_ptr) - this->unlink(); + assert(this->getCapture() == nullptr && "linking an already linked feature"); if(_cpt_ptr) { @@ -171,18 +170,4 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr) WOLF_WARN("Linking with nullptr"); } } - -void FeatureBase::unlink() -{ - if(this->getCapture()) - { - this->getCapture()->removeFeature(shared_from_this()); - this->setCapture(nullptr); - } - else - { - WOLF_WARN("Unlinking an unlinked feature"); - } -} - } // namespace wolf diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 03fdde2fa..04bbaa38f 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -388,8 +388,7 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr) void FrameBase::link(TrajectoryBasePtr _trj_ptr) { - if (this->getTrajectory()) - unlink(); + assert(this->getTrajectory() == nullptr && "linking an already linked frame"); if(_trj_ptr) { @@ -403,19 +402,6 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr) } } -void FrameBase::unlink() -{ - if(this->getTrajectory()) - { - this->getTrajectory()->addFrame(shared_from_this()); - this->setTrajectory(nullptr); - } - else - { - WOLF_WARN("Unlinking an unlinked frame"); - } -} - void FrameBase::setProblem(ProblemPtr _problem) { if (_problem == nullptr || _problem == this->getProblem()) @@ -429,51 +415,4 @@ void FrameBase::setProblem(ProblemPtr _problem) cap->setProblem(_problem); } -//FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x) -//{ -// assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!"); -// StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) ); -// StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) ); -// StateBlockPtr v_ptr ( nullptr ); -// FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) ); -// f->setType("PO 2D"); -// return f; -//} -// -//FrameBasePtr FrameBase::create_PO_3D(const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x) -//{ -// assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!"); -// StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); -// StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) ); -// StateBlockPtr v_ptr ( nullptr ); -// FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) ); -// f->setType("PO 3D"); -// return f; -//} -// -//FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp, -// const TimeStamp& _ts, -// const Eigen::VectorXs& _x) -//{ -// assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!"); -// StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); -// StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) ); -// StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) ); -// FrameBasePtr f ( std::make_shared<FrameBase>(_tp, _ts, p_ptr, o_ptr, v_ptr) ); -// f->setType("POV 3D"); -// return f; -//} - } // namespace wolf - -//#include "core/common/factory.h" -//namespace wolf -//{ -//namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); } -//namespace{ const bool WOLF_UNUSED Frame_PO_3D_Registered = FrameFactory::get().registerCreator("PO 3D", FrameBase::create_PO_3D ); } -//namespace{ const bool WOLF_UNUSED Frame_POV_3D_Registered = FrameFactory::get().registerCreator("POV 3D", FrameBase::create_POV_3D); } -//} // namespace wolf diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 95840f9da..cb3f7cb21 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -164,8 +164,7 @@ YAML::Node LandmarkBase::saveToYaml() const void LandmarkBase::link(MapBasePtr _map_ptr) { - if (this->getMap() && this->getMap() != _map_ptr) - unlink(); + assert(this->getMap() == nullptr && "linking an already linked landmark"); if(_map_ptr) { @@ -188,19 +187,6 @@ void LandmarkBase::setProblem(ProblemPtr _problem) this->registerNewStateBlocks(); } -void LandmarkBase::unlink() -{ - if(this->getMap()) - { - this->getMap()->removeLandmark(shared_from_this()); - this->setMap(nullptr); - } - else - { - WOLF_WARN("Unlinking an unlinked landmark"); - } -} - FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index ba7d2fdf0..fb9c7f921 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -81,11 +81,8 @@ void ProcessorBase::remove() void ProcessorBase::link(SensorBasePtr _sen_ptr) { - if(this->getSensor() && this->getSensor() != _sen_ptr) - { - WOLF_WARN("Trying to link an already linked processor with a different sensor. Skipping"); - return; - } + assert(this->getSensor() == nullptr && "linking an already linked landmark"); + if(_sen_ptr) { _sen_ptr->addProcessor(shared_from_this()); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 2661df0be..4f1bba034 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -409,12 +409,7 @@ void SensorBase::setProblem(ProblemPtr _problem) void SensorBase::link(HardwareBasePtr _hw_ptr) { - std::cout << "Linking SensorBase" << std::endl; - if(this->getHardware() && this->getHardware() != _hw_ptr) - { - WOLF_WARN("Trying to link an already linked sensor with a different hardware. Skipping"); - return; - } + assert(this->getHardware() == nullptr && "linking an already linked sensor"); if(_hw_ptr) { diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index a48478f8e..f02418eb9 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -133,10 +133,6 @@ TEST(FrameBase, LinksToTree) F1->setKey(); ASSERT_TRUE(F1->isKey()); ASSERT_TRUE(F1->isKeyOrAux()); - - // Unlink capture - C->unlink(); - ASSERT_TRUE(F1->getCaptureList().empty()); } #include "core/state_block/state_quaternion.h" -- GitLab