add ConstraintAbs(P/O/V) for priors

src/constraint_absolute_orientation.h

+
+#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

src/constraint_absolute_position.h

+
+#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

src/constraint_absolute_velocity.h

+
+#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

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