diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 9da4d31f5fa52928cbb73d6cdb257c4bead3a0da..f9e100b49bd4d59f21041ad16ec9475c8a7b16f6 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -146,10 +146,9 @@ class Problem : public std::enable_shared_from_this<Problem> protected: /** \brief Set the processor motion * - * Set the processor motion. + * Add a new processor of type is motion to the processor is motion list. */ - void addProcessorMotion(IsMotionPtr _processor_motion_ptr); - IsMotionPtr addProcessorMotion(const std::string& _unique_processor_name); + void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr); void clearProcessorIsMotion(IsMotionPtr proc); public: @@ -369,19 +368,12 @@ inline IsMotionPtr Problem::getProcessorIsMotion() return nullptr; } + inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList() { return processor_is_motion_list_; } -// linking error -> TODO -// void Problem::clearProcessorIsMotion(IsMotionPtr proc){ -// auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc); -// if (it != processor_is_motion_list_.end()){ -// processor_is_motion_list_.erase(it); -// } -// } - inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap() { std::lock_guard<std::mutex> lock(mut_state_block_notifications_); diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index ff5729536cdf97d0fa0bfb02a7488761db6943de..95d300f9729db24c49382d522d159e98f84ff95d 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -229,10 +229,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // adding processor is motion to the processor is motion vector - if (prc_ptr->isMotion()) - processor_is_motion_list_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr)); - return prc_ptr; } @@ -271,10 +267,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // adding processor is motion to the processor is motion vector - if (prc_ptr->isMotion()) - processor_is_motion_list_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr)); - return prc_ptr; } @@ -445,6 +437,18 @@ std::string Problem::getFrameStructure() const return frame_structure_; } +void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr) +{ + processor_is_motion_list_.push_back(_processor_motion_ptr); +} + +void Problem::clearProcessorIsMotion(IsMotionPtr proc){ + auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc); + if (it != processor_is_motion_list_.end()){ + processor_is_motion_list_.erase(it); + } +} + Eigen::VectorXd Problem::zeroState() const { Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize()); diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 637aaa8acab08d50997626d608ce682aff83e0b2..472bffb8d1a7ab3e470099eadf0084c544be1f88 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -60,27 +60,26 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr); } -// clearProcessorIsMotion liking error --> TODO void ProcessorBase::remove() { - // if (!is_removing_) - // { - // is_removing_ = true; - // ProcessorBasePtr this_p = shared_from_this(); - - // if (isMotion()) - // { - // ProblemPtr P = getProblem(); - // auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() ); - // if(P && this_proc_cast_attempt ) - // P->clearProcessorIsMotion(this_proc_cast_attempt); - // } - - // // remove from upstream - // SensorBasePtr sen = sensor_ptr_.lock(); - // if(sen) - // sen->removeProcessor(this_p); - // } + if (!is_removing_) + { + is_removing_ = true; + ProcessorBasePtr this_p = shared_from_this(); + + if (isMotion()) + { + ProblemPtr P = getProblem(); + auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() ); + if(P && this_proc_cast_attempt ) + P->clearProcessorIsMotion(this_proc_cast_attempt); + } + + // remove from upstream + SensorBasePtr sen = sensor_ptr_.lock(); + if(sen) + sen->removeProcessor(this_p); + } } void ProcessorBase::link(SensorBasePtr _sen_ptr) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index a153c1ec7f6bd7ea85c8fa7b64be3a479875937e..4259c0b7ccfebee600f9740039452b9a1cfa2f8e 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -628,7 +628,10 @@ void ProcessorMotion::setProblem(ProblemPtr _problem) // set the origin if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr) this->setOrigin(this->getProblem()->getLastKeyFrame()); -}; + + // adding processor is motion to the processor is motion vector + getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this())); +} bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr) { diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 4d3dfffbf09b9d1582c330b1d3d37f7067fc8d3c..983e2e127d32e46cd990440865050aed6f8dfe7e 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -125,7 +125,7 @@ TEST(ProcessorBase, KeyFrameCallback) capt_trk = make_shared<CaptureVoid>(t, sens_trk); proc_trk->captureCallback(capt_trk); -// problem->print(4,1,1,0); + problem->print(4,1,1,0); // Only odom creating KFs ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );