Main MRPT website > C++ reference for MRPT 1.5.6
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 
11 #include <mrpt/math/slerp.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/math/lightweight_geom_data.h>
14 #include <gtest/gtest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::utils;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 // Some
23 TEST(SLERP_tests, correctShortestPath)
24 {
25  { // Both poses at (0,0,0) angles:
26  const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
27  const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
28  {
29  CPose3D pose_interp;
30  mrpt::math::slerp(pose_a,pose_b,0,pose_interp);
31  const CPose3D expected(0,0,0,0,0,0);
32  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
33  }
34  {
35  CPose3D pose_interp;
36  mrpt::math::slerp(pose_a,pose_b,1,pose_interp);
37  const CPose3D expected(0,0,0,0,0,0);
38  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
39  }
40  {
41  CPose3D pose_interp;
42  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
43  const CPose3D expected(0,0,0,0,0,0);
44  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
45  }
46  }
47 
48  { // Poses at yaw=+-179deg
49  const CPose3D pose_a(0,0,0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(0));
50  const CPose3D pose_b(0,0,0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(0));
51  CPose3D pose_interp;
52  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
53  const CPose3D expected(0,0,0,DEG2RAD(-180),0,0);
54  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
55  }
56  { // Poses at yaw=-+179deg
57  const CPose3D pose_a(0,0,0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(0));
58  const CPose3D pose_b(0,0,0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(0));
59  CPose3D pose_interp;
60  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
61  const CPose3D expected(0,0,0,DEG2RAD(-180),0,0);
62  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
63  }
64  { // Poses at yaw=-+179deg
65  const TPose3D pose_a(0,0,0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(0));
66  const TPose3D pose_b(0,0,0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(0));
67  TPose3D pose_interp;
68  mrpt::math::slerp_ypr(pose_a,pose_b,0.5,pose_interp);
69  const TPose3D expected(0,0,0,DEG2RAD(-180),0,0);
70  EXPECT_NEAR(.0,(CPose3D(expected).getHomogeneousMatrixVal() - CPose3D(pose_interp).getHomogeneousMatrixVal()).array().abs().sum() , 1e-4 ) << "pose_a: " << pose_a.asString() << "\npose_b: " << pose_b.asString() << "\ninterp: " << pose_interp.asString() << endl;
71  }
72 
73  { // Poses at yaw=+-40
74  const CPose3D pose_a(0,0,0, DEG2RAD(40),DEG2RAD(0),DEG2RAD(0));
75  const CPose3D pose_b(0,0,0, DEG2RAD(-40),DEG2RAD(0),DEG2RAD(0));
76  CPose3D pose_interp;
77  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
78  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
79  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
80  }
81  { // Poses at yaw=-+40
82  const CPose3D pose_a(0,0,0, DEG2RAD(-40),DEG2RAD(0),DEG2RAD(0));
83  const CPose3D pose_b(0,0,0, DEG2RAD(40),DEG2RAD(0),DEG2RAD(0));
84  CPose3D pose_interp;
85  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
86  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
87  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
88  }
89 
90  { // Poses at pitch=+-40
91  const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD( 40),DEG2RAD(0));
92  const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(-40),DEG2RAD(0));
93  CPose3D pose_interp;
94  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
95  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
96  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
97  }
98  { // Poses at pitch=-+40
99  const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(-40),DEG2RAD(0));
100  const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD( 40),DEG2RAD(0));
101  CPose3D pose_interp;
102  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
103  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
104  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
105  }
106 
107  { // Poses at roll=-+40
108  const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(-40));
109  const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD( 40));
110  CPose3D pose_interp;
111  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
112  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
113  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
114  }
115  { // Poses at roll=+-40
116  const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD( 40));
117  const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(-40));
118  CPose3D pose_interp;
119  mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp);
120  const CPose3D expected(0,0,0,DEG2RAD(0),0,0);
121  EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl;
122  }
123 
124 
125 }
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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:23
mrpt::math::slerp_ypr
void BASE_IMPEXP slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:54
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:173
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
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:16
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:72
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
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:191


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019