From c8b354bd839a652e65409296fc090355294b270e Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Wed, 1 Apr 2020 11:10:59 +0200 Subject: [PATCH] Move processor dimension check to installProcessor --- include/core/processor/processor_base.h | 10 ++++++---- src/problem/problem.cpp | 13 +++++++++++++ src/processor/processor_base.cpp | 7 ------- src/processor/processor_motion.cpp | 10 ++-------- 4 files changed, 21 insertions(+), 19 deletions(-) diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index d0a250aa1..f3feeb56a 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -261,10 +261,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce unsigned int id() const; protected: - /** - * \brief set the pointer to problem - */ - virtual void setProblem(ProblemPtr) override; /** \brief process an incoming capture * * Each derived processor should implement this function. It will be called if: @@ -354,6 +350,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce void setVotingActive(bool _voting_active = true); + int getDim() const; + void link(SensorBasePtr); template<typename classType, typename... T> static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all); @@ -400,6 +398,10 @@ inline void ProcessorBase::setTimeTolerance(double _time_tolerance) { params_->time_tolerance = _time_tolerance; } +inline int ProcessorBase::getDim() const +{ + return dim_; +} template<typename classType, typename... T> std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index c79f3130d..c21ffe9b1 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -220,6 +220,12 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // } ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params); + + //Dimension check + int prc_dim = prc_ptr->getDim(); + auto prb = this; + assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension"); + prc_ptr->configure(_corresponding_sensor_ptr); prc_ptr->link(_corresponding_sensor_ptr); @@ -262,9 +268,16 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!"); ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server); + + //Dimension check + int prc_dim = prc_ptr->getDim(); + auto prb = this; + assert((prc_dim == 0 or prc_dim == prb->getDim()) && "Processor and Problem do not agree on dimension"); + prc_ptr->configure(sen_ptr); prc_ptr->link(sen_ptr); + // setting the origin in all processor motion if origin already setted if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 954531df6..a35097f2c 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -22,13 +22,6 @@ ProcessorBase::~ProcessorBase() // WOLF_DEBUG("destructed -p" , id()); } -void ProcessorBase::setProblem(ProblemPtr _problem) -{ - std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; - assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str()); - - NodeBase::setProblem(_problem); -} bool ProcessorBase::permittedKeyFrame() { return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this()); diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index e1746423e..634d1f6d1 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -366,7 +366,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const // Get delta and correct it with new calibration params VectorXd calib_preint = capture_motion->getCalibrationPreint(); Motion motion = capture_motion->getBuffer().getMotion(_ts); - + VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint); VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); @@ -621,13 +621,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() void ProcessorMotion::setProblem(ProblemPtr _problem) { - if (_problem == nullptr) - return; - - std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; - assert(((dim_ == 0) or (dim_ == _problem->getDim())) && str.c_str()); - - if (_problem == this->getProblem()) + if (_problem == nullptr or _problem == this->getProblem()) return; NodeBase::setProblem(_problem); -- GitLab