Use small-angle approx instead of null

src/local_parametrization_homogeneous.cpp

@@ -32,21 +32,25 @@ bool LocalParametrizationHomogeneous::plus(const Eigen::Map<const Eigen::VectorX
     using namespace Eigen;
 
     double angle = _delta.norm();
+    Quaternions dq;
     if (angle > Constants::EPS_SMALL)
     {
         // compute rotation axis -- this guarantees unity norm
         Vector3s axis = _delta.normalized();
 
-        // express delta as a quaternion -- this is exp(delta)
-        Quaternions dq(AngleAxis<Scalar>(angle, axis));
+        // express delta as a quaternion using the angle-axis helper
+        dq = AngleAxis<Scalar>(angle, axis);
 
-        // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere
-        _h_plus_delta = ( dq * Map<const Quaternions>( _h.data() ) ) .coeffs();
     }
-    else
+    else // Consider small angle approx
     {
-        _h_plus_delta = _h;
+        dq.w() = 1;
+        dq.vec() = _delta/2;
     }
+
+    // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere
+    _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs();
+
     return true;
 }
 

src/local_parametrization_quaternion.cpp

 #include "local_parametrization_quaternion.h"
-
+#include <iostream>
 namespace wolf {
 
 template <>
@@ -17,23 +17,26 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(const Eigen::Map<const
     using namespace Eigen;
 
     double angle = _delta_theta.norm();
+    Quaternions dq;
     if (angle > Constants::EPS_SMALL)
     {
         // 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 local reference: q * dq
-        _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+        dq = AngleAxis<Scalar>(angle, axis);
 
     }
-    else // Consider null rotation
+    else // Consider small angle approx
     {
-        _q_plus_delta_theta = _q;
+        dq.w() = 1;
+        dq.vec() = _delta_theta/2;
     }
+
+    // result as a quaternion
+    // the delta is in global reference: dq * q
+    _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+
     return true;
 }
 
@@ -52,22 +55,26 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(const Eigen::Map<cons
     using namespace Eigen;
 
     double angle = _delta_theta.norm();
+    Quaternions dq;
     if (angle > Constants::EPS_SMALL)
     {
         // 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));
+        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 // Consider null rotation
+    else // Consider small angle approx
     {
-        _q_plus_delta_theta = _q;
+        dq.w() = 1;
+        dq.vec() = _delta_theta/2;
     }
+
+    // result as a quaternion
+    // the delta is in global reference: dq * q
+    _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
+
     return true;
 }