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 {