Main MRPT website > C++ reference for MRPT 1.9.9
slerp_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/slerp.h>
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/math/lightweight_geom_data.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 // Some
22 TEST(SLERP_tests, correctShortestPath)
23 {
24  { // Both poses at (0,0,0) angles:
25  const CPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
26  const CPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
27  {
28  CPose3D pose_interp;
29  mrpt::math::slerp(pose_a, pose_b, 0, pose_interp);
30  const CPose3D expected(0, 0, 0, 0, 0, 0);
31  EXPECT_NEAR(
32  0, (pose_interp.getHomogeneousMatrixVal() -
33  expected.getHomogeneousMatrixVal())
34  .array()
35  .abs()
36  .sum(),
37  1e-4)
38  << "pose_a: " << pose_a << "\npose_b: " << pose_b
39  << "\ninterp: " << pose_interp << endl;
40  }
41  {
42  CPose3D pose_interp;
43  mrpt::math::slerp(pose_a, pose_b, 1, pose_interp);
44  const CPose3D expected(0, 0, 0, 0, 0, 0);
45  EXPECT_NEAR(
46  0, (pose_interp.getHomogeneousMatrixVal() -
47  expected.getHomogeneousMatrixVal())
48  .array()
49  .abs()
50  .sum(),
51  1e-4)
52  << "pose_a: " << pose_a << "\npose_b: " << pose_b
53  << "\ninterp: " << pose_interp << endl;
54  }
55  {
56  CPose3D pose_interp;
57  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
58  const CPose3D expected(0, 0, 0, 0, 0, 0);
59  EXPECT_NEAR(
60  0, (pose_interp.getHomogeneousMatrixVal() -
61  expected.getHomogeneousMatrixVal())
62  .array()
63  .abs()
64  .sum(),
65  1e-4)
66  << "pose_a: " << pose_a << "\npose_b: " << pose_b
67  << "\ninterp: " << pose_interp << endl;
68  }
69  }
70 
71  { // Poses at yaw=+-179deg
72  const CPose3D pose_a(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
73  const CPose3D pose_b(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
74  CPose3D pose_interp;
75  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
76  const CPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
77  EXPECT_NEAR(
78  0, (pose_interp.getHomogeneousMatrixVal() -
79  expected.getHomogeneousMatrixVal())
80  .array()
81  .abs()
82  .sum(),
83  1e-4)
84  << "pose_a: " << pose_a << "\npose_b: " << pose_b
85  << "\ninterp: " << pose_interp << endl;
86  }
87  { // Poses at yaw=-+179deg
88  const CPose3D pose_a(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
89  const CPose3D pose_b(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
90  CPose3D pose_interp;
91  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
92  const CPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
93  EXPECT_NEAR(
94  0, (pose_interp.getHomogeneousMatrixVal() -
95  expected.getHomogeneousMatrixVal())
96  .array()
97  .abs()
98  .sum(),
99  1e-4)
100  << "pose_a: " << pose_a << "\npose_b: " << pose_b
101  << "\ninterp: " << pose_interp << endl;
102  }
103  { // Poses at yaw=-+179deg
104  const TPose3D pose_a(0, 0, 0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(0));
105  const TPose3D pose_b(0, 0, 0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(0));
106  TPose3D pose_interp;
107  mrpt::math::slerp_ypr(pose_a, pose_b, 0.5, pose_interp);
108  const TPose3D expected(0, 0, 0, DEG2RAD(-180), 0, 0);
109  EXPECT_NEAR(
110  .0, (CPose3D(expected).getHomogeneousMatrixVal() -
111  CPose3D(pose_interp).getHomogeneousMatrixVal())
112  .array()
113  .abs()
114  .sum(),
115  1e-4)
116  << "pose_a: " << pose_a.asString()
117  << "\npose_b: " << pose_b.asString()
118  << "\ninterp: " << pose_interp.asString() << endl;
119  }
120 
121  { // Poses at yaw=+-40
122  const CPose3D pose_a(0, 0, 0, DEG2RAD(40), DEG2RAD(0), DEG2RAD(0));
123  const CPose3D pose_b(0, 0, 0, DEG2RAD(-40), DEG2RAD(0), DEG2RAD(0));
124  CPose3D pose_interp;
125  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
126  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
127  EXPECT_NEAR(
128  0, (pose_interp.getHomogeneousMatrixVal() -
129  expected.getHomogeneousMatrixVal())
130  .array()
131  .abs()
132  .sum(),
133  1e-4)
134  << "pose_a: " << pose_a << "\npose_b: " << pose_b
135  << "\ninterp: " << pose_interp << endl;
136  }
137  { // Poses at yaw=-+40
138  const CPose3D pose_a(0, 0, 0, DEG2RAD(-40), DEG2RAD(0), DEG2RAD(0));
139  const CPose3D pose_b(0, 0, 0, DEG2RAD(40), DEG2RAD(0), DEG2RAD(0));
140  CPose3D pose_interp;
141  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
142  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
143  EXPECT_NEAR(
144  0, (pose_interp.getHomogeneousMatrixVal() -
145  expected.getHomogeneousMatrixVal())
146  .array()
147  .abs()
148  .sum(),
149  1e-4)
150  << "pose_a: " << pose_a << "\npose_b: " << pose_b
151  << "\ninterp: " << pose_interp << endl;
152  }
153 
154  { // Poses at pitch=+-40
155  const CPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(40), DEG2RAD(0));
156  const CPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(-40), DEG2RAD(0));
157  CPose3D pose_interp;
158  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
159  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
160  EXPECT_NEAR(
161  0, (pose_interp.getHomogeneousMatrixVal() -
162  expected.getHomogeneousMatrixVal())
163  .array()
164  .abs()
165  .sum(),
166  1e-4)
167  << "pose_a: " << pose_a << "\npose_b: " << pose_b
168  << "\ninterp: " << pose_interp << endl;
169  }
170  { // Poses at pitch=-+40
171  const CPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(-40), DEG2RAD(0));
172  const CPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(40), DEG2RAD(0));
173  CPose3D pose_interp;
174  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
175  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
176  EXPECT_NEAR(
177  0, (pose_interp.getHomogeneousMatrixVal() -
178  expected.getHomogeneousMatrixVal())
179  .array()
180  .abs()
181  .sum(),
182  1e-4)
183  << "pose_a: " << pose_a << "\npose_b: " << pose_b
184  << "\ninterp: " << pose_interp << endl;
185  }
186 
187  { // Poses at roll=-+40
188  const CPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-40));
189  const CPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(40));
190  CPose3D pose_interp;
191  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
192  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
193  EXPECT_NEAR(
194  0, (pose_interp.getHomogeneousMatrixVal() -
195  expected.getHomogeneousMatrixVal())
196  .array()
197  .abs()
198  .sum(),
199  1e-4)
200  << "pose_a: " << pose_a << "\npose_b: " << pose_b
201  << "\ninterp: " << pose_interp << endl;
202  }
203  { // Poses at roll=+-40
204  const CPose3D pose_a(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(40));
205  const CPose3D pose_b(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-40));
206  CPose3D pose_interp;
207  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
208  const CPose3D expected(0, 0, 0, DEG2RAD(0), 0, 0);
209  EXPECT_NEAR(
210  0, (pose_interp.getHomogeneousMatrixVal() -
211  expected.getHomogeneousMatrixVal())
212  .array()
213  .abs()
214  .sum(),
215  1e-4)
216  << "pose_a: " << pose_a << "\npose_b: " << pose_b
217  << "\ninterp: " << pose_interp << endl;
218  }
219 }
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
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:35
std
STL namespace.
CPose3D.h
TEST
TEST(SLERP_tests, correctShortestPath)
Definition: slerp_unittest.cpp:22
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:46
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::poses::CPose3D::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:229
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
lightweight_geom_data.h
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:616
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: lightweight_geom_data.cpp:254


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