Completely replace old diff_drive sen and prc by new design

We keep the old names sensor_diff_drive, processor_diff_drive, factor_diff_drive, etc.

We change the CaptureWheelPosition onto CaptureDiffDrive.

We do not use FeatureDiffDrive as FeatureMotion suffices.

CMakeLists.txt

@@ -167,6 +167,7 @@ SET(HDRS_COMMON
   include/core/common/params_base.h
   )
 SET(HDRS_MATH
+  include/core/math/SE2.h
   include/core/math/SE3.h
   include/core/math/rotations.h
   )
@@ -215,7 +216,7 @@ SET(HDRS_CAPTURE
   include/core/capture/capture_pose.h
   include/core/capture/capture_velocity.h
   include/core/capture/capture_void.h
-  include/core/capture/capture_wheel_joint_position.h
+  include/core/capture/capture_diff_drive.h
   )
 SET(HDRS_FACTOR
   include/core/factor/factor_analytic.h
@@ -234,8 +235,8 @@ SET(HDRS_FACTOR
   )
 SET(HDRS_FEATURE
   include/core/feature/feature_base.h
-  include/core/feature/feature_diff_drive.h
   include/core/feature/feature_match.h
+  include/core/feature/feature_motion.h
   include/core/feature/feature_odom_2D.h
   include/core/feature/feature_pose.h
   )
@@ -245,8 +246,6 @@ SET(HDRS_LANDMARK
   )
 SET(HDRS_PROCESSOR
   include/core/processor/autoconf_processor_factory.h
-  include/core/processor/diff_drive_tools.h
-  include/core/processor/diff_drive_tools.hpp
   include/core/processor/motion_buffer.h
   include/core/processor/processor_base.h
   include/core/processor/processor_diff_drive.h
@@ -324,7 +323,7 @@ SET(SRCS_CAPTURE
   src/capture/capture_pose.cpp
   src/capture/capture_velocity.cpp
   src/capture/capture_void.cpp
-  src/capture/capture_wheel_joint_position.cpp
+  src/capture/capture_diff_drive.cpp
   )
 SET(SRCS_FACTOR
   src/factor/factor_analytic.cpp
@@ -332,7 +331,7 @@ SET(SRCS_FACTOR
   )
 SET(SRCS_FEATURE
   src/feature/feature_base.cpp
-  src/feature/feature_diff_drive.cpp
+  src/feature/feature_motion.cpp
   src/feature/feature_odom_2D.cpp
   src/feature/feature_pose.cpp
   )

include/core/capture/capture_diff_drive.h

+/**
+ * \file diff_drive_tools.h
+ *
+ *  Created on: Oct 20, 2016
+ *  \author: Jeremie Deray
+ */
+
+#ifndef CAPTURE_DIFF_DRIVE_H_
+#define CAPTURE_DIFF_DRIVE_H_
+
+//wolf includes
+#include "core/capture/capture_motion.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
+
+/**
+ * @brief The CaptureWheelJointPosition class
+ *
+ * Represents a list of wheel positions in radian.
+ */
+class CaptureDiffDrive : public CaptureMotion
+{
+
+public:
+
+        /**
+         * \brief Constructor
+         **/
+        CaptureDiffDrive(const TimeStamp& _ts,
+                         const SensorBasePtr& _sensor_ptr,
+                         const Eigen::VectorXs& _data,
+                         const Eigen::MatrixXs& _data_cov,
+                         FrameBasePtr _origin_frame_ptr,
+                         StateBlockPtr _p_ptr = nullptr,
+                         StateBlockPtr _o_ptr = nullptr,
+                         StateBlockPtr _intr_ptr = nullptr);
+
+        virtual ~CaptureDiffDrive() = default;
+
+        virtual Eigen::VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
+
+};
+
+} // namespace wolf
+
+#endif /* CAPTURE_DIFF_DRIVE_H_ */

include/core/capture/capture_motion.h

 /**
- * \file capture_motion2.h
+ * \file capture_motion.h
  *
  *  Created on: Mar 16, 2016
  *      \author: jsola
@@ -57,7 +57,8 @@ class CaptureMotion : public CaptureBase
         CaptureMotion(const std::string & _type,
                       const TimeStamp& _ts,
                       SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
+                      const Eigen::VectorXs& _data,
+                      const Eigen::MatrixXs& _data_cov,
                       SizeEigen _delta_size,
                       SizeEigen _delta_cov_size,
                       FrameBasePtr _origin_frame_ptr,
@@ -93,7 +94,7 @@ class CaptureMotion : public CaptureBase
         VectorXs getDeltaPreint(const TimeStamp& _ts);
         MatrixXs getDeltaPreintCov();
         MatrixXs getDeltaPreintCov(const TimeStamp& _ts);
-        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
+        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const;
 
         // Origin frame
         FrameBasePtr getOriginFrame();
@@ -101,10 +102,11 @@ class CaptureMotion : public CaptureBase
 
         // member data:
     private:
-        Eigen::VectorXs data_;          ///< Motion data in form of vector mandatory
-        Eigen::MatrixXs data_cov_;      ///< Motion data covariance
-        MotionBuffer buffer_;           ///< Buffer of motions between this Capture and the next one.
-        FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
+        Eigen::VectorXs data_;              ///< Motion data in form of vector mandatory
+        Eigen::MatrixXs data_cov_;          ///< Motion data covariance
+        Eigen::VectorXs calib_preint_;      ///< Calibration parameters used during pre-integration
+        MotionBuffer buffer_;               ///< Buffer of motions between this Capture and the next one.
+        FrameBaseWPtr origin_frame_ptr_;    ///< Pointer to the origin frame of the motion
 };
 
 inline const Eigen::VectorXs& CaptureMotion::getData() const
@@ -150,7 +152,7 @@ inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts)
     return getBuffer().getMotion(_ts).jacobian_calib_;
 }
 
-inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
+inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
 {
     WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
     return _delta + _delta_error;
@@ -168,12 +170,12 @@ inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
 
 inline VectorXs CaptureMotion::getCalibrationPreint() const
 {
-    return getBuffer().getCalibrationPreint();
+    return calib_preint_;
 }
 
 inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
 {
-    getBuffer().setCalibrationPreint(_calib_preint);
+    calib_preint_ = _calib_preint;
 }
 
 inline VectorXs CaptureMotion::getDeltaPreint()

include/core/capture/capture_odom_2D.h

@@ -33,11 +33,11 @@ class CaptureOdom2D : public CaptureMotion
 
         virtual ~CaptureOdom2D();
 
-        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
+        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
 
 };
 
-inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
+inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
 {
     Vector3s delta = _delta + _delta_error;
     delta(2) = pi2pi(delta(2));

include/core/capture/capture_odom_3D.h

@@ -33,7 +33,7 @@ class CaptureOdom3D : public CaptureMotion
 
         virtual ~CaptureOdom3D();
 
-        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
+        virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
 
 };
 

include/core/capture/capture_wheel_joint_position.h

-/**
- * \file diff_drive_tools.h
- *
- *  Created on: Oct 20, 2016
- *  \author: Jeremie Deray
- */
-
-#ifndef CAPTURE_WHEEL_JOINT_POSITION_H_
-#define CAPTURE_WHEEL_JOINT_POSITION_H_
-
-//wolf includes
-#include "core/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureWheelJointPosition)
-
-/**
- * @brief The CaptureWheelJointPosition class
- *
- * Represents a list of wheel positions in radian.
- */
-class CaptureWheelJointPosition : public CaptureMotion
-{
-protected:
-
-  using NodeBase::node_type_;
-
-public:
-
-  /**
-   * \brief Constructor
-   **/
-  CaptureWheelJointPosition(const TimeStamp& _ts,
-                            const SensorBasePtr& _sensor_ptr,
-                            const Eigen::VectorXs& _positions,
-                            FrameBasePtr _origin_frame_ptr);
-
-  CaptureWheelJointPosition(const TimeStamp& _ts,
-                            const SensorBasePtr& _sensor_ptr,
-                            const Eigen::VectorXs& _positions,
-                            const Eigen::MatrixXs& _positions_cov,
-                            FrameBasePtr _origin_frame_ptr,
-                            StateBlockPtr _p_ptr = nullptr,
-                            StateBlockPtr _o_ptr = nullptr,
-                            StateBlockPtr _intr_ptr = nullptr);
-
-  virtual ~CaptureWheelJointPosition() = default;
-
-  virtual VectorXs correctDelta(const VectorXs& _delta,
-                                const VectorXs& _delta_error) override;
-
-  const Eigen::VectorXs& getPositions() const;
-
-  const Eigen::MatrixXs& getPositionsCov() const;
-
-protected:
-
-  Eigen::VectorXs positions_;
-  Eigen::MatrixXs positions_cov_;
-};
-
-/// @todo Enforce some logic on the wheel joint pos data
-
-//template <typename E>
-//constexpr typename std::underlying_type<E>::type val(E&& e) noexcept
-//{
-//  return static_cast<typename std::underlying_type<E>::type>(std::forward<E>(e));
-//}
-
-//template <std::size_t N>
-//struct NumWheelTraits
-//{
-//  static constexpr std::size_t num_wheel = N;
-
-//  struct Positions
-//  {
-//    using data_t = Eigen::Matrix<Scalar, num_wheel, 1>;
-//  };
-//};
-
-//template <typename Derived>
-//struct MobileBaseControllerTraits
-//{
-//  using controller_t = Derived;
-
-//  static constexpr decltype(Derived::num_wheel) num_wheel = Derived::num_wheel;
-
-//  using wheel_index_t = typename Derived::WheelsIndexes;
-
-//  MobileBaseControllerTraits()
-//  {
-//    static_assert(true, "");
-//  }
-//};
-
-//struct DiffDriveController : NumWheelTraits<2>
-//{
-//  enum class WheelsIndexes : std::size_t
-//  {
-//    Left  = 0,
-//    Right = 1
-//  };
-
-//  class Positions
-//  {
-//    Eigen::Matrix<Scalar, num_wheel, 1> values_;
-
-//  public:
-
-//    Positions(const Scalar left_wheel_value,
-//              const Scalar rigth_wheel_value) :
-//    values_(Eigen::Matrix<Scalar, num_wheel, 1>(left_wheel_value, rigth_wheel_value))
-//    { }
-//  };
-//};
-
-//struct DiffDriveFourWheelsController : NumWheelTraits<4>
-//{
-//  enum class WheelsIndexes : std::size_t
-//  {
-//    Front_Left  = 0,
-//    Front_Right = 1,
-//    Rear_Left   = 2,
-//    Rear_Right  = 3
-//  };
-//};
-
-///**
-// * @brief The CaptureWheelJointPosition class
-// *
-// * Represents a list of wheel positions.
-// */
-//template <typename ControllerType>
-//class CaptureWheelJointPosition final :
-//    public CaptureBase, MobileBaseControllerTraits<ControllerType>
-//{
-//public:
-
-//  using MobileBaseControllerTraits<ControllerType>::controller_t;
-//  using typename MobileBaseControllerTraits<ControllerType>::wheel_index_t;
-//  using MobileBaseControllerTraits<ControllerType>::num_wheel;
-
-//  /**
-//   * \brief Constructor with ranges
-//   **/
-//  CaptureWheelJointPosition(const TimeStamp& _ts,
-//                            const SensorBasePtr& _sensor_ptr,
-//                            const Eigen::Matrix<Scalar, num_wheel, 1>& _positions) :
-//    CaptureBase("WHEELS POSITION", _ts, _sensor_ptr),
-//    positions_(_positions)
-//  {
-//    //
-//  }
-
-//  ~CaptureWheelJointPosition() = default;
-
-////  void setSensor(const SensorBasePtr sensor_ptr) override;
-
-//  std::size_t getNumWheels() const noexcept
-//  {
-//    return num_wheel;
-//  }
-
-//  template <wheel_index_t wheel_index>
-//  const Scalar& getPosition() const
-//  {
-//    return positions_(val(wheel_index));
-//  }
-
-//protected:
-
-//  Eigen::Matrix<Scalar, num_wheel, 1> positions_;
-
-//  //SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
-//};
-
-//using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
-
-} // namespace wolf
-
-#endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */

include/core/factor/factor_diff_drive.h

@@ -3,135 +3,157 @@
  *
  *  Created on: Oct 27, 2017
  *  \author: Jeremie Deray
+ *  \adapted: Joan Sola - July 2019
  */
 
 #ifndef WOLF_FACTOR_DIFF_DRIVE_H_
 #define WOLF_FACTOR_DIFF_DRIVE_H_
 
 //Wolf includes
+#include "core/capture/capture_diff_drive.h"
 #include "core/factor/factor_autodiff.h"
 #include "core/frame/frame_base.h"
-#include "core/feature/feature_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
+#include "core/feature/feature_motion.h"
 
 namespace
 {
 
-constexpr std::size_t RESIDUAL_SIZE                = 3;
-constexpr std::size_t POSITION_STATE_BLOCK_SIZE    = 2;
-constexpr std::size_t ORIENTATION_STATE_BLOCK_SIZE = 1;
-
-/// @todo This should be dynamic (2/3/5)
-constexpr std::size_t INTRINSICS_STATE_BLOCK_SIZE  = 3;
+constexpr std::size_t POSITION_SIZE     = 2;
+constexpr std::size_t ORIENTATION_SIZE  = 1;
+constexpr std::size_t INTRINSICS_SIZE   = 3;
+constexpr std::size_t RESIDUAL_SIZE     = 3;
 
 }
 
-namespace wolf {
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorDiffDrive);
 
-class FactorDiffDrive :
-    public FactorAutodiff< FactorDiffDrive,
-      RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
-      INTRINSICS_STATE_BLOCK_SIZE >
+class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
+        RESIDUAL_SIZE,
+        POSITION_SIZE,
+        ORIENTATION_SIZE,
+        POSITION_SIZE,
+        ORIENTATION_SIZE,
+        INTRINSICS_SIZE>
 {
-  using Base = FactorAutodiff<FactorDiffDrive,
-  RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
-  INTRINSICS_STATE_BLOCK_SIZE>;
-
-public:
-
-  FactorDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr,
-                      const CaptureWheelJointPositionPtr& _capture_origin_ptr,
-                      const ProcessorBasePtr& _processor_ptr = nullptr,
-                      const bool _apply_loss_function = false,
-                      const FactorStatus _status = FAC_ACTIVE) :
-    Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
-         nullptr, nullptr, _processor_ptr,
-         _apply_loss_function, _status,
-         _frame_ptr->getP(), _frame_ptr->getO(),
-         _capture_origin_ptr->getFrame()->getP(),
-         _capture_origin_ptr->getFrame()->getO(),
-         _capture_origin_ptr->getFrame()->getV(),
-         _capture_origin_ptr->getSensorIntrinsic(),
-         _ftr_ptr->getFrame()->getP(),
-         _ftr_ptr->getFrame()->getO(),
-         _ftr_ptr->getFrame()->getV(),
-         _ftr_ptr->getCapture()->getSensorIntrinsic()),
-    J_delta_calib_(_ftr_ptr->getJacobianFactor())
-  {
-    //
-  }
-
-  /**
-   * \brief Default destructor (not recommended)
-   *
-   * Default destructor.
-   *
-   **/
-  virtual ~FactorDiffDrive() = default;
-
-  template<typename T>
-  bool operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
-                   const T* const _p2, const T* const _o2, const T* const _c2,
-                   T* _residuals) const;
-
-  /**
-   * \brief Returns the jacobians computation method
-   **/
-  virtual JacobianMethod getJacobianMethod() const
-  {
-    return JAC_AUTO;
-  }
-
-protected:
-
-  Eigen::MatrixXs J_delta_calib_;
+        using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
+                RESIDUAL_SIZE,
+                POSITION_SIZE,
+                ORIENTATION_SIZE,
+                POSITION_SIZE,
+                ORIENTATION_SIZE,
+                INTRINSICS_SIZE>;
+
+    public:
+
+        FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
+                        const CaptureBasePtr& _capture_origin_ptr,
+                        const ProcessorBasePtr& _processor_ptr = nullptr,
+                        const bool _apply_loss_function = false,
+                        const FactorStatus _status = FAC_ACTIVE) :
+                Base("DIFF DRIVE",
+                     _capture_origin_ptr->getFrame(),
+                     _capture_origin_ptr,
+                     nullptr,
+                     nullptr,
+                     _processor_ptr,
+                     _apply_loss_function,
+                     _status,
+                     _capture_origin_ptr->getFrame()->getP(),
+                     _capture_origin_ptr->getFrame()->getO(),
+                     _ftr_ptr->getFrame()->getP(),
+                     _ftr_ptr->getFrame()->getO(),
+                     _capture_origin_ptr->getSensorIntrinsic()),
+                     calib_preint_(_ftr_ptr->getCalibrationPreint()),
+                     J_delta_calib_(_ftr_ptr->getJacobianCalibration())
+        {
+            //
+        }
+
+        /**
+         * \brief Default destructor (not recommended)
+         *
+         * Default destructor.
+         *
+         **/
+        virtual ~FactorDiffDrive() = default;
+
+        template<typename T>
+        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+                         const T* const _o2, const T* const _c, T* _residuals) const;
+
+        VectorXs residual();
+
+//        /**
+//         * \brief Returns the jacobians computation method
+//         **/
+//        virtual JacobianMethod getJacobianMethod() const
+//        {
+//            return JAC_AUTO;
+//        }
+
+    protected:
+
+        Eigen::Vector3s calib_preint_;
+        Eigen::MatrixXs J_delta_calib_;
 };
 
 template<typename T>
