MRPT  1.9.9
CQuaternion_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
12 #include <mrpt/math/CQuaternion.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPose3DQuat.h>
16 #include <Eigen/Dense>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 template class mrpt::CTraitsTest<mrpt::poses::CPose3DQuat>;
24 template class mrpt::CTraitsTest<mrpt::poses::CPose3D>;
25 template class mrpt::CTraitsTest<mrpt::math::CQuaternionDouble>;
26 
27 class QuaternionTests : public ::testing::Test
28 {
29  protected:
30  void SetUp() override {}
31  void TearDown() override {}
32  void test_gimbalLock(double YAW, double PITCH, double ROLL)
33  {
34  CQuaternionDouble q1, q1r;
35  CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
36  p1.getAsQuaternion(q1);
37  CPose3D(q1, 0, 0, 0).getAsQuaternion(q1r);
38 
40  0,
41  std::abs((CPose3D(q1, 0, 0, 0).asVectorVal() -
42  CPose3D(q1r, 0, 0, 0).asVectorVal())
43  .sum()),
44  1e-6);
45  }
46 
47  void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
48  {
49  CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
50  CPose3DQuat q1(p1);
51  CPose3D p2 = CPose3D(q1);
52 
54  0,
56  .array()
57  .abs()
58  .sum(),
59  1e-4)
60  << "ypr->quat->ypr failed with:" << endl
61  << " p1:" << p1 << endl
62  << " q1:" << q1 << endl
63  << " p2:" << p2 << endl;
64 
65  CPose3D p3(q1.quat(), q1[0], q1[1], q1[2]);
67  0,
68  (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);
83  pp.getAsQuaternion(q1);
84 
85  auto 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)
90  << "q1:\n"
91  << q1 << endl
92  << "q2:\n"
93  << q2 << endl
94  << "Error:\n"
95  << (q1 - q2) << endl;
96  }
97 
98  void test_ExpAndLnMatches(double v0, double v1, double v2)
99  {
101  v[0] = v0;
102  v[1] = v1;
103  v[2] = v2;
104 
105  const CQuaternionDouble q1 = CQuaternionDouble::exp(v);
106  auto q1_ln = q1.ln<CVectorFixedDouble<3>>();
107 
108  // q1_ln should be == v
109  EXPECT_NEAR(0, (q1_ln - v).array().abs().sum(), 1e-10)
110  << "v:\n"
111  << v << endl
112  << "q1_ln:\n"
113  << q1_ln << endl
114  << "Error:\n"
115  << (q1_ln - v) << 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, 10.0_deg, 30.0_deg, -20.0_deg);
125  p1.getAsQuaternion(q1);
126 
127  CPose3D p2(0, 0, 0, 30.0_deg, -20.0_deg, 10.0_deg);
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,
137  std::abs((p3.asVectorVal() - CPose3D(q3, 0, 0, 0).asVectorVal()).sum()),
138  1e-6)
139  << "q1 = " << q1 << endl
140  << "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl
141  << "q2 = " << q2 << endl
142  << "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl
143  << "q3 = q1 * q2 = " << q3 << endl
144  << "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl
145  << "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
146 }
147 
148 // Use special cases: gimbal lock:
150 {
151  test_gimbalLock(20.0_deg, 90.0_deg, 0.0_deg);
152  test_gimbalLock(20.0_deg, -90.0_deg, 0.0_deg);
153 }
154 
155 TEST_F(QuaternionTests, ToYPRAndBack)
156 {
157  test_toYPRAndBack(20.0_deg, 30.0_deg, 40.0_deg);
158  test_toYPRAndBack(20.0_deg, 30.0_deg, 40.0_deg);
159  test_toYPRAndBack(30.0_deg, 90.0_deg, 0.0_deg);
160  test_toYPRAndBack(-30.0_deg, 90.0_deg, 0.0_deg);
161  test_toYPRAndBack(-30.0_deg, 88.0_deg, 60.0_deg);
162  test_toYPRAndBack(-30.0_deg, 10.0_deg, 60.0_deg);
163 }
164 
165 TEST_F(QuaternionTests, LnAndExpMatches)
166 {
167  const double list_test_YPR_angles_degrees[][3] = {
168  {0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
169  {0, 1, 0}, {0, 0, -1}, {0, 0, 1}, {40, 0, 0},
170  {0, 40, 0}, {0, 0, 40}, {-40, 0, 0}, {0, -40, 0},
171  {0, 0, -40}, {-30, 20, 50}, {-30, 20, -50}, {30, -20, -50}};
172 
173  for (const auto& list_test_YPR_angles_degree : list_test_YPR_angles_degrees)
174  test_lnAndExpMatches(
175  DEG2RAD(list_test_YPR_angles_degree[0]),
176  DEG2RAD(list_test_YPR_angles_degree[1]),
177  DEG2RAD(list_test_YPR_angles_degree[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  for (const auto& i : list_test_XYZ) test_ExpAndLnMatches(i[0], i[1], i[2]);
192 }
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void test_gimbalLock(double YAW, double PITCH, double ROLL)
STL namespace.
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:501
This base provides a set of functions for maths stuff.
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:231
constexpr double DEG2RAD(const double x)
Degrees to radians.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void test_ExpAndLnMatches(double v0, double v1, double v2)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:183
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void SetUp() override
void TearDown() override
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
TEST_F(QuaternionTests, crossProduct)
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020