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 );