From 3cf38a8b0434cd500876e946f4cebe31fe0872c2 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 1 Sep 2016 10:53:39 +0200
Subject: [PATCH] [C++] Add firstOrderNormalize for quaternion whose norm is
almost 1.
---
src/math/quaternion.hpp | 15 +++++++++++++++
1 file changed, 15 insertions(+)
diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp
index 8f20cc57b..c66e150b4 100644
--- a/src/math/quaternion.hpp
+++ b/src/math/quaternion.hpp
@@ -61,5 +61,20 @@ namespace se3
return (q1.coeffs().isApprox(q2.coeffs()) || q1.coeffs().isApprox(-q2.coeffs()) );
}
+ /// Approximately normalize by applying the first order limited development
+ /// of the normalization function.
+ ///
+ /// Only additions and multiplications are required. Neither square root nor
+ /// division are used (except a division by 2).
+ ///
+ /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
+ template <typename D> void
+ firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
+ {
+ typedef typename D::Scalar Scalar;
+ const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2;
+ const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() = alpha * q.derived().coeffs();
+ }
+
}
#endif //#ifndef __math_quaternion_hpp__
--
GitLab