Skip to content
Snippets Groups Projects
Commit c9bb8c6a authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove odd test that was not passing

parent 593e1f4b
No related branches found
No related tags found
No related merge requests found
...@@ -106,23 +106,22 @@ inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase ...@@ -106,23 +106,22 @@ inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase
typedef typename Derived::Scalar T; typedef typename Derived::Scalar T;
Eigen::Quaternion<T> q;
T angle_squared = _v.squaredNorm(); T angle_squared = _v.squaredNorm();
T angle = sqrt(angle_squared); T angle = sqrt(angle_squared);
T angle_half = angle / (T)2.0; T angle_half = angle / (T)2.0;
Eigen::Quaternion<T> q;
if (angle > (T)(wolf::Constants::EPS)) if (angle > (T)(wolf::Constants::EPS))
{ {
q.w() = cos(angle_half); q.w() = cos(angle_half);
q.vec() = _v / angle * sin(angle_half); q.vec() = _v / angle * sin(angle_half);
return q;
} }
else else
{ {
q.w() = (T)1.0 - angle_squared/(T)2; // Taylor expansion of cos(x) = 1 - x^2/2!; q.w() = (T)1.0 - angle_squared/(T)2; // Taylor expansion of cos(x) = 1 - x^2/2!;
q.vec() = _v * ((T)2.0 - angle_squared / (T)48.0); // Taylor series of sinc(x) ~ 1 - x^2/3!, and have q.vec = v/2 * sinc(angle_half) q.vec() = _v * ((T)2.0 - angle_squared / (T)48.0); // Taylor series of sinc(x) ~ 1 - x^2/3!, and have q.vec = v/2 * sinc(angle_half)
return q;
} }
return q;
} }
/** \brief Quaternion logarithmic map /** \brief Quaternion logarithmic map
......
...@@ -342,51 +342,6 @@ TEST(rotations, AngleAxis_limits) ...@@ -342,51 +342,6 @@ TEST(rotations, AngleAxis_limits)
} }
} }
TEST(rotations, AngleAxis_limits2)
{
using namespace wolf;
Matrix3s rotation_mat;
Vector3s rv;
AngleAxis<wolf::Scalar> aa0;
//FIX ME : 5. Building a rot mat doing this is not safe; You cannot guarantee that R is valid.
// Highlight limitation of AngleAxis
rotation_mat = skew(Vector3s::Random().eval()) * 1e-8;
rotation_mat(0,0) = 1;
rotation_mat(1,1) = 0.999999;
rotation_mat(2,2) = 1;
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
ASSERT_FALSE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
rotation_mat(0,0) = 1.0;
rotation_mat(1,1) = 1.0;
rotation_mat(2,2) = 1.0;
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
ASSERT_MATRIX_APPROX(rv, Eigen::Vector3s::Zero(), wolf::Constants::EPS);
rotation_mat = skew(Vector3s::Random().eval()) *0.1;
rotation_mat(0,0) = 1;
rotation_mat(1,1) = 0.9999999;
rotation_mat(2,2) = 1;
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
ASSERT_FALSE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
rotation_mat(0,0) = 1.0;
rotation_mat(1,1) = 1.0;
rotation_mat(2,2) = 1.0;
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
ASSERT_FALSE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
}
TEST(rotations, Quat_compos_const_rateOfTurn) TEST(rotations, Quat_compos_const_rateOfTurn)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment