MRPT  2.0.0
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-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 <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, 0.0_deg, 0.0_deg, 0.0_deg);
25  const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
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, 179.0_deg, 0.0_deg, 0.0_deg);
60  const TPose3D pose_b(0, 0, 0, -179.0_deg, 0.0_deg, 0.0_deg);
61  TPose3D pose_interp;
62  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
63  const TPose3D expected(0, 0, 0, -180.0_deg, 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, -179.0_deg, 0.0_deg, 0.0_deg);
72  const TPose3D pose_b(0, 0, 0, 179.0_deg, 0.0_deg, 0.0_deg);
73  TPose3D pose_interp;
74  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
75  const TPose3D expected(0, 0, 0, -180.0_deg, 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, -179.0_deg, 0.0_deg, 0.0_deg);
84  const TPose3D pose_b(0, 0, 0, 179.0_deg, 0.0_deg, 0.0_deg);
85  TPose3D pose_interp;
86  mrpt::math::slerp_ypr(pose_a, pose_b, 0.5, pose_interp);
87  const TPose3D expected(0, 0, 0, -180.0_deg, 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, 40.0_deg, 0.0_deg, 0.0_deg);
98  const TPose3D pose_b(0, 0, 0, -40.0_deg, 0.0_deg, 0.0_deg);
99  TPose3D pose_interp;
100  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
101  const TPose3D expected(0, 0, 0, 0.0_deg, 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, -40.0_deg, 0.0_deg, 0.0_deg);
110  const TPose3D pose_b(0, 0, 0, 40.0_deg, 0.0_deg, 0.0_deg);
111  TPose3D pose_interp;
112  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
113  const TPose3D expected(0, 0, 0, 0.0_deg, 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, 0.0_deg, 40.0_deg, 0.0_deg);
123  const TPose3D pose_b(0, 0, 0, 0.0_deg, -40.0_deg, 0.0_deg);
124  TPose3D pose_interp;
125  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
126  const TPose3D expected(0, 0, 0, 0.0_deg, 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, 0.0_deg, -40.0_deg, 0.0_deg);
135  const TPose3D pose_b(0, 0, 0, 0.0_deg, 40.0_deg, 0.0_deg);
136  TPose3D pose_interp;
137  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
138  const TPose3D expected(0, 0, 0, 0.0_deg, 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, 0.0_deg, 0.0_deg, -40.0_deg);
148  const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, 40.0_deg);
149  TPose3D pose_interp;
150  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
151  const TPose3D expected(0, 0, 0, 0.0_deg, 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, 0.0_deg, 0.0_deg, 40.0_deg);
160  const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, -40.0_deg);
161  TPose3D pose_interp;
162  mrpt::math::slerp(pose_a, pose_b, 0.5, pose_interp);
163  const TPose3D expected(0, 0, 0, 0.0_deg, 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::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: TPose3D.cpp:36
CMatrixFixed.h
mrpt::math::TPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
Definition: TPose3D.cpp:217
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)


Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mié abr 1 00:50:30 CEST 2020