diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2c46be342ed1d00b8d3a269779170cdb7ba47e27..e626b36bac416515f416f3d5a2035c601c6618b4 100644
--- a/CMakeLists.txt
+++ b/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
)
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
new file mode 100644
index 0000000000000000000000000000000000000000..d50a728160c9d1d393fd552501a1d3f58a5dbe8f
--- /dev/null
+++ b/include/core/capture/capture_diff_drive.h
@@ -0,0 +1,48 @@
+/**
+ * \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_ */
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index c6cb1ef9ee2ca0bf96cabc0641e4f21937489280..b9875aaea28c99b29fcbcc3fa7b8a1a1132edd94 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -1,5 +1,5 @@
/**
- * \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()
diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2D.h
index 7fedfde3aeafc06ffa8fb652e34c4df67127990f..6b6258cea271baf716324a3888a2725557fd4027 100644
--- a/include/core/capture/capture_odom_2D.h
+++ b/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));
diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3D.h
index a8c5d651e8ee8c7dad31d95ee7b66623e08e2e41..c7e0eecb59e5fc500c5a67c07e1557d65681f5cb 100644
--- a/include/core/capture/capture_odom_3D.h
+++ b/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;
};
diff --git a/include/core/capture/capture_wheel_joint_position.h b/include/core/capture/capture_wheel_joint_position.h
deleted file mode 100644
index a2dc8b9571d4cd87ebdfccfc3297e2d3711edcaf..0000000000000000000000000000000000000000
--- a/include/core/capture/capture_wheel_joint_position.h
+++ /dev/null
@@ -1,182 +0,0 @@
-/**
- * \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_ */
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 95c44592da4006391c246e5bd2be1e0caa7ca6b9..e9501f3b71bdb47c23d5cbd9d5f9bd4f243bfb5c 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/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
diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h
index 6052c24406a2dcee27909e424948b67322311cb7..1261db31dc17429733b0d3fc6bf09b96d3e8b11b 100644
--- a/include/core/feature/feature_diff_drive.h
+++ b/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 */
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index 7471510337a30c36c0acf5a525efa01739224d02..9f531f06103412cf34f053c8cb2bfef4fd9ba851 100644
--- a/include/core/feature/feature_motion.h
+++ b/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 */
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
new file mode 100644
index 0000000000000000000000000000000000000000..925359e9bf04758777909d8ae866cb8b2a0919a2
--- /dev/null
+++ b/include/core/math/SE2.h
@@ -0,0 +1,116 @@
+/**
+ * \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_ */
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 6e2630efed88721857a0c4cd70a4c7d08277d7cb..3e16968591d9d49457598420092e64924d62ba31 100644
--- a/include/core/math/rotations.h
+++ b/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
diff --git a/include/core/processor/diff_drive_tools.h b/include/core/processor/diff_drive_tools.h
deleted file mode 100644
index 50c20c0a021fd98ef882fe549d366b55cc7966e8..0000000000000000000000000000000000000000
--- a/include/core/processor/diff_drive_tools.h
+++ /dev/null
@@ -1,424 +0,0 @@
-/**
- * \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_ */
diff --git a/include/core/processor/diff_drive_tools.hpp b/include/core/processor/diff_drive_tools.hpp
deleted file mode 100644
index f70ca2c373796de8eccfff4debb6cb9ae9b7fab9..0000000000000000000000000000000000000000
--- a/include/core/processor/diff_drive_tools.hpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * \file diff_drive_tools.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
-#define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_
-
-// un-comment for IDE highlight
-//#include "diff_drive_tools.h"
-#include <tuple>
-
-namespace wolf {
-namespace detail {
-
-template <>
-template <typename T0, typename T1, typename T2>
-std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
-wheelPositionIncrementToVelocityHelper<DiffDriveModel::Two_Factor_Model>::wheelPositionIncrementToVelocity(
- const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar /*dt*/)
-{
- Eigen::VectorSizeCheck<2>::check(position_increment);
- Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
-
- Eigen::VectorSizeCheck<2>::check(factors);
-
- using T = typename T0::Scalar;
-
- const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
- const T right_wheel_vel = (right_radius * factors(0)) * position_increment(1);
-
- const T o_5(0.5);
- const T s_f = separation * factors(1);
-
- const Eigen::Matrix<T, 2, 1> comand =
- Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
- (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
-
- const T p_r = position_increment(1) * right_radius;
- const T p_l = position_increment(0) * left_radius;
-
- Eigen::Matrix<T, 2, 2> J_vel_posinc;
-
- J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
- left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
-
- Eigen::Matrix<T, 2, 3> J_vel_factor;
-
- J_vel_factor << (p_l + p_r) * o_5, 0,
- (p_l - p_r) / s_f, ((p_r - p_l) * factors(0)) / (s_f * factors(1));
-
- const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
-
- return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
-}
-
-template <>
-template <typename T0, typename T1, typename T2>
-std::tuple<VelocityComand<typename T0::Scalar>, Eigen::Matrix<typename T0::Scalar, 2, 2>, Eigen::Matrix<typename T0::Scalar, 2, 3>>
-wheelPositionIncrementToVelocityHelper<DiffDriveModel::Three_Factor_Model>::wheelPositionIncrementToVelocity(
- const Eigen::MatrixBase<T0>& position_increment,
- const Eigen::MatrixBase<T1>& position_increment_cov,
- const typename T0::Scalar left_radius,
- const typename T0::Scalar right_radius,
- const typename T0::Scalar separation,
- const Eigen::MatrixBase<T2>& factors,
- const typename T0::Scalar /*dt*/)
-{
- Eigen::VectorSizeCheck<2>::check(position_increment);
- Eigen::MatrixSizeCheck<2,2>::check(position_increment_cov);
-
- Eigen::VectorSizeCheck<3>::check(factors);
-
- using T = typename T0::Scalar;
-
- const T left_wheel_vel = (left_radius * factors(0)) * position_increment(0);
- const T right_wheel_vel = (right_radius * factors(1)) * position_increment(1);
-
- const T o_5(0.5);
- const T s_f = separation * factors(2);
-
- const Eigen::Matrix<T, 2, 1> comand =
- Eigen::Matrix<T, 2, 1>((right_wheel_vel + left_wheel_vel) * o_5,
- (right_wheel_vel - left_wheel_vel) / s_f) /* * dt*/;
-
- const T p_l = position_increment(0) * left_radius;
- const T p_r = position_increment(1) * right_radius;
-
- Eigen::Matrix<T, 2, 2> J_vel_posinc;
-
- J_vel_posinc << left_radius * factors(0) * o_5 , right_radius * factors(1) * o_5,
- left_radius * factors(0) / s_f, -right_radius * factors(1) / s_f;
-
- Eigen::Matrix<T, 2, 3> J_vel_factor;
-
- J_vel_factor << p_l * o_5, p_r * o_5, 0,
- p_l / s_f, -p_r / s_f, (p_r * factors(1) - p_l * factors(0)) / (s_f * factors(2));
-
- const Eigen::Matrix<T, 2, 2> comand_cov = J_vel_posinc * position_increment_cov * J_vel_posinc.transpose();
-
- return std::make_tuple(VelocityComand<T>{comand, comand_cov}, J_vel_posinc, J_vel_factor);
-}
-
-} /* namespace detail */
-} /* namespace wolf */
-
-#endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_HPP_ */
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 9b64ff56252a8229c27a73cbb8880adaa7689ae9..6809c92eedd000c803961b3b8629920e17c5b449 100644
--- a/include/core/processor/motion_buffer.h
+++ b/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_ */
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 831fec13696365facbee70749bc197d5d7c3f3cd..d1ffd344f06c5788382236eb83b4aaa76167f144 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -1,121 +1,72 @@
/**
* \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_ */
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 8f68501f60feb22588032b60291b0a3ffcbfab2a..b06cf3390b6175ac989df88d9265542759955407 100644
--- a/include/core/processor/processor_motion.h
+++ b/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)
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 7c7c1cd0a431d4c4c0053598fa9567854cc09742..de53f9fb400df6098785d144d11a9210dab9ebda 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/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";
}
};
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 78e15380b96fc05bbd818b25aa4211bdeff4e44e..653447cd77b01361e57f79e130a5d4da63390b2d 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -28,7 +28,7 @@ struct IntrinsicsBase: public ParamsBase
using ParamsBase::ParamsBase;
std::string print()
{
- return "";
+ return "";
}
};
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index e6737438050dba3f7bff3b51130937572cc5ef23..c7465756a1cbb347f3e877e15056d8e45082931c 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -1,133 +1,73 @@
/**
* \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_ */
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..02d9eab183156528ffe991b5f01a7867fd5d5568
--- /dev/null
+++ b/src/capture/capture_diff_drive.cpp
@@ -0,0 +1,39 @@
+
+
+#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
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index be77087388afc8a3ae05eb85333b54934bb2e90e..9f1e2216665806f7c7ac51b53928f4ea0cd19016 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -1,5 +1,5 @@
/**
- * \file capture_motion2.cpp
+ * \file capture_motion.cpp
*
* Created on: Oct 14, 2017
* \author: jsola
@@ -20,7 +20,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
- buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
+ buffer_(_data.size(), _delta_size, _delta_cov_size),
origin_frame_ptr_(_origin_frame_ptr)
{
//
@@ -40,7 +40,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
data_(_data),
data_cov_(_data_cov),
- buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
+ buffer_(_data.size(), _delta_size, _delta_cov_size),
origin_frame_ptr_(_origin_frame_ptr)
{
//
@@ -63,11 +63,10 @@ Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
{
- VectorXs calib_preint = getBuffer().getCalibrationPreint();
Motion motion = getBuffer().getMotion(_ts);
VectorXs delta_preint = motion.delta_integr_;
MatrixXs jac_calib = motion.jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
+ VectorXs delta_error = jac_calib * (_calib_current - calib_preint_);
VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index f456b992b2b4136d74690d1cafb8676dabe5762f..d3236a2ab89c312320d0f3c9a6caf830733d3ef2 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -34,7 +34,7 @@ CaptureOdom3D::~CaptureOdom3D()
//
}
-Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
+Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
{
VectorXs delta(7);
delta.head(3) = _delta.head(3) + _delta_error.head(3);
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
deleted file mode 100644
index 10532e397c53455e8d9893cc286ec3ef5797f803..0000000000000000000000000000000000000000
--- a/src/capture/capture_wheel_joint_position.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, Eigen::Matrix2s::Zero(), 3, 3,
- _origin_frame_ptr/*, nullptr, nullptr, std::make_shared<StateBlock>(3, false)*/)
-{
-//
-
- const IntrinsicsDiffDrive intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
-
- setDataCovariance(computeWheelJointPositionCov(getPositions(),
- intrinsics.left_resolution_,
- intrinsics.right_resolution_,
- intrinsics.left_gain_,
- intrinsics.right_gain_));
-}
-
-CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _positions,
- const Eigen::MatrixXs& _positions_cov,
- FrameBasePtr _origin_frame_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("WHEEL JOINT POSITION", _ts, _sensor_ptr, _positions, _positions_cov, 3, 3,
- _origin_frame_ptr, _p_ptr, _o_ptr, _intr_ptr)
-{
-//
-}
-
-Eigen::VectorXs CaptureWheelJointPosition::correctDelta(const VectorXs& _delta,
- const VectorXs& _delta_error)
-{
- Vector3s delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
- return delta;
-}
-
-const Eigen::VectorXs& CaptureWheelJointPosition::getPositions() const
-{
- return getData();
-}
-
-const Eigen::MatrixXs& CaptureWheelJointPosition::getPositionsCov() const
-{
- return getDataCovariance();
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index 0796e18bc73708fd31f55ea4530aff70ac2918f2..ecf92ab78148361f036d7df23cae237a998a98d4 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -1,21 +1,19 @@
#include "core/feature/feature_diff_drive.h"
-namespace wolf {
+namespace wolf
+{
FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::VectorXs& _diff_drive_factors,
- const Eigen::MatrixXs& _jacobian_diff_drive_factors) :
- FeatureBase("DIFF DRIVE", _delta_preintegrated, _delta_preintegrated_covariance),
- diff_drive_factors_(_diff_drive_factors),
- jacobian_diff_drive_factors_(_jacobian_diff_drive_factors)
-{
- //
-}
-
-const Eigen::MatrixXs& FeatureDiffDrive::getJacobianFactor() const
+ const Eigen::VectorXs& _diff_drive_params,
+ const Eigen::MatrixXs& _jacobian_diff_drive_params) :
+ FeatureMotion("DIFF DRIVE",
+ _delta_preintegrated,
+ _delta_preintegrated_covariance,
+ _diff_drive_params,
+ _jacobian_diff_drive_params)
{
- return jacobian_diff_drive_factors_;
+ //
}
} /* namespace wolf */
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index 3e01bd662cf69b07230a6c7f939c4dd18a754a07..078776584865b462f67e355fcfb30c643ad1578f 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -5,15 +5,19 @@
* Author: jsola
*/
-#include "feature_motion.h"
+#include "core/feature/feature_motion.h"
namespace wolf
{
-FeatureMotion::FeatureMotion(const std::string& _type, const CaptureMotionPtr& _capture_motion) :
- FeatureBase(_type, _capture_motion->getDeltaPreint(), _capture_motion->getDeltaPreintCov()),
- calib_preint_(_capture_motion->getCalibrationPreint()),
- jacobian_calib_(_capture_motion->getJacobianCalib())
+FeatureMotion::FeatureMotion(const std::string& _type,
+ const VectorXs& _delta_preint,
+ const MatrixXs _delta_preint_cov,
+ const VectorXs& _calib_preint,
+ const MatrixXs& _jacobian_calib) :
+ FeatureBase(_type, _delta_preint, _delta_preint_cov),
+ calib_preint_(_calib_preint),
+ jacobian_calib_(_jacobian_calib)
{
//
}
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 37f840a505c86754b6e6a4d865a45e441d6feac1..635004edd7dfd33854acc33cdb7c69ba51263ad9 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -64,13 +64,10 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
MotionBuffer::MotionBuffer(SizeEigen _data_size,
SizeEigen _delta_size,
- SizeEigen _cov_size,
- SizeEigen _calib_size) :
+ SizeEigen _cov_size) :
data_size_(_data_size),
delta_size_(_delta_size),
- cov_size_(_cov_size),
- calib_size_(_calib_size),
- calib_preint_(_calib_size)
+ cov_size_(_cov_size)
{
container_.clear();
}
@@ -110,8 +107,6 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- _buffer_part_before_ts.setCalibrationPreint(calib_preint_);
-
auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
@@ -132,48 +127,6 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
}
}
-//MatrixXs MotionBuffer::integrateCovariance() const
-//{
-// Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
-// delta_integr_cov.setZero();
-// for (Motion mot : container_)
-// {
-// delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return delta_integr_cov;
-//}
-//
-//MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts) const
-//{
-// Eigen::MatrixXs delta_integr_cov(cov_size_, cov_size_);
-// delta_integr_cov.setZero();
-// for (Motion mot : container_)
-// {
-// if (mot.ts_ > _ts)
-// break;
-//
-// delta_integr_cov = mot.jacobian_delta_integr_ * delta_integr_cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return delta_integr_cov;
-//}
-//
-//MatrixXs MotionBuffer::integrateCovariance(const TimeStamp& _ts_1, const TimeStamp _ts_2) const
-//{
-// Eigen::MatrixXs cov(cov_size_, cov_size_);
-// cov.setZero();
-// for (Motion mot : container_)
-// {
-// if (mot.ts_ > _ts_2)
-// break;
-//
-// if (mot.ts_ >= _ts_1)
-// cov = mot.jacobian_delta_integr_ * cov * mot.jacobian_delta_integr_.transpose()
-// + mot.jacobian_delta_ * mot.delta_cov_ * mot.jacobian_delta_.transpose();
-// }
-// return cov;
-//}
void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs)
{
@@ -193,8 +146,6 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
for (Motion mot : container_)
{
cout << "-- Motion (" << mot.ts_ << ")" << endl;
-// if (show_ts)
-// cout << " ts: " << mot.ts_ << endl;
if (show_data)
{
cout << " data: " << mot.data_.transpose() << endl;
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 4d9e937028360e651dfbb0377927ad2186244b8e..d3d025b7b43090a87f4abb2de6e4e86ffccfd6f6 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -47,7 +47,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _
void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
{
- WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
// if trigger, process directly without buffering
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index a1abf31c8acfb5d58006b6c3ad170893bdfb6b1e..7af4c10f8a09807f234799c8fc5d6ea034aa9fc1 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -1,200 +1,144 @@
+/**
+ * \file processor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
#include "core/processor/processor_diff_drive.h"
#include "core/sensor/sensor_diff_drive.h"
+#include "core/feature/feature_motion.h"
+#include "core/factor/factor_diff_drive.h"
-#include "core/capture/capture_wheel_joint_position.h"
-#include "core/capture/capture_velocity.h"
-
-#include "core/math/rotations.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/feature/feature_diff_drive.h"
+#include "core/math/SE2.h"
namespace wolf
{
ProcessorDiffDrive::ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params) :
- ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, _params),
- params_motion_diff_drive_(_params)
+ ProcessorOdom2D(_params),
+ params_prc_diff_drv_selfcal_(_params),
+ radians_per_tick_(0.0) // This params needs further initialization. See this->configure(sensor).
{
- unmeasured_perturbation_cov_ = Matrix3s::Identity() * params_motion_diff_drive_->unmeasured_perturbation_std * params_motion_diff_drive_->unmeasured_perturbation_std;
+ setType("DIFF DRIVE"); // overwrite odom2D setting
+ calib_size_ = 3; // overwrite odom2D setting
+ unmeasured_perturbation_cov_ = Matrix1s(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2D setting
}
-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)
+ProcessorDiffDrive::~ProcessorDiffDrive()
{
- 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>(getSensor())->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()
+ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
{
- // Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_motion_diff_drive_->dist_traveled)
- {
- //WOLF_PROCESSOR_DEBUG("vote for key-frame on distance criterion.");
- return true;
- }
-
- if (std::abs(getBuffer().get().back().delta_integr_.tail<1>()(0)) > params_motion_diff_drive_->angle_turned)
- {
- //WOLF_PROCESSOR_DEBUG("vote for key-frame on rotation criterion.");
- return true;
- }
-
- return false;
+
+ ProcessorDiffDrivePtr prc_ptr;
+
+ std::shared_ptr<ProcessorParamsDiffDrive> params;
+ if (_params)
+ params = std::static_pointer_cast<ProcessorParamsDiffDrive>(_params);
+ else
+ params = std::make_shared<ProcessorParamsDiffDrive>();
+
+ prc_ptr = std::make_shared<ProcessorDiffDrive>(params);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
}
-void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar /*_Dt2*/,
- Eigen::VectorXs& _delta1_plus_delta2)
+void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
- 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(_sensor && "Provided sensor is nullptr");
- /// Simple 2d frame composition
+ SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
- _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));
+ radians_per_tick_ = sensor_diff_drive->getParams()->radians_per_tick;
}
-void ProcessorDiffDrive::deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
- const Scalar /*_Dt2*/,
- Eigen::VectorXs& _delta1_plus_delta2,
- MatrixXs& _jacobian1, MatrixXs& _jacobian2)
+
+
+//////////// MOTION INTEGRATION ///////////////
+
+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_calib)
{
- using std::sin;
- using std::cos;
+ /*
+ * Differential drive model -----------------------------------------------
+ *
+ * Variables:
+ * k : encoder + gear multiplication, [radians per encoder tick]
+ * r_r, r_l : right and left wheel radii [m]
+ * d : wheel separation [m]
+ * dl : arc length [m]
+ * dth : arc angle [rad]
+ */
+ const Scalar& k = radians_per_tick_;
+ const Scalar& r_l = _calib(0); // wheel radii
+ const Scalar& r_r = _calib(1);
+ const Scalar& d = _calib(2); // wheel separation
+ Scalar dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0;
+ Scalar dth = k * (_data(1) * r_r - _data(0) * r_l) / d;
- 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
+ /// 1. Tangent space: calibrate and go to se2 tangent -----------------------------------------------
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() +
- Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+ Vector3s tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance for allowing skidding.
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+ Matrix3s J_tangent_calib;
+ J_tangent_calib(0,0) = k * _data(0) / 2.0; // d dl / d r_l
+ J_tangent_calib(0,1) = k * _data(1) / 2.0; // d dl / d r_r
+ J_tangent_calib(0,2) = 0.0; // d dl / d d
+ J_tangent_calib(1,0) = 0.0; // d ds / d r_l
+ J_tangent_calib(1,1) = 0.0; // d ds / d r_r
+ J_tangent_calib(1,2) = 0.0; // d ds / d d
+ J_tangent_calib(2,0) = - k * _data(0) / d; // d dth / d r_l
+ J_tangent_calib(2,1) = k * _data(1) / d; // d dth / d r_r
+ J_tangent_calib(2,2) = - dth / d; // d dth / d d
- // 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);
+ Matrix<Scalar, 3, 2> J_tangent_data;
+ J_tangent_data(0,0) = k * r_l / 2.0; // d dl / d data(0)
+ J_tangent_data(0,1) = k * r_r / 2.0; // d dl / d data(1)
+ J_tangent_data(1,0) = 0.0; // d ds / d data(0)
+ J_tangent_data(1,1) = 0.0; // d ds / d data(1)
+ J_tangent_data(2,0) = - k * r_l / d; // d dth / d data(0)
+ J_tangent_data(2,1) = k * r_r / d; // d dth / d data(1)
- // jac wrt delta
- _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian2.topLeftCorner<2,2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
-}
+ Matrix<Scalar, 3, 1> J_tangent_side;
+ J_tangent_side(0,0) = 0.0; // d dl / d ds
+ J_tangent_side(1,0) = 1.0; // d ds / d ds
+ J_tangent_side(2,0) = 0.0; // d dth / d ds
-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");
+ /// 2. Current delta: go to SE2 manifold -----------------------------------------------
- // 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));
-}
+ Matrix3s J_delta_tangent;
-Eigen::VectorXs ProcessorDiffDrive::deltaZero() const
-{
- return Eigen::VectorXs::Zero(delta_size_);
-}
+ exp_SE2(tangent, _delta, J_delta_tangent);
-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;
-
- return ProcessorMotion::interpolate(_ref, _second, _ts);
+ /// 3. delta covariance -----------------------------------------------
+
+ // Compose Jacobians -- chain rule
+ Matrix<Scalar, 3, 2> J_delta_data = J_delta_tangent * J_tangent_data;
+ Matrix<Scalar, 3, 1> J_delta_side = J_delta_tangent * J_tangent_side;
+
+ // Propagate covariance
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose()
+ + J_delta_side * unmeasured_perturbation_cov_ * J_delta_side.transpose();
+
+ /// 4. Jacobian of delta wrt calibration params -----------------------------------------------
+
+ _jacobian_calib = J_delta_tangent * J_tangent_calib;
}
+
CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
@@ -204,79 +148,73 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
const VectorXs& _calib_preint,
const FrameBasePtr& _frame_origin)
{
- StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
- std::make_shared<StateBlock>(3, false) : nullptr;
-
- auto cap_motion = CaptureBase::emplace<CaptureWheelJointPosition>(_frame_own,
- _ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin,
- nullptr,
- nullptr,
- i_ptr);
- cap_motion->setCalibration(_calib);
+ auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _frame_origin);
+ cap_motion->setCalibration (_calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
}
+FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+ auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
+ "DIFF DRIVE",
+ _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().get().back().delta_integr_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().get().back().jacobian_calib_);
+
+ return key_feature_ptr;
+}
+
FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin)
+ CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature,
- _feature,
- _capture_origin->getFrame(),
- shared_from_this());
+ auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
+ auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+ ftr_motion,
+ _capture_origin,
+ shared_from_this());
return fac_odom;
}
-FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+///////////// FACTORIES ///////////////
+
+ProcessorBasePtr ProcessorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr)
{
- FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
- _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_);
+ ProcessorDiffDrivePtr prc_ptr;
- return key_feature_ptr;
-}
+ std::shared_ptr<ProcessorParamsDiffDrive> params;
+ params = std::make_shared<ProcessorParamsDiffDrive>();
+ params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active");
+ params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance");
+ params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span");
+ params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled"); // Will make KFs automatically every 1m displacement
+ params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned");
+ params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std");
-ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
-{
- const auto params = std::static_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;
+ prc_ptr = std::make_shared<ProcessorDiffDrive>(params);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
}
-} // namespace wolf
+} /* namespace wolf */
+
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive)
} // namespace wolf
+#include "core/processor/autoconf_processor_factory.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR_AUTO("DIFF DRIVE", ProcessorDiffDrive)
+} // namespace wolf
+
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index 8448d1913bd6cee2e5b76aa23aa4aa2fbc22c008..c604f080a9919d749ed04bb77db4cdddaaacb83f 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -5,7 +5,7 @@
* \author: Pierre Guetschel
*/
-#include <core/processor/processor_loopclosure.h>
+#include "core/processor/processor_loopclosure.h"
namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 27b56edaa822a7d9e0ceee7def1a64d0aa55dde3..9f997e1f51e71d2405cf5c0c36abc459149f29ee 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -65,11 +65,11 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
// Update the existing capture
_capture_source->setOriginFrame(_keyframe_target);
-// // Get optimized calibration params from 'origin' keyframe
-// VectorXs calib_preint_optim = _capture_source->getOriginFrame()->getCaptureOf(getSensor())->getCalibration();
-//
-// // Write the calib params into the capture before re-integration
-// _capture_source->setCalibrationPreint(calib_preint_optim);
+ // Get optimized calibration params from 'origin' keyframe
+ VectorXs calib_preint_optim = _capture_source->getOriginFrame()->getCaptureOf(getSensor())->getCalibration();
+
+ // Write the calib params into the capture before re-integration
+ _capture_source->setCalibrationPreint(calib_preint_optim);
// re-integrate target capture's buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_target);
@@ -280,7 +280,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
VectorXs calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
- VectorXs calib_preint = capture_motion->getBuffer().getCalibrationPreint();
+ VectorXs calib_preint = capture_motion->getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
VectorXs delta_step = motion.jacobian_calib_ * (calib - calib_preint);
@@ -362,7 +362,7 @@ void ProcessorMotion::integrateOneStep()
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
- calib_preint_ = getBuffer().getCalibrationPreint();
+ calib_preint_ = getLast()->getCalibrationPreint();
// get data and convert it to delta, and obtain also the delta covariance
computeCurrentDelta(incoming_ptr_->getData(),
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 16253a32fa230da41fb6f123f151304640cdef8f..664ee4ae943248b81140b6663a3987f3a80dbf24 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -29,18 +29,18 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
_delta(2) = _data(1);
// Fill delta covariance
- Eigen::MatrixXs J(delta_cov_size_, data_size_);
- J(0, 0) = cos(_data(1) / 2);
- J(1, 0) = sin(_data(1) / 2);
- J(2, 0) = 0;
- J(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
- J(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
- J(2, 1) = 1;
+ Eigen::MatrixXs J_delta_data(delta_cov_size_, data_size_);
+ J_delta_data(0, 0) = cos(_data(1) / 2);
+ J_delta_data(1, 0) = sin(_data(1) / 2);
+ J_delta_data(2, 0) = 0;
+ J_delta_data(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
+ J_delta_data(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
+ J_delta_data(2, 1) = 1;
// Since input data is size 2, and delta is size 3, the noise model must be given by:
// 1. Covariance of the input data: J*Q*J.tr
// 2. Fixed variance term to be added: var*Id
- _delta_cov = J * _data_cov * J.transpose() + unmeasured_perturbation_cov_;
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose() + unmeasured_perturbation_cov_;
}
void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
@@ -74,7 +74,7 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
// 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);
+ _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_);
@@ -94,24 +94,7 @@ void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
Motion ProcessorOdom2D::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;
-
return ProcessorMotion::interpolate(_ref, _second, _ts);
-
}
Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
@@ -124,21 +107,24 @@ bool ProcessorOdom2D::voteForKeyFrame()
// Distance criterion
if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2D_->dist_traveled)
{
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2D_->angle_turned)
{
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
return true;
}
// Uncertainty criterion
if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2D_->cov_det)
{
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
return true;
}
// Time criterion
- WOLF_TRACE("orig t: ", origin_ptr_->getFrame()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get());
if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2D_->max_time_span)
{
+ WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
return true;
}
return false;
@@ -218,7 +204,7 @@ ProcessorBasePtr ProcessorOdom2D::createAutoConf(const std::string& _unique_name
return prc_ptr;
}
-}
+} /* namespace wolf */
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 727e8bfb6308bef33798785eabc9beff109f2bc2..6cfe3c6121d8422d2744b3bae1f94cc737071303 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -1,133 +1,82 @@
+/**
+ * \file sensor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
#include "core/sensor/sensor_diff_drive.h"
-#include "core/state_block/state_block.h"
-#include "core/capture/capture_motion.h"
-#include "core/utils/eigen_assert.h"
+#include "core/state_block/state_angle.h"
-namespace wolf {
+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, false),
- intrinsics_ptr_{_intrinsics}
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
+ const IntrinsicsDiffDrivePtr& _intrinsics) :
+ SensorBase("DIFF DRIVE",
+ std::make_shared<StateBlock>(_extrinsics.head(2), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ std::make_shared<StateBlock>(3, false),
+ 2),
+ params_diff_drive_(_intrinsics)
{
- //
+ params_diff_drive_->radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
+ getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
+ getIntrinsic()->unfix();
}
-// Define the factory method
-SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
+SensorDiffDrive::~SensorDiffDrive()
{
- 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_, false);
-
- SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
-
- odo->setName(_unique_name);
-
- /// @todo make calibration optional at creation
- //if (calibrate)
- // odo->unfixIntrinsics();
-
- return odo;
+ // TODO Auto-generated destructor stub
}
-/// @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::initIntrisics()
-//{
-// 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;
+// Define the factory method
+SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, const Eigen::VectorXs& extrinsics,
+ const IntrinsicsBasePtr _intrinsics)
+{
+ // extrinsics
+ assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-// /// 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;
+ // intrinsics
+ assert(_intrinsics != nullptr && "Intrinsics provided are nullptr.");
+ IntrinsicsDiffDrivePtr params = std::static_pointer_cast<IntrinsicsDiffDrive>(_intrinsics);
-// /// Set covariance matrix (diagonal):
-// _data_cov.diagonal() << dp_var_l + dp_var_avg,
-// dp_var_r + dp_var_avg;
-//}
+ // build sensor
+ SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params);
-// 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)
-//{
-// if (intrinsics_.data_is_position_)
-// {
-// Eigen::Vector2s data = _capture_ptr->getData();
+ // last details
+ diff_drive->setName(_unique_name);
-// // 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);
+ return diff_drive;
+}
-// _capture_ptr->setData(data);
+SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server)
+{
+ // extrinsics
+ Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos");
+ assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-// Eigen::Matrix2s data_cov;
-// data_cov << 0.00001, 0, 0, 0.00001; // Todo
+ // intrinsics
+ IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server);
-// computeDataCov(data, data_cov);
+ // build sensor
+ SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params);
-// _capture_ptr->setDataCovariance(data_cov);
-// }
+ // last details
+ diff_drive ->setName(_unique_name);
-// /// @todo tofix
-// return false;//SensorBase::addCapture(_capture_ptr);
-//}
+ return diff_drive;
+}
-} // namespace wolf
+} /* namespace wolf */
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
} // namespace wolf
+#include "core/sensor/autoconf_sensor_factory.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR_AUTO("DIFF DRIVE", SensorDiffDrive)
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b8c0f9856393c54108ce7afe4d93b2f68515450b..49f5dfbb1ddb89fc2b9c212b4cadc9fbea300a29 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,7 +28,7 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
# Create a specific test executable for gtest_example #
wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PROJECT_NAME} dummy) #
+target_link_libraries(gtest_example ${PROJECT_NAME}) #
# #
###########################################################
set(SRC_DUMMY
@@ -45,23 +45,23 @@ add_library(dummy ${SRC_DUMMY})
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_capture_base ${PROJECT_NAME})
# CaptureBase class test
#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
-#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME} dummy)
+#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
# Converter from String to various types used by the parameters server
wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_converter ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
@@ -69,23 +69,23 @@ target_link_libraries(gtest_emplace ${PROJECT_NAME} dummy)
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_feature_base ${PROJECT_NAME})
# FrameBase classes test
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_frame_base ${PROJECT_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_local_param ${PROJECT_NAME})
# Logging test
wolf_add_gtest(gtest_logging gtest_logging.cpp)
-target_link_libraries(gtest_logging ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_logging ${PROJECT_NAME})
# MotionBuffer class test
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
# PackKFBuffer
wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
@@ -93,11 +93,11 @@ target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME} dummy)
# Parameters server
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_param_server ${PROJECT_NAME})
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
@@ -109,7 +109,7 @@ target_link_libraries(gtest_processor_base ${PROJECT_NAME} dummy)
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
@@ -119,83 +119,91 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
# shared_from_this test
wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
# SolverManager test
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
# TrackMatrix class test
wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
# TrajectoryBase class test
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_trajectory ${PROJECT_NAME})
# ------- Now Derived classes ----------
IF (Ceres_FOUND)
# CeresManager test
wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp)
-target_link_libraries(gtest_ceres_manager ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_ceres_manager ${PROJECT_NAME})
ENDIF(Ceres_FOUND)
# FactorAbs(P/O/V) classes test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
# FactorAutodiffDistance3D test
wolf_add_gtest(gtest_factor_autodiff_distance_3D gtest_factor_autodiff_distance_3D.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME})
+
+# FactorOdom3D class test
+wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
+target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
# FactorOdom3D class test
wolf_add_gtest(gtest_factor_odom_3D gtest_factor_odom_3D.cpp)
-target_link_libraries(gtest_factor_odom_3D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_odom_3D ${PROJECT_NAME})
# FactorPose2D class test
wolf_add_gtest(gtest_factor_pose_2D gtest_factor_pose_2D.cpp)
-target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
# FactorPose3D class test
wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
-target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
# Map yaml save/load test
wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
-target_link_libraries(gtest_map_yaml ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+
+# ProcessorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+target_link_libraries(gtest_processor_diff_drive ${PROJECT_NAME})
# ProcessorLoopClosureBase class test
wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
-target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME} dummy)
+# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
# ProcessorMotion in 2D
wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
-target_link_libraries(gtest_odom_2D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
# ProcessorOdom3D class test
wolf_add_gtest(gtest_processor_odom_3D gtest_processor_odom_3D.cpp)
-target_link_libraries(gtest_processor_odom_3D ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_odom_3D ${PROJECT_NAME})
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
@@ -205,8 +213,12 @@ target_link_libraries(gtest_processor_tracker_feature_dummy ${PROJECT_NAME} dumm
wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dummy)
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
+target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME})
+
# yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
-target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1e8f781fe2d50604e6b5a5f605f8b78ee1cd888
--- /dev/null
+++ b/test/gtest_factor_diff_drive.cpp
@@ -0,0 +1,648 @@
+/**
+ * \file gtest_factor_diff_drive.cpp
+ *
+ * Created on: Jul 24, 2019
+ * \author: jsola
+ */
+
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_diff_drive.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+#include "core/frame/frame_base.h"
+#include "core/capture/capture_motion.h"
+#include "core/feature/feature_motion.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
+
+class ProcessorDiffDrivePublic : public ProcessorDiffDrive
+{
+ using Base = ProcessorDiffDrive;
+ public:
+ ProcessorDiffDrivePublic(ProcessorParamsDiffDrivePtr _params_motion) :
+ ProcessorDiffDrive(_params_motion)
+ {
+ //
+ }
+ virtual void configure(SensorBasePtr _sensor)
+ {
+ Base::configure(_sensor);
+ }
+ 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)
+ {
+ Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
+ }
+
+ virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
+ const Eigen::VectorXs& _delta2,
+ const Scalar _dt2,
+ Eigen::VectorXs& _delta1_plus_delta2)
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+ }
+
+ 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)
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+ }
+
+ virtual void statePlusDelta(const Eigen::VectorXs& _x,
+ const Eigen::VectorXs& _delta,
+ const Scalar _dt,
+ Eigen::VectorXs& _x_plus_delta)
+ {
+ Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ }
+
+ virtual Eigen::VectorXs deltaZero() const
+ {
+ return Base::deltaZero();
+ }
+
+ 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)
+ {
+ return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin);
+ }
+
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own)
+ {
+ return Base::emplaceFeature(_capture_own);
+ }
+
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin)
+ {
+ return Base::emplaceFactor(_feature_motion, _capture_origin);
+ }
+
+ ProcessorParamsDiffDrivePtr getParams()
+ {
+ return params_prc_diff_drv_selfcal_;
+ }
+
+ Scalar getRadPerTick()
+ {
+ return radians_per_tick_;
+ }
+
+};
+
+
+
+class FactorDiffDriveTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ CeresManagerPtr solver;
+ TrajectoryBasePtr trajectory;
+ IntrinsicsDiffDrivePtr intr;
+ SensorDiffDrivePtr sensor;
+ ProcessorParamsDiffDrivePtr params;
+ ProcessorDiffDrivePublicPtr processor;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorDiffDrivePtr c1;
+
+ Vector2s data0, data1;
+ Matrix2s data_cov;
+ Vector3s delta0, delta1;
+ Matrix3s delta0_cov, delta1_cov;
+ Vector3s calib;
+ Vector3s residual;
+
+ virtual void SetUp()
+ {
+ problem = Problem::create("PO", 2);
+ trajectory = problem->getTrajectory();
+
+ solver = std::make_shared<CeresManager>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3s extr(0, 0, 0);
+ auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ calib = sensor->getIntrinsic()->getState();
+
+ // params and processor
+ params = std::make_shared<ProcessorParamsDiffDrive>();
+ params->angle_turned = 1;
+ params->dist_traveled = 2;
+ params->max_buff_length = 3;
+ auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+ // frames
+ F0 = FrameBase::emplace<FrameBase>(trajectory,
+ "PO",
+ 2,
+ KEY,
+ 0.0,
+ Vector3s(0,0,0));
+ F1 = FrameBase::emplace<FrameBase>(trajectory,
+ "PO",
+ 2,
+ KEY,
+ 1.0,
+ Vector3s(1,0,0));
+
+ // captures
+ C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
+ 0.0,
+ sensor,
+ data0.Zero(),
+ data_cov,
+ nullptr);
+
+ data1 << 100/(2*M_PI), 100/(2*M_PI); // we go straight -- this can be changed later on
+ data_cov.setIdentity();
+ delta1 << 1,0,0;
+ delta1_cov.setIdentity();
+
+ C1 = CaptureBase::emplace<CaptureDiffDrive>(F1,
+ 1.0,
+ sensor,
+ data1.Zero(),
+ data_cov,
+ nullptr);
+
+
+ // feature
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "DIFF DRIVE",
+ delta1,
+ delta1_cov,
+ C0->getCalibration(),
+ Matrix3s::Identity()); // TODO Jacobian?
+
+
+ // final checks
+ startup_config_for_all_tests();
+ }
+
+ virtual void TearDown()
+ {
+ }
+
+ void startup_config_for_all_tests()
+ {
+ ASSERT_NE(sensor, nullptr);
+ ASSERT_FALSE(sensor->getIntrinsic()->isFixed());
+
+ ASSERT_NE(processor, nullptr);
+
+ ASSERT_NE(C0, nullptr);
+
+ ASSERT_NE(f1, nullptr);
+ ASSERT_MATRIX_APPROX(f1->getMeasurement(), delta1, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getMeasurementCovariance(), delta1_cov, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getJacobianCalibration(), Matrix3s::Identity(), 1e-6);
+ }
+
+};
+
+TEST_F(FactorDiffDriveTest, constructor)
+{
+ // plain constructor
+ auto c1_obj = FactorDiffDrive(f1, C0, processor);
+ ASSERT_EQ(c1_obj.getType(), "DIFF DRIVE");
+
+ // std: make_shared
+ c1 = std::make_shared<FactorDiffDrive>(f1,
+ C0,
+ processor);
+
+ ASSERT_NE(c1, nullptr);
+ ASSERT_EQ(c1->getType(), "DIFF DRIVE");
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1,
+ f1,
+ C0,
+ processor);
+
+ ASSERT_NE(c1, nullptr);
+ ASSERT_EQ(c1->getType(), "DIFF DRIVE");
+ ASSERT_EQ(c1->getCaptureOther()->getSensorIntrinsic(), sensor->getIntrinsic());
+
+}
+
+TEST_F(FactorDiffDriveTest, residual_zeros)
+{
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1,
+ f1,
+ C0,
+ processor);
+
+ residual = c1->residual();
+
+ ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-12);
+}
+
+TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
+{
+ MatrixXs J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXs delta(3), delta2(3);
+ MatrixXs delta_cov(3,3);
+ Scalar dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+
+ f1->setMeasurement(delta2);
+ F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+
+ residual = c1->residual();
+
+ ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-12);
+}
+
+TEST_F(FactorDiffDriveTest, solve_F1)
+{
+ MatrixXs J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXs delta(3), delta2(3);
+ MatrixXs delta_cov(3,3);
+ Scalar dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ f1->setMeasurement(delta2);
+ F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+
+ residual = c1->residual();
+
+ // Fix all but F1 ; perturb F1 ; solve ; print and assert values of F1
+ F0->fix();
+ sensor->fixIntrinsics();
+ F1->setState(F1->getState() + Vector3s::Random() * 0.1);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+// WOLF_TRACE("\n", report);
+ problem->print(1,0,1,1);
+
+ ASSERT_MATRIX_APPROX(F1->getState(), delta, 1e-6);
+
+}
+
+TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
+{
+ MatrixXs J_delta_calib(3,3);
+
+ // right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ VectorXs delta(3), delta2(3);
+ MatrixXs delta_cov(3,3);
+ Scalar dt = 1.0;
+
+ data1(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
+
+ f1->setMeasurement(delta2);
+ F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+
+ // wolf: emplace
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+
+ residual = c1->residual();
+
+ // Fix all but S ; perturb Sensor ; solve ; print and assert values of Sensor
+ F0->fix();
+ F1->fix();
+ sensor->unfixIntrinsics();
+ sensor->getIntrinsic()->setState(calib + Vector3s::Random() * 0.1);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+// WOLF_TRACE("\n", report);
+ problem->print(1,0,1,1);
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib, 1e-6);
+
+}
+
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
+{
+
+ /*
+ * The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+ * Passage poses are:
+ * x0: Initial: 0 , 0 , 0
+ * x1: Half : 1.5, -1.5 , -pi/2
+ * x2: Final : 3 , -3 , 0
+ *
+ * x0 1.5 3
+ * *> ------+--------+------> X
+ * | *
+ * |
+ * | *
+ * |
+ * -1.5 + * x1
+ * | v
+ * | *
+ * |
+ * | * x2
+ * -3 + *>
+ * |
+ * v
+ * -Y
+ *
+ * Notes:
+ * - We make two arcs because the intrinsics are not observable with just one arc.
+ * - The deltas are pre-integrated with ground truth values of the intrinsics. They are perturbed before solving.
+ * - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+ * - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+ * - The total of deltas is 12.
+ * - The total of keyframes is 7.
+ * - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+ * - The other 5 keyframes including x1 are estimated.
+ * - The sensor intrinsics are also estimated.
+ */
+
+ ProblemPtr problem = Problem::create("PO", 2);
+ CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3s extr(0, 0, 0);
+ auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ auto calib_preint = sensor->getIntrinsic()->getState();
+ Vector3s calib_gt(1,1,1);
+
+ // params and processor
+ ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+ params->angle_turned = 99;
+ params->dist_traveled = 99;
+ params->max_buff_length = 99;
+ params->voting_active = true;
+ params->max_time_span = 1.5;
+ auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+
+ TimeStamp t(0.0);
+ Scalar dt = 1.0;
+ Vector3s x0(0,0,0);
+ Vector3s x1(1.5, -1.5, -M_PI/2.0);
+ Vector3s x2(3.0, -3.0, 0.0);
+ Matrix3s P0; P0.setIdentity();
+
+ auto F0 = problem->setPrior(x0, P0, t, 0.1);
+
+ // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+ int N = 6;
+ Vector3s data;
+ Matrix2s data_cov; data_cov.setIdentity();
+
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ // left turn 90 deg in N steps --> ends up in (3, -3, 0)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+ C->setData(data);
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ auto F2 = problem->getLastKeyFrame();
+
+
+ // Fix boundaries and unfix S ;
+ F0->fix();
+ F2->fix();
+ sensor->unfixIntrinsics();
+
+ // Perturb S
+ Vector3s calib_pert = calib_gt + Vector3s::Random()*0.2;
+ sensor->getIntrinsic()->setState(calib_pert);
+
+ WOLF_TRACE("\n ========== SOLVE =========");
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE("\n", report);
+
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
+ WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("calib_preint : ", calib_preint.transpose());
+ WOLF_TRACE("calib_pert : ", calib_pert.transpose());
+ WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
+ WOLF_TRACE("calib_gt : ", calib_gt.transpose());
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-6);
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-6);
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+
+ std::cout << "\n\n" << std::endl;
+
+}
+
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
+{
+ /*
+ * The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+ * Passage poses are:
+ * x0: Initial: 0 , 0 , 0
+ * x1: Half : 1.5, -1.5 , -pi/2
+ * x2: Final : 3 , -3 , 0
+ *
+ * x0 1.5 3
+ * *> ------+--------+------> X
+ * | *
+ * |
+ * | *
+ * |
+ * -1.5 + * x1
+ * | v
+ * | *
+ * |
+ * | * x2
+ * -3 + *>
+ * |
+ * v
+ * -Y
+ *
+ * Notes:
+ * - We make two arcs because the intrinsics are not observable with just one arc.
+ * - The deltas are pre-integrated with wrong values of the intrinsics. The true values must be found by solving.
+ * - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+ * - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+ * - The total of deltas is 12.
+ * - The total of keyframes is 7.
+ * - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+ * - The other 5 keyframes including x1 are estimated.
+ * - The sensor intrinsics are also estimated.
+ */
+
+
+
+ ProblemPtr problem = Problem::create("PO", 2);
+ CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radius_left = 0.95;
+ intr->radius_right = 0.98;
+ intr->wheel_separation = 1.05;
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3s extr(0, 0, 0);
+ auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ auto calib_preint = sensor->getIntrinsic()->getState();
+ Vector3s calib_gt(1.0, 1.0, 1.0); // ground truth calib
+
+ // params and processor
+ ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+ params->angle_turned = 99;
+ params->dist_traveled = 99;
+ params->max_buff_length = 99;
+ params->voting_active = true;
+ params->max_time_span = 1.5;
+ auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+
+ TimeStamp t(0.0);
+ Scalar dt = 1.0;
+ Vector3s x0(0,0,0);
+ Vector3s x1(1.5, -1.5, -M_PI/2.0);
+ Vector3s x2(3.0, -3.0, 0.0);
+ Matrix3s P0; P0.setIdentity();
+
+ auto F0 = problem->setPrior(x0, P0, t, 0.1);
+
+ // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+ int N = 6;
+ Vector3s data;
+ Matrix2s data_cov; data_cov.setIdentity();
+
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ // left turn 90 deg in N steps --> ends up in (3, -3, 0)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+
+ C->setData(data);
+
+ for (int n = 0; n < N; n++)
+ {
+ t += dt;
+ C->setTimeStamp(t);
+ C->process();
+ }
+
+ auto F2 = problem->getLastKeyFrame();
+
+ F2->setState(x2); // Impose known final state regardless of integrated value.
+
+ // Fix boundaries and unfix S ;
+ F0->fix();
+ F2->fix();
+ sensor->unfixIntrinsics();
+
+
+ WOLF_TRACE("\n ========== SOLVE =========");
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE("\n", report);
+
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
+ WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("calib_preint : ", calib_preint.transpose());
+ WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
+ WOLF_TRACE("calib_GT : ", calib_gt.transpose());
+
+
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.*";
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.preintegrate_and_solve_sensor_intrinsics";
+return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 8bd5f665921b35b0087d4636e86cd74691869cda..af262f1170bdcfffa0c633e858cebc5d9fc41562 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -40,7 +40,7 @@ Motion m4 = newMotion(t4, 4, 10, 1, .1, 1);
TEST(MotionBuffer, QueryTimeStamps)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB(1,1,1);
MB.get().push_back(m0);
MB.get().push_back(m1);
@@ -64,7 +64,7 @@ TEST(MotionBuffer, QueryTimeStamps)
TEST(MotionBuffer, getMotion)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB(1,1,1);
MB.get().push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
@@ -78,7 +78,7 @@ TEST(MotionBuffer, getMotion)
TEST(MotionBuffer, getDelta)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB(1,1,1);
MB.get().push_back(m0);
@@ -92,7 +92,7 @@ TEST(MotionBuffer, getDelta)
TEST(MotionBuffer, Split)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB(1,1,1);
MB.get().push_back(m0);
MB.get().push_back(m1);
@@ -100,7 +100,7 @@ TEST(MotionBuffer, Split)
MB.get().push_back(m3);
MB.get().push_back(m4); // put 5 motions
- MotionBuffer MB_old(1,1,1,0);
+ MotionBuffer MB_old(1,1,1);
TimeStamp t = 1.5; // between m1 and m2
MB.split(t, MB_old);
@@ -156,13 +156,13 @@ TEST(MotionBuffer, Split)
// Eigen::MatrixXs cov = MB.integrateCovariance(t1, t3);
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
//
-// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integate
+// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integrate
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
// }
TEST(MotionBuffer, print)
{
- MotionBuffer MB(1,1,1,0);
+ MotionBuffer MB(1,1,1);
MB.get().push_back(m0);
MB.get().push_back(m1);
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f021ba4248e88e85353354abbb52cdee33de7ef0
--- /dev/null
+++ b/test/gtest_processor_diff_drive.cpp
@@ -0,0 +1,344 @@
+/**
+ * \file gtest_processor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/utils/utils_gtest.h"
+
+#include "core/capture/capture_diff_drive.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
+
+class ProcessorDiffDrivePublic : public ProcessorDiffDrive
+{
+ using Base = ProcessorDiffDrive;
+ public:
+ ProcessorDiffDrivePublic(ProcessorParamsDiffDrivePtr _params_motion) :
+ ProcessorDiffDrive(_params_motion)
+ {
+ //
+ }
+ virtual void configure(SensorBasePtr _sensor)
+ {
+ Base::configure(_sensor);
+ }
+ 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)
+ {
+ Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
+ }
+
+ virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
+ const Eigen::VectorXs& _delta2,
+ const Scalar _dt2,
+ Eigen::VectorXs& _delta1_plus_delta2)
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+ }
+
+ 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)
+ {
+ Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+ }
+
+ virtual void statePlusDelta(const Eigen::VectorXs& _x,
+ const Eigen::VectorXs& _delta,
+ const Scalar _dt,
+ Eigen::VectorXs& _x_plus_delta)
+ {
+ Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ }
+
+ virtual Eigen::VectorXs deltaZero() const
+ {
+ return Base::deltaZero();
+ }
+
+ 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)
+ {
+ return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin);
+ }
+
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own)
+ {
+ return Base::emplaceFeature(_capture_own);
+ }
+
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin)
+ {
+ return Base::emplaceFactor(_feature_motion, _capture_origin);
+ }
+
+ ProcessorParamsDiffDrivePtr getParams()
+ {
+ return params_prc_diff_drv_selfcal_;
+ }
+
+ Scalar getRadPerTick()
+ {
+ return radians_per_tick_;
+ }
+
+};
+
+class ProcessorDiffDriveTest : public testing::Test
+{
+ public:
+ IntrinsicsDiffDrivePtr intr;
+ SensorDiffDrivePtr sensor;
+ ProcessorParamsDiffDrivePtr params;
+ ProcessorDiffDrivePublicPtr processor;
+ ProblemPtr problem;
+
+ virtual void SetUp()
+ {
+ problem = Problem::create("PO", 2);
+
+ // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+ intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
+ intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+ Vector3s extr(0,0,0);
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("DIFF DRIVE", "sensor diff drive", extr, intr));
+
+ // params and processor
+ params = std::make_shared<ProcessorParamsDiffDrive>();
+ params->voting_active = true;
+ params->angle_turned = 1;
+ params->dist_traveled = 2;
+ params->max_buff_length = 3;
+ params->max_time_span = 2.5;
+ params->unmeasured_perturbation_std = 1e-4;
+ processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("DIFF DRIVE", "processor diff drive", sensor, params));
+ }
+
+ virtual void TearDown(){}
+
+};
+
+TEST(ProcessorDiffDrive, constructor)
+{
+ auto params = std::make_shared<ProcessorParamsDiffDrive>();
+
+ ASSERT_NE(params, nullptr);
+
+ auto prc = std::make_shared<ProcessorDiffDrive>(params);
+
+ ASSERT_NE(prc, nullptr);
+
+ ASSERT_EQ(prc->getType(), "DIFF DRIVE");
+}
+
+TEST(ProcessorDiffDrive, create)
+{
+ // make a sensor first
+ auto intr = std::make_shared<IntrinsicsDiffDrive>();
+ Vector3s extr(1,2,3);
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ // params
+ auto params = std::make_shared<ProcessorParamsDiffDrive>();
+
+ // processor
+ auto prc_base = ProcessorDiffDrive::create("prc", params, sen);
+
+ auto prc = std::static_pointer_cast<ProcessorDiffDrive>(prc_base);
+
+ ASSERT_NE(prc, nullptr);
+
+ ASSERT_EQ(prc->getType(), "DIFF DRIVE");
+}
+
+TEST_F(ProcessorDiffDriveTest, params)
+{
+ // read
+ ASSERT_EQ(processor->getParams()->angle_turned , 1.0);
+ ASSERT_EQ(processor->getParams()->dist_traveled, 2.0);
+
+ // write
+ processor->getParams()->angle_turned = 5;
+ ASSERT_EQ(processor->getParams()->angle_turned, 5.0);
+}
+
+TEST_F(ProcessorDiffDriveTest, configure)
+{
+ processor->configure(sensor);
+ ASSERT_EQ(processor->getRadPerTick(), 2.0*M_PI/100); // 100 ticks per revolution
+}
+
+TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
+{
+ // 1. zero delta
+ Vector2s data(0,0);
+ Matrix2s data_cov; data_cov . setIdentity();
+ Vector3s calib(1,1,1);
+ Scalar dt = 1.0;
+ VectorXs delta(3);
+ MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3s::Zero(), 1e-8);
+
+ // 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ ASSERT_MATRIX_APPROX(delta, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
+{
+
+ Vector2s data;
+ Matrix2s data_cov; data_cov . setIdentity();
+ Vector3s calib(1,1,1);
+ Scalar dt = 1.0;
+ VectorXs delta(3), delta1(3), delta2(3);
+ MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+
+ // 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 30
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 60
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ delta1 .setZero();
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 22.5
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 45
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 67.5
+ delta1 = delta2;
+ processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
+
+ ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, statePlusDelta)
+{
+
+ Vector2s data;
+ Matrix2s data_cov; data_cov . setIdentity();
+ Vector3s calib(1,1,1);
+ Scalar dt = 1.0;
+ VectorXs delta(3), x1(3), x2(3);
+ MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+
+ // 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ x1 .setZero();
+ processor->statePlusDelta(x1, delta, dt, x2); // 30
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 60
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 90
+
+ ASSERT_MATRIX_APPROX(x2, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+
+ // 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
+ data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
+
+ x1 .setZero();
+ processor->statePlusDelta(x1, delta, dt, x2); // 22.5
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 45
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 67.5
+ x1 = x2;
+ processor->statePlusDelta(x1, delta, dt, x2); // 90
+
+ ASSERT_MATRIX_APPROX(x2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+}
+
+TEST_F(ProcessorDiffDriveTest, process)
+{
+
+ Vector2s data;
+ Matrix2s data_cov; data_cov . setIdentity();
+ TimeStamp t = 0.0;
+ Scalar dt = 1.0;
+ Vector3s x(0,0,0);
+ Matrix3s P; P.setIdentity();
+
+ auto F0 = problem->setPrior(x, P, t, 0.1);
+
+ // 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
+ int N = 9;
+ data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+ WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ for (int n = 1; n <= N; n++)
+ {
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+
+ t += dt;
+ C->setTimeStamp(t);
+ }
+
+ problem->print(4,1,1,1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29ccea249acb27a6b0ac3bbbfb88b7bc0516d05c
--- /dev/null
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -0,0 +1,81 @@
+/**
+ * \file gtest_sensor_diff_drive.cpp
+ *
+ * Created on: Jul 22, 2019
+ * \author: jsola
+ */
+
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/utils/utils_gtest.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(DiffDrive, constructor)
+{
+
+ auto intr = std::make_shared<IntrinsicsDiffDrive>();
+ Vector3s extr(1,2,3);
+
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), Vector2s(1,2), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), Vector1s(3), 1e-12);
+}
+
+TEST(DiffDrive, getParams)
+{
+ auto intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radians_per_tick = 1;
+ intr->radius_left = 2;
+ intr->radius_right = 3;
+ intr->ticks_per_wheel_revolution = 4;
+ intr->wheel_separation = 5;
+
+ Vector3s extr(1,2,3);
+
+ auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+
+ ASSERT_NE(sen->getParams(), nullptr);
+
+ ASSERT_EQ(sen->getParams()->radians_per_tick, 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getParams()->radius_left, 2);
+ ASSERT_EQ(sen->getParams()->radius_right, 3);
+ ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
+ ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+}
+
+TEST(DiffDrive, create)
+{
+ auto intr = std::make_shared<IntrinsicsDiffDrive>();
+ intr->radians_per_tick = 1;
+ intr->radius_left = 2;
+ intr->radius_right = 3;
+ intr->ticks_per_wheel_revolution = 4;
+ intr->wheel_separation = 5;
+
+ Vector3s extr(1,2,3);
+
+ auto sen_base = SensorDiffDrive::create("name", extr, intr);
+
+ auto sen = std::static_pointer_cast<SensorDiffDrive>(sen_base);
+
+ ASSERT_NE(sen->getParams(), nullptr);
+
+ ASSERT_EQ(sen->getParams()->radians_per_tick, 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
+ ASSERT_EQ(sen->getParams()->radius_left, 2);
+ ASSERT_EQ(sen->getParams()->radius_right, 3);
+ ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
+ ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+