From ded514db17cfe7057f71a8f3eb432e80fb1fec9f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 20 Sep 2016 18:30:59 +0200
Subject: [PATCH] [C++] Update firstOrderNormalize doc and assertions.
---
src/math/quaternion.hpp | 21 +++++++++++++++++----
src/multibody/joint/joint-free-flyer.hpp | 2 +-
2 files changed, 18 insertions(+), 5 deletions(-)
diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp
index 2815cbdf7..063846710 100644
--- a/src/math/quaternion.hpp
+++ b/src/math/quaternion.hpp
@@ -66,19 +66,32 @@ namespace se3
///
/// Only additions and multiplications are required. Neither square root nor
/// division are used (except a division by 2).
+ /// The output quaternion is guaranted to statisfy the following:
+ /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
+ /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
+ /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
///
/// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
///
/// \note See
/// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
/// to know the reason why the argument is const.
- template <typename D> void
- firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
+ template <typename D>
+ void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
{
- assert(std::fabs(q.norm() - 1) < 1e-2);
typedef typename D::Scalar Scalar;
- const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2;
+ const Scalar N2 = q.squaredNorm();
+#ifndef NDEBUG
+ const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
+ assert(std::fabs(N2-1.) <= epsilon);
+#endif
+ const Scalar alpha = ((Scalar)3 - N2) / 2;
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
+#ifndef NDEBUG
+ const Scalar M = 3 * std::pow((Scalar)1-epsilon, (Scalar)-5/2) / 4;
+ assert(std::fabs(q.norm() - 1) <=
+ std::max(M * sqrt(N2) * (N2 - 1)*(N2 - 1) / 2, Eigen::NumTraits<Scalar>::epsilon()));
+#endif
}
/// Uniformly random quaternion sphere.
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index ba60aa49c..a58e019bc 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -213,7 +213,7 @@ namespace se3
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
- assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
+ assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
M.translation(q_joint.template head<3>());
--
GitLab