add SensorDiffDrive & diff-drive tools

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}

src/sensors/CMakeLists.txt

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

src/sensors/diff_drive_tools.h

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

src/sensors/diff_drive_tools.hpp

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

src/sensors/sensor_diff_drive.cpp

+#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

src/sensors/sensor_diff_drive.h

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