diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ee30f2bd219cbe58f27c45baa459586e3a857a84..ef6340c265683804c3f07f0c1c6c425da0800660 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -438,6 +438,9 @@ ENDIF(OpenCV_FOUND) # Add the capture sub-directory ADD_SUBDIRECTORY(captures) +# Add the constraints sub-directory +ADD_SUBDIRECTORY(constraints) + # Add the features sub-directory ADD_SUBDIRECTORY(features) @@ -488,6 +491,7 @@ ADD_LIBRARY(${PROJECT_NAME} ${SRCS_BASE} ${SRCS} ${SRCS_CAPTURE} + ${SRCS_CONSTRAINT} ${SRCS_FEATURE} ${SRCS_SENSOR} ${SRCS_PROCESSOR} @@ -542,6 +546,8 @@ INSTALL(FILES ${HDRS} # DESTINATION include/iri-algorithms/wolf/data_association) INSTALL(FILES ${HDRS_CAPTURE} DESTINATION include/iri-algorithms/wolf/captures) +INSTALL(FILES ${HDRS_CONSTRAINT} + DESTINATION include/iri-algorithms/wolf/constraints) INSTALL(FILES ${HDRS_FEATURE} DESTINATION include/iri-algorithms/wolf/features) INSTALL(FILES ${HDRS_SENSOR} diff --git a/src/constraints/CMakeLists.txt b/src/constraints/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..7730d0f99921698ca23a90904fe99b08d8077bf9 --- /dev/null +++ b/src/constraints/CMakeLists.txt @@ -0,0 +1,8 @@ +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) + +# Forward var to parent scope + +SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} + ${CMAKE_CURRENT_SOURCE_DIR}/constraint_diff_drive.h PARENT_SCOPE) + +SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE) diff --git a/src/constraints/constraint_diff_drive.h b/src/constraints/constraint_diff_drive.h new file mode 100644 index 0000000000000000000000000000000000000000..e6f54dd4ee3f73f95af4f926635b3cc4605fd69a --- /dev/null +++ b/src/constraints/constraint_diff_drive.h @@ -0,0 +1,139 @@ +/** + * \file constraint_diff_drive.h + * + * Created on: Oct 27, 2017 + * \author: Jeremie Deray + */ + +#ifndef WOLF_CONSTRAINT_DIFF_DRIVE_H_ +#define WOLF_CONSTRAINT_DIFF_DRIVE_H_ + +//Wolf includes +#include "../constraint_autodiff.h" +#include "../frame_base.h" +#include "../features/feature_diff_drive.h" +#include "../captures/capture_wheel_joint_position.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; + +} + +namespace wolf { + +class ConstraintDiffDrive : + public ConstraintAutodiff< ConstraintDiffDrive, + RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE, + INTRINSICS_STATE_BLOCK_SIZE > +{ + using Base = ConstraintAutodiff<ConstraintDiffDrive, + RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE, + INTRINSICS_STATE_BLOCK_SIZE>; + +public: + + ConstraintDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr, + const CaptureWheelJointPositionPtr& _capture_origin_ptr, + const ProcessorBasePtr& _processor_ptr = nullptr, + const bool _apply_loss_function = false, + const ConstraintStatus _status = CTR_ACTIVE) : + Base(CTR_DIFF_DRIVE, _capture_origin_ptr->getFramePtr(), _capture_origin_ptr, + nullptr, nullptr, _processor_ptr, + _apply_loss_function, _status, + _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), + _capture_origin_ptr->getFramePtr()->getPPtr(), + _capture_origin_ptr->getFramePtr()->getOPtr(), + _capture_origin_ptr->getFramePtr()->getVPtr(), + _capture_origin_ptr->getSensorIntrinsicPtr(), + _ftr_ptr->getFramePtr()->getPPtr(), + _ftr_ptr->getFramePtr()->getOPtr(), + _ftr_ptr->getFramePtr()->getVPtr(), + _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()), + J_delta_calib_(_ftr_ptr->getJacobianFactor()) + { + setType("DIFF DRIVE"); + } + + /** + * \brief Default destructor (not recommended) + * + * Default destructor. + * + **/ + virtual ~ConstraintDiffDrive() = 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_; +}; + +template<typename T> +inline bool +ConstraintDiffDrive::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 +{ + // MAPS + Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals_map(_residuals); + + 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_STATE_BLOCK_SIZE, 1> > c1_map(_c1); + Eigen::Map<const Eigen::Matrix<T, INTRINSICS_STATE_BLOCK_SIZE, 1> > c2_map(_c2); + + // Compute corrected delta + + /// Is this my delta_preint ? + const Eigen::Matrix<T, 3, 1> measurement = getMeasurement().cast<T>(); + + Eigen::Matrix<T, 3, 1> delta_corrected = measurement + J_delta_calib_.cast<T>() * (c2_map - c1_map); + + // First 2d pose residual + + Eigen::Matrix<T, 3, 1> delta_predicted; + + // position + delta_predicted.head(2) = Eigen::Rotation2D<T>(-_o1[0]) * (p2_map - p1_map); + + // orientation + delta_predicted(2) = _o2[0] - _o1[0]; + + // residual + residuals_map = 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); + + // weighted residual + residuals_map = getMeasurementSquareRootInformationTransposed().cast<T>() * residuals_map; + + return true; +} + +} // namespace wolf + +#endif /* WOLF_CONSTRAINT_DIFF_DRIVE_H_ */ diff --git a/src/wolf.h b/src/wolf.h index 2a3bbbf5ac12dff1bc7dd0fa97d04624d3abbf97..3e9aeb78f0910ef9c958b5f13e38f257d10f5dc9 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -226,7 +226,8 @@ typedef enum CTR_EPIPOLAR, ///< Epipolar constraint CTR_AHP, ///< Anchored Homogeneous Point constraint CTR_AHP_NL, ///< Anchored Homogeneous Point constraint (temporal, to be removed) - CTR_IMU ///< IMU constraint + CTR_IMU, ///< IMU constraint + CTR_DIFF_DRIVE ///< Diff-drive constraint } ConstraintType;