From d62a109f4c3eff6a5cd354c03f8c48121b5465a9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 26 Jun 2019 16:23:10 +0200
Subject: [PATCH] implemented move() and use it in PT
---
include/core/capture/capture_base.h | 1 +
src/capture/capture_base.cpp | 26 +++++++++++++++++++++++---
src/processor/processor_tracker.cpp | 12 +++++-------
3 files changed, 29 insertions(+), 10 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index f0670bfa6..1a7230987 100644
--- a/include/core/capture/capture_base.h
+++ b/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);
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 5f0ce6880..830b04c57 100644
--- a/src/capture/capture_base.cpp
+++ b/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());
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 0b4da2dba..2d65827f1 100644
--- a/src/processor/processor_tracker.cpp
+++ b/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();
--
GitLab