From b04912ace73cc5110bcb3eb184c4a58c0feee19d Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Fri, 27 Oct 2017 12:44:41 +0200
Subject: [PATCH] add FeatureDiffDrive
---
src/CMakeLists.txt | 6 +++++
src/features/CMakeLists.txt | 9 +++++++
src/features/feature_diff_drive.cpp | 21 ++++++++++++++++
src/features/feature_diff_drive.h | 39 +++++++++++++++++++++++++++++
4 files changed, 75 insertions(+)
create mode 100644 src/features/CMakeLists.txt
create mode 100644 src/features/feature_diff_drive.cpp
create mode 100644 src/features/feature_diff_drive.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ec7455517..178832312 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -436,6 +436,9 @@ ENDIF(OpenCV_FOUND)
# Add the capture sub-directory
ADD_SUBDIRECTORY(captures)
+# Add the features sub-directory
+ADD_SUBDIRECTORY(features)
+
# Add the processor sub-directory
ADD_SUBDIRECTORY(processors)
@@ -483,6 +486,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_BASE}
${SRCS}
${SRCS_CAPTURE}
+ ${SRCS_FEATURE}
${SRCS_SENSOR}
${SRCS_PROCESSOR}
#${SRCS_DTASSC}
@@ -536,6 +540,8 @@ INSTALL(FILES ${HDRS}
# DESTINATION include/iri-algorithms/wolf/data_association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/captures)
+INSTALL(FILES ${HDRS_FEATURE}
+ DESTINATION include/iri-algorithms/wolf/features)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/sensors)
INSTALL(FILES ${HDRS_PROCESSOR}
diff --git a/src/features/CMakeLists.txt b/src/features/CMakeLists.txt
new file mode 100644
index 000000000..d3cecc7f4
--- /dev/null
+++ b/src/features/CMakeLists.txt
@@ -0,0 +1,9 @@
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_FEATURE ${HDRS_FEATURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h PARENT_SCOPE)
+
+SET(SRCS_FEATURE ${SRCS_FEATURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp PARENT_SCOPE)
diff --git a/src/features/feature_diff_drive.cpp b/src/features/feature_diff_drive.cpp
new file mode 100644
index 000000000..ae1f8a2b3
--- /dev/null
+++ b/src/features/feature_diff_drive.cpp
@@ -0,0 +1,21 @@
+#include "feature_diff_drive.h"
+
+namespace wolf {
+
+FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
+ const Eigen::MatrixXs& _delta_preintegrated_covariance,
+ const Eigen::VectorXs& _diff_drive_factors,
+ const Eigen::MatrixXs& _jacobian_diff_drive_factors) :
+ FeatureBase("DIFF DRIVE", _delta_preintegrated, _delta_preintegrated_covariance),
+ diff_drive_factors_(_diff_drive_factors),
+ jacobian_diff_drive_factors_(_jacobian_diff_drive_factors)
+{
+ //
+}
+
+const Eigen::VectorXs& FeatureDiffDrive::getJacobianFactor() const
+{
+ return jacobian_diff_drive_factors_;
+}
+
+} /* namespace wolf */
diff --git a/src/features/feature_diff_drive.h b/src/features/feature_diff_drive.h
new file mode 100644
index 000000000..ac13c4907
--- /dev/null
+++ b/src/features/feature_diff_drive.h
@@ -0,0 +1,39 @@
+/**
+ * \file feature_diff_drive.h
+ *
+ * Created on: Oct 27, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_FEATURE_DIFF_DRIVE_H_
+#define _WOLF_FEATURE_DIFF_DRIVE_H_
+
+//Wolf includes
+#include "../feature_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
+
+class FeatureDiffDrive : public FeatureBase
+{
+public:
+
+ FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
+ const Eigen::MatrixXs& _delta_preintegrated_covariance,
+ const Eigen::VectorXs& _diff_drive_factors,
+ const Eigen::MatrixXs& _jacobian_diff_drive_factors);
+
+ virtual ~FeatureDiffDrive() = default;
+
+ const Eigen::VectorXs& getJacobianFactor() const;
+
+protected:
+
+ Eigen::VectorXs diff_drive_factors_;
+ Eigen::VectorXs jacobian_diff_drive_factors_;
+};
+
+} /* namespace wolf */
+
+#endif /* _WOLF_FEATURE_DIFF_DRIVE_H_ */
--
GitLab