From cae88cf7098fc96a78ab64b7d5edfab1002edb83 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 17 Jan 2018 11:10:39 +0100
Subject: [PATCH] quaternion error using minus_left
---
src/constraint_quaternion_absolute.h | 10 +++-------
1 file changed, 3 insertions(+), 7 deletions(-)
diff --git a/src/constraint_quaternion_absolute.h b/src/constraint_quaternion_absolute.h
index 8ed9cc2e6..8fba0ebbb 100644
--- a/src/constraint_quaternion_absolute.h
+++ b/src/constraint_quaternion_absolute.h
@@ -59,16 +59,12 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res
* q2 = exp(diff) * q1 ==> exp on the left means global.
*
* In rotations.h, we have
- * minus(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
- * But we could use this function to make a global 'minus' :
- * minus(q2.conjugate(),q1.conjugate()) = log_q(q2 * q1.conjugate());
- *
- * Other option : define plus_left(), plus_right(), minus_left() and minus_right(),
- * as we defined Jac_SO3_left() and Jac_SO3_right() in rotations.h.
+ * 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 = log_q( q2.cast<T>() * q1.conjugate() );
+ er = minus_left( q1, q2.cast<T>() );
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
--
GitLab