From af820d3e2bf1410a028c15cfe7ad7cec84850079 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Thu, 26 Oct 2017 11:47:14 +0200
Subject: [PATCH] add CaptureVelocity
---
src/CMakeLists.txt | 8 ++++-
src/captures/CMakeLists.txt | 9 +++++
src/captures/capture_velocity.cpp | 42 ++++++++++++++++++++++
src/captures/capture_velocity.h | 59 +++++++++++++++++++++++++++++++
4 files changed, 117 insertions(+), 1 deletion(-)
create mode 100644 src/captures/CMakeLists.txt
create mode 100644 src/captures/capture_velocity.cpp
create mode 100644 src/captures/capture_velocity.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1943a400c..e7e224afa 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -433,6 +433,9 @@ IF (OpenCV_FOUND)
)
ENDIF(OpenCV_FOUND)
+# Add the capture sub-directory
+ADD_SUBDIRECTORY(captures)
+
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
@@ -472,7 +475,8 @@ ENDIF(YAMLCPP_FOUND)
ADD_LIBRARY(${PROJECT_NAME}
SHARED
${SRCS_BASE}
- ${SRCS}
+ ${SRCS}
+ ${SRCS_CAPTURE}
#${SRCS_DTASSC}
${SRCS_WRAPPER}
)
@@ -522,6 +526,8 @@ INSTALL(FILES ${HDRS}
DESTINATION include/iri-algorithms/wolf)
#INSTALL(FILES ${HDRS_DTASSC}
# DESTINATION include/iri-algorithms/wolf/data_association)
+INSTALL(FILES ${HDRS_CAPTURE}
+ DESTINATION include/iri-algorithms/wolf/captures)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER}
diff --git a/src/captures/CMakeLists.txt b/src/captures/CMakeLists.txt
new file mode 100644
index 000000000..216253c75
--- /dev/null
+++ b/src/captures/CMakeLists.txt
@@ -0,0 +1,9 @@
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_CAPTURE ${HDRS_CAPTURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h PARENT_SCOPE)
+
+SET(SRCS_CAPTURE ${SRCS_CAPTURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp PARENT_SCOPE)
diff --git a/src/captures/capture_velocity.cpp b/src/captures/capture_velocity.cpp
new file mode 100644
index 000000000..45384d37e
--- /dev/null
+++ b/src/captures/capture_velocity.cpp
@@ -0,0 +1,42 @@
+#include "capture_velocity.h"
+
+namespace wolf {
+
+CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXs& _velocity,
+ Size _delta_size, Size _delta_cov_size,
+ FrameBasePtr _origin_frame_ptr) :
+ CaptureMotion(_ts, _sensor_ptr, _velocity,
+ _delta_size, _delta_cov_size, _origin_frame_ptr)
+{
+ setType("VELOCITY");
+}
+
+CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXs& _velocity,
+ const Eigen::MatrixXs& _velocity_cov,
+ Size _delta_size, Size _delta_cov_size,
+ FrameBasePtr _origin_frame_ptr,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr) :
+ CaptureMotion(_ts, _sensor_ptr, _velocity, _velocity_cov,
+ _delta_size, _delta_cov_size, _origin_frame_ptr,
+ _p_ptr, _o_ptr, _intr_ptr)
+{
+ setType("VELOCITY");
+}
+
+const Eigen::VectorXs& CaptureVelocity::getVelocity() const
+{
+ return getData();
+}
+
+const Eigen::MatrixXs& CaptureVelocity::getVelocityCov() const
+{
+ return getDataCovariance();
+}
+
+} // namespace wolf
diff --git a/src/captures/capture_velocity.h b/src/captures/capture_velocity.h
new file mode 100644
index 000000000..5b3607833
--- /dev/null
+++ b/src/captures/capture_velocity.h
@@ -0,0 +1,59 @@
+/**
+ * \file capture_velocity.h
+ *
+ * Created on: Oct 20, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_CAPTURE_VELOCITY_H_
+#define _WOLF_CAPTURE_VELOCITY_H_
+
+//wolf includes
+#include "capture_motion.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureVelocity);
+
+/**
+ * @brief The CaptureVelocity class
+ *
+ * Represents a velocity.
+ */
+class CaptureVelocity : public CaptureMotion
+{
+protected:
+
+ using NodeBase::node_type_;
+
+public:
+
+ /**
+ * \brief Constructor
+ **/
+ CaptureVelocity(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXs& _velocity,
+ Size _delta_size, Size _delta_cov_size,
+ FrameBasePtr _origin_frame_ptr);
+
+ CaptureVelocity(const TimeStamp& _ts,
+ const SensorBasePtr& _sensor_ptr,
+ const Eigen::VectorXs& _velocity,
+ const Eigen::MatrixXs& _velocity_cov,
+ Size _delta_size, Size _delta_cov_size,
+ FrameBasePtr _origin_frame_ptr,
+ StateBlockPtr _p_ptr = nullptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _intr_ptr = nullptr);
+
+ virtual ~CaptureVelocity() = default;
+
+ const Eigen::VectorXs& getVelocity() const;
+
+ const Eigen::MatrixXs& getVelocityCov() const;
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_CAPTURE_VELOCITY_H_ */
--
GitLab