diff --git a/src/constraint_absolute_orientation.h b/src/constraint_absolute_orientation.h new file mode 100644 index 0000000000000000000000000000000000000000..ab1cc460538c645e3a92c5681532eea3a9e9d003 --- /dev/null +++ b/src/constraint_absolute_orientation.h @@ -0,0 +1,61 @@ + +#ifndef CONSTRAINT_ABSOLUTE_ORIENTATION_H_ +#define CONSTRAINT_ABSOLUTE_ORIENTATION_H_ + +//Wolf includes +#include "constraint_autodiff.h" +#include "frame_base.h" +#include "rotations.h" + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(ConstraintAbsO); + +//class +class ConstraintAbsO: public ConstraintAutodiff<ConstraintAbsO,6,3,4> +{ + public: + + ConstraintAbsO(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : + ConstraintAutodiff<ConstraintAbsO,3,4>(CTR_ABS_Q, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getOPtr()) + { + setType("ABS O"); + } + + virtual ~ConstraintAbsO() = default; + + template<typename T> + bool operator ()(const T* const _p, const T* const _o, T* _residuals) const; + + virtual JacobianMethod getJacobianMethod() const override + { + return JAC_AUTO; + } + +}; + +template<typename T> +inline bool ConstraintAbsO::operator ()(const T* const _o, T* _residuals) const +{ + + // state + Eigen::Quaternion<T> q(_o); + + // measurements + Eigen::Quaternions q_measured(getMeasurement().data() + 0); + + // error + Eigen::Matrix<T, 3, 1> er; + er = q2v(q.conjugate() * q_measured.cast<T>()); + + // residual + Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); + res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er; + + return true; +} + +} // namespace wolf + +#endif diff --git a/src/constraint_absolute_position.h b/src/constraint_absolute_position.h new file mode 100644 index 0000000000000000000000000000000000000000..4721930d7627e479272d1f123a066a229584a4c8 --- /dev/null +++ b/src/constraint_absolute_position.h @@ -0,0 +1,60 @@ + +#ifndef CONSTRAINT_ABSOLUTE_POSITION_H_ +#define CONSTRAINT_ABSOLUTE_POSITION_H_ + +//Wolf includes +#include "constraint_autodiff.h" +#include "frame_base.h" + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(ConstraintAbsP); + +//class +class ConstraintAbsP: public ConstraintAutodiff<ConstraintAbsP,6,3> +{ + public: + + ConstraintAbsP(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : + ConstraintAutodiff<ConstraintAbsP,3,3>(CTR_ABS_P, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr()) + { + setType("FIX P"); + } + + virtual ~ConstraintAbsP() = default; + + template<typename T> + bool operator ()(const T* const _p, T* _residuals) const; + + virtual JacobianMethod getJacobianMethod() const override + { + return JAC_AUTO; + } + +}; + +template<typename T> +inline bool ConstraintAbsP::operator ()(const T* const _p, T* _residuals) const +{ + + // states + Eigen::Matrix<T, 3, 1> p(_p); + + // measurements + Eigen::Vector3s p_measured(getMeasurement().data() + 0); + + // error + Eigen::Matrix<T, 3, 1> er; + er = p_measured.cast<T>() - p; + + // residual + Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); + res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er; + + return true; +} + +} // namespace wolf + +#endif diff --git a/src/constraint_absolute_velocity.h b/src/constraint_absolute_velocity.h new file mode 100644 index 0000000000000000000000000000000000000000..6e4d136ec0c09e6d6031c2fac3180626adc6140d --- /dev/null +++ b/src/constraint_absolute_velocity.h @@ -0,0 +1,60 @@ + +#ifndef CONSTRAINT_ABSOLUTE_VELOCITY_H_ +#define CONSTRAINT_ABSOLUTE_VELOCITY_H_ + +//Wolf includes +#include "constraint_autodiff.h" +#include "frame_base.h" + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(ConstraintAbsV); + +//class +class ConstraintAbsV: public ConstraintAutodiff<ConstraintAbsV,6,3> +{ + public: + + ConstraintAbsV(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : + ConstraintAutodiff<ConstraintAbsV,3,3>(CTR_ABS_V, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getVPtr()) + { + setType("FIX V"); + } + + virtual ~ConstraintAbsV() = default; + + template<typename T> + bool operator ()(const T* const _v, T* _residuals) const; + + virtual JacobianMethod getJacobianMethod() const override + { + return JAC_AUTO; + } + +}; + +template<typename T> +inline bool ConstraintAbsV::operator ()(const T* const _v, T* _residuals) const +{ + + // states + Eigen::Matrix<T, 3, 1> v(_v); + + // measurements + Eigen::Vector3s v_measured(getMeasurement().data() + 0); + + // error + Eigen::Matrix<T, 3, 1> er; + er = v_measured.cast<T>() - v; + + // residual + Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); + res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er; + + return true; +} + +} // namespace wolf + +#endif diff --git a/src/wolf.h b/src/wolf.h index a21847739404bed8b8150f1abe0b920a51e82640..0a71bb0018913f89a7271f0f16da00c2b4ae1415 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -228,7 +228,10 @@ typedef enum CTR_AHP_NL, ///< Anchored Homogeneous Point constraint (temporal, to be removed) CTR_IMU, ///< IMU constraint CTR_DIFF_DRIVE, ///< Diff-drive constraint - CTR_BEARING_2D ///< 2D bearing + CTR_BEARING_2D, ///< 2D bearing + CTR_ABS_P, ///< absolute position constraint (for priors) + CTR_ABS_O, ///< absolute orientation constraint (for priors) + CTR_ABS_V ///< absolute velocity constraint (for priors) } ConstraintType; /** \brief Enumeration of constraint status