diff --git a/src/constraint_quaternion_absolute.h b/src/constraint_quaternion_absolute.h index 6984d904fcaee6f5e0ebb1f6297229509bd6a315..d1e24db8e76db5a5ed595b8311a6dc1c750e7337 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);