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

src/constraint_quaternion_absolute.h

@@ -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);

[Joan Solà Ortega]

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.

[Dinesh Atchuthan]

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.

[Joan Solà Ortega]

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

[Joan Solà Ortega]

Done and pushed to master. You can merge from master.

[Joan Solà Ortega]

so now you can use minus_left(q1, q2)