-inline bool
-FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
-                                 const T* const _p2, const T* const _o2, const T* const _c2,
-                                 T* _residuals) const
+inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+                                         const T* const _o2, const T* const _c, T* _residuals) const
 {
-  // MAPS
-  Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals_map(_residuals);
+    // MAPS
+    Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals(_residuals);
+
+    Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p1(_p1);
+    Eigen::Map<const Eigen::Matrix<T, POSITION_SIZE, 1> > p2(_p2);
 
-  Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p1_map(_p1);
-  Eigen::Map<const Eigen::Matrix<T, POSITION_STATE_BLOCK_SIZE, 1> > p2_map(_p2);
+    Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
 
-  Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c1_map(_c1);
-  Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c2_map(_c2);
+    // Compute corrected delta
 
-  // Compute corrected delta
+    /// Is this my delta_preint ? Yes!
+    const Eigen::Matrix<T, 3, 1> delta_preint = getMeasurement().cast<T>();
 
-  /// Is this my delta_preint ?
-  const Eigen::Matrix<T, 3, 1> measurement = getMeasurement().cast<T>();
+    Matrix<T, 3, 1> c_preint = calib_preint_.cast<T>();
 
-  Eigen::Matrix<T, 3, 1> delta_corrected = measurement + J_delta_calib_.cast<T>() * (c2_map - c1_map);
+    Eigen::Matrix<T, 3, 1> delta_corrected = delta_preint + J_delta_calib_.cast<T>() * (c - c_preint);
 
-  // First 2d pose residual
+    // 2d pose residual
 
-  Eigen::Matrix<T, 3, 1> delta_predicted;
+    Eigen::Matrix<T, 3, 1> delta_predicted;
 
-  // position
-  delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2_map - p1_map);
+    // position
+    delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2 - p1);
 
