From b30207289c594192ff302f22f75e12bad7032c72 Mon Sep 17 00:00:00 2001 From: Joaquim Casals <jcasals@iri.upc.edu> Date: Tue, 2 Apr 2019 15:11:33 +0200 Subject: [PATCH] WIP: Added all functionality in link(). Old functionality not removed. --- include/base/factor/factor_base.h | 28 ++++++---------------------- src/capture/capture_base.cpp | 2 ++ src/factor/factor_base.cpp | 24 ++++++++++++++++++++++++ src/feature/feature_base.cpp | 1 + src/frame_base.cpp | 2 ++ src/landmark/landmark_base.cpp | 4 +++- src/processor/processor_base.cpp | 1 + test/gtest_emplace.cpp | 2 +- 8 files changed, 40 insertions(+), 24 deletions(-) diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h index f3c074597..f0659caf0 100644 --- a/include/base/factor/factor_base.h +++ b/include/base/factor/factor_base.h @@ -171,10 +171,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa */ void setProcessor(const ProcessorBasePtr& _processor_ptr); - template<typename otherType> - void link(otherType); - template<typename otherType, typename classType, typename... T> - static std::shared_ptr<FactorBase> emplace(otherType _oth_ptr, T&&... all); + void link(FeatureBasePtr); + template<typename classType, typename... T> + static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all); // protected: // template<typename D> @@ -214,29 +213,14 @@ namespace wolf{ // } //} -template<typename OtherPtr, typename classType, typename... T> -std::shared_ptr<FactorBase> FactorBase::emplace(OtherPtr _oth_ptr, T&&... all) +template<typename classType, typename... T> +std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all) { FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...); - ctr->link(_oth_ptr); + ctr->link(_ftr_ptr); return ctr; } -template<typename otherType> -void FactorBase::link(otherType _oth_ptr) -{ - std::cout << "Linking FactorBase" << std::endl; - // _oth_ptr->addConstrainedBy(shared_from_this()); - _oth_ptr->addFactor(shared_from_this()); - auto frame_other = this->frame_other_ptr_.lock(); - if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this()); - auto capture_other = this->capture_other_ptr_.lock(); - if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this()); - auto feature_other = this->feature_other_ptr_.lock(); - if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this()); - auto landmark_other = this->landmark_other_ptr_.lock(); - if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this()); -} inline unsigned int FactorBase::id() const { diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 960263211..3d8a9fed3 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -298,6 +298,8 @@ void CaptureBase::link(FrameBasePtr _frm_ptr) std::cout << "Linking CaptureBase" << std::endl; _frm_ptr->addCapture(shared_from_this()); this->setFramePtr(_frm_ptr); + this->setProblem(_frm_ptr->getProblem()); + this->registerNewStateBlocks(); } } // namespace wolf diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 76ff548b6..00db51449 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -145,4 +145,28 @@ void FactorBase::setApplyLossFunction(const bool _apply) } } } +void FactorBase::link(FeatureBasePtr _ftr_ptr) +{ + std::cout << "Linking FactorBase" << std::endl; + // _ftr_ptr->addConstrainedBy(shared_from_this()); + _ftr_ptr->addFactor(shared_from_this()); + this->setFeaturePtr(_ftr_ptr); + this->setProblem(_ftr_ptr->getProblem()); + // add factor to be added in solver + if (this->getProblem() != nullptr) + { + if (this->getStatus() == FAC_ACTIVE) + this->getProblem()->addFactor(shared_from_this()); + } + else + WOLF_TRACE("WARNING: ADDING CONSTRAINT ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM."); + auto frame_other = this->frame_other_ptr_.lock(); + if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this()); + auto capture_other = this->capture_other_ptr_.lock(); + if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this()); + auto feature_other = this->feature_other_ptr_.lock(); + if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this()); + auto landmark_other = this->landmark_other_ptr_.lock(); + if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this()); +} } // namespace wolf diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 085697be4..3d9e5b497 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -148,6 +148,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con std::cout << "Linking FeatureBase" << std::endl; _cpt_ptr->addFeature(shared_from_this()); this->setCapturePtr(_cpt_ptr); + this->setProblem(getProblem()); } } // namespace wolf diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 465c18ad8..aea78b744 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -386,6 +386,8 @@ void FrameBase::link(TrajectoryBasePtr _ptr) std::cout << "Linking FrameBase" << std::endl; _ptr->addFrame(shared_from_this()); this->setTrajectoryPtr(_ptr); + this->setProblem(_ptr->getProblem()); + if (this->isKey()) this->registerNewStateBlocks(); } } // namespace wolf diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index d987cb365..e1388b726 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -185,7 +185,9 @@ YAML::Node LandmarkBase::saveToYaml() const std::cout << "Linking LandmarkBase" << std::endl; // this->map_ptr_.lock()->addLandmark(shared_from_this()); this->setMapPtr(_map_ptr); - this->map_ptr_.lock()->addLandmark(shared_from_this()); + _map_ptr->addLandmark(shared_from_this()); + this->setProblem(_map_ptr->getProblem()); + this->registerNewStateBlocks(); } FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr) diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index c396dd235..84923e65b 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -77,6 +77,7 @@ void ProcessorBase::remove() std::cout << "Linking ProcessorBase" << std::endl; _sen_ptr->addProcessor(shared_from_this()); this->setSensorPtr(_sen_ptr); + this->setProblem(_sen_ptr->getProblem()); } ///////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 5278a49cf..ef16e5ae6 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -112,7 +112,7 @@ TEST(Emplace, Factor) ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); - auto cnt = FactorBase::emplace<FeatureBasePtr,FactorOdom2D>(ftr, ftr, frm); + auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm); ASSERT_NE(nullptr, ftr->getFactorList().front().get()); } -- GitLab