diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 7045ff5134c2916a82032d7e3e9cbea2b8be5a9b..f0670bfa64e6f3cacf460e22d312c8dd9e57fa80 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 42089e0e52bbc769bc5e3dc88a98cfa9ba6402e1..c039c319445c82d5b8909a46ba2fa695da88b118 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 b1cd6c7c545d9e9f607c61b814dc994d892b4543..2d3506fc96cea72a4db1ed6102edf60169e54682 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 b307a10e22b556d9ac9c44bc8eca30b7971c71be..8d251380532bfc842028405d1bf5d8f08299401f 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 07c116e818bb59361098d7f39d0a832996751235..99a4179542d3857ed670b4dd95ac8607c1e588b5 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 07ac0ce110493710760ed46de6fde18a3aa3e261..57d294acfe4404cb16fc3abdcc15e5225d2053ac 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 136f42f80aab5c41063a8de140de9c707f2e1032..d4d5f4fb7de8356cf53fe05283c53093842ebb6c 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 7c171a11f3761fbb313655b314bdf35c090b6665..72912c21db54fbc92baea6a06e891f3a2df975f0 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 277abfc1ee9e6d7aefd5883785e376e6bddeee58..e5333b510e3dd5ac65392826ee8d966ddcf74231 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 5d22163797d38016d1a62f14f4bceeae9cd066b2..c41bf09830811009d415a748fbdca7a918a72f72 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 db2c85222b5413b731f7f9a6b5760c9b8e8ad8dd..db12b5847f5a5a5d93a791ba11727947b982b260 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 11df542fb2650306ff959107812f819134f64eda..7f769587f9678bf5feeb52882031071c0832bc01 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 d6596861e1c73f1bd33a34e35b2f9ec78f69f7ad..2929eb17fc4bb44f7b6b6094c05590c62a9e6ceb 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 03fdde2fa4cf53374ee51089fe8c495e99603e82..04bbaa38fff74496753765f6fd6628dc60d6b68b 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 95840f9dabd6d22128ea8a1e8bc7b8853b4b3c36..cb3f7cb21d3c0172e99ee482018f48365206d15f 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 ba7d2fdf003a667a5351b64d8b282bfa9556688c..fb9c7f921c6869976b5d5ce01fb3ebb7ce5950a7 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 2661df0be77597611894c06780b7aabdac6001b3..4f1bba034ee2da85ec3b06aad5ec29984de023b5 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 a48478f8e287ea265a8b2c15aa5f06e789c5ca22..f02418eb99ce6d49eeaaee923755314c85c8fcc4 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"