-  // orientation
-  delta_predicted(2) = _o2[0] - _o1[0];
+    // orientation
+    delta_predicted(2) = pi2pi(_o2[0] - _o1[0]);
 
-  // residual
-  residuals_map = delta_corrected - delta_predicted;
+    // residual
+    residuals = delta_corrected - delta_predicted;
 
-  // angle remapping
-  while (residuals_map(2) > T(M_PI))
-    residuals_map(2) = residuals_map(2) - T(2. * M_PI);
-  while (residuals_map(2) <= T(-M_PI))
-    residuals_map(2) = residuals_map(2) + T(2. * M_PI);
+    // angle remapping
+    while (residuals(2) > T(M_PI))
+        residuals(2) = residuals(2) - T(2. * M_PI);
+    while (residuals(2) <= T(-M_PI))
+        residuals(2) = residuals(2) + T(2. * M_PI);
 
-  // weighted residual
-  residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
+    // weighted residual
+    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * residuals;
 
-  return true;
+    return true;
+}
+
+inline Eigen::VectorXs FactorDiffDrive::residual()
+{
+    VectorXs residual(3);
+    operator ()(getFrameOther()->getP()->getState().data(), getFrameOther()->getO()->getState().data(),
+                getCapture()->getFrame()->getP()->getState().data(),
+                getCapture()->getFrame()->getO()->getState().data(),
+                getCaptureOther()->getSensorIntrinsic()->getState().data(), residual.data());
+    return residual;
 }
 
 } // namespace wolf

