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-2018, 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 <CTraitsTest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template class mrpt::CTraitsTest<mrpt::poses::CPose3DQuat>;
23 template class mrpt::CTraitsTest<mrpt::poses::CPose3D>;
24 template class mrpt::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  EXPECT_NEAR(
39  0, std::abs(
40  (CPose3D(q1, 0, 0, 0).getAsVectorVal() -
41  CPose3D(q1r, 0, 0, 0).getAsVectorVal())
42  .sum()),
43  1e-6);
44  }
45 
46  void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
47  {
48  CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
49  CPose3DQuat q1(p1);
50  CPose3D p2 = CPose3D(q1);
51 
52  EXPECT_NEAR(
53  0, (p1.getRotationMatrix() - p2.getRotationMatrix())
54  .array()
55  .abs()
56  .sum(),
57  1e-4)
58  << "ypr->quat->ypr failed with:" << endl
59  << " p1:" << p1 << endl
60  << " q1:" << q1 << endl
61  << " p2:" << p2 << endl;
62 
63  CPose3D p3(q1.quat(), q1[0], q1[1], q1[2]);
64  EXPECT_NEAR(
65  0, (p1.getRotationMatrix() - p3.getRotationMatrix())
66  .array()
67  .abs()
68  .sum(),
69  1e-4)
70  << "pose constructor from quat failed with:" << endl
71  << " p1:" << p1 << endl
72  << " q1:" << q1 << endl
73  << " p3:" << p3 << endl;
74  }
75 
76  void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
77  {
78  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
79  CQuaternionDouble q1;
80  pp.getAsQuaternion(q1);
81 
82  mrpt::math::CVectorDouble q1_ln = q1.ln<mrpt::math::CVectorDouble>();
83  const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln);
84 
85  // q2 should be == q1
86  EXPECT_NEAR(0, (q1 - q2).array().abs().sum(), 1e-10) << "q1:\n"
87  << q1 << endl
88  << "q2:\n"
89  << q2 << endl
90  << "Error:\n"
91  << (q1 - q2)
92  << endl;
93  }
94 
95  void test_ExpAndLnMatches(double v0, double v1, double v2)
96  {
97  CArrayDouble<3> v;
98  v[0] = v0;
99  v[1] = v1;
100  v[2] = v2;
101 
102  const CQuaternionDouble q1 = CQuaternionDouble::exp(v);
103  CArrayDouble<3> q1_ln = q1.ln<CArrayDouble<3>>();
104 
105  // q1_ln should be == v
106  EXPECT_NEAR(0, (q1_ln - v).array().abs().sum(), 1e-10) << "v:\n"
107  << v << endl
108  << "q1_ln:\n"
109  << q1_ln << endl
110  << "Error:\n"
111  << (q1_ln - v)
112  << endl;
113  }
114 };
115 
116 TEST_F(QuaternionTests, crossProduct)
117 {
118  CQuaternionDouble q1, q2, q3;
119 
120  // q1 = CQuaternionDouble(1,2,3,4); q1.normalize();
121  CPose3D p1(0, 0, 0, DEG2RAD(10), DEG2RAD(30), DEG2RAD(-20));
122  p1.getAsQuaternion(q1);
123 
124  CPose3D p2(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(10));
125  p2.getAsQuaternion(q2);
126 
127  // q3 = q1 x q2
128  q3.crossProduct(q1, q2);
129 
130  const CPose3D p3 = p1 + p2;
131 
132  EXPECT_NEAR(
133  0, std::abs(
134  (p3.getAsVectorVal() - CPose3D(q3, 0, 0, 0).getAsVectorVal())
135  .sum()),
136  1e-6)
137  << "q1 = " << q1 << endl
138  << "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl
139  << "q2 = " << q2 << endl
140  << "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl
141  << "q3 = q1 * q2 = " << q3 << endl
142  << "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl
143  << "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
144 }
145 
146 // Use special cases: gimbal lock:
147 TEST_F(QuaternionTests, gimbalLock)
148 {
149  test_gimbalLock(DEG2RAD(20), DEG2RAD(90), DEG2RAD(0));
150  test_gimbalLock(DEG2RAD(20), DEG2RAD(-90), DEG2RAD(0));
151 }
152 
153 TEST_F(QuaternionTests, ToYPRAndBack)
154 {
155  test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
156  test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
157  test_toYPRAndBack(DEG2RAD(30), DEG2RAD(90), DEG2RAD(0));
158  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(90), DEG2RAD(0));
159  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(88), DEG2RAD(60));
160  test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
161 }
162 
163 TEST_F(QuaternionTests, LnAndExpMatches)
164 {
165  const double list_test_YPR_angles_degrees[][3] = {
166  {0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
167  {0, 1, 0}, {0, 0, -1}, {0, 0, 1}, {40, 0, 0},
168  {0, 40, 0}, {0, 0, 40}, {-40, 0, 0}, {0, -40, 0},
169  {0, 0, -40}, {-30, 20, 50}, {-30, 20, -50}, {30, -20, -50}};
170 
171  const size_t N = sizeof(list_test_YPR_angles_degrees) /
172  sizeof(list_test_YPR_angles_degrees[0]);
173  for (size_t i = 0; i < N; i++)
174  test_lnAndExpMatches(
175  DEG2RAD(list_test_YPR_angles_degrees[i][0]),
176  DEG2RAD(list_test_YPR_angles_degrees[i][1]),
177  DEG2RAD(list_test_YPR_angles_degrees[i][2]));
178 }
179 
180 TEST_F(QuaternionTests, ExpAndLnMatches)
181 {
182  const double list_test_XYZ[][3] = {
183  {0, 0, 0}, {-1, 0, 0}, {1, 0, 0},
184  {0, -1, 0}, {0, 1, 0}, {0, 0, -1},
185  {0, 0, 1}, {1e-3, 0, 0}, {0, 1e-3, 0},
186  {0, 0, 1e-3}, {-1e-3, 0, 0}, {0, -1e-3, 0},
187  {0, 0, -1e-3}, {-0.1, 0.2, 0.3}, {-0.1, -0.2, 0.3},
188  {-0.1, -0.2, -0.3}, {-0.1, 0.2, -0.3}, {0.1, 0.2, -0.3},
189  {0.1, 0.2, 0.3}};
190 
191  const size_t N = sizeof(list_test_XYZ) / sizeof(list_test_XYZ[0]);
192  for (size_t i = 0; i < N; i++)
193  test_ExpAndLnMatches(
194  list_test_XYZ[i][0], list_test_XYZ[i][1], list_test_XYZ[i][2]);
195 }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
QuaternionTests::TearDown
virtual void TearDown()
Definition: CQuaternion_unittest.cpp:30
TEST_F
TEST_F(QuaternionTests, crossProduct)
Definition: CQuaternion_unittest.cpp:116
CQuaternion.h
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:232
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:48
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
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:510
QuaternionTests::test_lnAndExpMatches
void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
Definition: CQuaternion_unittest.cpp:76
v
const GLdouble * v
Definition: glext.h:3678
v1
GLfloat GLfloat v1
Definition: glext.h:4105
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:61
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:263
QuaternionTests
Definition: CQuaternion_unittest.cpp:26
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:4107
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
CPose3DQuat.h
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:203
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:25
QuaternionTests::test_ExpAndLnMatches
void test_ExpAndLnMatches(double v0, double v1, double v2)
Definition: CQuaternion_unittest.cpp:95
QuaternionTests::SetUp
virtual void SetUp()
Definition: CQuaternion_unittest.cpp:29
CPose3D.h
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::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
QuaternionTests::test_toYPRAndBack
void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
Definition: CQuaternion_unittest.cpp:46
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:155
v0
GLfloat v0
Definition: glext.h:4103
QuaternionTests::test_gimbalLock
void test_gimbalLock(double YAW, double PITCH, double ROLL)
Definition: CQuaternion_unittest.cpp:31
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST