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(); +} +