add ProcessorDiffDrive

src/CMakeLists.txt

@@ -436,6 +436,9 @@ ENDIF(OpenCV_FOUND)
 # Add the capture sub-directory
 ADD_SUBDIRECTORY(captures)
 
+# Add the processor sub-directory
+ADD_SUBDIRECTORY(processors)
+
 # Add the sensor sub-directory
 ADD_SUBDIRECTORY(sensors)
 
@@ -481,6 +484,7 @@ ADD_LIBRARY(${PROJECT_NAME}
             ${SRCS}
             ${SRCS_CAPTURE}
             ${SRCS_SENSOR}
+            ${SRCS_PROCESSOR}
             #${SRCS_DTASSC} 
             ${SRCS_WRAPPER}
             )
@@ -534,6 +538,8 @@ INSTALL(FILES ${HDRS_CAPTURE}
     DESTINATION include/iri-algorithms/wolf/captures)
 INSTALL(FILES ${HDRS_SENSOR}
     DESTINATION include/iri-algorithms/wolf/sensors)
+INSTALL(FILES ${HDRS_PROCESSOR}
+    DESTINATION include/iri-algorithms/wolf/processors)
 INSTALL(FILES ${HDRS_WRAPPER}
     DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
 INSTALL(FILES ${HDRS_SOLVER}

src/processors/CMakeLists.txt

+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h PARENT_SCOPE)
+
+SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp PARENT_SCOPE)

src/processors/processor_diff_drive.cpp

