From 9492839751b6972e757b0158a9bf0d589364bd16 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Fri, 15 Dec 2017 14:40:19 +0100
Subject: [PATCH] added constraint_quaternion_absolute
---
src/CMakeLists.txt | 1 +
src/constraint_quaternion_absolute.h | 66 ++++++++++++++++++++++++++++
2 files changed, 67 insertions(+)
create mode 100644 src/constraint_quaternion_absolute.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 93fe9bc18..f146f97a9 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 000000000..60a1f8e09
--- /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
--
GitLab