Check quaternion norm and eventually normalize it if it's not too bad

include/core/state_block/state_quaternion.h

@@ -51,12 +51,19 @@ class StateQuaternion : public StateBlock
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
         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!");
+    state_.normalize();
 }
 
 inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
         StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
 {
-    assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
+    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!");
+    state_.normalize();
 }
 
 inline StateQuaternion::StateQuaternion(bool _fixed) :