diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp
index c66e150b4b8448f6b6dd4a0a920106bb8253e7cf..228aff35f07df3301c878110ce7e2d3a63567bb1 100644
--- a/src/math/quaternion.hpp
+++ b/src/math/quaternion.hpp
@@ -71,6 +71,7 @@ namespace se3
   template <typename D> void
     firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
   {
+    assert(std::fabs(q.norm() - 1) < 1e-2);
     typedef typename D::Scalar Scalar;
     const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2;
     const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() = alpha * q.derived().coeffs();