include/core/feature/feature_diff_drive.h

@@ -9,29 +9,25 @@
 #define _WOLF_FEATURE_DIFF_DRIVE_H_
 
 //Wolf includes
-#include "core/feature/feature_base.h"
+#include "core/feature/feature_motion.h"
 
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
 
-class FeatureDiffDrive : public FeatureBase
+class FeatureDiffDrive : public FeatureMotion
 {
 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);
+                   const Eigen::VectorXs& _diff_drive_params,
+                   const Eigen::MatrixXs& _jacobian_diff_drive_params);
 
   virtual ~FeatureDiffDrive() = default;
 
-  const Eigen::MatrixXs& getJacobianFactor() const;
-
 protected:
 
-  Eigen::VectorXs diff_drive_factors_;
-  Eigen::MatrixXs jacobian_diff_drive_factors_;
 };
 
 } /* namespace wolf */

include/core/feature/feature_motion.h

@@ -14,46 +14,42 @@
 namespace wolf
 {
 
+WOLF_PTR_TYPEDEFS(FeatureMotion);
+
 class FeatureMotion : public FeatureBase
 {
     public:
-        FeatureMotion(const std::string& _type, const CaptureMotionPtr& _capture_motion);
+        FeatureMotion(const std::string& _type,
+                      const VectorXs& _delta_preint,
+                      const MatrixXs _delta_preint_cov,
+                      const VectorXs& _calib_preint,
+                      const MatrixXs& _jacobian_calib);
         virtual ~FeatureMotion();
 
-        Eigen::VectorXs getCalibrationPreint();
-        Eigen::MatrixXs getJacobianCalibration();
-
-        Eigen::VectorXs computeDeltaCorrection(const Eigen::VectorXs& _calib) const;
-        Eigen::VectorXs getDeltaCorrected(const Eigen::VectorXs& _calib) const;
-
-        virtual Eigen::VectorXs correctDelta(const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta_correction) const = 0;
+        const Eigen::VectorXs& getDeltaPreint() const; ///< A new name for getMeasurement()
+        const Eigen::VectorXs& getCalibrationPreint() const;
+        const Eigen::MatrixXs& getJacobianCalibration() const;
 
     private:
-        Eigen::VectorXs& calib_preint_;
-        Eigen::MatrixXs& jacobian_calib_;
+        Eigen::VectorXs calib_preint_;
+        Eigen::MatrixXs jacobian_calib_;
 };
 
-inline Eigen::VectorXs FeatureMotion::getCalibrationPreint()
+inline const Eigen::VectorXs& FeatureMotion::getDeltaPreint() const
 {
-    return calib_preint_;
+    return measurement_;
 }
 
-inline Eigen::MatrixXs FeatureMotion::getJacobianCalibration()
+inline const Eigen::VectorXs& FeatureMotion::getCalibrationPreint() const
 {
-    return jacobian_calib_;
+    return calib_preint_;
 }
 
-inline Eigen::VectorXs FeatureMotion::computeDeltaCorrection(const Eigen::VectorXs& _calib) const
+inline const Eigen::MatrixXs& FeatureMotion::getJacobianCalibration() const
 {
-    return jacobian_calib_ * (_calib - calib_preint_);
+    return jacobian_calib_;
 }
 
-inline Eigen::VectorXs FeatureMotion::getDeltaCorrected(const Eigen::VectorXs& _calib) const
-{
-    // delta_preint is stored in FeatureBase::measurement_;
-    Eigen::VectorXs delta_step = computeDeltaCorrection(_calib);
-    return correctDelta(measurement_, delta_step);
-}
 
 } /* namespace wolf */
 