+#include "processor_diff_drive.h"
+
+#include "../sensors/sensor_diff_drive.h"
+
+#include "../captures/capture_wheel_joint_position.h"
+#include "../captures/capture_velocity.h"
+
+#include "../rotations.h"
+#include "../constraint_odom_2D.h"
+#include "../features/feature_diff_drive.h"
+
+namespace wolf
+{
+
+ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive &params) :
+  ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 0.15, 3),
+  unmeasured_perturbation_cov_(Matrix3s::Identity()*
+                               params.unmeasured_perturbation_std_*
+                               params.unmeasured_perturbation_std_),
+  params_(params)
+{
+  //
+}
+
+void ProcessorDiffDrive::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)
+{
+  assert(_data.size()     == data_size_  && "Wrong _data vector size");
+  assert(_data_cov.rows() == data_size_  && "Wrong _data_cov size");
+  assert(_data_cov.cols() == data_size_  && "Wrong _data_cov size");
+  assert(_calib.size()    == calib_size_ && "Wrong _calib vector size");
+
+  /// Retrieve sensor intrinsics
+  const IntrinsicsDiffDrive& intrinsics =
+      *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
+
+  VelocityComand<Scalar> vel;
+  Eigen::MatrixXs J_vel_data;
+  Eigen::MatrixXs J_vel_calib;
+
+  switch (intrinsics.model_) {
+  case DiffDriveModel::Two_Factor_Model:
+    std::tie(vel, J_vel_data, J_vel_calib) =
+        wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
+          _data, _data_cov,
+          intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
+          _calib, _dt);
+    break;
+  case DiffDriveModel::Three_Factor_Model:
+    std::tie(vel, J_vel_data, J_vel_calib) =
+        wheelPositionIncrementToVelocity<DiffDriveModel::Three_Factor_Model>(
+          _data, _data_cov,
+          intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
+          _calib, _dt);
+    break;
+  case DiffDriveModel::Five_Factor_Model:
+    //      std::tie(vel, J_vel_data, J_vel_calib) =
+    //          wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
+    //            data, data_cov,
+    //            intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
+    //            _calib, _dt);
+    throw std::runtime_error("DiffDriveModel::Five_Factor_Model not implemented !");
+    break;
+  default:
+    throw std::runtime_error("Unknown DiffDrive model.");
+    break;
+  }
+
+  /// Integrate delta vel to zero vel thus
+  /// Convert delta vel to delta 2d pose
+  Eigen::MatrixXs J_delta_vel;
+  integrate(vel.comand, vel.comand_cov, _delta, _delta_cov, J_delta_vel);
+
+  _delta_cov += unmeasured_perturbation_cov_;
+
+  _jacobian_delta_calib = J_delta_vel * J_vel_calib;
+}
+
+bool ProcessorDiffDrive::voteForKeyFrame()
+{
+  // Distance criterion
+  if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_.dist_traveled_th_)
+  {
+    //WOLF_PROCESSOR_DEBUG("vote for key-frame on distance criterion.");
+      return true;
+  }
+//  else
+//  {
+//    WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.head<2>().norm(),
+//                         " < ", params_.dist_traveled_th_);
+//  }
+
+  if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_.theta_traveled_th_)
+  {
+    //WOLF_PROCESSOR_DEBUG("vote for key-frame on rotation criterion.");
+    return true;
+  }
+//  else
+//  {
+//    WOLF_PROCESSOR_DEBUG(getBuffer().get().back().delta_integr_.tail<1>()(0),
+//                         " < ", params_.theta_traveled_th_);
+//  }
+
+  return false;
+}
+
+void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
+                                        const Eigen::VectorXs& _delta2,
+                                        const Scalar /*_Dt2*/,
+                                        Eigen::VectorXs& _delta1_plus_delta2)
+{
+  assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
+  assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
+  assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
+
+  /// Simple 2d frame composition
+
+  _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+  _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+}
+
+void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
+                                        const Eigen::VectorXs& _delta2,
+                                        const Scalar /*_Dt2*/,
+                                        Eigen::VectorXs& _delta1_plus_delta2,
+                                        MatrixXs& _jacobian1, MatrixXs& _jacobian2)
+{
+  using std::sin;
+  using std::cos;
+
+  assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
+  assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
+  assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
+  assert(_jacobian1.rows() == delta_cov_size_ && "Wrong _jacobian1 size");
+  assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
+  assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
+  assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
+
+  /// Simple 2d frame composition
+
+  _delta1_plus_delta2.head<2>() = _delta1.head<2>() +
+      Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+
+  _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+  // Jac wrt delta_integrated
+  _jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_,delta_cov_size_);
+  _jacobian1(0,2) = -sin(_delta1(2))*_delta2(0) - cos(_delta1(2))*_delta2(1);
+  _jacobian1(1,2) =  cos(_delta1(2))*_delta2(0) - sin(_delta1(2))*_delta2(1);
+
+  // jac wrt delta
+  _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
+  _jacobian2.topLeftCorner<2,2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
+}
+
+void ProcessorDiffDrive::statePlusDelta(const Eigen::VectorXs& _x,
+                                        const Eigen::VectorXs& _delta,
+                                        const Scalar /*_Dt*/,
+                                        Eigen::VectorXs& _x_plus_delta)
+{
+  assert(_x.size() == x_size_ && "Wrong _x vector size");
+  assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
+
+  // This is just a frame composition in 2D
+  _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
+  _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
+}
+
+Eigen::VectorXs ProcessorDiffDrive::deltaZero() const
+{
+  return Eigen::VectorXs::Zero(delta_size_);
+}
+
+Motion ProcessorDiffDrive::interpolate(const Motion& _ref,
+                                       Motion& /*_second*/,
+                                       TimeStamp& _ts)
+
+{
+  // TODO: Implement actual interpolation
+  // Implementation: motion ref keeps the same
+  //
+  Motion _interpolated(_ref);
+  _interpolated.ts_                   = _ts;
+  _interpolated.data_                 = Vector3s::Zero();
+  _interpolated.data_cov_             = Matrix3s::Zero();
+  _interpolated.delta_                = deltaZero();
+  _interpolated.delta_cov_            = Eigen::MatrixXs::Zero(delta_size_, delta_size_);
+  _interpolated.delta_integr_         = _ref.delta_integr_;
+  _interpolated.delta_integr_cov_     = _ref.delta_integr_cov_;
+  _interpolated.jacobian_delta_integr_. setIdentity();
+  _interpolated.jacobian_delta_       . setZero();
+  _interpolated.jacobian_calib_       . setZero();
+  return _interpolated;
+}
+
+CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
+                                                   const SensorBasePtr& _sensor,
+                                                   const VectorXs& _data,
+                                                   const MatrixXs& _data_cov,
+                                                   const FrameBasePtr& _frame_origin)
+{
+
+  StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
+        std::make_shared<StateBlock>(3, false) : nullptr;
+
+  return std::make_shared<CaptureWheelJointPosition>(_ts, _sensor, _data, _data_cov,
+                                                     _frame_origin, nullptr, nullptr, i_ptr);
+}
+
+ConstraintBasePtr ProcessorDiffDrive::emplaceConstraint(FeatureBasePtr _feature,
+                                                        CaptureBasePtr _capture_origin)
+{
+  ConstraintOdom2DPtr ctr_odom =
+      std::make_shared<ConstraintOdom2D>(_feature, _capture_origin->getFramePtr(),
+                                         shared_from_this());
+
+  _feature->addConstraint(ctr_odom);
+  _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
+
+  return ctr_odom;
+}
+
+FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
+{
+    FeatureBasePtr key_feature_ptr = std::make_shared<FeatureDiffDrive>(
+            _capture_motion->getBuffer().get().back().delta_integr_,
+            _capture_motion->getBuffer().get().back().delta_integr_cov_,
+            _capture_motion->getBuffer().getCalibrationPreint(),
+            _capture_motion->getBuffer().get().back().jacobian_calib_);
+
+    WOLF_INFO(_capture_motion->getBuffer().getCalibrationPreint());
+    WOLF_INFO(_capture_motion->getBuffer().get().back().jacobian_calib_);
+
+    return key_feature_ptr;
+}
+
+bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
+{
+  return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol);
+}
+
+ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
+                                            const ProcessorParamsBasePtr _params,
+                                            const SensorBasePtr _sensor_ptr)
+{
+  const auto params = std::dynamic_pointer_cast<ProcessorParamsDiffDrive>(_params);
+
+  if (params == nullptr)
+  {
+    WOLF_ERROR("ProcessorDiffDrive::create : input arg"
+               " _params is NOT of type 'ProcessorParamsDiffDrive' !");
+    return nullptr;
+  }
+
+  const auto sensor_ptr = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor_ptr);
+
+  if (sensor_ptr == nullptr)
+  {
+    WOLF_ERROR("ProcessorDiffDrive::create : input arg"
+               " '_sensor_ptr' is NOT of type 'SensorDiffDrive' !");
+    return nullptr;
+  }
+
+  ProcessorBasePtr prc_ptr = std::make_shared<ProcessorDiffDrive>(*params);
+  prc_ptr->setName(_unique_name);
+  return prc_ptr;
+}
+
+} // namespace wolf
+
+// Register in the ProcessorFactory
+#include "processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
+} // namespace wolf

