diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 93de8a4bc9455e83e2f10e181cfb681ff40457e8..1477cdc248844ab181e44194a77c04573381a155 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -29,6 +29,7 @@ before_script: - cd build - ls - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF .. + - make -j$(nproc) - make install - cd ../.. @@ -54,6 +55,7 @@ before_script: - cd build - ls - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF .. + - make -j$(nproc) - make install - cd ../.. @@ -67,4 +69,5 @@ wolf_build_and_test: - ls # we can check whether the directory was already full - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. - make -j$(nproc) - - ctest + - ctest -j$(nproc) + - make install diff --git a/CMakeLists.txt b/CMakeLists.txt index 4aa56e351ef94fb74f64eb72a2e126eb29c53ac2..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,11 +246,8 @@ 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_capture_holder.h include/core/processor/processor_diff_drive.h include/core/processor/processor_factory.h include/core/processor/processor_logging.h @@ -325,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 @@ -333,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 ) @@ -343,7 +341,6 @@ SET(SRCS_LANDMARK SET(SRCS_PROCESSOR src/processor/motion_buffer.cpp src/processor/processor_base.cpp - src/processor/processor_capture_holder.cpp src/processor/processor_diff_drive.cpp src/processor/processor_loopclosure.cpp src/processor/processor_motion.cpp diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index be22913c2594f8c912efd7e69d4ddbb529a5f222..5685c411998415689aa8d795b4f8c61417382f3d 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -75,7 +75,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) { // new landmark: // - create landmark - lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing))); + lmk = LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)); WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose()); // - add to known landmarks known_lmks.emplace(id, lmk); diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index acecc0f7c6a9aecd4de40a34e178b08654f46dc5..387fd71d5c82dd05fc4d14aab782254cca28557b 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -107,7 +107,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void move(FrameBasePtr); void link(FrameBasePtr); template<typename classType, typename... T> - static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all); + static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all); protected: SizeEigen computeCalibSize() const; @@ -130,9 +130,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture namespace wolf{ template<typename classType, typename... T> -std::shared_ptr<CaptureBase> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all) +std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all) { - CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...); cpt->link(_frm_ptr); return cpt; } 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_base.h b/include/core/factor/factor_base.h index b6b7953940279fef26735a06b3d012910590be77..6593383e5137629b94a5f60cf17bc053b0b9991b 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -165,7 +165,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa void link(FeatureBasePtr ftr); template<typename classType, typename... T> - static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all); + static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all); private: @@ -191,9 +191,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa namespace wolf{ template<typename classType, typename... T> -std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all) +std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all) { - FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> ctr = std::make_shared<classType>(std::forward<T>(all)...); ctr->link(_ftr_ptr); return ctr; } 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_base.h b/include/core/feature/feature_base.h index eb4094d1aa0581478aeef801845abd8ee98ca1b1..65e9228f5b48a6c9687c95d3c1d10c5cce58eb3b 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -98,7 +98,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature void link(CaptureBasePtr cap_ptr); template<typename classType, typename... T> - static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all); + static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all); protected: @@ -122,9 +122,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature namespace wolf{ template<typename classType, typename... T> - std::shared_ptr<FeatureBase> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all) + std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all) { - FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...); ftr->link(_cpt_ptr); return ftr; } 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/frame/frame_base.h b/include/core/frame/frame_base.h index 189b5b5a06ceb0275fa1d11c59ff25a8dad4bc89..e3548b5300b69f29f9bca6992a12f4b2a64c4f04 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -149,7 +149,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase const FactorBasePtrList& getConstrainedByList() const; void link(TrajectoryBasePtr); template<typename classType, typename... T> - static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all); + static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all); private: @@ -173,9 +173,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase namespace wolf { template<typename classType, typename... T> -std::shared_ptr<FrameBase> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all) +std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all) { - FrameBasePtr frm = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...); frm->link(_ptr); return frm; } diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index d4843590bd3974d02bcded1360616bac13cf8adb..e33351c3b3bf416baed99b4afa2601cdd923f67e 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -93,7 +93,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma MapBasePtr getMap(); void link(MapBasePtr); template<typename classType, typename... T> - static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all); + static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all); /** \brief Creator for Factory<LandmarkBase, YAML::Node> * Caution: This creator does not set the landmark's anchor frame and sensor. @@ -117,9 +117,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma namespace wolf{ template<typename classType, typename... T> -std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all) +std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all) { - LandmarkBasePtr lmk = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...); lmk->link(_map_ptr); return lmk; } 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_base.h b/include/core/processor/processor_base.h index 744e03fe6cb4b47fa8f8d17723ded75f2f78921c..3537f96032ba64bbbb901f722bd64bdf10bbe0ab 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -304,7 +304,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce void link(SensorBasePtr); template<typename classType, typename... T> - static std::shared_ptr<ProcessorBase> emplace(SensorBasePtr _sen_ptr, T&&... all); + static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all); void setVotingAuxActive(bool _voting_active = true); }; @@ -336,9 +336,9 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active) namespace wolf { template<typename classType, typename... T> -std::shared_ptr<ProcessorBase> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) +std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) { - ProcessorBasePtr prc = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...); prc->link(_sen_ptr); return prc; } diff --git a/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h deleted file mode 100644 index 19b33f8f2629cf4e6d1a61888732ce3d1d197568..0000000000000000000000000000000000000000 --- a/include/core/processor/processor_capture_holder.h +++ /dev/null @@ -1,86 +0,0 @@ -/** - * \file processor_capture_holder.h - * - * Created on: Jul 12, 2017 - * \author: Jeremie Deray - */ - -#ifndef _WOLF_PROCESSOR_CAPTURE_HOLDER_H_ -#define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_ - -//Wolf includes -#include "core/processor/processor_base.h" -#include "core/capture/capture_base.h" - -namespace wolf { - -WOLF_PTR_TYPEDEFS(ProcessorCaptureHolder); -WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder); - -/** - * \brief ProcessorParamsCaptureHolder - */ -struct ProcessorParamsCaptureHolder : public ProcessorParamsBase -{ - using ProcessorParamsBase::ProcessorParamsBase; -}; - -/** - * \brief ProcessorCaptureHolder - */ -class ProcessorCaptureHolder : public ProcessorBase -{ - public: - - ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder); - virtual ~ProcessorCaptureHolder() = default; - virtual void configure(SensorBasePtr _sensor) override { }; - - protected: - - /** \brief process an incoming capture - * - * The ProcessorCaptureHolder is only triggered in KF (see triggerInCapture()) so this function is not called. - */ - virtual void processCapture(CaptureBasePtr) override {}; - - /** \brief process an incoming key-frame - * - * Each derived processor should implement this function. It will be called if: - * - A new KF arrived and triggerInKF() returned true. - */ - virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override; - - /** \brief trigger in capture - * - * The ProcessorCaptureHolder only processes incoming KF, then it returns false. - */ - virtual bool triggerInCapture(CaptureBasePtr) override {return false;} - - /** \brief trigger in key-frame - * - * Returns true if processKeyFrame() should be called after the provided KF arrived. - */ - virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) override { return true; } - - /** \brief Vote for KeyFrame generation - * - * If a KeyFrame criterion is validated, this function returns true, - * meaning that it wants to create a KeyFrame at the \b last Capture. - * - * WARNING! This function only votes! It does not create KeyFrames! - */ - virtual bool voteForKeyFrame() override { return false; } - - ProcessorParamsCaptureHolderPtr params_capture_holder_; - - public: - - static ProcessorBasePtr create(const std::string& _unique_name, - const ProcessorParamsBasePtr _params, - const SensorBasePtr sensor_ptr = nullptr); -}; - -} // namespace wolf - -#endif // _WOLF_PROCESSOR_CAPTURE_HOLDER_H_ diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 36b6d5aa0885d9815135ab6221f7b0f029395602..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"); - } - 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 a5ff4c53382b3d6a1b6ace1fce41a28dbbc9bfab..6a96be5ef0c13a299e687bafb77bf38fbd48ede1 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 7b1acb5b2a22d8cdd7f8cc013ce14bfd0bad3321..de53f9fb400df6098785d144d11a9210dab9ebda 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -21,17 +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"); } + std::string print() { return "\n" + ProcessorParamsMotion::print() - + "cov_det: " + std::to_string(cov_det) + "\n"; + + "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 fb7444439d51bb68ef0c27d3a48cd7d860973299..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 ""; } }; @@ -199,7 +199,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa void link(HardwareBasePtr); template<typename classType, typename... T> - static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all); + static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all); protected: SizeEigen computeCalibSize() const; @@ -218,9 +218,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa namespace wolf{ template<typename classType, typename... T> -std::shared_ptr<SensorBase> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all) +std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all) { - SensorBasePtr sen = std::make_shared<classType>(std::forward<T>(all)...); + std::shared_ptr<classType> sen = std::make_shared<classType>(std::forward<T>(all)...); sen->link(_hwd_ptr); return sen; } diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 2988669c965c55d398938b49ae27167ca3dfd5fa..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"); - - 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"); - right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain"); - } - 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/include/core/utils/converter.h b/include/core/utils/converter.h index 36d59e759018377bb81266932a57d7a5d827b318..dceaf3e01bce136b7841c5b9bd015203de200386 100644 --- a/include/core/utils/converter.h +++ b/include/core/utils/converter.h @@ -1,12 +1,19 @@ #ifndef CONVERTER_H #define CONVERTER_H -#include <vector> + +// wolf +#include "core/common/wolf.h" + // Eigen #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> + +// std #include <regex> #include <iostream> #include <array> +#include <vector> +#include <stack> /** @file @@ -35,7 +42,7 @@ namespace utils{ /** @Brief Returns all the substrings of @val that match @exp * @param val String to be matched * @param exp Regular expression - * @return <b>{std::vector<std::string>}</b> Collection of matching subtrings + * @return <b>{std::vector<std::string>}</b> Collection of matching substrings */ static inline std::vector<std::string> getMatches(std::string val, std::regex exp){ std::smatch res; @@ -91,6 +98,55 @@ namespace utils{ } return result; } + static inline std::vector<std::string> parseList(std::string val){ + std::stack<char> limiters; + std::stack<std::string> word_stack; + std::string current_word; + std::vector<std::string> words; + std::vector<char> chars(val.begin(), val.end()); + for(const char ¤t : chars){ + if(current == '['){ + limiters.push(current); + word_stack.push(current_word); + current_word = ""; + }else if(current == ']'){ + if(limiters.empty()) throw std::runtime_error("Unmatched delimiter"); + if(limiters.top() == '[') { + if(limiters.size() > 1) { + if(word_stack.empty()) word_stack.push(""); + current_word = word_stack.top() + "[" + current_word + "]"; + word_stack.pop(); + }else if(limiters.size() == 1 and current_word != "") words.push_back(current_word); + else current_word += current; + limiters.pop(); + }else throw std::runtime_error("Unmatched delimiter"); + }else if(current == '{'){ + limiters.push(current); + word_stack.push(current_word); + current_word = ""; + }else if(current == '}'){ + if(limiters.top() == '{') { + if(limiters.size() > 1) { + if(word_stack.empty()) word_stack.push(""); + current_word = word_stack.top() + "{" + current_word + "}"; + word_stack.pop(); + }else if(limiters.size() == 1) words.push_back(current_word); + else current_word += current; + limiters.pop(); + }else throw std::runtime_error("Unmatched delimiter"); + }else if(current == ',') { + if(limiters.size() == 1 and current_word != "") { + words.push_back(current_word); + current_word = ""; + }else if(limiters.size() > 1) current_word += current; + }else { + if(limiters.empty()) throw std::runtime_error("Found non-delimited text"); + current_word += current; + } + } + if(not limiters.empty()) throw std::runtime_error("Unclosed delimiter [] or {}"); + return words; + } } namespace wolf{ @@ -102,14 +158,16 @@ struct converter{ }; template<typename A> struct converter<utils::list<A>>{ - static utils::list<A> convert(std::string val){ + static utils::list<A> convert(std::string val){ std::regex rgxP("\\[([^,]+)?(,[^,]+)*\\]"); utils::list<A> result = utils::list<A>(); if(std::regex_match(val, rgxP)) { - std::string aux = val.substr(1,val.size()-2); - auto l = utils::getMatches(aux, std::regex("([^,]+)")); + // std::string aux = val.substr(1,val.size()-2); + // auto l = utils::getMatches(aux, std::regex("([^,]+)")); + auto l = utils::parseList(val); for(auto it : l){ - result.push_back(converter<A>::convert(it)); + // WOLF_DEBUG("Asking to convert in list ", it); + result.push_back(converter<A>::convert(it)); } } else throw std::runtime_error("Invalid string format representing a list-like structure. Correct format is [(value)?(,value)*]. String provided: " + val); return result; @@ -118,11 +176,11 @@ struct converter<utils::list<A>>{ std::string aux = ""; bool first = true; for(auto it : val){ - if(not first) aux += "," + it; - else{ - first = false; - aux = it; - } + if(not first) aux += "," + converter<A>::convert(it); + else{ + first = false; + aux = converter<A>::convert(it); + } } return "[" + aux + "]"; } @@ -264,8 +322,11 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo template<typename A> struct converter<std::map<std::string,A>>{ static std::map<std::string,A> convert(std::string val){ - auto str_map = utils::splitMapStringRepresentation(val); - auto v = utils::pairSplitter(str_map); + std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]"); + if(not std::regex_match(val, rgxM)) + throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val); + + auto v = utils::parseList(val); auto map = std::map<std::string, A>(); for(auto it : v){ auto p = converter<std::pair<std::string, A>>::convert(it); diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp index 837d47ddabe2c52a88720ce2dd8570d2091b8e42..beb5a589a87aa99dec1996712190c146ddd8d504 100644 --- a/include/core/yaml/parser_yaml.hpp +++ b/include/core/yaml/parser_yaml.hpp @@ -14,52 +14,67 @@ #include <numeric> namespace { - /** @Brief Generates a std::string [v1,v2,v3,...] representing the YAML sequence node - * @param n a vector of YAML::Node where each node should be of type YAML::Node::Scalar - * @return <b>{std::string}</b> [v1,v2,v3,...] - */ - std::string fromSequenceToString(std::vector<YAML::Node> n){ - std::string aux = "["; - bool first = true; - for(auto it : n){ - assert(it.Type() == YAML::NodeType::Scalar && "fromSequenceToString requires that the sequence be a sequence of Scalars"); - if(first) { - aux = aux + it.Scalar(); - first = false; - }else{ - aux = aux + "," + it.Scalar(); - } - } - aux = aux + "]"; - return aux; - } - /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars. - * @param n a vector of YAML::Node that represents a YAML::Sequence - * @return <b>{std::string}</b> representing the YAML sequence - */ - std::string parseScalarSequence(std::vector<YAML::Node> n){ - std::string aux = "["; - bool first = true; - std::string separator = ""; - for(auto it : n){ - if(it.Type() == YAML::NodeType::Scalar) aux = aux + separator + it.Scalar(); - else { - auto seq = std::vector<YAML::Node>(); - for(auto itt : it){ - //I'm going to assume that the node is a sequence of scalars - assert(itt.Type() == YAML::NodeType::Scalar && "The sequence should be a sequence of Scalars"); - seq.push_back(itt); - } - aux = aux + separator + fromSequenceToString(seq); - } - if(first){ - separator = ","; - first = false; - } + //====== START OF FORWARD DECLARATION ======== + std::string parseAtomicNode(YAML::Node); + std::string fetchMapEntry(YAML::Node); + std::string mapToString(std::map<std::string,std::string>); + //====== END OF FORWARD DECLARATION ======== + +/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences. + * @param n the node representing a map + * @return std::map<std::string, std::string> populated with the key,value pairs in n + */ +std::map<std::string, std::string> fetchAsMap(YAML::Node n){ + assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); + auto m = std::map<std::string, std::string>(); + for(const auto& kv : n){ + std::string key = kv.first.as<std::string>(); + switch (kv.second.Type()) { + case YAML::NodeType::Scalar : { + std::string value = kv.second.Scalar(); + m.insert(std::pair<std::string,std::string>(key, value)); + break; + } + case YAML::NodeType::Sequence : { + std::string aux = parseAtomicNode(kv.second); + m.insert(std::pair<std::string,std::string>(key, aux)); + break; + } + case YAML::NodeType::Map : { + std::string value = fetchMapEntry(kv.second); + std::regex r("^\\$.*"); + if (std::regex_match(key, r)) key = key.substr(1,key.size()-1); + m.insert(std::pair<std::string,std::string>(key, value)); + break; } - aux = aux + "]"; - return aux; + default: + assert(1 == 0 && "Unsupported node Type at fetchAsMap"); + break; + } + } + return m; +} + std::string fetchMapEntry(YAML::Node n){ + switch (n.Type()) { + case YAML::NodeType::Scalar: { + return n.Scalar(); + break; + } + case YAML::NodeType::Sequence: { + return parseAtomicNode(n); + break; + } + case YAML::NodeType::Map: { + return mapToString(fetchAsMap(n)); + break; + } + default: { + assert(1 == 0 && "Unsupported node Type at fetchMapEntry"); + return ""; + break; + } } + } /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...] * @param _map just a std::map<std::string,std::string> * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...] @@ -77,6 +92,76 @@ namespace { else accumulated = ""; return "[" + accumulated + "]"; } + /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars. + * @param n a vector of YAML::Node that represents a YAML::Sequence + * @return <b>{std::string}</b> representing the YAML sequence + */ + std::string parseAtomicNode(YAML::Node n){ + std::string aux = ""; + bool first = true; + std::string separator = ""; + switch(n.Type()){ + case YAML::NodeType::Scalar: + return n.Scalar(); + break; + case YAML::NodeType::Sequence: + for(auto it : n){ + aux += separator + parseAtomicNode(it); + if(first){ + separator = ","; + first = false; + } + } + return "[" + aux + "]"; + break; + case YAML::NodeType::Map: + return mapToString(fetchAsMap(n)); + break; + default: + return ""; + break; + } + } + + /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type + * Scalar, Sequence or Map. + * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map + * @param n node to be test for atomicity + */ + bool isAtomic(std::string key, YAML::Node n){ + assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node"); + std::regex r("^\\$.*"); + bool is_atomic = true; + switch(n.Type()){ + case YAML::NodeType::Scalar: + return true; + break; + case YAML::NodeType::Sequence: + for(auto it : n) { + switch(it.Type()){ + case YAML::NodeType::Map: + for(const auto& kv : it){ + is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it); + } + break; + default: + is_atomic = is_atomic and isAtomic("", it); + break; + } + } + return is_atomic; + break; + case YAML::NodeType::Map: + is_atomic = std::regex_match(key, r); + return is_atomic; + break; + default: + throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type())); + return false; + break; + } + return false; + } } class ParserYAML { struct ParamsInitSensor{ @@ -158,7 +243,6 @@ public: std::vector<std::array<std::string, 2>> getProblem(); std::map<std::string,std::string> getParams(); void parse(); - std::map<std::string, std::string> fetchAsMap(YAML::Node); }; std::string ParserYAML::generatePath(std::string path){ std::regex r("^/.*"); @@ -193,7 +277,7 @@ void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std: n = YAML::LoadFile(generatePath(file)); walkTreeR(n, tags, hdr); } -/** @Brief Recursively walks the YAML tree while filling a map with the values oarsed from the file +/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file * @param YAML node to be parsed * @param tags represents the path from the root of the YAML tree to the current node * @param hdr is the name of the current YAML node @@ -211,67 +295,59 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st break; } case YAML::NodeType::Sequence : { - std::vector<YAML::Node> scalar_and_seqs; - std::vector<YAML::Node> maps; - for(const auto& kv : n){ - if(kv.Type() == YAML::NodeType::Scalar or kv.Type() == YAML::NodeType::Sequence) scalar_and_seqs.push_back(kv); - else if(kv.Type() == YAML::NodeType::Map) maps.push_back(kv); - } - assert(scalar_and_seqs.size() * maps.size() == 0); - if(scalar_and_seqs.size() > 0 and maps.size() == 0){ - _params.insert(std::pair<std::string,std::string>(hdr, parseScalarSequence(scalar_and_seqs))); - }else if(scalar_and_seqs.size() == 0 and maps.size() > 0){ - for(const auto& kv : maps){ + if(isAtomic("", n)){ + _params.insert(std::pair<std::string,std::string>(hdr, parseAtomicNode(n))); + }else{ + for(const auto& kv : n){ walkTreeR(kv, tags, hdr); } } break; } case YAML::NodeType::Map : { - for(const auto& kv : n){ - //If the key's value starts with a $ (i.e. $key) then its value is parsed as an atomic map, - //otherwise the parser recursively parses the map. - std::regex r("^\\$.*"); - if(not std::regex_match(kv.first.as<std::string>(), r)){ - /* - If key=="follow" then the parser will assume that the value is a path and will parse - the (expected) yaml file at the specified path. Note that this does not increase the header depth. - The following example shows how the header remains unafected: - @my_main_config | @some_path - - cov_det: 1 | - my_value : 23 - - follow: "@some_path" | - - var: 1.2 | - Resulting map: - cov_det -> 1 - my_value-> 23 - var: 1.2 - Instead of: - cov_det -> 1 - follow/my_value-> 23 - var: 1.2 - Which would result from the following yaml files - @my_main_config | @some_path - - cov_det: 1 | - my_value : 23 - - $follow: "@some_path" | - - var: 1.2 | - */ - std::regex rr("follow"); - if(not std::regex_match(kv.first.as<std::string>(), rr)) { - tags.push_back(kv.first.as<std::string>()); - if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>()); - walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>()); - tags.pop_back(); - if(tags.size() == 1) this->updateActiveName(""); - }else{ - walkTree(kv.second.as<std::string>(), tags, hdr); - } - }else{ - std::string key = kv.first.as<std::string>(); - key = key.substr(1,key.size() - 1); - auto fm = fetchAsMap(kv.second); - _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, mapToString(fm))); - } - } + for(const auto& kv : n){ + if(isAtomic(kv.first.as<std::string>(), n)){ + std::string key = kv.first.as<std::string>(); + //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key); + key = key.substr(1,key.size() - 1); + _params.insert(std::pair<std::string,std::string>(hdr + "/" + key, parseAtomicNode(kv.second))); + }else{ + /* + If key=="follow" then the parser will assume that the value is a path and will parse + the (expected) yaml file at the specified path. Note that this does not increase the header depth. + The following example shows how the header remains unafected: + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - follow: "@some_path" | + - var: 1.2 | + Resulting map: + cov_det -> 1 + my_value-> 23 + var: 1.2 + Instead of: + cov_det -> 1 + follow/my_value-> 23 + var: 1.2 + Which would result from the following yaml files + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - $follow: "@some_path" | + - var: 1.2 | + */ + std::string key = kv.first.as<std::string>(); + //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key); + std::regex rr("follow"); + if(not std::regex_match(kv.first.as<std::string>(), rr)) { + tags.push_back(kv.first.as<std::string>()); + if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>()); + walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>()); + tags.pop_back(); + if(tags.size() == 1) this->updateActiveName(""); + }else{ + walkTree(kv.second.as<std::string>(), tags, hdr); + } + } + } break; } default: @@ -350,65 +426,4 @@ void ParserYAML::parse(){ this->walkTreeR(it.n , tags , it._name); } } - -std::string fetchMapEntry(YAML::Node n){ - switch (n.Type()) { - case YAML::NodeType::Scalar : { - return n.Scalar(); - break; - } - case YAML::NodeType::Sequence : { - std::vector<YAML::Node> nodes; - for(auto it : n){ - nodes.push_back(it); - } - return parseScalarSequence(nodes); - break; - } - default: { - assert(1 == 0 && "Unsupported node Type at fetchMapEntry"); - return ""; - break; - } - } -} -/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences. - * @param n the node representing a map - * @return std::map<std::string, std::string> populated with the key,value pairs in n - */ -std::map<std::string, std::string> ParserYAML::fetchAsMap(YAML::Node n){ - assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); - auto m = std::map<std::string, std::string>(); - for(const auto& kv : n){ - std::string key = kv.first.as<std::string>(); - switch (kv.second.Type()) { - case YAML::NodeType::Scalar : { - std::string value = kv.second.Scalar(); - m.insert(std::pair<std::string,std::string>(key, value)); - break; - } - case YAML::NodeType::Sequence : { - std::vector<YAML::Node> scalars; - std::vector<YAML::Node> non_scalars; - for(const auto& kvv : kv.second){ - if(kvv.Type() == YAML::NodeType::Scalar or kvv.Type() == YAML::NodeType::Sequence) scalars.push_back(kvv); - else non_scalars.push_back(kvv); - } - if(non_scalars.size() > 0) WOLF_WARN("Ignoring non-scalar members of sequence in atomic map..."); - std::string aux = parseScalarSequence(scalars); - m.insert(std::pair<std::string,std::string>(key, aux)); - break; - } - case YAML::NodeType::Map : { - std::string value = fetchMapEntry(kv.second); - m.insert(std::pair<std::string,std::string>(key, value)); - break; - } - default: - assert(1 == 0 && "Unsupported node Type at fetchAsMap"); - break; - } - } - return m; -} #endif 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/factor/CMakeLists.txt b/src/factor/CMakeLists.txt deleted file mode 100644 index d910e757557c66ff81a85c5c8397c1aff9bb1fff..0000000000000000000000000000000000000000 --- a/src/factor/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) - -#========================================= -#========================================= -# Add in this section the CONDITIONAL CLUES [IF/ELSE] -# for external libraries and move inside them the respective lines from above. - -IF (OPENCV_FOUND AND Apriltag_FOUND) - SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} - ${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_apriltag.h - ) -ENDIF(OPENCV_FOUND AND Apriltag_FOUND) - -#========================================= -#========================================= - -SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} - ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_distance_3D.h - ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_trifocal.h - ${CMAKE_CURRENT_SOURCE_DIR}/factor_diff_drive.h - ) - -SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} - ) - -# Forward var to parent scope -# These lines always at the end -SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} PARENT_SCOPE) -SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE) - - \ No newline at end of file diff --git a/src/feature/CMakeLists.txt b/src/feature/CMakeLists.txt deleted file mode 100644 index ca674167dd7c8d7a4bd36bf0e9af40f24232f926..0000000000000000000000000000000000000000 --- a/src/feature/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) - -#========================================= -#========================================= -# Add in this section the CONDITIONAL CLUES [IF/ELSE] -# for external libraries and move inside them the respective lines from above. - -IF (OPENCV_FOUND AND Apriltag_FOUND) - SET(HDRS_FEATURE ${HDRS_FEATURE} - ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.h - ) - SET(SRCS_FEATURE ${SRCS_FEATURE} - ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.cpp - ) -ENDIF(OPENCV_FOUND AND Apriltag_FOUND) - -#========================================= -#========================================= - -SET(HDRS_FEATURE ${HDRS_FEATURE} - ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h - ) - -SET(SRCS_FEATURE ${SRCS_FEATURE} - ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp - ) - -# Forward var to parent scope -# These lines always at the end -SET(HDRS_FEATURE ${HDRS_FEATURE} PARENT_SCOPE) -SET(SRCS_FEATURE ${SRCS_FEATURE} PARENT_SCOPE) - \ No newline at end of file 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/problem/problem.cpp b/src/problem/problem.cpp index 19b594237b58a303f0f320fc2635c7d8c73d9f62..b0052f8066b04250051d912727797fdb20569d40 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -759,16 +759,10 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: // create origin capture with the given state as data // Capture fix only takes 3D position and Quaternion orientation CapturePosePtr init_capture; - CaptureBasePtr init_capture_base; - // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); - // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov); if (this->getFrameStructure() == "POV" and this->getDim() == 3) - init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); + init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); else - init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov); - - init_capture = std::static_pointer_cast<CapturePose>(init_capture_base); - // origin_keyframe->addCapture(init_capture); + init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov); // emplace feature and factor init_capture->emplaceFeatureAndFactor(); 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_capture_holder.cpp b/src/processor/processor_capture_holder.cpp deleted file mode 100644 index 5cad88a4e5c5eca8e94d6af99a67eb22ba9e3147..0000000000000000000000000000000000000000 --- a/src/processor/processor_capture_holder.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/** - * \file processor_capture_holder.h - * - * Created on: Jul 12, 2017 - * \author: Jeremie Deray - */ - -//Wolf includes -#include "core/processor/processor_capture_holder.h" - -namespace wolf { - -ProcessorCaptureHolder::ProcessorCaptureHolder(ProcessorParamsCaptureHolderPtr _params_capture_holder) : - ProcessorBase("CAPTURE HOLDER", _params_capture_holder), - params_capture_holder_(_params_capture_holder) -{ - // -} - -void ProcessorCaptureHolder::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) -{ - assert(_keyframe_ptr->getTrajectory() != nullptr - && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory."); - - // get keyframe's time stamp - const TimeStamp new_ts = _keyframe_ptr->getTimeStamp(); - - // find capture whose buffer is affected by the new keyframe - CaptureBasePtr existing_capture = buffer_capture_.selectFirstBefore(new_ts, _time_tolerance); - buffer_capture_.removeUpTo(existing_capture->getTimeStamp()); - - if (existing_capture == nullptr) - { - WOLF_WARN("Could not find a capture at ts: ", new_ts.get(), " within time tolerance: ", _time_tolerance); - return; - } - - WOLF_DEBUG("ProcessorCaptureHolder::keyFrameCallback time tolerance ", _time_tolerance); - WOLF_DEBUG("Capture of type : ", existing_capture->getType()); - WOLF_DEBUG("CaptureBuffer size : ", buffer_capture_.size()); - - // add capture to keyframe - auto frame_ptr = existing_capture->getFrame(); - - if (frame_ptr != nullptr && frame_ptr->isKey()) - { - WOLF_WARN_COND(frame_ptr != _keyframe_ptr, "found a capture already in a different KF"); - WOLF_INFO_COND(frame_ptr == _keyframe_ptr, "found a capture already in a this KF"); - } - else - { - WOLF_INFO("Adding capture laser !"); - existing_capture->link(_keyframe_ptr); - - if (frame_ptr) - frame_ptr->remove(); - } -} - -ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name, - const ProcessorParamsBasePtr _params, - const SensorBasePtr) -{ - ProcessorParamsCaptureHolderPtr params; - - params = std::static_pointer_cast<ProcessorParamsCaptureHolder>(_params); - - // if cast failed use default value - if (params == nullptr) - params = std::make_shared<ProcessorParamsCaptureHolder>(); - - ProcessorCaptureHolderPtr prc_ptr = std::make_shared<ProcessorCaptureHolder>(params); - prc_ptr->setName(_unique_name); - - return prc_ptr; -} - -} // namespace wolf - -// Register in the ProcessorFactory -#include "core/processor/processor_factory.h" -namespace wolf { -WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder) -} // namespace wolf diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 395a2520b00ddbb85ecabae25d4c03bc6e09dda2..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,77 +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 = std::static_pointer_cast<CaptureMotion>(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 1d61bd14b4e167bbfcdc259a1c4407c1a2030123..7a026854728ee7a8bf4bddf3a63c8b651cb47334 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; @@ -153,7 +139,7 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own, const VectorXs& _calib_preint, const FrameBasePtr& _frame_origin) { - auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin)); + auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin); cap_motion->setCalibration(_calib); cap_motion->setCalibrationPreint(_calib_preint); @@ -162,7 +148,9 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own, FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(), + auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, + _feature, + _capture_origin->getFrame(), shared_from_this()); return fac_odom; } @@ -209,7 +197,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/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 76fc35d3bc166eb30e878ab2e61b4f4fbe2950e3..3168276f93aa8da7c5220aaba098503863ff0e68 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -404,7 +404,7 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own, const VectorXs& _calib_preint, const FrameBasePtr& _frame_origin) { - auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin)); + auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin); cap_motion->setCalibration(_calib); cap_motion->setCalibrationPreint(_calib_preint); 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_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index 1f508dbe5cb760e6c5360d4cab16d5f4eb378ac7..da5a01b80e98cf776957422d5bca8729b89ff038 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -321,7 +321,7 @@ TEST(CeresManager, AddFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f); // update solver ceres_manager_ptr->update(); @@ -343,7 +343,7 @@ TEST(CeresManager, DoubleAddFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f); // add factor again P->notifyFactor(c,ADD); @@ -368,7 +368,7 @@ TEST(CeresManager, RemoveFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f); // update solver ceres_manager_ptr->update(); @@ -396,7 +396,7 @@ TEST(CeresManager, AddRemoveFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f); // remove factor P->notifyFactor(c,REMOVE); @@ -423,7 +423,7 @@ TEST(CeresManager, DoubleRemoveFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f); // update solver ceres_manager_ptr->update(); @@ -550,8 +550,8 @@ TEST(CeresManager, FactorsRemoveLocalParam) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); + FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()); + FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()); // update solver ceres_manager_ptr->update(); @@ -592,8 +592,8 @@ TEST(CeresManager, FactorsUpdateLocalParam) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); + FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()); + FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()); // update solver ceres_manager_ptr->update(); diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp index d36d8665d7c3f9226e945a445ff922c20b8c1e00..d068e024a3f9145f347a8645d7ba52dedd036c38 100644 --- a/test/gtest_converter.cpp +++ b/test/gtest_converter.cpp @@ -57,6 +57,19 @@ TEST(Converter, ParseToMap) map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}}; ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]"); } +TEST(Converter, extraTests) +{ + string str = "[{x:1},{y:[[{x:[1,2]},{y:[]},{z:[3]}]]},{z:[1,2,3,4,5]}]"; + auto v = converter<vector<string>>::convert(str); + ASSERT_EQ(v[0], "{x:1}"); + ASSERT_EQ(v[1], "{y:[[{x:[1,2]},{y:[]},{z:[3]}]]}"); + ASSERT_EQ(v[2], "{z:[1,2,3,4,5]}"); + + string str2 = "[]"; + auto v2 = converter<vector<string>>::convert(str2); + // EXPECT_EQ(v2.size(), 1); + EXPECT_TRUE(v2.empty()); +} TEST(Converter, noGeneralConvert) { class DUMMY{}; diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 7e32bf8fad08c13274168d176c8767c51f642ae7..f8901a4d9e75edc815913289b697d2a39dd455fe 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -55,6 +55,11 @@ TEST(Emplace, Processor) ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem()); ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor()); ASSERT_EQ(prc, sen->getProcessorList().front()); + + SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false); + ProcessorOdom2DPtr prc2 = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen2, std::make_shared<ProcessorParamsOdom2D>()); + ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor()); + ASSERT_EQ(prc2, sen2->getProcessorList().front()); } TEST(Emplace, Capture) @@ -119,7 +124,7 @@ TEST(Emplace, EmplaceDerived) auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D()); auto cov = Eigen::MatrixXs::Identity(2,2); auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm); - auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt); + // auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt); auto m = Eigen::Matrix<Scalar,9,6>(); for(int i = 0; i < 9; i++) for(int j = 0; j < 6; j++) @@ -133,6 +138,20 @@ TEST(Emplace, Nullpointer) { CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, nullptr); } +TEST(Emplace, ReturnDerived) +{ + ProblemPtr P = Problem::create("POV", 3); + + ASSERT_NE(P->getTrajectory(), nullptr); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); + auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); + auto cov = Eigen::MatrixXs::Identity(2,2); + auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov); + auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm); + + FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm); + +} int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index af452608f8a602ad2128ea5c44b81cf832b6004b..7213c1084cf13077a0413cbbea48af0676b330d8 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -38,7 +38,6 @@ CeresManager ceres_mgr(problem_ptr); FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0)); // Capture, feature and factor -// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr)); auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr); //////////////////////////////////////////////////////// @@ -49,9 +48,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos TEST(FactorBlockAbs, fac_block_abs_p) { - // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()); - // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP())); FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP()); ASSERT_TRUE(problem_ptr->check(0)); @@ -69,9 +66,7 @@ TEST(FactorBlockAbs, fac_block_abs_p) TEST(FactorBlockAbs, fac_block_abs_p_tail2) { - // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>())); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()); - // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2)); FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2); // Unfix frame 0, perturb frm0 @@ -87,9 +82,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2) TEST(FactorBlockAbs, fac_block_abs_v) { - // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()); - // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV())); FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV()); ASSERT_TRUE(problem_ptr->check(0)); @@ -107,9 +100,7 @@ TEST(FactorBlockAbs, fac_block_abs_v) TEST(FactorQuatAbs, fac_block_abs_o) { - // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)); - // fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO())); FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO()); ASSERT_TRUE(problem_ptr->check(0)); diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index 108ae75cdb54fa50a3caf5eaa9733a096b893578..1ad1635a2a980b73771171e4b238e3db222f1d19 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -29,19 +29,12 @@ TEST(CaptureAutodiff, ConstructorOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - // fr2_ptr->addCapture(capture_ptr); auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE - // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); - // capture_ptr->addFeature(feature_ptr); auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); // FACTOR - // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - // feature_ptr->addFactor(factor_ptr); - // fr1_ptr->addConstrainedBy(factor_ptr); auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); ASSERT_TRUE(factor_ptr->getFeature()); @@ -67,23 +60,15 @@ TEST(CaptureAutodiff, ResidualOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - // fr2_ptr->addCapture(capture_ptr); auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - // capture_ptr->addFeature(feature_ptr); auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); // FACTOR - // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - // feature_ptr->addFactor(factor_ptr); - // fr1_ptr->addConstrainedBy(factor_ptr); - auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); // EVALUATE @@ -119,21 +104,14 @@ TEST(CaptureAutodiff, JacobianOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - // fr2_ptr->addCapture(capture_ptr); auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - // capture_ptr->addFeature(feature_ptr); auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); // FACTOR - // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - // feature_ptr->addFactor(factor_ptr); - // fr1_ptr->addConstrainedBy(factor_ptr); auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); // COMPUTE JACOBIANS @@ -204,25 +182,15 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - // fr2_ptr->addCapture(capture_ptr); auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - // capture_ptr->addFeature(feature_ptr); auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); // FACTOR - // FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - // feature_ptr->addFactor(fac_autodiff_ptr); - // fr1_ptr->addConstrainedBy(fac_autodiff_ptr); auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); - // FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr); - // feature_ptr->addFactor(fac_analytic_ptr); - // fr1_ptr->addConstrainedBy(fac_analytic_ptr); auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr); // COMPUTE JACOBIANS diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp index 6fb4c644e4abc8045d1c842d533e5a73286cb431..bb5e6092e414b5520ff399b40803856f0f91e6cd 100644 --- a/test/gtest_factor_autodiff_distance_3D.cpp +++ b/test/gtest_factor_autodiff_distance_3D.cpp @@ -60,7 +60,7 @@ class FactorAutodiffDistance3D_Test : public testing::Test F2 = problem->emplaceFrame (KEY, pose2, 2.0); C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0); f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov); - c2 = std::static_pointer_cast<FactorAutodiffDistance3D>(FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE)); + c2 = FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE); } 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_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp index 7c239323a93e009a7e8f4c71eca650022ee2cd94..d6eb4ddf7ea7b07cdb675b96cb7af81e3adc3c8d 100644 --- a/test/gtest_factor_odom_3D.cpp +++ b/test/gtest_factor_odom_3D.cpp @@ -41,13 +41,9 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), Tim FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1)); // Capture, feature and factor from frm1 to frm0 -// CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr)); auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr); -// FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov)); auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov); -// FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add -FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr)); // create and add -// FactorBasePtr dummy = frm0->addConstrainedBy(ctr1); +FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr); // create and add //////////////////////////////////////////////////////// diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp index c28a8946ef393b2e41de96d424108dd44691d84a..3c88521c95ab2966b839cd6ab551ef6224b27390 100644 --- a/test/gtest_factor_pose_2D.cpp +++ b/test/gtest_factor_pose_2D.cpp @@ -38,7 +38,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pos // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov)); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov); // FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0))); -FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(fea0, fea0)); +FactorPose2DPtr ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0); //////////////////////////////////////////////////////// diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp index eda34b179b9d579cfc535dc3d488c5fef74cbc7b..0b5032a27978d172432b7f68a04fdea28afc64ff 100644 --- a/test/gtest_factor_pose_3D.cpp +++ b/test/gtest_factor_pose_3D.cpp @@ -39,12 +39,9 @@ CeresManager ceres_mgr(problem); FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0)); // Capture, feature and factor -// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr)); auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr); -// FEATUREBASEPTR fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov)); auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov); -// FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0))); -FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(fea0, fea0)); +FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0); //////////////////////////////////////////////////////// diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index f02418eb99ce6d49eeaaee923755314c85c8fcc4..03f991c07c4a144f560a3759c4c081cdd2410ed1 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -68,25 +68,13 @@ TEST(FrameBase, LinksToTree) IntrinsicsOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 1; intrinsics_odo.k_rot_to_rot = 1; - // SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - // P->getHardware()->addSensor(S); auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo); - // FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - // T->addFrame(F1); auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - // FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - // T->addFrame(F2); auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - // CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); - // F1->addCapture(C); auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); /// @todo link sensor & proccessor ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>()); - // FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); - // C->addFeature(f); auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); - // FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p); - // f->addFactor(c); auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p); //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic. @@ -97,9 +85,6 @@ TEST(FrameBase, LinksToTree) // tree is inconsistent since we are missing the constrained_by link // ASSERT_FALSE(P->check(0)); - // establish constrained_by link F2 -> c - // F2->addConstrainedBy(c); - // tree is now consistent ASSERT_TRUE(P->check(0)); 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_parser_yaml.cpp b/test/gtest_parser_yaml.cpp index 7dc34e134fa96361c698e61f03a227ff8d1e8125..de005340e981a6d62a6b24cec3e190f52d09882f 100644 --- a/test/gtest_parser_yaml.cpp +++ b/test/gtest_parser_yaml.cpp @@ -28,7 +28,10 @@ TEST(ParserYAML, ParseMap) { auto parser = parse("test/yaml/params2.yaml", wolf_root); auto params = parser.getParams(); + // for(auto it : params) + // cout << it.first << " %% " << it.second << endl; EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]"); + // EXPECT_EQ(params["processor1/$mymap/k1"], "v1"); } TEST(ParserYAML, JumpFile) { diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 9a4204d158505cb8647358567bfb8bbe3eaa2678..5c64c90b5dd35b0efa6bc85cc77d39b383b4940f 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -76,8 +76,6 @@ TEST(Problem, Sensors) ProblemPtr P = Problem::create("POV", 3); // add a dummy sensor - // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false); - // P->addSensor(S); auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false); // check pointers @@ -94,14 +92,10 @@ TEST(Problem, Processor) ASSERT_FALSE(P->getProcessorMotion()); // add a motion sensor and processor - // SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics - // P->addSensor(Sm); auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // add motion processor - // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>()); - // Sm->addProcessor(Pm); - auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>())); + auto Pm = ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()); // check motion processor IS NOT by emplace ASSERT_EQ(P->getProcessorMotion(), Pm); 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(); +} + diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index de653851d9de3aa625421088b4fb076e9b586afb..2616d61040d4f224cfd65020faec182d93e02376 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -518,7 +518,7 @@ TEST(SolverManager, AddFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f); // notification Notification notif; @@ -545,7 +545,7 @@ TEST(SolverManager, RemoveFactor) FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0)); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f); // update solver solver_manager_ptr->update(); @@ -579,7 +579,7 @@ TEST(SolverManager, AddRemoveFactor) auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f); // notification Notification notif; @@ -614,7 +614,7 @@ TEST(SolverManager, DoubleRemoveFactor) auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); + FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f); // update solver solver_manager_ptr->update();