diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b226ca51a9860f66fcf7b17faaf50b2d22271ecf..b157ef55deaf606f4730ef5a08447b305af26af8 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -236,6 +236,7 @@ SET(HDRS
capture_odom_2D.h
capture_odom_3D.h
capture_void.h
+ constraint_block_absolute.h
constraint_container.h
constraint_corner_2D.h
constraint_AHP.h
@@ -252,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_block_absolute.h b/src/constraint_block_absolute.h
new file mode 100644
index 0000000000000000000000000000000000000000..737de58a81d0625dd9e31c877d2f227110076519
--- /dev/null
+++ b/src/constraint_block_absolute.h
@@ -0,0 +1,66 @@
+/**
+ * \file constraint_block_absolute.h
+ *
+ * Created on: Dec 15, 2017
+ * \author: AtDinesh
+ */
+
+#ifndef CONSTRAINT_BLOCK_ABSOLUTE_H_
+#define CONSTRAINT_BLOCK_ABSOLUTE_H_
+
+//Wolf includes
+#include "constraint_autodiff.h"
+#include "frame_base.h"
+
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ConstraintBlockAbsolute);
+
+//class
+class ConstraintBlockAbsolute: public ConstraintAutodiff<ConstraintBlockAbsolute,3,3>
+{
+ public:
+
+ ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAutodiff<ConstraintBlockAbsolute,3,3>(CTR_BLOCK_ABS, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+ {
+ setType("FIX SB");
+ }
+
+ virtual ~ConstraintBlockAbsolute() = default;
+
+ template<typename T>
+ bool operator ()(const T* const _sb, T* _residuals) const;
+
+ virtual JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+};
+
+template<typename T>
+inline bool ConstraintBlockAbsolute::operator ()(const T* const _sb, T* _residuals) const
+{
+
+ // states
+ Eigen::Matrix<T, 3, 1> sb(_sb);
+
+ // measurements
+ Eigen::Vector3s measured_state(getMeasurement().data() + 0);
+
+ // error
+ Eigen::Matrix<T, 3, 1> er;
+ er = measured_state.cast<T>() - sb;
+
+ // 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_quaternion_absolute.h b/src/constraint_quaternion_absolute.h
new file mode 100644
index 0000000000000000000000000000000000000000..8fba0ebbb95169af6a7a3faf0cfbda2b06471ffb
--- /dev/null
+++ b/src/constraint_quaternion_absolute.h
@@ -0,0 +1,78 @@
+/**
+ * \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 "local_parametrization_quaternion.h"
+#include "frame_base.h"
+#include "rotations.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> q1(_o);
+
+ // measurements
+ Eigen::Quaternions q2(getMeasurement().data() + 0); //q_measured
+
+ /* error
+ * to compute the difference between both quaternions, we do
+ * diff = log(q2 * q1.conj)
+ * isolating q2 we get
+ * q2 = exp(diff) * q1 ==> exp on the left means global.
+ *
+ * In rotations.h, we have
+ * minus(q1,q2) = minus_right(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
+ * minus_left(q1,q2) = log_q(q2.conjugate() * q1); --> this is a global 'minus'
+ */
+
+ Eigen::Matrix<T, 3, 1> er;
+ er = minus_left( q1, q2.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/test/CMakeLists.txt b/src/test/CMakeLists.txt
index a60a4a027d6efa8bf83fb2bb60c960cd175db24c..6014f353067b002c80144ee66608f3cf062f0fe5 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -90,6 +90,10 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME})
# ------- Now Derived classes ----------
+# ConstraintAbs(P/O/V) classes test
+wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp)
+target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME})
+
# ConstraintFix3D class test
wolf_add_gtest(gtest_constraint_fix_3D gtest_constraint_fix_3D.cpp)
target_link_libraries(gtest_constraint_fix_3D ${PROJECT_NAME})
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f190fef2ec73affcbdc37ff70a2c730c2c23c5e
--- /dev/null
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -0,0 +1,138 @@
+/**
+ * \file gtest_constraint_absolute.cpp
+ *
+ * Created on: Dec 9, 2017
+ * \author: datchuth
+ */
+
+
+#include "utils_gtest.h"
+#include "constraint_block_absolute.h"
+#include "constraint_quaternion_absolute.h"
+#include "capture_motion.h"
+
+#include "ceres_wrapper/ceres_manager.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+Vector10s pose9toPose10(Vector9s _pose9)
+{
+ return (Vector10s() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
+}
+
+// Input pose9 and covariance
+Vector9s pose(Vector9s::Random());
+Vector10s pose10 = pose9toPose10(pose);
+Vector9s data_var((Vector9s() << 0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2).finished());
+Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = data_var.asDiagonal();
+
+// perturbated priors
+Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
+
+// Problem and solver
+ProblemPtr problem = Problem::create("POV 3D");
+CeresManager ceres_mgr(problem);
+
+// Two frames
+FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+
+// Capture, feature and constraint
+CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose10, 10, 9, nullptr));
+
+////////////////////////////////////////////////////////
+/* In the tests below, we check that ConstraintBlockAbsolute and ConstraintQuaternionAbsolute are working fine
+ * These are absolute constraints related to a specific part of the frame's state
+ * Both features and constraints are created in the TEST(). Hence, tests will not interfere each others.
+ */
+
+TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
+ );
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
+ );
+
+ // Unfix frame 0, perturb frm0
+ frm0->unfix();
+ frm0->setState(x0);
+
+ // solve for frm0
+ std::string brief_report = ceres_mgr.solve(1);
+
+ //only orientation is constrained
+ ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6);
+}
+
+TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
+ );
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
+ );
+
+ // Unfix frame 0, perturb frm0
+ frm0->unfix();
+ frm0->setState(x0);
+
+ // solve for frm0
+ std::string brief_report = ceres_mgr.solve(1);
+
+ //only velocity is constrained
+ ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
+}
+
+TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
+ );
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
+{
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+ fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
+ );
+
+ // Unfix frame 0, perturb frm0
+ frm0->unfix();
+ frm0->setState(x0);
+
+ // solve for frm0
+ std::string brief_report = ceres_mgr.solve(1);
+
+ //only velocity is constrained
+ ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/wolf.h b/src/wolf.h
index a21847739404bed8b8150f1abe0b920a51e82640..2978cc6032a9ae433de40ef237e94b219933b9a4 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -228,7 +228,8 @@ 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_BLOCK_ABS ///< absolute constraint to Poisition or Velocity depending on argument StateBlockPtr (for priors)
} ConstraintType;
/** \brief Enumeration of constraint status