diff --git a/src/factory.h b/src/factory.h
index cec1b2d090ddca7d7f7b0cd7bd54261a3de0834b..00d2263c2657c183bd6ac1204198ece06ce4ea9e 100644
--- a/src/factory.h
+++ b/src/factory.h
@@ -374,7 +374,7 @@ inline std::string FrameFactory::getClass()
 
 #define WOLF_REGISTER_FRAME(FrameType, FrameName) \
   namespace{ const bool WOLF_UNUSED FrameName##Registered = \
-    FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
+    wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\
 
 
 } /* namespace wolf */
diff --git a/src/processor_base.h b/src/processor_base.h
index fb828e8c4cadffd926dc5fa971e321302d80610d..f87df2394d54ad7abf44637d971fa550b857968a 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -23,8 +23,11 @@ namespace wolf {
  */
 struct ProcessorParamsBase
 {
-        std::string type;
-        std::string name;
+    ProcessorParamsBase()          = default;
+    virtual ~ProcessorParamsBase() = default;
+
+    std::string type;
+    std::string name;
 };
 
 //class ProcessorBase
@@ -33,8 +36,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     private:
         SensorBaseWPtr sensor_ptr_;
 
-        static unsigned int processor_id_count_;
         bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+        static unsigned int processor_id_count_;
 
     public:
         ProcessorBase(const std::string& _type, const Scalar& _time_tolerance = 0);
diff --git a/src/processor_factory.h b/src/processor_factory.h
index d558d458e488e77663db9a5bdfe901ce98d0d332..00d2d50f6bfdc5605b3890ed6f49a2ca38023ba7 100644
--- a/src/processor_factory.h
+++ b/src/processor_factory.h
@@ -178,7 +178,7 @@ inline std::string ProcessorFactory::getClass()
 
 #define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \
   namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \
-    ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\
+    wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\
 
 } /* namespace wolf */
 
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 0cbdd2863930306a61213126f905714f60d0a6f7..227f476e0ecbacb3d20c2f248c120766e0014aab 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -67,8 +67,10 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
 //        std::cout << "PM: RUNNING" << std::endl;
     }
 
+    incoming_ptr_ = getIncomingCaptureMotion(_incoming_ptr);
 
-    incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+    /// @todo Anything else to do ?
+    if (incoming_ptr_ == nullptr) return;
 
     preProcess();
 
@@ -92,7 +94,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
         FeatureBasePtr key_feature_ptr = emplaceFeature(last_ptr_, key_frame_ptr);
 
         // create motion constraint and link it to parent feature and other frame (which is origin's frame)
-        auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_->getFramePtr());
+        /*auto ctr_ptr =*/ emplaceConstraint(key_feature_ptr, origin_ptr_->getFramePtr());
 
         // new capture
         CaptureMotionPtr new_capture_ptr = std::make_shared<CaptureMotion>(key_frame_ptr->getTimeStamp(),
@@ -372,6 +374,11 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
     }
 }
 
+CaptureMotionPtr ProcessorMotion::getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr)
+{
+  return std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+}
+
 CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const TimeStamp& _ts)
 {
     // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
diff --git a/src/processor_motion.h b/src/processor_motion.h
index de7ee71a6048f8a71b18cb543a7d8e8c72f41aee..cd1c41c7f9e2fbe784abfef79ba91768e2afba95 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -269,6 +269,14 @@ class ProcessorMotion : public ProcessorBase
          */
         virtual void postProcess() { };
 
+        /**
+         * @brief Get the incoming CaptureBasePtr and returns a CaptureMotionPtr out of it.
+         * If not overloaded, the base class calls
+         * std::static_pointer_cast<CaptureMotion>(_incoming_ptr)
+         * @return CaptureMotionPtr.
+         */
+        virtual CaptureMotionPtr getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr);
+
 
         // These are the pure virtual functions doing the mathematics
     protected:
diff --git a/src/rotations.h b/src/rotations.h
index 6b5605ea6d4db0d5ba655b2f98c3a4b60de782de..9160256067eae0ba5d2ce8020ca951f31add6b9a 100644
--- a/src/rotations.h
+++ b/src/rotations.h
@@ -381,6 +381,19 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eige
     return Eigen::Matrix<T, 3, 3>::Identity() - (T)0.5 * W + M; //is this really more optimized?
 }
 
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
+                                                 const T pitch,
+                                                 const T yaw)
+{
+  const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T, 3, 1>::UnitX());
+  const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+  const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T, 3, 1>::UnitZ());
+
+  return (az * ay * ax).toRotationMatrix().matrix();
+}
+
+
 } // namespace wolf
 
 #endif /* ROTATIONS_H_ */
diff --git a/src/sensor_base.h b/src/sensor_base.h
index cd36d2881ca75aaeb8116c84b9678feac37e8663..007580f8d2ccbcd463224858d05a7a15eb330319 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -23,8 +23,11 @@ namespace wolf {
  */
 struct IntrinsicsBase
 {
-        std::string type;
-        std::string name;
+  IntrinsicsBase()          = default;
+  virtual ~IntrinsicsBase() = default;
+
+  std::string type;
+  std::string name;
 };
 
 class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
diff --git a/src/sensor_factory.h b/src/sensor_factory.h
index 3e7e1aabc300d286fbf1c37373de0ba7614f75f1..abdba1e604a199b81b8a32c765c98197bdd87ff3 100644
--- a/src/sensor_factory.h
+++ b/src/sensor_factory.h
@@ -220,7 +220,7 @@ inline std::string SensorFactory::getClass()
 
 #define WOLF_REGISTER_SENSOR(SensorType, SensorName) \
   namespace{ const bool WOLF_UNUSED SensorName##Registered = \
-    SensorFactory::get().registerCreator(SensorType, SensorName::create); }\
+    wolf::SensorFactory::get().registerCreator(SensorType, SensorName::create); }\
 
 } /* namespace wolf */
 
diff --git a/src/wolf.h b/src/wolf.h
index dc63786c3e1f45347d305c718bfe99144803760f..0e945f49d9ea47c7ed4c73657c4e9f44f77977c4 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -104,7 +104,11 @@ typedef Quaternion<wolf::Scalar> Quaternions;               ///< Quaternion of r
 typedef AngleAxis<wolf::Scalar> AngleAxiss;                 ///< Angle-Axis of real Scalar type
 typedef Rotation2D<wolf::Scalar> Rotation2Ds;               ///< Rotation2D of real Scalar type
 
+typedef Transform<wolf::Scalar,2,Affine> Affine2ds;         ///< Affine2d of real Scalar type
 typedef Transform<wolf::Scalar,3,Affine> Affine3ds;         ///< Affine3d of real Scalar type
+
+typedef Transform<wolf::Scalar,2,Isometry> Isometry2ds;     ///< Isometry2d of real Scalar type
+typedef Transform<wolf::Scalar,3,Isometry> Isometry3ds;     ///< Isometry3d of real Scalar type
 }
 
 namespace wolf {