include/core/math/SE2.h

+/**
+ * \file SE2.h
+ *
+ *  Created on: Jul 27, 2019
+ *      \author: jsola
+ */
+
+#ifndef MATH_SE2_H_
+#define MATH_SE2_H_
+
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+
+/*
+ * Some of the functions below are based on:
+ *
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
+ */
+
+namespace wolf
+{
+
+template<typename T>
+Eigen::Matrix<T, 2, 2> exp_SO2(const T theta)
+{
+    return Eigen::Rotation2D<T>(theta).matrix();
+}
+
+template<typename T>
+Eigen::Matrix2s V_SE2(const T theta)
+{
+    T s;   //   sin(theta)   / theta
+    T c_1; // (1-cos(theta)) / theta
+
+    if (fabs(theta) > T(Constants::EPS))
+    {
+        // [1] eq. 158
+        s = sin(theta) / theta;
+        c_1 = (T(1.0) - cos(theta)) / theta;
+    }
+    else
+    {
+        // approx of [1] eq. 158
+        s = T(1.0);              // sin(x) ~= x
+        c_1 = theta / T(2.0);      // cos(x) ~= 1 - x^2/2
+    }
+
+    return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
+}
+
+template<class D1, class D2>
+void exp_SE2(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
+{
+    MatrixSizeCheck<3, 1>::check(_tau);
+    MatrixSizeCheck<3, 1>::check(_delta);
+
+    // [1] eq. 156
+    _delta.head(2) = V_SE2(_tau(2)) * _tau.head(2);
+    _delta(2) = pi2pi(_tau(2));
+}
+
+template<class D, typename T>
+Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
+{
+    MatrixSizeCheck<2, 1>::check (p);
+
+    // Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here.
+    T x = p(0);
+    T y = p(1);
+    Matrix<T, 2, 1> J_Vp_th;
+
+    if (fabs(theta) > T(Constants::EPS))
+    {
+        T s_th = sin(theta);
+        T c_th = cos(theta);
+        T theta2 = theta * theta;
+        J_Vp_th << -(y * c_th - y + x * s_th - theta * x * c_th + theta * y * s_th) / theta2,
+                    (x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
+    }
+    else
+    {   // sin(x) ~= x
+        // cos(x) ~= 1 - x^2/2
+        J_Vp_th << -y / 2.0, x / 2.0;
+    }
+
+    return J_Vp_th;
+}
+
+template<class D1, class D2, class D3>
+void exp_SE2(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+{
+    MatrixSizeCheck<3, 1>::check(_tau);
+    MatrixSizeCheck<3, 1>::check(_delta);
+    MatrixSizeCheck<3, 3>::check(_J_delta_tau);
+
+    typedef typename D1::Scalar T;
+
+    // [1] eq. 156
+    T theta = pi2pi(_tau(2));
+    Eigen::Matrix<T, 2, 2> V = V_SE2(theta);
+    _delta.head(2) = V * _tau.head(2);
+    _delta(2) = theta;
+
+    // Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
+    //   J_Vp_p = V: see V_SE2 below, eq. 158
+    //   J_Vp_theta: see fcn helper below
+    //   J_theta_theta = 1; eq. 126
+    _J_delta_tau << V, J_Vp_theta(_tau.template head<2>(), theta), 0.0, 0.0, 1.0;
+}
+
+} // namespacs wolf
+
+#endif /* MATH_SE2_H_ */

include/core/math/rotations.h

@@ -22,13 +22,15 @@ namespace wolf
  */
 template<typename T>
 inline T
-pi2pi(const T angle)
+pi2pi(const T _angle)
 {
-    using std::fmod;
+    T angle = _angle;
+    while (angle > T( M_PI ))
+        angle -=   T( 2 * M_PI );
+    while (angle < T( -M_PI ))
+        angle +=   T( 2 * M_PI );
 
-    return (angle > (T)0 ?
-            fmod(angle + (T)M_PI, (T)(2*M_PI)) - (T)M_PI :
-            fmod(angle - (T)M_PI, (T)(2*M_PI)) + (T)M_PI);
+    return angle;
 }
 
 /** \brief Conversion to radians

include/core/processor/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 "core/utils/eigen_assert.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.
- *
- * Introduction to Autonomous Mobile Robots, 1st Edition, 2004
- * Roland Siegwart, Illah R. Nourbakhsh
- * Section: 5.2.4 'An error model for odometric position estimation' (pp. 186-191)
- */
-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 "core/processor/diff_drive_tools.hpp"
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */

include/core/processor/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_ */

include/core/processor/motion_buffer.h

@@ -44,7 +44,7 @@ struct Motion
                const MatrixXs& _delta_integr_cov,
                const MatrixXs& _jac_delta,
                const MatrixXs& _jac_delta_int,
-               const MatrixXs& _jacobian_calib);// = MatrixXs::Zero(0,0));
+               const MatrixXs& _jacobian_calib);
         ~Motion();
     private:
         void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
@@ -74,23 +74,17 @@ struct Motion
  */
 class MotionBuffer{
     public:
-        SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
+        SizeEigen data_size_, delta_size_, cov_size_;
         MotionBuffer() = delete;
-        MotionBuffer(SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
+        MotionBuffer(SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size);
         std::list<Motion>& get();
         const std::list<Motion>& get() const;
-        const VectorXs& getCalibrationPreint() const;
-        void setCalibrationPreint(const VectorXs& _calib_preint);
         const Motion& getMotion(const TimeStamp& _ts) const;
         void getMotion(const TimeStamp& _ts, Motion& _motion) const;
         void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
-//        MatrixXs integrateCovariance() const; // Integrate all buffer
-//        MatrixXs integrateCovariance(const TimeStamp& _ts) const; // Integrate up to time stamp (included)
-//        MatrixXs integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const; // integrate between time stamps (both included)
         void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
 
     private:
-        VectorXs calib_preint_;         ///< value of the calibration params during pre-integration
         std::list<Motion> container_;
 };
 
@@ -104,18 +98,6 @@ inline const std::list<Motion>& MotionBuffer::get() const
     return container_;
 }
 
-inline const VectorXs& MotionBuffer::getCalibrationPreint() const
-{
-    return calib_preint_;
-}
-
-inline void MotionBuffer::setCalibrationPreint(const VectorXs& _calib_preint)
-{
-    assert(_calib_preint.size() == calib_size_ && "Wrong size of calibration parameters");
-
-    calib_preint_ = _calib_preint;
-}
-
 } // namespace wolf
 
 #endif /* SRC_MOTIONBUFFER_H_ */

include/core/processor/processor_diff_drive.h

 /**
  * \file processor_diff_drive.h
  *
- *  Created on: Oct 15, 2016
- *  \author: Jeremie Deray
+ *  Created on: Jul 22, 2019
+ *      \author: jsola
  */
 
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
+#ifndef PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
+#define PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
 
-#include "core/processor/processor_motion.h"
-#include "core/processor/diff_drive_tools.h"
-#include "core/utils/params_server.hpp"
+#include "core/processor/processor_odom_2D.h"
 
-namespace wolf {
+namespace wolf
+{
 
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
 
-struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
+struct ProcessorParamsDiffDrive : public ProcessorParamsOdom2D
 {
-  Scalar unmeasured_perturbation_std = 0.0001;
-  ProcessorParamsDiffDrive() = default;
-  ProcessorParamsDiffDrive(std::string _unique_name, const ParamsServer& _server):
-    ProcessorParamsMotion(_unique_name, _server)
-  {
-    unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
-  }
-  std::string print()
-  {
-    return "\n" + ProcessorParamsMotion::print() + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
-  }
+        ProcessorParamsDiffDrive() = default;
+        ProcessorParamsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
+            ProcessorParamsOdom2D(_unique_name, _server)
+        {
+        }
+        std::string print()
+        {
+            return "\n" + ProcessorParamsOdom2D::print();
+        }
 };
 
-/**
- * @brief The ProcessorDiffDrive class.
- *
- * Velocity motion model.
- *
- * Integrate odometry from joint position.
- *
- * velocity : data is [d_vx, d_w]
- * position : data is [left_position_increment, right_position_increment]
- *
- * delta is [dx, dy, dtheta]
- */
-
 WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
 
-class ProcessorDiffDrive : public ProcessorMotion
+class ProcessorDiffDrive : public ProcessorOdom2D
 {
-public:
-
-  ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params);
-
-  virtual ~ProcessorDiffDrive() = default;
-  virtual void configure(SensorBasePtr _sensor) override { }
-
-  virtual bool voteForKeyFrame() override;
-
-protected:
-
-  /// @brief Intrinsic params
-  ProcessorParamsDiffDrivePtr params_motion_diff_drive_;
-
-  virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
-                                   const Eigen::MatrixXs& _data_cov,
-                                   const Eigen::VectorXs& _calib,
-                                   const Scalar _dt,
-                                   Eigen::VectorXs& _delta,
-                                   Eigen::MatrixXs& _delta_cov,
-                                   Eigen::MatrixXs& _jacobian_delta_calib) override;
-
-  virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
-                              const Eigen::VectorXs& _delta2,
-                              const Scalar _Dt2,
-                              Eigen::VectorXs& _delta1_plus_delta2) override;
-
-  virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
-                              const Eigen::VectorXs& _delta2,
-                              const Scalar _Dt2,
-                              Eigen::VectorXs& _delta1_plus_delta2,
-                              Eigen::MatrixXs& _jacobian1,
-                              Eigen::MatrixXs& _jacobian2) override;
-
-  virtual void statePlusDelta(const Eigen::VectorXs& _x,
-                          const Eigen::VectorXs& _delta,
-                          const Scalar _Dt,
-                          Eigen::VectorXs& _x_plus_delta) override;
-
-  virtual Eigen::VectorXs deltaZero() const override;
-
-  virtual Motion interpolate(const Motion& _ref,
-                             Motion& _second,
-                             TimeStamp& _ts) override;
-
-  virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                                  const SensorBasePtr& _sensor,
-                                                  const TimeStamp& _ts,
-                                                  const VectorXs& _data,
-                                                  const MatrixXs& _data_cov,
-                                                  const VectorXs& _calib,
-                                                  const VectorXs& _calib_preint,
-                                                  const FrameBasePtr& _frame_origin) override;
-
-  virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
-                                              CaptureBasePtr _capture_origin) override;
-
-  virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
-
-public:
+    public:
+        ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params_motion);
+        static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
+        static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr = nullptr);
+        virtual ~ProcessorDiffDrive();
+
+    protected:
+        virtual void configure(SensorBasePtr _sensor) override;
+        virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
+                                         const Eigen::MatrixXs& _data_cov,
+                                         const Eigen::VectorXs& _calib,
+                                         const Scalar _dt,
+                                         Eigen::VectorXs& _delta,
+                                         Eigen::MatrixXs& _delta_cov,
+                                         Eigen::MatrixXs& _jacobian_calib) override;
+        virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+                                                const SensorBasePtr& _sensor,
+                                                const TimeStamp& _ts,
+                                                const VectorXs& _data,
+                                                const MatrixXs& _data_cov,
+                                                const VectorXs& _calib,
+                                                const VectorXs& _calib_preint,
+                                                const FrameBasePtr& _frame_origin) override;
+        virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+        virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+                                            CaptureBasePtr _capture_origin) override;
+
+
+    protected:
+        ProcessorParamsDiffDrivePtr params_prc_diff_drv_selfcal_;
+        Scalar radians_per_tick_;
 
