WIP: Added all functionality in link(). Old functionality not removed.

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
 {

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

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

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

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
 

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)

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());
     }
 /////////////////////////////////////////////////////////////////////////////////////////
 

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());
 }