src/processors/processor_diff_drive.h

+/**
+ * \file processor_diff_drive.h
+ *
+ *  Created on: Oct 15, 2016
+ *  \author: Jeremie Deray
+ */
+
+#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_
+#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
+
+#include "../processor_motion.h"
+#include "../sensors/diff_drive_tools.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
+
+struct ProcessorParamsDiffDrive : public ProcessorParamsBase
+{
+  ProcessorParamsDiffDrive(const Scalar _dist_travel_th,
+                           const Scalar _theta_traveled_th,
+                           const Scalar _cov_det_th,
+                           const Scalar _unmeasured_perturbation_std = 0.0001) :
+    dist_traveled_th_(_dist_travel_th),
+    theta_traveled_th_(_theta_traveled_th),
+    cov_det_th_(_cov_det_th),
+    unmeasured_perturbation_std_(_unmeasured_perturbation_std)
+  {
+    //
+  }
+
+  Scalar dist_traveled_th_;
+  Scalar theta_traveled_th_;
+  Scalar cov_det_th_;
+  Scalar unmeasured_perturbation_std_ = 0.0001;
+};
+
+/**
+ * @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
+{
+public:
+
+  ProcessorDiffDrive(const ProcessorParamsDiffDrive& params);
+
+  virtual ~ProcessorDiffDrive() = default;
+
+  virtual bool voteForKeyFrame() override;
+
+protected:
+
+  /// @brief Covariance to be added to the unmeasured perturbation.
+  Matrix3s unmeasured_perturbation_cov_;
+
+  /// @brief Intrinsic params
+  ProcessorParamsDiffDrive params_;
+
+  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 createCapture(const TimeStamp& _ts,
+                                         const SensorBasePtr& _sensor,
+                                         const VectorXs& _data,
+                                         const MatrixXs& _data_cov,
+                                         const FrameBasePtr& _frame_origin) override;
+
+  virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature,
+                                              CaptureBasePtr _capture_origin) override;
+
+  virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
+
+  bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override;
+
+public:
+
+  /// @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_ */
+