Merge branch '218-allowed-changes-in-wolf-nodes' into 'devel'

Resolve "Allowed changes in wolf nodes"

Closes #218

See merge request !292

include/core/capture/capture_base.h

@@ -103,6 +103,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         SizeEigen getCalibSize() const;
         virtual Eigen::VectorXs getCalibration() const;
         void setCalibration(const Eigen::VectorXs& _calib);
+        void move(FrameBasePtr);
         void link(FrameBasePtr);
         template<typename classType, typename... T>
         static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);

src/capture/capture_base.cpp

@@ -289,16 +289,36 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
     }
 }
 
-void CaptureBase::link(FrameBasePtr _frm_ptr)
+void CaptureBase::move(FrameBasePtr _frm_ptr)
 {
-    assert((this->getFrame() == nullptr || !this->getFrame()->isKey()) && "linking a capture already linked to a KF");
+    WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
+    WOLF_WARN_COND(_frm_ptr != nullptr, "moving a capture to a null FrameBasePtr");
 
-    // unlink from previous non-key frame
+    // Unlink
     if (this->getFrame())
+    {
+        if (this->getFrame()->isKey())
+        {
+            WOLF_ERROR("moving a capture linked to a KF");
+            return;
+        }
+
+        // unlink from previous non-key frame
         this->getFrame()->removeCapture(shared_from_this());
+    }
+
+    // link
+    link(_frm_ptr);
+}
+
+
+void CaptureBase::link(FrameBasePtr _frm_ptr)
+{
+    assert(this->getFrame() == nullptr && "linking a capture already linked");
 
     if(_frm_ptr)
     {
+
         _frm_ptr->addCapture(shared_from_this());
         this->setFrame(_frm_ptr);
         this->setProblem(_frm_ptr->getProblem());

src/factor/factor_base.cpp

@@ -145,20 +145,22 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
 {
     assert(this->getFeature() == nullptr && "linking an already linked factor");
 
-    if(_ftr_ptr)
+    // not link if nullptr
+    if(_ftr_ptr == nullptr)
     {
-        _ftr_ptr->addFactor(shared_from_this());
-        this->setFeature(_ftr_ptr);
-        this->setProblem(_ftr_ptr->getProblem());
-        // add factor to be added in solver
-        if (this->getProblem() == nullptr)
-        {
-            WOLF_WARN("ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
-        }
-    }
-    else
         WOLF_WARN("Linking with nullptr");
+        return;
+    }
+
+    // link with feature
+    _ftr_ptr->addFactor(shared_from_this());
+    this->setFeature(_ftr_ptr);
+
+    // set problem ( and register factor )
+    WOLF_WARN_COND(this->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+    this->setProblem(_ftr_ptr->getProblem());
 
+    // constrained by
     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();

src/processor/processor_tracker.cpp

@@ -58,7 +58,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
 
             // Append incoming to KF
-            // pack->key_frame->addCapture(incoming_ptr_);
             incoming_ptr_->link(pack->key_frame);
 
             // Process info
@@ -137,7 +136,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             // Capture last_ is added to the new keyframe
             FrameBasePtr last_old_frame = last_ptr_->getFrame();
-            last_ptr_->link(pack->key_frame); // automatically calling last_ptr_->unlink();
+            last_ptr_->move(pack->key_frame);
             last_old_frame->remove();
 
             // Create new frame
@@ -204,7 +203,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // make F; append incoming to new F
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
-                frm->addCapture(incoming_ptr_);
+                incoming_ptr_->link(frm);
 
                 // Establish factors
                 establishFactors();
@@ -227,10 +226,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // We do not create a KF
 
                 // Advance this
-                // last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
-                incoming_ptr_->link(last_ptr_->getFrame());
-                last_ptr_->remove();
-                incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
+                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+                incoming_ptr_->link(frm);
+                last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
 
                 // Update pointers
                 advanceDerived();