diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 93fe9bc18a1a5d82fb1a700e6194e458eb51f514..f146f97a9495155379db9436128baa12789a4b1d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -253,6 +253,7 @@ SET(HDRS constraint_odom_3D.h constraint_point_2D.h constraint_point_to_line_2D.h + constraint_quaternion_absolute.h constraint_relative_2D_analytic.h feature_corner_2D.h feature_gps_fix.h diff --git a/src/constraint_quaternion_absolute.h b/src/constraint_quaternion_absolute.h new file mode 100644 index 0000000000000000000000000000000000000000..60a1f8e09a2f21b0a74db06eeb5010f1e504e206 --- /dev/null +++ b/src/constraint_quaternion_absolute.h @@ -0,0 +1,66 @@ +/** + * \file constraint_quaternion_absolute.h + * + * Created on: Dec 15, 2017 + * \author: AtDinesh + */ + +#ifndef CONSTRAINT_QUATERNION_ABSOLUTE_H_ +#define CONSTRAINT_QUATERNION_ABSOLUTE_H_ + +//Wolf includes +#include "constraint_autodiff.h" +#include "frame_base.h" + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(ConstraintQuaternionAbsolute); + +//class +class ConstraintQuaternionAbsolute: public ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4> +{ + public: + + ConstraintQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : + ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4>(CTR_BLOCK_ABS, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr) + { + setType("FIX Q"); + } + + virtual ~ConstraintQuaternionAbsolute() = default; + + template<typename T> + bool operator ()(const T* const _o, T* _residuals) const; + + virtual JacobianMethod getJacobianMethod() const override + { + return JAC_AUTO; + } + +}; + +template<typename T> +inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) const +{ + + // states + 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