Main MRPT website > C++ reference for MRPT 1.9.9
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 #include <mrpt/utils/CTraitsTest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::utils;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template class mrpt::utils::CTraitsTest<mrpt::poses::CPose3DQuat>;
23 template class mrpt::utils::CTraitsTest<mrpt::poses::CPose3D>;
24 template class mrpt::utils::CTraitsTest<mrpt::math::CQuaternionDouble>;
25 
26 class QuaternionTests : public ::testing::Test
27 {
28  protected:
29  virtual void SetUp() {}
30  virtual void TearDown() {}
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(
42  0, std::abs(
43  (CPose3D(q1, 0, 0, 0).getAsVectorVal() -
44  CPose3D(q1r, 0, 0, 0).getAsVectorVal())
45  .sum()),
46  1e-6);
47  }
48 
49  void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
50  {
51  CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
52  CPose3DQuat q1(p1);
53  CPose3D p2 = CPose3D(q1);
54 
55  EXPECT_NEAR(
56  0, (p1.getRotationMatrix() - p2.getRotationMatrix())
57  .array()
58  .abs()
59  .sum(),
60  1e-4)
61  << "ypr->quat->ypr failed with:" << endl
62  << " p1:" << p1 << endl
63  << " q1:" << q1 << endl
64  << " p2:" << p2 << endl;
65 
66  CPose3D p3(q1.quat(), q1[0], q1[1], q1[2]);
67  EXPECT_NEAR(
68  0, (p1.getRotationMatrix() - p3.getRotationMatrix())
69  .array()
70  .abs()
71  .sum(),
72  1e-4)
73  << "pose constructor from quat failed with:" << endl
74  << " p1:" << p1 << endl
75  << " q1:" << q1 << endl
76  << " p3:" << p3 << endl;
77  }
78 
79  void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
80  {
81  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
82  CQuaternionDouble q1;
83  pp.getAsQuaternion(q1);
84 
85  mrpt::math::CVectorDouble q1_ln = q1.ln<mrpt::math::CVectorDouble>();
86  const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln);
87 
88  // q2 should be == q1
89  EXPECT_NEAR(0, (q1 - q2).array().abs().sum(), 1e-10) << "q1:\n"
90  << q1 << endl
91  << "q2:\n"
92  << q2 << endl
93  << "Error:\n"
94  << (q1 - q2)
95  << endl;
96  }
97 
98  void test_ExpAndLnMatches(double v0, double v1, double v2)
99  {
100  CArrayDouble<3> v;
101  v[0] = v0;
102  v[1] = v1;
103  v[2] = v2;
104 
105  const CQuaternionDouble q1 = CQuaternionDouble::exp(v);
106  CArrayDouble<3> q1_ln = q1.ln<CArrayDouble<3>>();
107 
108  // q1_ln should be == v
109  EXPECT_NEAR(0, (q1_ln - v).array().abs().sum(), 1e-10) << "v:\n"
110  << v << endl
111  << "q1_ln:\n"
112  << q1_ln << endl
113  << "Error:\n"
114  << (q1_ln - v)
115  << endl;
116  }
117 };
118 
119 TEST_F(QuaternionTests, crossProduct)
120 {
121  CQuaternionDouble q1, q2, q3;
122 
123  // q1 = CQuaternionDouble(1,2,3,4); q1.normalize();
124  CPose3D p1(0, 0, 0, DEG2RAD(10), DEG2RAD(30), DEG2RAD(-20));
125  p1.getAsQuaternion(q1);
126 
127  CPose3D p2(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(10));
128  p2.getAsQuaternion(q2);
129 
130  // q3 = q1 x q2
131  q3.crossProduct(q1, q2);
132 
133  const CPose3D p3 = p1 + p2;
134 
135  EXPECT_NEAR(
136  0, std::abs(
137  (p3.getAsVectorVal() - CPose3D(q3, 0, 0, 0).getAsVectorVal())
138  .sum()),
139  1e-6)
140  << "q1 = " << q1 << endl
141  << "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl
142  << "q2 = " << q2 << endl
143  << "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl
144  << "q3 = q1 * q2 = " << q3 << endl
145  << "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl
146  << "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
147 }
148 
149 // Use special cases: gimbal lock:
150 TEST_F(QuaternionTests, gimbalLock)
151 {
152  test_gimbalLock(DEG2RAD(20), DEG2RAD(90), DEG2RAD(0));
153  test_gimbalLock(DEG2RAD(20), DEG2RAD(-90), DEG2RAD(0));
154 }
155 
156 TEST_F(QuaternionTests, ToYPRAndBack)
157 {
158  test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
159  test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
160  test_toYPRAndBack(DEG2RAD(30), DEG2RAD(90), DEG2RAD(0));
161  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(90), DEG2RAD(0));
162  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(88), DEG2RAD(60));
163  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
164 }
165 
166 TEST_F(QuaternionTests, LnAndExpMatches)
167 {
168  const double list_test_YPR_angles_degrees[][3] = {
169  {0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
170  {0, 1, 0}, {0, 0, -1}, {0, 0, 1}, {40, 0, 0},
171  {0, 40, 0}, {0, 0, 40}, {-40, 0, 0}, {0, -40, 0},
172  {0, 0, -40}, {-30, 20, 50}, {-30, 20, -50}, {30, -20, -50}};
173 
174  const size_t N = sizeof(list_test_YPR_angles_degrees) /
175  sizeof(list_test_YPR_angles_degrees[0]);
176  for (size_t i = 0; i < N; i++)
177  test_lnAndExpMatches(
178  DEG2RAD(list_test_YPR_angles_degrees[i][0]),
179  DEG2RAD(list_test_YPR_angles_degrees[i][1]),
180  DEG2RAD(list_test_YPR_angles_degrees[i][2]));
181 }
182 
183 TEST_F(QuaternionTests, ExpAndLnMatches)
184 {
185  const double list_test_XYZ[][3] = {
186  {0, 0, 0}, {-1, 0, 0}, {1, 0, 0},
187  {0, -1, 0}, {0, 1, 0}, {0, 0, -1},
188  {0, 0, 1}, {1e-3, 0, 0}, {0, 1e-3, 0},
189  {0, 0, 1e-3}, {-1e-3, 0, 0}, {0, -1e-3, 0},
190  {0, 0, -1e-3}, {-0.1, 0.2, 0.3}, {-0.1, -0.2, 0.3},
191  {-0.1, -0.2, -0.3}, {-0.1, 0.2, -0.3}, {0.1, 0.2, -0.3},
192  {0.1, 0.2, 0.3}};
193 
194  const size_t N = sizeof(list_test_XYZ) / sizeof(list_test_XYZ[0]);
195  for (size_t i = 0; i < N; i++)
196  test_ExpAndLnMatches(
197  list_test_XYZ[i][0], list_test_XYZ[i][1], list_test_XYZ[i][2]);
198 }
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:60
QuaternionTests::TearDown
virtual void TearDown()
Definition: CQuaternion_unittest.cpp:30
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
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:265
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
mrpt::poses::CPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:584
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:202
CTraitsTest.h
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
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:48
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
QuaternionTests::test_lnAndExpMatches
void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
Definition: CQuaternion_unittest.cpp:79
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:98
v
const GLdouble * v
Definition: glext.h:3678
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
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:154
v1
GLfloat GLfloat v1
Definition: glext.h:4105
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
v0
GLfloat v0
Definition: glext.h:4103
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:616
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:4107
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:46
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:237
QuaternionTests::test_toYPRAndBack
void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
Definition: CQuaternion_unittest.cpp:49
TEST_F
TEST_F(QuaternionTests, crossProduct)
Definition: CQuaternion_unittest.cpp:119


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019