-  /// @brief Factory method
-  static ProcessorBasePtr create(const std::string& _unique_name,
-                                 const ProcessorParamsBasePtr _params,
-                                 const SensorBasePtr _sensor_ptr);
 };
 
-} // namespace wolf
+}
 
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_H_ */
 
+#endif /* PROCESSOR_PROCESSOR_DIFF_DRIVE_H_ */

include/core/processor/processor_motion.h

@@ -29,6 +29,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
         Scalar dist_traveled    = 5;
         Scalar angle_turned     = 0.5;
         Scalar unmeasured_perturbation_std  = 1e-4;
+
         ProcessorParamsMotion() = default;
         ProcessorParamsMotion(std::string _unique_name, const ParamsServer& _server):
             ProcessorParamsBase(_unique_name, _server)

include/core/processor/processor_odom_2D.h

@@ -21,16 +21,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
 
 struct ProcessorParamsOdom2D : public ProcessorParamsMotion
 {
-        Scalar cov_det = 1.0;      // 1 rad^2
+        Scalar cov_det = 1.0;
+
         ProcessorParamsOdom2D() = default;
         ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) :
-                ProcessorParamsMotion(_unique_name, _server)
+            ProcessorParamsMotion(_unique_name, _server)
         {
-            cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
+            cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det");
         }
+
         std::string print()
         {
-            return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n";
+            return "\n" + ProcessorParamsMotion::print()
+            + "cov_det: " + std::to_string(cov_det) + "\n";
         }
 };
 

include/core/sensor/sensor_base.h

@@ -28,7 +28,7 @@ struct IntrinsicsBase: public ParamsBase
     using ParamsBase::ParamsBase;
     std::string print()
     {
-      return "";
+        return "";
     }
 };
 

include/core/sensor/sensor_diff_drive.h

 /**
  * \file sensor_diff_drive.h
  *
- *  Created on: Oct 20, 2016
- *  \author: Jeremie Deray
+ *  Created on: Jul 22, 2019
+ *      \author: jsola
  */
 
-#ifndef WOLF_SENSOR_DIFF_DRIVE_H_
-#define WOLF_SENSOR_DIFF_DRIVE_H_
+#ifndef SENSOR_SENSOR_DIFF_DRIVE_H_
+#define SENSOR_SENSOR_DIFF_DRIVE_H_
 
-//wolf includes
 #include "core/sensor/sensor_base.h"
-#include "core/processor/diff_drive_tools.h"
-#include "core/utils/params_server.hpp"
 
