Skip to content
Snippets Groups Projects
Commit 885eca58 authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

fix for absolute quaternion constraint

parent c6c037f5
No related branches found
No related tags found
1 merge request!149Constraint abs
...@@ -52,8 +52,12 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res ...@@ -52,8 +52,12 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res
Eigen::Quaternions q_measured(getMeasurement().data() + 0); Eigen::Quaternions q_measured(getMeasurement().data() + 0);
// error // 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; Eigen::Matrix<T, 3, 1> er;
er = q2v(q.conjugate() * q_measured.cast<T>()); er = q2v(q_measured.cast<T>() * q.conjugate());
// residual // residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals); Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment