diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 6995b7f8caaa626b36444d08e71d941d49aea8b6..62c6ea0aaff95d1eab6049ba96a0304a06068f9d 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/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;
 }
 
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index fe222dcaf52ac481ca0b1022442c7b88945e0ae1..fddf08b73acb33bc74a2e65c3d4d4a993500088e 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -1,5 +1,5 @@
 #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;
 }