Main MRPT website > C++ reference for MRPT 1.9.9
CPose3DRotVec_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-2018, 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 
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPose3DQuat.h>
13 #include <CTraitsTest.h>
14 #include <gtest/gtest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template class mrpt::CTraitsTest<CPose3DRotVec>;
23 
24 class Pose3DRotVecTests : public ::testing::Test
25 {
26  protected:
27  virtual void SetUp() {}
28  virtual void TearDown() {}
30  {
31  // CPose3DRotVec -> CPose3D -> CPose3DRotVec
32  CPose3DRotVec poseRVT(0.2, 0.3, 0.4, 1, 2, 3);
33  CPose3D pose3D(poseRVT);
34  CPose3DRotVec newPoseRVT_1(pose3D);
35 
36  EXPECT_NEAR(
37  0, (poseRVT.getAsVectorVal() - newPoseRVT_1.getAsVectorVal())
38  .array()
39  .abs()
40  .sum(),
41  1e-5)
42  << "EULER: " << endl
43  << "pRVT : " << poseRVT << endl
44  << "newPoseRVT : " << newPoseRVT_1 << endl;
45 
46  // CPose3DRotVec -> CPose3DQuat -> CPose3DRotVec
47  CPose3DQuat poseQuat = CPose3DQuat(CPose3D(poseRVT));
48  CPose3DRotVec newPoseRVT_2 = CPose3DRotVec(CPose3D(poseQuat));
49 
50  EXPECT_NEAR(
51  0, (poseRVT.getAsVectorVal() - newPoseRVT_2.getAsVectorVal())
52  .array()
53  .abs()
54  .sum(),
55  1e-5)
56  << "Quat: " << endl
57  << "pRVT : " << poseRVT << endl
58  << "newPoseRVT : " << newPoseRVT_2 << endl;
59  }
60 
61  void test_default_values(const CPose3DRotVec& p, const std::string& label)
62  {
63  MRPT_UNUSED_PARAM(label);
64  EXPECT_EQ(p.x(), 0);
65  EXPECT_EQ(p.y(), 0);
66  EXPECT_EQ(p.z(), 0);
67  EXPECT_EQ(p.rx(), 0);
68  EXPECT_EQ(p.ry(), 0);
69  EXPECT_EQ(p.rz(), 0);
70 
71 #if 0
72  CMatrixDouble44 HM = p.getHomogeneousMatrixVal<CMatrixDouble44>();
73  for (size_t i=0;i<4;i++)
74  for (size_t j=0;j<4;j++)
75  EXPECT_NEAR(HM(i,j), i==j ? 1.0 : 0.0, 1e-8 )
76  << "Failed for (i,j)=" << i << "," << j << endl
77  << "Matrix is: " << endl << HM << endl
78  << "case was: " << label << endl;
79 #endif
80  }
81 
83  double x1, double y1, double z1, double yaw1, double pitch1,
84  double roll1, double x2, double y2, double z2, double yaw2,
85  double pitch2, double roll2)
86  {
87  CPose3DRotVec p1;
88  p1.setFromXYZAndAngles(x1, y1, z1, yaw1, pitch1, roll1);
89  // double md = p1.m_rotvec.norm();
90  // p1.m_rotvec /= md;
91  // p1.m_rotvec *= 0.009;
92 
93  CPose3DRotVec p2;
94  p2.setFromXYZAndAngles(x2, y2, z2, yaw2, pitch2, roll2);
95  // md = p2.m_rotvec.norm();
96  // p2.m_rotvec /= md;
97  // p2.m_rotvec *= 0.009;
98  // cout << "p1: " << p1.m_rotvec << endl;
99  // cout << "p2: " << p2.m_rotvec << endl;
100 
101  // cout << "Compose: " << endl;
102  const CPose3DRotVec p1_c_p2 = p1 + p2;
103  const CPose3DRotVec p1_i_p2 = p1 - p2;
104 
105  const CPose3DRotVec p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2
106  const CPose3DRotVec p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1
107 
108  CMatrixDouble44 M1, M2;
109  p1.getHomogeneousMatrix(M1);
110  p2.getHomogeneousMatrix(M2);
111 
112  CPose3DQuat q1(M1), q2(M2), q1_c_q2(UNINITIALIZED_POSE);
113  q1_c_q2 = q1 + q2;
114  const CPose3DRotVec p3 = CPose3DRotVec(CPose3D(q1_c_q2));
115 
116  EXPECT_NEAR(
117  0, (p1_c_p2_i_p2.getAsVectorVal() - p2.getAsVectorVal())
118  .array()
119  .abs()
120  .sum(),
121  1e-5)
122  << "p1 : " << p1 << endl
123  << "p2 : " << p2 << endl
124  << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
125 
126  EXPECT_NEAR(
127  0, (p2_c_p1_i_p2.getAsVectorVal() - p1.getAsVectorVal())
128  .array()
129  .abs()
130  .sum(),
131  1e-5)
132  << "p1 : " << p1 << endl
133  << "p2 : " << p2 << endl
134  << "p2 matrix : " << endl
136  << "p1_i_p2 : " << p1_i_p2 << endl
137  << "p1_i_p2 matrix: " << endl
138  << p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
139  << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
140 
141  EXPECT_NEAR(
142  0, (p3.getAsVectorVal() - p1_c_p2.getAsVectorVal())
143  .array()
144  .abs()
145  .sum(),
146  1e-5)
147  << "p3 : " << p3 << endl
148  << "p1_c_p2 : " << p1_c_p2 << endl;
149  }
150 };
151 
152 // Elemental tests:
153 TEST_F(Pose3DRotVecTests, DefaultValues)
154 {
155  {
157  test_default_values(p, "Default");
158  }
159 }
160 
161 TEST_F(Pose3DRotVecTests, Conversions) { test_conversions(); }
162 TEST_F(Pose3DRotVecTests, Initialization)
163 {
164  CPose3DRotVec p(0.2, 0.3, 0.4, 1, 2, 3);
165  EXPECT_NEAR(p.m_coords[0], 1, 1e-7);
166  EXPECT_NEAR(p.m_coords[1], 2, 1e-7);
167  EXPECT_NEAR(p.m_coords[2], 3, 1e-7);
168  EXPECT_NEAR(p.m_rotvec[0], 0.2, 1e-7);
169  EXPECT_NEAR(p.m_rotvec[1], 0.3, 1e-7);
170  EXPECT_NEAR(p.m_rotvec[2], 0.4, 1e-7);
171 }
172 
174 {
175  test_compose(
176  25.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(89), DEG2RAD(0), -10.0, 4.0, -8.0,
177  DEG2RAD(20), DEG2RAD(9), DEG2RAD(0));
178 }
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:263
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
double DEG2RAD(const double x)
Degrees to radians.
STL namespace.
void test_compose(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
void test_default_values(const CPose3DRotVec &p, const std::string &label)
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TEST_F(Pose3DRotVecTests, DefaultValues)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:275
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:44
GLfloat GLfloat p
Definition: glext.h:6305
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019