quaternion.cpp

Go to the documentation of this file.

//
// Copyright (c) 2019-2020 INRIA CNRS
//

#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/se3.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_assignQuaternion)
{
  using namespace pinocchio;
  const int max_tests = 1e5;
  for(int k = 0; k < max_tests; ++k)
  {
    const SE3 M(SE3::Random());
    SE3::Quaternion quat_ref(M.rotation());
    
    SE3::Quaternion quat;
    quaternion::assignQuaternion(quat,M.rotation());
    
    BOOST_CHECK(quat.coeffs().isApprox(quat_ref.coeffs()));
  }
}

BOOST_AUTO_TEST_CASE(test_uniformRandom)
{
  srand(0);

  using namespace pinocchio;
  Eigen::Quaternion<double> q;

  for (int i = 0; i < (1 << 10); ++i) {
    quaternion::uniformRandom(q);
    BOOST_CHECK_MESSAGE((q.coeffs().array().abs() <= 1).all(),
        "Quaternion coeffs out of bounds: " << i << ' ' << q.coeffs().transpose());
  }
}

BOOST_AUTO_TEST_CASE(test_isNormalized)
{
  srand(0);

  using namespace pinocchio;
  typedef Eigen::Quaternion<double> Quaternion;
  typedef Quaternion::Coefficients Vector4;
  
#ifdef NDEBUG
  const int max_test = 1e6;
#else
  const int max_test = 1e2;
#endif
  for(int i = 0; i < max_test; ++i)
  {
    Quaternion q;
    q.coeffs() = Vector4::Random() + Vector4::Constant(2);
    BOOST_CHECK(!quaternion::isNormalized(q));
    
    q.normalize();
    BOOST_CHECK(quaternion::isNormalized(q));
  }
  
  // Specific check for the Zero vector
  BOOST_CHECK(!quaternion::isNormalized(Quaternion(Vector4::Zero())));
}

BOOST_AUTO_TEST_SUITE_END()