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