From 885eca58996fd51681897887e3a127bc52052169 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Mon, 18 Dec 2017 14:21:01 +0100
Subject: [PATCH] fix for absolute quaternion constraint
---
src/constraint_quaternion_absolute.h | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/src/constraint_quaternion_absolute.h b/src/constraint_quaternion_absolute.h
index 6984d904f..d1e24db8e 100644
--- a/src/constraint_quaternion_absolute.h
+++ b/src/constraint_quaternion_absolute.h
@@ -52,8 +52,12 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res
Eigen::Quaternions q_measured(getMeasurement().data() + 0);
// 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.
Eigen::Matrix<T, 3, 1> er;
- er = q2v(q.conjugate() * q_measured.cast<T>());
+ er = q2v(q_measured.cast<T>() * q.conjugate());
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
--
GitLab