made LocalParametrizationQuaternion template & use partial specialization

src/examples/test_local_param.cpp

@@ -37,8 +37,8 @@ int main(){
     cout << "q  = " << q.transpose() << "   with norm = " << q.norm() << "\nda = " << da.transpose() << endl;
     cout << "qo = " << qo.transpose() << "   with norm = " << qo.norm() << endl;
 
-    LocalParametrizationQuaternion Qpar;
-    LocalParametrizationQuaternion Qpar_loc(DQ_LOCAL);
+    LocalParametrizationQuaternion<DQ_GLOBAL> Qpar;
+    LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
 
     cout << "\nGLOBAL D_QUAT plus()" << endl;
     Map<const VectorXs> q_const(q.data(),4);

src/local_parametrization_quaternion.cpp

@@ -2,19 +2,10 @@
 
 namespace wolf {
 
-LocalParametrizationQuaternion::LocalParametrizationQuaternion(QuaternionDeltaReference _delta_ref) :
-        LocalParametrizationBase(4, 3),
-        delta_reference_(_delta_ref)
-{
-}
-
-LocalParametrizationQuaternion::~LocalParametrizationQuaternion()
-{
-}
-
-bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
-                                          const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
-                                          Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+                                                          const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+                                                          Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
 {
 
     assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -35,12 +26,8 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs
         Quaternions dq(AngleAxis<Scalar>(angle, axis));
 
         // result as a quaternion
-        if (delta_reference_ == DQ_GLOBAL)
-            // the delta is in global reference: dq * q
-            _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
-        else
-            // the delta is in local reference: q * dq
-            _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+        // the delta is in local reference: q * dq
+        _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
 
     }
     else // Consider null rotation
@@ -50,30 +37,72 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs
     return true;
 }
 
-bool LocalParametrizationQuaternion::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
-                                                     Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+                                                           const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+                                                           Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
 {
+
     assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
-    assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+    assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
+    assert(_q_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
+
+    assert(fabs(1.0 - _q.norm()) < Constants::EPS && "Quaternion not normalized.");
 
     using namespace Eigen;
-    if (delta_reference_ == DQ_GLOBAL) // See comments in method plus()
+
+    double angle = _delta_theta.norm();
+    if (angle > Constants::EPS)
     {
-        _jacobian << -_q(0), -_q(1), -_q(2),
-                      _q(3),  _q(2), -_q(1),
-                     -_q(2),  _q(3),  _q(0),
-                      _q(1), -_q(0),  _q(3);
-        _jacobian /= 2;
+        // compute rotation axis -- this guarantees unity norm
+        Vector3s axis = _delta_theta / angle;
+
+        // express delta_theta as a quaternion using the angle-axis helper
+        Quaternions dq(AngleAxis<Scalar>(angle, axis));
+
+        // result as a quaternion
+        // the delta is in global reference: dq * q
+        _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
     }
-    else
+    else // Consider null rotation
     {
-        _jacobian << -_q(0), -_q(1), -_q(2),
-                      _q(3), -_q(2),  _q(1),
-                      _q(2),  _q(3), -_q(0),
-                     -_q(1),  _q(0),  _q(3);
-        _jacobian /= 2;
+        _q_plus_delta_theta = _q;
     }
     return true;
 }
 
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+                                                                     Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+{
+    assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
+    assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+
+    using namespace Eigen;
+    _jacobian << -_q(0), -_q(1), -_q(2),
+                  _q(3), -_q(2),  _q(1),
+                  _q(2),  _q(3), -_q(0),
+                 -_q(1),  _q(0),  _q(3);
+    _jacobian /= 2;
+
+    return true;
+}
+
+template <>
+bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+                                                                     Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+{
+    assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
+    assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
+
+    using namespace Eigen;
+    _jacobian << -_q(0), -_q(1), -_q(2),
+                  _q(3),  _q(2), -_q(1),
+                 -_q(2),  _q(3),  _q(0),
+                  _q(1), -_q(0),  _q(3);
+    _jacobian /= 2;
+
+    return true;
+}
+
 } // namespace wolf

src/local_parametrization_quaternion.h

@@ -48,20 +48,38 @@ typedef enum {
  *   \f]
  *
  */
+template <unsigned int DeltaReference/* = DQ_GLOBAL*/>
 class LocalParametrizationQuaternion : public LocalParametrizationBase
 {
-    private:
-        QuaternionDeltaReference delta_reference_;
-    public:
-        LocalParametrizationQuaternion(QuaternionDeltaReference _delta_ref = DQ_GLOBAL);
-        virtual ~LocalParametrizationQuaternion();
+protected:
 
-        virtual bool plus(const Eigen::Map<const Eigen::VectorXs>& _q,
-                          const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
-                          Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
-        virtual bool computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
+  QuaternionDeltaReference delta_reference_;
+
+public:
+
+  LocalParametrizationQuaternion() :
+    LocalParametrizationBase(4, 3)
+  {
+    //
+  }
+
+  virtual ~LocalParametrizationQuaternion()
+  {
+    //
+  }
+
+  virtual bool plus(const Eigen::Map<const Eigen::VectorXs>& _q,
+                    const Eigen::Map<const Eigen::VectorXs>& _delta_theta,
+                    Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
+
+  virtual bool computeJacobian(const Eigen::Map<const Eigen::VectorXs>& _q,
+                               Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
 };
 
+// @TODO : considere typedef
+// typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionG;
+// typedef LocalParametrizationQuaternion<DQ_LOCAL>  LocalParametrizationQuaternionL;
+
 } // namespace wolf
 
 #endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */

src/state_quaternion.h

@@ -28,13 +28,13 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaternions _quaternion, bo
 }
 
 inline StateQuaternion::StateQuaternion(const Eigen::VectorXs _state, bool _fixed) :
-        StateBlock(_state, _fixed, new LocalParametrizationQuaternion)
+        StateBlock(_state, _fixed, new LocalParametrizationQuaternion<DQ_GLOBAL>)
 {
     assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
 }
 
 inline StateQuaternion::StateQuaternion(bool _fixed) :
-        StateBlock(4, _fixed, new LocalParametrizationQuaternion)
+        StateBlock(4, _fixed, new LocalParametrizationQuaternion<DQ_GLOBAL>)
 {
     state_ = Eigen::Quaternions::Identity().coeffs();
     //