diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index a57d0de768ad6cac5d01d15dd5e3ba3a54fce619..460835cb9bf03c92cfb73f8516248816212c6571 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -27,7 +27,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase
         bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
                            Eigen::Map<const Eigen::VectorXd>& _x2,
                            Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
-        bool isValid(const Eigen::VectorXd& state) override;
+        bool isValid(const Eigen::VectorXd& state, double tolerance) override;
 };
 
 inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -64,7 +64,7 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>&
     return true;
 }
 
-inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state)
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
 {
     //Any real is a valid angle because we use the pi2pi function. Also
     //the types don't match. In this case the argument is
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 48417d92f98df0aecccdd0380402119b2b4f9d0d..8494ec3a89215af8287584c8255b241af3486b45 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -31,7 +31,7 @@ class LocalParametrizationBase{
         virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
                            Eigen::Map<const Eigen::VectorXd>& _x2,
                            Eigen::Map<Eigen::VectorXd>&       _x2_minus_x1) = 0;
-        virtual bool isValid(const Eigen::VectorXd& state) = 0;
+        virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
         unsigned int getLocalSize() const;
         unsigned int getGlobalSize() const;
 };
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 834a070e7beb8dda9d8671ab1925341b9f2f5cd7..b8bbb981c6cec93c9e4fbfa29049a12f16d137a9 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -47,7 +47,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
         bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
                            Eigen::Map<const Eigen::VectorXd>& _h2,
                            Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
-        bool isValid(const Eigen::VectorXd& state) override;
+        bool isValid(const Eigen::VectorXd& state, double tolerance) override;
 };
 
 } // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 35b905f8d202d04461e1d1133be2bafbc818abbd..c637ab6bb07a23b0e5dd5528d6a1c240327e1473 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -75,9 +75,9 @@ public:
                      Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
   // inline bool isValid(const Eigen::VectorXd& state) override;
   // template<QuaternionDeltaReference DeltaReference>
-  bool isValid(const Eigen::VectorXd& _state) override
+  bool isValid(const Eigen::VectorXd& _state, double tolerance) override
   {
-      return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < Constants::EPS;
+      return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
   }
 };
 
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 1773daf16c1f9a85ffc9c2a92d0f3e4ac7e4f103..5406fe0c105c67d87e4793ba86a2e52f81bbfa87 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -159,7 +159,7 @@ public:
 
         void plus(const Eigen::VectorXd& _dv);
 
-        bool isValid();
+        bool isValid(double tolerance = Constants::EPS);
 
         static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
 
@@ -321,9 +321,9 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi
 {
     return std::make_shared<StateBlock>(_state, _fixed);
 }
-inline bool StateBlock::isValid()
+inline bool StateBlock::isValid(double tolerance)
 {
-    return local_param_ptr_ ? local_param_ptr_->isValid(state_) : true;
+    return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
 }
 }// namespace wolf
 
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 284360b729154405daef97a3e2b3f07bcf7756a3..846b14a49934c70c0d3879efd03f454ffab74605 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -60,7 +60,7 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _
     return true;
 }
 
-bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state)
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance)
 {
     return _state.size() == global_size_;
 }