-namespace wolf {
+namespace wolf
+{
 
 WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
-WOLF_PTR_TYPEDEFS(SensorDiffDrive);
 
 struct IntrinsicsDiffDrive : public IntrinsicsBase
 {
-  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;
-
-    IntrinsicsDiffDrive()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
-    IntrinsicsDiffDrive(std::string _unique_name, const ParamsServer& _server):
-        IntrinsicsBase(_unique_name, _server)
-    {
-
-        left_radius_ = _server.getParam<Scalar>(_unique_name + "/left_radius_");
-        right_radius_ = _server.getParam<Scalar>(_unique_name + "/right_radius_");
-        separation_ = _server.getParam<Scalar>(_unique_name + "/separation_");
-
-        auto model_str = _server.getParam<std::string>(_unique_name + "/model");
-        if(model_str.compare("Two_Factor_Model")) model_ = DiffDriveModel::Two_Factor_Model;
-        else if(model_str.compare("Three_Factor_Model")) model_ = DiffDriveModel::Three_Factor_Model;
-        else if(model_str.compare("Five_Factor_Model")) model_ = DiffDriveModel::Five_Factor_Model;
-        else throw std::runtime_error("Failed to fetch a valid value for the enumerate DiffDriveModel. Value provided: " + model_str);
-
-        factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors", "[1,1,1]");
-
-        left_resolution_ = _server.getParam<Scalar>(_unique_name + "/left_resolution_");
-        right_resolution_ = _server.getParam<Scalar>(_unique_name + "/right_resolution_");
-
-        left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain", "0.01");
-        right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01");
-    }
-  virtual ~IntrinsicsDiffDrive() = default;
-  std::string print()
-  {
-    std::string model_string;
-
-    if(model_ == DiffDriveModel::Two_Factor_Model) model_string = "Two Factor Model";
-    else if(model_ == DiffDriveModel::Three_Factor_Model) model_string = "Three Factor Model";
-    else if(model_ == DiffDriveModel::Five_Factor_Model) model_string = "Five Factor Model";
-
-    return "\n" + IntrinsicsBase::print() + "left_radius: " + std::to_string(left_radius_) + "\n"
-      + "right_radius: "  + std::to_string(right_radius_) + "\n"
-      + "separation_: " + std::to_string(separation_) + "\n"
-      + "model_string: " + model_string + "\n"
-      + "factors_: " + converter<std::string>::convert(factors_) + "\n"
-      + "left_resolution_: " + std::to_string(left_resolution_) + "\n"
-      + "right_resolution_: " + std::to_string(right_resolution_) + "\n"
-      + "left_gain_: " + std::to_string(left_gain_) + "\n"
-      + "right_gain_: " + std::to_string(right_gain_) + "\n";
-  }
+        Scalar radius_left;
+        Scalar radius_right;
+        Scalar wheel_separation;
+        unsigned int ticks_per_wheel_revolution;
+
+        Scalar radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
+
+
+        IntrinsicsDiffDrive() = default;
+        IntrinsicsDiffDrive(std::string _unique_name,
+                                     const wolf::ParamsServer & _server) :
+                                         IntrinsicsBase(_unique_name, _server)
+        {
+            radius_left                 = _server.getParam<Scalar>(_unique_name         + "/radius_left");
+            radius_right                = _server.getParam<Scalar>(_unique_name         + "/radius_right");
+            wheel_separation            = _server.getParam<Scalar>(_unique_name         + "/wheel_separation");
+            ticks_per_wheel_revolution  = _server.getParam<unsigned int>(_unique_name   + "/ticks_per_wheel_revolution");
+            radians_per_tick            = 2.0 * M_PI / ticks_per_wheel_revolution;
+        }
+        std::string print()
+        {
+            return "\n" + IntrinsicsBase::print()
+            + "radius_left: "                   + std::to_string(radius_left)               + "\n"
+            + "radius_right: "                  + std::to_string(radius_right)              + "\n"
+            + "wheel_separation: "              + std::to_string(wheel_separation)          + "\n"
+            + "ticks_per_wheel_revolution: "    + std::to_string(ticks_per_wheel_revolution)+ "\n"
+            + "radians_per_tick: "              + std::to_string(radians_per_tick)          + "\n";
+        }
+
 };
 
-typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
+WOLF_PTR_TYPEDEFS(SensorDiffDrive);
 
 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);
+    public:
+        SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
+                                 const IntrinsicsDiffDrivePtr& _intrinsics);
+        virtual ~SensorDiffDrive();
+        IntrinsicsDiffDriveConstPtr getParams() const {return params_diff_drive_;}
+
+    protected:
+        IntrinsicsDiffDrivePtr params_diff_drive_;
+
+
+    public:
+        static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
+        static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server);
+
 };
 
-} // namespace wolf
+} /* namespace wolf */
 
-#endif /* WOLF_SENSOR_DIFF_DRIVE_H_ */
+#endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */

src/capture/capture_diff_drive.cpp

+
+
+#include "core/capture/capture_diff_drive.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
+                                   const SensorBasePtr& _sensor_ptr,
+                                   const Eigen::VectorXs& _data,
+                                   const Eigen::MatrixXs& _data_cov,
+                                   FrameBasePtr _origin_frame_ptr,
+                                   StateBlockPtr _p_ptr,
+                                   StateBlockPtr _o_ptr,
+                                   StateBlockPtr _intr_ptr) :
+  CaptureMotion("DIFF DRIVE",
+                _ts,
+                _sensor_ptr,
+                _data,
+                _data_cov,
+                3,
+                3,
+                _origin_frame_ptr,
+                _p_ptr,
+                _o_ptr,
+                _intr_ptr)
+{
+    //
+}
+
+Eigen::VectorXs CaptureDiffDrive::correctDelta(const VectorXs& _delta,
+                                               const VectorXs& _delta_error) const
+{
+    Vector3s delta_corrected = _delta + _delta_error;
+    delta_corrected(2) = pi2pi(delta_corrected(2));
+    return delta_corrected;
+}
+
+} // namespace wolf