Draft: Resolve "Implementation of new nodes creation"

Closes #454, #318, #364, #449, #381, #376, #208, #485

include/core/state_block/state_quaternion.h

@@ -52,7 +52,9 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
         StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
 {
     // TODO: remove these lines after issue #381 is addressed
-    assert(isValid(1e-5)  && "Quaternion unit norm is worse than 1e-5 tolerance!");
+    if(not isValid(1e-5))
+        throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+        
     state_.normalize();
 }
 
@@ -62,7 +64,9 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
     assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
 
     // TODO: remove these lines after issue #381 is addressed
-    assert(isValid(1e-5)  && "Quaternion unit norm is worse than 1e-5 tolerance!");
+    if(not isValid(1e-5))
+        throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+
     state_.normalize();
 }