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

use minus from rotations.h to compute error in ConstraintQuaternionAbsolute::operator ()

parent e71647a9
No related branches found
No related tags found
1 merge request!149Constraint abs
......@@ -10,6 +10,7 @@
//Wolf includes
#include "constraint_autodiff.h"
#include "local_parametrization_quaternion.h"
#include "frame_base.h"
#include "rotations.h"
......@@ -46,18 +47,25 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res
{
// states
Eigen::Quaternion<T> q(_o);
Eigen::Quaternion<T> q1(_o);
// measurements
Eigen::Quaternions q_measured(getMeasurement().data() + 0);
Eigen::Quaternions q2(getMeasurement().data() + 0); //q_measured
/* 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.
*
* In rotations.h, we have
* minus(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
* But we can use this function to make a global 'minus' :
* minus(q2.conjugate(),q1.conjugate()) = log_q(q2 * q1.conjugate());
*/
// 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_measured.cast<T>() * q.conjugate());
er = minus(q2.cast<T>().conjugate(), q1.conjugate());
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
......
  • Dinesh Atchuthan @AtDinesh

    mentioned in issue #147 (closed)

    ·

    mentioned in issue #147 (closed)

    Toggle commit list
  • In my opinion, as first option, in line 68 it is clearer to write:

    er = log_q( q2.cast<T> * q1.conjugate() ); // This is a minus operation in global frame

    than to twist the code with unclear shortcuts.

    Otherwise, we could define plus_left(), plus_right(), minus_left() and minus_right(), as we defined Jac_SO3_left() and Jac_SO3_right() in rotations.h. But I would go for the first option.

    Edited by Joan Solà Ortega
  • OK then let's use log_q( q2.cast<T>() * q1.conjugate() ) as temporary fix and define new functions in rotations.h if really needed in other places.

    Edited by Dinesh Atchuthan
  • I'm on it. Will do the following:

    • create plus_right(), plus_left(), minus_right(), minus_left()
    • have plus() = plus_right(), and minus() = minus_right()
    • leave diff() = minus() as only option by now
  • Done and pushed to master. You can merge from master.

  • so now you can use minus_left(q1, q2)

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