From 2b3c7e61b17c5c8f90951ffb757e9f4eac6d8547 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 28 Apr 2022 16:19:39 +0200
Subject: [PATCH] Check quaternion norm and eventually normalize it if it's not
too bad
---
include/core/state_block/state_quaternion.h | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 4200f81f7..f22772908 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/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) :
--
GitLab