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