diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 7e757c1210aacc1886568794283f45b7c9acc269..d8ff7e15a2028a1781fe3a7f0ede56e7fca2877d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -438,6 +438,9 @@ ENDIF(OpenCV_FOUND)
# Add the capture sub-directory
ADD_SUBDIRECTORY(captures)
+# Add the sensor sub-directory
+ADD_SUBDIRECTORY(sensors)
+
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
@@ -479,6 +482,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_BASE}
${SRCS}
${SRCS_CAPTURE}
+ ${SRCS_SENSOR}
#${SRCS_DTASSC}
${SRCS_WRAPPER}
)
@@ -530,6 +534,8 @@ INSTALL(FILES ${HDRS}
# DESTINATION include/iri-algorithms/wolf/data_association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/captures)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/sensors)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
INSTALL(FILES ${HDRS_SOLVER}
diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..65520638ce8de1fd3e7200873e0b95f0b1ba22c3
--- /dev/null
+++ b/src/sensors/CMakeLists.txt
@@ -0,0 +1,11 @@
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_SENSOR ${HDRS_SENSOR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h PARENT_SCOPE)
+
+SET(SRCS_SENSOR ${SRCS_SENSOR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp PARENT_SCOPE)
diff --git a/src/sensors/diff_drive_tools.h b/src/sensors/diff_drive_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..c95784001db1a2f952ae1cd4b870766294fd0c4f
--- /dev/null
+++ b/src/sensors/diff_drive_tools.h
@@ -0,0 +1,421 @@
+/**
+ * \file diff_drive_tools.h
+ *
+ * Created on: Oct 20, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
+#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_
+
+#include "../eigen_checks.h"
+
+namespace wolf {
+
+enum class DiffDriveModel : std::size_t
+{
+ Two_Factor_Model = 0,
+ Three_Factor_Model = 1,
+ Five_Factor_Model = 2
+};
+
+constexpr double ANGULAR_VELOCITY_MIN(1e-8);
+
+/**
+ * @brief computeDiffDriveComand. Compute wheels velocity comands given
+ * linear and angular velocity.
+ *
+ * @param comand. Linear and angular velocity comands.
+ * @param wheel_comand. Wheels velocity comands.
+ *
+ * @param left_radius. Left wheel radius.
+ * @param right_radius. Right wheel radius.
+ * @param separation. Wheels separation.
+ */
+//template <typename T0, typename T1>
+//void computeDiffDriveComand(const Eigen::MatrixBase<T0> &comand,
+// Eigen::MatrixBase<T1> &wheel_comand,
+// const typename T1::Scalar left_radius,
+// const typename T1::Scalar right_radius,
+// const typename T1::Scalar separation)
+//{
+// Eigen::VectorSizeCheck<2>::check(comand);
+
+// using T = typename T1::Scalar;
+
+// const T linear = comand(0);
+// const T angular = comand(1);
+
+// wheel_comand = Eigen::Matrix<T, 2, 1>((linear - angular * separation * T(0.5)) / left_radius,
+// (linear + angular * separation * T(0.5)) / right_radius);
+//}
+
+/**
+ * @brief computeDiffDriveComand. Compute wheels velocity comands given
+ * linear and angular velocity.
+ *
+ * @param comand. Linear and angular velocity comands.
+ * @param wheel_comand. Wheels velocity comands.
+ *
+ * @param wheel_radius. Wheel radius.
+ * @param separation. Wheels separation.
+ */
+//template <typename T0, typename T1>
+//void computeDiffDriveComand(const Eigen::MatrixBase<T0> &comand,
+// Eigen::MatrixBase<T1> &wheel_comand,
+// const typename T1::Scalar wheel_radius,
+// const typename T1::Scalar separation)
+//{
+// computeDiffDriveComand(comand, wheel_comand,
+// wheel_radius, wheel_radius, separation);
+//}
+
+/**
+ * @brief VelocityComand. Holds a velocity comand vector
+ * together with its covariance.
+ *
+ * @note
+ * 2d : [linear_x, angular]
+ *
+ */
+template <typename Scalar = double>
+struct VelocityComand
+{
+ Eigen::Matrix<Scalar, Eigen::Dynamic, 1> comand;
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> comand_cov;
+};
+
+namespace detail {
+
+template <DiffDriveModel>
+struct wheelPositionIncrementToVelocityHelper
+{
+ template <typename T0, typename T1, typename T2>
+ static std::tuple<VelocityComand<typename T0::Scalar>,
+ Eigen::Matrix<typename T0::Scalar, 2, 2>,
+ Eigen::Matrix<typename T0::Scalar, 2, 3>>
+ wheelPositionIncrementToVelocity(const Eigen::MatrixBase<T0>& position_increment,
+ const Eigen::MatrixBase<T1>& position_increment_cov,
+ const typename T0::Scalar left_radius,
+ const typename T0::Scalar right_radius,
+ const typename T0::Scalar separation,
+ const Eigen::MatrixBase<T2>& factors,
+ const typename T0::Scalar dt);
+};
+
+} /* namespace detail */
+
+/**
+ * @brief Convert from wheels joint positions to per-wheel velocity comands.
+ * @param[in] position_increment. A vector containing the wheels position increments.
+ * @param[in] position_increment_cov. The covariance associated to the vector position_increment.
+ * @param[in] left_radius. The left wheel radius.
+ * @param[in] right_radius. The right wheel radius.
+ * @param[in] separation. The distance separing both wheels.
+ * @param[in] factors. The diff-drive correction factors (calibration).
+ * @param[in] dt. UNUSED.
+ *
+ * @return std::tuple. First element is the computed VelocityComand,
+ * second element is the Jacobian matrix J_vel_data,
+ * third element is the Jacobian matrix J_vel_factor.
+ */
+template <DiffDriveModel M, typename T0, typename T1, typename T2>
+std::tuple<VelocityComand<typename T0::Scalar>,
+ Eigen::Matrix<typename T0::Scalar, 2, 2>,
+ Eigen::Matrix<typename T0::Scalar, 2, 3>>
+wheelPositionIncrementToVelocity(const Eigen::MatrixBase<T0>& position_increment,
+ const Eigen::MatrixBase<T1>& position_increment_cov,
+ const typename T0::Scalar left_radius,
+ const typename T0::Scalar right_radius,
+ const typename T0::Scalar separation,
+ const Eigen::MatrixBase<T2>& factors,
+ const typename T0::Scalar dt)
+{
+ return detail::wheelPositionIncrementToVelocityHelper<M>::wheelPositionIncrementToVelocity(
+ position_increment, position_increment_cov,
+ left_radius, right_radius, separation,
+ factors, dt);
+}
+
+/**
+ * @brief WheelPositionAbsoluteToIncrement.
+ * Simple class to convert from absolute position to increment position.
+ *
+ * @todo handle num wheels 4-6-etc
+ */
+template <typename T>
+class WheelPositionAbsoluteToIncrement
+{
+public:
+
+ WheelPositionAbsoluteToIncrement() = default;
+ ~WheelPositionAbsoluteToIncrement() = default;
+
+ template <typename T0>
+ WheelPositionAbsoluteToIncrement(const Eigen::MatrixBase<T0>& positions)
+ {
+ init(positions);
+ }
+
+ inline bool init() const noexcept { return init_; }
+
+ template <typename T0>
+ void update(const Eigen::MatrixBase<T0>& positions)
+ {
+ Eigen::VectorSizeCheck<2>::check(positions);
+
+ positions_ = positions.template cast<T>();
+ }
+
+ template <typename T0>
+ void init(const Eigen::MatrixBase<T0>& positions)
+ {
+ update(positions);
+
+ init_ = true;
+ }
+
+ template <typename T0, typename T1>
+ void toIncrement(const Eigen::MatrixBase<T0>& positions,
+ Eigen::MatrixBase<T1>& positions_increment)
+ {
+ Eigen::VectorSizeCheck<2>::check(positions);
+
+ if (!init_) init(positions);
+
+ positions_increment =
+ (positions - positions_.template cast<typename T0::Scalar>()).
+ template cast<typename T1::Scalar>();
+ }
+
+ template <typename T0, typename T1>
+ void operator() (const Eigen::MatrixBase<T0>& positions,
+ Eigen::MatrixBase<T1>& positions_increment)
+ {
+ toIncrement(positions, positions_increment);
+ update(positions);
+ }
+
+protected:
+
+ bool init_ = false;
+
+ Eigen::Matrix<T, Eigen::Dynamic, 1> positions_;
+};
+
+/**
+ * @brief integrateRungeKutta. Runge-Kutta 2nd order integration.
+ *
+ * @param[in] data. The input linear and angular velocities.
+ * @param[in] data_cov. The covariance matrix associated to data.
+ * @param[out] delta. The computed delta (2d).
+ * @param[out] delta_cov. The covariance matrix associated to delta.
+ * @param[out] jacobian. The Jacobian matrix J_delta_vel
+ *
+ * @note
+ *
+ * Linear velocity [m] (linear displacement, i.e. m/s * dt)
+ * Angular velocity [rad] (angular displacement, i.e. rad/s * dt)
+ *
+ * MATHS of delta
+ * dx = dv * cos( dw * 0.5 )
+ * dy = dv * sin( dw * 0.5 )
+ * dth = dw
+ */
+template <typename T0, typename T1, typename T2, typename T3, typename T4>
+void integrateRungeKutta(const Eigen::MatrixBase<T0> &data,
+ const Eigen::MatrixBase<T1> &data_cov,
+ Eigen::MatrixBase<T2> &delta,
+ Eigen::MatrixBase<T3> &delta_cov,
+ Eigen::MatrixBase<T4> &jacobian)
+{
+ using std::sin;
+ using std::cos;
+
+ /// @note data is size 3 -> linear vel on x + angular vel on yaw
+ Eigen::VectorSizeCheck<2>::check(data);
+ /// @todo check dim
+ Eigen::MatrixSizeCheck<2,2>::check(data_cov);
+
+ using T = typename T2::Scalar;
+
+ const T v = data(0);
+ const T w = data(1);
+
+ const T w_05 = w * T(.5);
+
+ const T cos_w_05 = cos(w_05);
+ const T sin_w_05 = sin(w_05);
+
+ delta = Eigen::Matrix<T, 3, 1>(cos_w_05 * v,
+ sin_w_05 * v,
+ w );
+ // Fill delta covariance
+ Eigen::Matrix<typename T3::Scalar,
+ Eigen::MatrixBase<T2>::RowsAtCompileTime,
+ Eigen::MatrixBase<T0>::RowsAtCompileTime> J;
+
+ J.setZero(delta.rows(), data.rows());
+
+ // Compute jacobian
+ jacobian =
+ Eigen::Matrix<typename T4::Scalar,
+ Eigen::MatrixBase<T2>::RowsAtCompileTime,
+ Eigen::MatrixBase<T0>::RowsAtCompileTime>::Zero(delta.rows(), data.rows());
+
+ Eigen::MatrixSizeCheck<3,2>::check(jacobian);
+
+ jacobian(0,0) = cos_w_05;
+ jacobian(1,0) = sin_w_05;
+ jacobian(2,0) = typename T3::Scalar(0);
+
+ jacobian(0,1) = -v * sin_w_05 * typename T3::Scalar(.5);
+ jacobian(1,1) = v * cos_w_05 * typename T3::Scalar(.5);
+ jacobian(2,1) = typename T3::Scalar(1);
+
+ // Fill delta covariance
+ delta_cov = J * data_cov * J.transpose();
+}
+
+/**
+ * @brief integrateExact. Exact integration.
+ *
+ * @param[in] data. The input linear and angular velocities.
+ * @param[in] data_cov. The covariance matrix associated to data.
+ * @param[out] delta. The computed delta (2d).
+ * @param[out] delta_cov. The covariance matrix associated to delta.
+ * @param[out] jacobian. The Jacobian matrix J_delta_vel
+ *
+ * @note
+ *
+ * Linear velocity [m] (linear displacement, i.e. m/s * dt)
+ * Angular velocity [rad] (angular displacement, i.e. rad/s * dt)
+ *
+ * MATHS of delta
+ * dx = dv / dw * (sin(dw) - sin(0))
+ * dy = -dv / dw * (cos(dw) - cos(0))
+ * dth = dw
+ */
+template <typename T0, typename T1, typename T2, typename T3, typename T4>
+void integrateExact(const Eigen::MatrixBase<T0> &data,
+ const Eigen::MatrixBase<T1> &data_cov,
+ Eigen::MatrixBase<T2> &delta,
+ Eigen::MatrixBase<T3> &delta_cov,
+ Eigen::MatrixBase<T4> &jacobian)
+{
+ using std::sin;
+ using std::cos;
+
+ /// @note data is size 3 -> linear vel on x & y + angular vel on yaw
+ Eigen::VectorSizeCheck<2>::check(data);
+ /// @todo check dim
+ Eigen::MatrixSizeCheck<2,2>::check(data_cov);
+
+ using T = typename T2::Scalar;
+
+ // Compute delta
+
+ const T v = data(0);
+ const T w = data(1);
+
+ const T inv_w = T(1) / w;
+
+ const T r = v * inv_w;
+
+ const T theta_1 = w;
+
+ const T sin_theta_0 = 0;
+ const T cos_theta_0 = 1;
+
+ const T sin_theta_1 = sin(theta_1);
+ const T cos_theta_1 = cos(theta_1);
+
+ const T sin_diff = sin_theta_1 - sin_theta_0;
+ const T cos_diff = cos_theta_1 - cos_theta_0;
+
+ delta = Eigen::Matrix<T, 3, 1>( r * sin_diff,
+ -r * cos_diff,
+ w );
+
+ const T inv_w2 = inv_w * inv_w;
+
+ // Compute jacobian
+ jacobian =
+ Eigen::Matrix<typename T4::Scalar,
+ Eigen::MatrixBase<T2>::RowsAtCompileTime,
+ Eigen::MatrixBase<T0>::RowsAtCompileTime>::Zero(delta.rows(), data.rows());
+
+ Eigen::MatrixSizeCheck<3,2>::check(jacobian);
+
+ jacobian(0,0) = sin_diff * inv_w;
+ jacobian(1,0) = -cos_diff * inv_w;
+ jacobian(2,0) = T(0);
+
+ jacobian(0,1) = (v * cos_theta_1 * inv_w) - (v * sin_diff * inv_w2);
+ jacobian(1,1) = (v * sin_theta_1 * inv_w) + (v * cos_diff * inv_w2);
+ jacobian(2,1) = T(1);
+
+ // Fill delta covariance
+ delta_cov = jacobian * data_cov * jacobian.transpose();
+}
+
+
+/**
+ * @brief integrate. Helper function to call either
+ * `integrateRung` or `integrateExact` depending on the
+ * angular velocity value.
+ *
+ * @see integrateRungeKutta
+ * @see integrateExact
+ * @see ANGULAR_VELOCITY_MIN
+ */
+template <typename T0, typename T1, typename T2, typename T3, typename T4>
+void integrate(const Eigen::MatrixBase<T0>& data,
+ const Eigen::MatrixBase<T1>& data_cov,
+ Eigen::MatrixBase<T2>& delta,
+ Eigen::MatrixBase<T3>& delta_cov,
+ Eigen::MatrixBase<T4>& jacobian)
+{
+ (data(1) < ANGULAR_VELOCITY_MIN) ?
+ integrateRungeKutta(data, data_cov, delta, delta_cov, jacobian) :
+ integrateExact(data, data_cov, delta, delta_cov, jacobian);
+}
+
+/**
+ * @brief computeWheelJointPositionCov. Compute a covariance matrix to associate
+ * to wheel position increment.
+ *
+ * Set covariance matrix (diagonal):
+ * second term is the wheel resolution covariance,
+ * similar to a DC offset equal to half of the resolution,
+ * which is the theoretical average error.
+ */
+template <typename T>
+Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov(
+ const Eigen::MatrixBase<T>& data,
+ const typename T::Scalar left_wheel_resolution,
+ const typename T::Scalar right_wheel_resolution,
+ const typename T::Scalar left_wheel_cov_gain,
+ const typename T::Scalar right_wheel_cov_gain)
+{
+ using std::abs;
+
+ Eigen::VectorSizeCheck<2>::check(data);
+
+ using S = typename T::Scalar;
+
+ const S dp_var_l = left_wheel_cov_gain * abs(data(0));
+ const S dp_var_r = right_wheel_cov_gain * abs(data(1));
+
+ Eigen::Matrix<S, 2, 2> data_cov;
+ data_cov << dp_var_l + (left_wheel_resolution * S(0.5)), 0,
+ 0, dp_var_r + (right_wheel_resolution * S(0.5));
+
+ return data_cov;
+}
+
+} // namespace wolf
+
+#include "diff_drive_tools.hpp"
+
+#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */
diff --git a/src/sensors/diff_drive_tools.hpp b/src/sensors/diff_drive_tools.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f70ca2c373796de8eccfff4debb6cb9ae9b7fab9
--- /dev/null
+++ b/src/sensors/diff_drive_tools.hpp
@@ -0,0 +1,115 @@
+/**
+ * \file diff_drive_tools.h
+ *
+ * Created on: Oct 20, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
+#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
+
+// un-comment for IDE highlight
+//#include "diff_drive_tools.h"
+#include <tuple>
+
+namespace wolf {
+namespace detail {
+
+template <>
+template <typename T0, typename T1, typename T2>
+std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
+wheelPositionIncrementToVelocityHelper<DiffDriveModel::Two_Factor_Model>::wheelPositionIncrementToVelocity(
+ const Eigen::MatrixBase<T0>& position_increment,
+ const Eigen::MatrixBase<T1>& position_increment_cov,
+ const typename T0::Scalar left_radius,
+ const typename T0::Scalar right_radius,
+ const typename T0::Scalar separation,
+ const Eigen::MatrixBase<T2>& factors,
+ const typename T0::Scalar /*dt*/)
+{
+ Eigen::VectorSizeCheck<2>::check(position_increment);
+ Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
+
+ Eigen::VectorSizeCheck<2>::check(factors);
+
+ using T = typename T0::Scalar;
+
+ const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
+ const T right_wheel_vel = (right_radius * factors(0)) * position_increment(1);
+
+ const T o_5(0.5);
+ const T s_f = separation * factors(1);
+
+ const Eigen::Matrix<T, 2, 1> comand =
+ Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
+ (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
+
+ const T p_r = position_increment(1) * right_radius;
+ const T p_l = position_increment(0) * left_radius;
+
+ Eigen::Matrix<T, 2, 2> J_vel_posinc;
+
+ J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
+ left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
+
+ Eigen::Matrix<T, 2, 3> J_vel_factor;
+
+ J_vel_factor << (p_l + p_r) * o_5, 0,
+ (p_l - p_r) / s_f, ((p_r - p_l) * factors(0)) / (s_f * factors(1));
+
+ const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
+
+ return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
+}
+
+template <>
+template <typename T0, typename T1, typename T2>
+std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
+wheelPositionIncrementToVelocityHelper<DiffDriveModel::Three_Factor_Model>::wheelPositionIncrementToVelocity(
+ const Eigen::MatrixBase<T0>& position_increment,
+ const Eigen::MatrixBase<T1>& position_increment_cov,
+ const typename T0::Scalar left_radius,
+ const typename T0::Scalar right_radius,
+ const typename T0::Scalar separation,
+ const Eigen::MatrixBase<T2>& factors,
+ const typename T0::Scalar /*dt*/)
+{
+ Eigen::VectorSizeCheck<2>::check(position_increment);
+ Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
+
+ Eigen::VectorSizeCheck<3>::check(factors);
+
+ using T = typename T0::Scalar;
+
+ const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
+ const T right_wheel_vel = (right_radius * factors(1)) * position_increment(1);
+
+ const T o_5(0.5);
+ const T s_f = separation * factors(2);
+
+ const Eigen::Matrix<T, 2, 1> comand =
+ Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
+ (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
+
+ const T p_l = position_increment(0) * left_radius;
+ const T p_r = position_increment(1) * right_radius;
+
+ Eigen::Matrix<T, 2, 2> J_vel_posinc;
+
+ J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
+ left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
+
+ Eigen::Matrix<T, 2, 3> J_vel_factor;
+
+ J_vel_factor << p_l * o_5, p_r * o_5, 0,
+ p_l / s_f, -p_r / s_f, (p_r * factors(1) - p_l * factors(0)) / (s_f * factors(2));
+
+ const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
+
+ return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
+}
+
+} /* namespace detail */
+} /* namespace wolf */
+
+#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_ */
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae484a9c3f15cba753544a6b8d0b6ca45595a464
--- /dev/null
+++ b/src/sensors/sensor_diff_drive.cpp
@@ -0,0 +1,133 @@
+#include "sensor_diff_drive.h"
+#include "state_block.h"
+#include "capture_motion.h"
+#include "eigen_checks.h"
+
+namespace wolf {
+
+SensorDiffDrive::SensorDiffDrive(const StateBlockPtr& _p_ptr,
+ const StateBlockPtr& _o_ptr,
+ const StateBlockPtr& _i_ptr,
+ const IntrinsicsDiffDrivePtr& _intrinsics) :
+ SensorBase("DIFF DRIVE", _p_ptr, _o_ptr, _i_ptr, 2, false, true),
+ intrinsics_ptr_{_intrinsics}
+{
+ //
+}
+
+// Define the factory method
+SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
+ const Eigen::VectorXs& _extrinsics_po,
+ const IntrinsicsBasePtr _intrinsics)
+{
+ Eigen::VectorSizeCheck<3>::check(_extrinsics_po);
+
+ // cast intrinsics into derived type
+ IntrinsicsDiffDrivePtr params = std::dynamic_pointer_cast<IntrinsicsDiffDrive>(_intrinsics);
+
+ if (params == nullptr)
+ {
+ WOLF_ERROR("SensorDiffDrive::create expected IntrinsicsDiffDrive !");
+ return nullptr;
+ }
+
+ StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
+ StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
+ StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true);
+
+ SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
+
+ odo->setName(_unique_name);
+
+ return odo;
+}
+
+
+/// @todo Further work to enforce wheel model
+
+//const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
+//{
+//// if (intrinsic_ptr_)
+//// {
+//// const auto& intrinsics = intrinsic_ptr_->getVector();
+
+//// intrinsics_.left_radius_factor_ = intrinsics(0);
+//// intrinsics_.right_radius_factor_ = intrinsics(1);
+//// intrinsics_.separation_factor_ = intrinsics(2);
+//// }
+
+// return intrinsics_;
+//}
+
+//void SensorDiffDrive::initIntrisicsPtr()
+//{
+// assert(intrinsic_ptr_ == nullptr &&
+// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
+
+// Eigen::Vector3s state;
+// state << intrinsics_.left_radius_factor_,
+// intrinsics_.right_radius_factor_,
+// intrinsics_.separation_factor_;
+
+// assert(state(0) > 0 && "The left_radius_factor should be rather close to 1.");
+// assert(state(1) > 0 && "The right_radius_factor should be rather close to 1.");
+// assert(state(2) > 0 && "The separation_factor should be rather close to 1.");
+
+// intrinsic_ptr_ = new StateBlock(state);
+//}
+
+//void SensorDiffDrive::computeDataCov(const Eigen::Vector2s &_data, Eigen::Matrix2s &_data_cov)
+//{
+// const double dp_std_l = intrinsics_.left_gain_ * _data(0);
+// const double dp_std_r = intrinsics_.right_gain_ * _data(1);
+
+// const double dp_var_l = dp_std_l * dp_std_l;
+// const double dp_var_r = dp_std_r * dp_std_r;
+
+// /// Wheel resolution covariance, which is like a DC offset equal to half of
+// /// the resolution, which is the theoretical average error:
+// const double dp_std_avg = Scalar(0.5) * intrinsics_.left_resolution_;
+// const double dp_var_avg = dp_std_avg * dp_std_avg;
+
+// /// Set covariance matrix (diagonal):
+// _data_cov.diagonal() << dp_var_l + dp_var_avg,
+// dp_var_r + dp_var_avg;
+//}
+
+// This overload is probably not the best solution as it forces
+// me to call addCapture from a SensorDiffDrivePtr whereas
+// problem->installSensor() return a SensorBasePtr.
+//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
+//{
+// std::shared_ptr<CaptureMotion> capture_ptr = std::static_pointer_cast<CaptureMotion>(_capture_ptr);
+
+// if (intrinsics_.data_is_position_)
+// {
+// Eigen::Vector2s data = capture_ptr->getData();
+
+// // dt is set to one as we are dealing with wheel position
+// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
+// intrinsics_.separation_, 1);
+
+// capture_ptr->setData(data);
+
+// Eigen::Matrix2s data_cov;
+// data_cov << 0.00001, 0, 0, 0.00001; // Todo
+
+// computeDataCov(data, data_cov);
+
+// capture_ptr->setDataCovariance(data_cov);
+// }
+
+// /// @todo tofix
+// return false;//SensorBase::addCapture(_capture_ptr);
+//}
+
+} // namespace wolf
+
+
+// Register in the SensorFactory
+#include "sensor_factory.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
+} // namespace wolf
diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h
new file mode 100644
index 0000000000000000000000000000000000000000..334fea3895676f923f84625997caf79b4637bafc
--- /dev/null
+++ b/src/sensors/sensor_diff_drive.h
@@ -0,0 +1,107 @@
+/**
+ * \file sensor_diff_drive.h
+ *
+ * Created on: Oct 20, 2016
+ * \author: Jeremie Deray
+ */
+
+#ifndef WOLF_SENSOR_DIFF_DRIVE_H_
+#define WOLF_SENSOR_DIFF_DRIVE_H_
+
+//wolf includes
+#include "../sensor_base.h"
+#include "diff_drive_tools.h"
+
+namespace wolf {
+
+struct IntrinsicsDiffDrive : public IntrinsicsBase
+{
+ IntrinsicsDiffDrive() = default;
+
+ IntrinsicsDiffDrive(const Scalar _left_radius,
+ const Scalar _right_radius,
+ const Scalar _separation,
+ const DiffDriveModel _model,
+ const Eigen::VectorXs& _factors,
+ const Scalar _left_resolution,
+ const Scalar _right_resolution,
+ const Scalar _left_gain,
+ const Scalar _right_gain) :
+ left_radius_(_left_radius),
+ right_radius_(_right_radius),
+ separation_(_separation),
+ model_(_model),
+ factors_(_factors),
+ left_resolution_(_left_resolution),
+ right_resolution_(_right_resolution),
+ left_gain_(_left_gain),
+ right_gain_(_right_gain)
+ {
+ //
+ }
+
+ Scalar left_radius_;
+ Scalar right_radius_;
+ Scalar separation_;
+
+ DiffDriveModel model_ = DiffDriveModel::Three_Factor_Model;
+
+ Eigen::VectorXs factors_ = Eigen::Vector3s(1, 1, 1);
+
+ Scalar left_resolution_;
+ Scalar right_resolution_;
+
+ Scalar left_gain_ = 0.01;
+ Scalar right_gain_ = 0.01;
+};
+
+typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
+
+class SensorDiffDrive : public SensorBase
+{
+public:
+
+ /**
+ * \brief Constructor with arguments
+ *
+ * Constructor with arguments
+ * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base.
+ * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base.
+ * \param _i_ptr StateBlock pointer to the sensor dynamic intrinsics (factors).
+ * \param _intrinsics The intrinsics parameters of the sensor.
+ *
+ **/
+ SensorDiffDrive(const StateBlockPtr& _p_ptr,
+ const StateBlockPtr& _o_ptr,
+ const StateBlockPtr& _i_ptr,
+ const IntrinsicsDiffDrivePtr& _intrinsics);
+
+ /**
+ * \brief Default destructor (not recommended)
+ **/
+ virtual ~SensorDiffDrive() = default;
+
+ inline IntrinsicsDiffDrivePtr getIntrinsics() const {return intrinsics_ptr_;}
+
+protected:
+
+ IntrinsicsDiffDrivePtr intrinsics_ptr_;
+
+public:
+
+ /**
+ * @brief create. Factory function to create a SensorDiffDrive.
+ * @param _unique_name. A unique name for the sensor.
+ * @param _extrinsics_po. The (2d) position of the sensor w.r.t to the robot base frame.
+ * @param _intrinsics. The sensor extrinsics parameters.
+ * @return a SensorBasePtr holding the sensor. If the sensor creation failed,
+ * the returned pointer is nullptr.
+ */
+ static SensorBasePtr create(const std::string& _unique_name,
+ const Eigen::VectorXs& _extrinsics_po,
+ const IntrinsicsBasePtr _intrinsics);
+};
+
+} // namespace wolf
+
+#endif /* WOLF_SENSOR_DIFF_DRIVE_H_ */