MRPT  1.9.9
slerp_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-2019, 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 <gtest/gtest.h>
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/math/TPose3D.h>
13 #include <mrpt/math/slerp.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::math;
17 using namespace std;
18 
19 // Some
20 TEST(SLERP_tests, correctShortestPath)
21 {
22  CMatrixDouble44 HM, HMe;
23  { // Both poses at (0,0,0) angles:
24  const TPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
25  const TPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
26  {
27  TPose3D pose_interp;
28  mrpt::math::slerp(pose_a, pose_b, 0, pose_interp);
29  const TPose3D expected(0, 0, 0, 0, 0, 0);
30  pose_interp.getHomogeneousMatrix(HM);
31  expected.getHomogeneousMatrix(HMe);
32  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
33  << "pose_a: " << pose_a << "\npose_b: " << pose_b
34  << "\ninterp: " << pose_interp << endl;
35  }
36  {
37  TPose3D pose_interp;
38  mrpt::math::slerp(pose_a, pose_b, 1, pose_interp);
39  const TPose3D expected(0, 0, 0, 0, 0, 0);
40  pose_interp.getHomogeneousMatrix(HM);
41  expected.getHomogeneousMatrix(HMe);
42  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
43  << "pose_a: " << pose_a << "\npose_b: " << pose_b
44  << "\ninterp: " << pose_interp << endl;
45  }
46  {
47  TPose3D pose_interp;
48  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
49  const TPose3D expected(0, 0, 0, 0, 0, 0);
50  pose_interp.getHomogeneousMatrix(HM);
51  expected.getHomogeneousMatrix(HMe);
52  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
53  << "pose_a: " << pose_a << "\npose_b: " << pose_b
54  << "\ninterp: " << pose_interp << endl;
55  }
56  }
57 
58  { // Poses at yaw=+-179deg
59  const TPose3D pose_a(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
60  const TPose3D pose_b(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
61  TPose3D pose_interp;
62  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
63  const TPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
64  pose_interp.getHomogeneousMatrix(HM);
65  expected.getHomogeneousMatrix(HMe);
66  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
67  << "pose_a: " << pose_a << "\npose_b: " << pose_b
68  << "\ninterp: " << pose_interp << endl;
69  }
70  { // Poses at yaw=-+179deg
71  const TPose3D pose_a(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
72  const TPose3D pose_b(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
73  TPose3D pose_interp;
74  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
75  const TPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
76  pose_interp.getHomogeneousMatrix(HM);
77  expected.getHomogeneousMatrix(HMe);
78  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
79  << "pose_a: " << pose_a << "\npose_b: " << pose_b
80  << "\ninterp: " << pose_interp << endl;
81  }
82  { // Poses at yaw=-+179deg
83  const TPose3D pose_a(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
84  const TPose3D pose_b(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
85  TPose3D pose_interp;
86  mrpt::math::slerp_ypr(pose_a, pose_b, 0.5, pose_interp);
87  const TPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
88  pose_interp.getHomogeneousMatrix(HM);
89  expected.getHomogeneousMatrix(HMe);
90  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
91  << "pose_a: " << pose_a.asString()
92  << "\npose_b: " << pose_b.asString()
93  << "\ninterp: " << pose_interp.asString() << endl;
94  }
95 
96  { // Poses at yaw=+-40
97  const TPose3D pose_a(0, 0, 0, DEG2RAD(40), DEG2RAD(0), DEG2RAD(0));
98  const TPose3D pose_b(0, 0, 0, DEG2RAD(-40), DEG2RAD(0), DEG2RAD(0));
99  TPose3D pose_interp;
100  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
101  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
102  pose_interp.getHomogeneousMatrix(HM);
103  expected.getHomogeneousMatrix(HMe);
104  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
105  << "pose_a: " << pose_a << "\npose_b: " << pose_b
106  << "\ninterp: " << pose_interp << endl;
107  }
108  { // Poses at yaw=-+40
109  const TPose3D pose_a(0, 0, 0, DEG2RAD(-40), DEG2RAD(0), DEG2RAD(0));
110  const TPose3D pose_b(0, 0, 0, DEG2RAD(40), DEG2RAD(0), DEG2RAD(0));
111  TPose3D pose_interp;
112  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
113  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
114  pose_interp.getHomogeneousMatrix(HM);
115  expected.getHomogeneousMatrix(HMe);
116  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
117  << "pose_a: " << pose_a << "\npose_b: " << pose_b
118  << "\ninterp: " << pose_interp << endl;
119  }
120 
121  { // Poses at pitch=+-40
122  const TPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(40), DEG2RAD(0));
123  const TPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(-40), DEG2RAD(0));
124  TPose3D pose_interp;
125  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
126  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
127  pose_interp.getHomogeneousMatrix(HM);
128  expected.getHomogeneousMatrix(HMe);
129  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
130  << "pose_a: " << pose_a << "\npose_b: " << pose_b
131  << "\ninterp: " << pose_interp << endl;
132  }
133  { // Poses at pitch=-+40
134  const TPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(-40), DEG2RAD(0));
135  const TPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(40), DEG2RAD(0));
136  TPose3D pose_interp;
137  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
138  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
139  pose_interp.getHomogeneousMatrix(HM);
140  expected.getHomogeneousMatrix(HMe);
141  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
142  << "pose_a: " << pose_a << "\npose_b: " << pose_b
143  << "\ninterp: " << pose_interp << endl;
144  }
145 
146  { // Poses at roll=-+40
147  const TPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-40));
148  const TPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(40));
149  TPose3D pose_interp;
150  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
151  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
152  pose_interp.getHomogeneousMatrix(HM);
153  expected.getHomogeneousMatrix(HMe);
154  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
155  << "pose_a: " << pose_a << "\npose_b: " << pose_b
156  << "\ninterp: " << pose_interp << endl;
157  }
158  { // Poses at roll=+-40
159  const TPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(40));
160  const TPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-40));
161  TPose3D pose_interp;
162  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
163  const TPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
164  pose_interp.getHomogeneousMatrix(HM);
165  expected.getHomogeneousMatrix(HMe);
166  EXPECT_NEAR(0, (HM - HMe).sum_abs(), 1e-4)
167  << "pose_a: " << pose_a << "\npose_b: " << pose_b
168  << "\ninterp: " << pose_interp << endl;
169  }
170 }
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
TPose3D.h
mrpt::math::slerp_ypr
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
mrpt::math::slerp
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:32
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:43
CMatrixFixed.h
std
STL namespace.
TEST
TEST(SLERP_tests, correctShortestPath)
Definition: slerp_unittest.cpp:20
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::TPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
Definition: TPoseOrPoint.cpp:465
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:27
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
mrpt::math::TPose3D::asString
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
Definition: TPoseOrPoint.cpp:284


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019