Merge branch 'miscellaneous' into 'master'

Add ProcessorMotion::processIncomingCapture  &  Miscellaneous

See merge request !127

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 */

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

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 */
 

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

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:

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_ */

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>

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 */
 

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 {