Main MRPT website > C++ reference for MRPT 1.5.7
CQuaternion_unittest.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include <mrpt/math/CQuaternion.h>
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPose3DQuat.h>
13 #include <gtest/gtest.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 
22 
23 class QuaternionTests : public ::testing::Test {
24 protected:
25  virtual void SetUp()
26  {
27  }
28 
29  virtual void TearDown() { }
30 
31  void test_gimbalLock(double YAW,double PITCH,double ROLL)
32  {
33  CQuaternionDouble q1, q1r;
34  CPose3D p1(0,0,0, YAW,PITCH,ROLL);
35  p1.getAsQuaternion(q1);
36  CPose3D(q1,0,0,0).getAsQuaternion(q1r);
37 
38  TPose3D t1(CPose3D(q1,0,0,0));
39  TPose3D t2(CPose3D(q1r,0,0,0));
40 
41  EXPECT_NEAR(0, std::abs((CPose3D(q1,0,0,0).getAsVectorVal()-CPose3D(q1r,0,0,0).getAsVectorVal()).sum()), 1e-6);
42  }
43 
44  void test_toYPRAndBack(double YAW,double PITCH,double ROLL)
45  {
46  CPose3D p1(0,0,0, YAW,PITCH,ROLL);
47  CPose3DQuat q1(p1);
48  CPose3D p2 = CPose3D(q1);
49 
50  EXPECT_NEAR(0,(p1.getRotationMatrix()-p2.getRotationMatrix()).array().abs().sum(), 1e-4) <<
51  "ypr->quat->ypr failed with:" << endl
52  << " p1:" << p1 << endl
53  << " q1:" << q1 << endl
54  << " p2:" << p2 << endl;
55 
56  CPose3D p3(q1.quat(),q1[0],q1[1],q1[2] );
57  EXPECT_NEAR(0,(p1.getRotationMatrix()-p3.getRotationMatrix()).array().abs().sum(), 1e-4) <<
58  "pose constructor from quat failed with:" << endl
59  << " p1:" << p1 << endl
60  << " q1:" << q1 << endl
61  << " p3:" << p3 << endl;
62  }
63 
64  void test_lnAndExpMatches(double yaw1,double pitch1,double roll1)
65  {
66  const CPose3D pp(0,0,0,yaw1,pitch1,roll1);
67  CQuaternionDouble q1;
68  pp.getAsQuaternion(q1);
69 
70  mrpt::math::CVectorDouble q1_ln = q1.ln<mrpt::math::CVectorDouble>();
71  const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln);
72 
73  // q2 should be == q1
74  EXPECT_NEAR(0, (q1-q2).array().abs().sum(), 1e-10 )
75  << "q1:\n" << q1 << endl
76  << "q2:\n" << q2 << endl
77  << "Error:\n" << (q1-q2) << endl;
78  }
79 
80  void test_ExpAndLnMatches(double v0,double v1,double v2)
81  {
82  CArrayDouble<3> v;
83  v[0]=v0;
84  v[1]=v1;
85  v[2]=v2;
86 
87  const CQuaternionDouble q1 = CQuaternionDouble::exp(v);
88  CArrayDouble<3> q1_ln = q1.ln<CArrayDouble<3> >();
89 
90  // q1_ln should be == v
91  EXPECT_NEAR(0, (q1_ln-v).array().abs().sum(), 1e-10 )
92  << "v:\n" << v << endl
93  << "q1_ln:\n" << q1_ln << endl
94  << "Error:\n" << (q1_ln-v) << endl;
95  }
96 };
97 
98 TEST_F(QuaternionTests, crossProduct)
99 {
100  CQuaternionDouble q1,q2,q3;
101 
102  //q1 = CQuaternionDouble(1,2,3,4); q1.normalize();
103  CPose3D p1(0,0,0, DEG2RAD(10),DEG2RAD(30),DEG2RAD(-20));
104  p1.getAsQuaternion(q1);
105 
106  CPose3D p2(0,0,0, DEG2RAD(30),DEG2RAD(-20),DEG2RAD(10));
107  p2.getAsQuaternion(q2);
108 
109  // q3 = q1 x q2
110  q3.crossProduct(q1,q2);
111 
112  const CPose3D p3 = p1 + p2;
113 
114  EXPECT_NEAR(0,std::abs((p3.getAsVectorVal()-CPose3D(q3,0,0,0).getAsVectorVal()).sum()), 1e-6) <<
115  "q1 = " << q1 << endl <<
116  "q1 as CPose3D = " << CPose3D(q1,0,0,0) << endl <<
117  "q2 = " << q2 << endl <<
118  "q2 as CPose3D = " << CPose3D(q2,0,0,0) << endl <<
119  "q3 = q1 * q2 = " << q3 << endl <<
120  "q3 as CPose3D = " << CPose3D(q3,0,0,0) << endl <<
121  "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
122 }
123 
124 // Use special cases: gimbal lock:
125 TEST_F(QuaternionTests, gimbalLock)
126 {
127  test_gimbalLock(DEG2RAD(20),DEG2RAD(90),DEG2RAD(0));
128  test_gimbalLock(DEG2RAD(20),DEG2RAD(-90),DEG2RAD(0));
129 }
130 
131 TEST_F(QuaternionTests,ToYPRAndBack)
132 {
133  test_toYPRAndBack(DEG2RAD(20),DEG2RAD(30),DEG2RAD(40));
134  test_toYPRAndBack(DEG2RAD(20),DEG2RAD(30),DEG2RAD(40));
135  test_toYPRAndBack(DEG2RAD(30),DEG2RAD(90),DEG2RAD(0));
136  test_toYPRAndBack(DEG2RAD(-30),DEG2RAD(90),DEG2RAD(0));
137  test_toYPRAndBack(DEG2RAD(-30),DEG2RAD(88),DEG2RAD(60));
138  test_toYPRAndBack(DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
139 
140 }
141 
142 TEST_F(QuaternionTests,LnAndExpMatches)
143 {
144  const double list_test_YPR_angles_degrees[][3] = {
145  { 0, 0, 0 },
146  { -1, 0, 0 }, { 1, 0, 0 },
147  { 0,-1, 0 }, { 0, 1, 0 },
148  { 0, 0, -1}, { 0, 0, 1},
149  { 40, 0, 0 }, { 0, 40, 0 }, { 0, 0, 40 },
150  { -40, 0, 0 }, { 0,-40, 0 }, { 0, 0,-40 },
151  { -30, 20, 50 }, { -30, 20,-50 }, { 30,-20,-50 }
152  };
153 
154  const size_t N = sizeof(list_test_YPR_angles_degrees)/sizeof(list_test_YPR_angles_degrees[0]);
155  for (size_t i=0;i<N;i++)
156  test_lnAndExpMatches(DEG2RAD(list_test_YPR_angles_degrees[i][0]),DEG2RAD(list_test_YPR_angles_degrees[i][1]),DEG2RAD(list_test_YPR_angles_degrees[i][2]));
157 }
158 
159 TEST_F(QuaternionTests,ExpAndLnMatches)
160 {
161  const double list_test_XYZ[][3] = {
162  { 0, 0, 0 },
163  { -1, 0, 0 }, { 1, 0, 0 },
164  { 0,-1, 0 }, { 0, 1, 0 },
165  { 0, 0, -1}, { 0, 0, 1},
166  { 1e-3, 0, 0}, { 0, 1e-3, 0}, {0,0, 1e-3},
167  { -1e-3, 0, 0}, { 0,-1e-3, 0}, {0,0,-1e-3},
168  { -0.1,0.2,0.3 },{-0.1,-0.2,0.3 },{-0.1,-0.2,-0.3 },{-0.1,0.2,-0.3 },{0.1,0.2,-0.3 },{0.1,0.2,0.3}
169  };
170 
171  const size_t N = sizeof(list_test_XYZ)/sizeof(list_test_XYZ[0]);
172  for (size_t i=0;i<N;i++)
173  test_ExpAndLnMatches(list_test_XYZ[i][0],list_test_XYZ[i][1],list_test_XYZ[i][2]);
174 }
175 
176 
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
QuaternionTests::TearDown
virtual void TearDown()
Definition: CQuaternion_unittest.cpp:29
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3DQuat.h
QuaternionTests::test_gimbalLock
void test_gimbalLock(double YAW, double PITCH, double ROLL)
Definition: CQuaternion_unittest.cpp:31
mrpt::poses::CPoseOrPoint::getAsVectorVal
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:181
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
std
STL namespace.
CPose3D.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CQuaternion::crossProduct
void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...
Definition: CQuaternion.h:179
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
QuaternionTests::test_lnAndExpMatches
void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
Definition: CQuaternion_unittest.cpp:64
CQuaternion.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
QuaternionTests::test_ExpAndLnMatches
void test_ExpAndLnMatches(double v0, double v1, double v2)
Definition: CQuaternion_unittest.cpp:80
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::math::CQuaternion::ln
void ln(ARRAYLIKE3 &out_ln) const
Logarithm of the 3x3 matrix defined by this pose, generating the corresponding vector in the SO(3) Li...
Definition: CQuaternion.h:132
v1
GLfloat GLfloat v1
Definition: glext.h:3922
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
v0
GLfloat v0
Definition: glext.h:3921
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
QuaternionTests::test_toYPRAndBack
void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
Definition: CQuaternion_unittest.cpp:44
mrpt::poses::CPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:553
TEST_F
TEST_F(QuaternionTests, crossProduct)
Definition: CQuaternion_unittest.cpp:98


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019