Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPose3DQuat.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 class Pose3DRotVecTests : public ::testing::Test {
22 protected:
23  virtual void SetUp()
24  {
25  }
26 
27  virtual void TearDown() { }
28 
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(0, (poseRVT.getAsVectorVal()-newPoseRVT_1.getAsVectorVal()).array().abs().sum(), 1e-5)
37  << "EULER: " << endl
38  << "pRVT : " << poseRVT << endl
39  << "newPoseRVT : " << newPoseRVT_1 << endl;
40 
41  // CPose3DRotVec -> CPose3DQuat -> CPose3DRotVec
42  CPose3DQuat poseQuat= CPose3DQuat(CPose3D(poseRVT));
43  CPose3DRotVec newPoseRVT_2= CPose3DRotVec(CPose3D(poseQuat));
44 
45  EXPECT_NEAR(0, (poseRVT.getAsVectorVal()-newPoseRVT_2.getAsVectorVal()).array().abs().sum(), 1e-5)
46  << "Quat: " << endl
47  << "pRVT : " << poseRVT << endl
48  << "newPoseRVT : " << newPoseRVT_2 << endl;
49 
50  }
51 
52  void test_default_values(const CPose3DRotVec &p, const std::string & label)
53  {
54  MRPT_UNUSED_PARAM(label);
55  EXPECT_EQ(p.x(),0);
56  EXPECT_EQ(p.y(),0);
57  EXPECT_EQ(p.z(),0);
58  EXPECT_EQ(p.rx(),0);
59  EXPECT_EQ(p.ry(),0);
60  EXPECT_EQ(p.rz(),0);
61 
62 #if 0
63  CMatrixDouble44 HM = p.getHomogeneousMatrixVal();
64  for (size_t i=0;i<4;i++)
65  for (size_t j=0;j<4;j++)
66  EXPECT_NEAR(HM(i,j), i==j ? 1.0 : 0.0, 1e-8 )
67  << "Failed for (i,j)=" << i << "," << j << endl
68  << "Matrix is: " << endl << HM << endl
69  << "case was: " << label << endl;
70 #endif
71  }
72 
73  void test_compose(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
74  double x2,double y2,double z2, double yaw2,double pitch2,double roll2)
75  {
76  CPose3DRotVec p1;
77  p1.setFromXYZAndAngles(x1,y1,z1,yaw1,pitch1,roll1);
78 // double md = p1.m_rotvec.norm();
79 // p1.m_rotvec /= md;
80 // p1.m_rotvec *= 0.009;
81 
82  CPose3DRotVec p2;
83  p2.setFromXYZAndAngles(x2,y2,z2,yaw2,pitch2,roll2);
84 // md = p2.m_rotvec.norm();
85 // p2.m_rotvec /= md;
86 // p2.m_rotvec *= 0.009;
87 // cout << "p1: " << p1.m_rotvec << endl;
88 // cout << "p2: " << p2.m_rotvec << endl;
89 
90 // cout << "Compose: " << endl;
91  const CPose3DRotVec p1_c_p2 = p1 + p2;
92  const CPose3DRotVec p1_i_p2 = p1 - p2;
93 
94  const CPose3DRotVec p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2
95  const CPose3DRotVec p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1
96 
97  CMatrixDouble44 M1,M2;
98  p1.getHomogeneousMatrix(M1);
99  p2.getHomogeneousMatrix(M2);
100 
101  CPose3DQuat q1(M1), q2(M2), q1_c_q2(UNINITIALIZED_POSE);
102  q1_c_q2 = q1 + q2;
103  const CPose3DRotVec p3 = CPose3DRotVec(CPose3D(q1_c_q2));
104 
105  EXPECT_NEAR(0, (p1_c_p2_i_p2.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(), 1e-5)
106  << "p1 : " << p1 << endl
107  << "p2 : " << p2 << endl
108  << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
109 
110  EXPECT_NEAR(0, (p2_c_p1_i_p2.getAsVectorVal()-p1.getAsVectorVal()).array().abs().sum(), 1e-5)
111  << "p1 : " << p1 << endl
112  << "p2 : " << p2 << endl
113  << "p2 matrix : " << endl << p2.getHomogeneousMatrixVal() << endl
114  << "p1_i_p2 : " << p1_i_p2 << endl
115  << "p1_i_p2 matrix: " << endl << p1_i_p2.getHomogeneousMatrixVal() << endl
116  << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
117 
118  EXPECT_NEAR(0, (p3.getAsVectorVal()-p1_c_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
119  << "p3 : " << p3 << endl
120  << "p1_c_p2 : " << p1_c_p2 << endl;
121  }
122 };
123 
124 // Elemental tests:
125 TEST_F(Pose3DRotVecTests,DefaultValues)
126 {
127  {
129  test_default_values(p, "Default");
130  }
131 }
132 
134 {
135  test_conversions();
136 }
137 
138 TEST_F(Pose3DRotVecTests,Initialization)
139 {
140  CPose3DRotVec p(0.2,0.3,0.4,1,2,3);
141  EXPECT_NEAR(p.m_coords[0],1, 1e-7);
142  EXPECT_NEAR(p.m_coords[1],2, 1e-7);
143  EXPECT_NEAR(p.m_coords[2],3, 1e-7);
144  EXPECT_NEAR(p.m_rotvec[0],0.2, 1e-7);
145  EXPECT_NEAR(p.m_rotvec[1],0.3, 1e-7);
146  EXPECT_NEAR(p.m_rotvec[2],0.4, 1e-7);
147 }
148 
150 {
151  test_compose( 25.0, 2.0, 3.0, DEG2RAD(-30),DEG2RAD(89),DEG2RAD(0),
152  -10.0, 4.0, -8.0, DEG2RAD( 20),DEG2RAD( 9),DEG2RAD(0));
153 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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:181
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
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.
Definition: CArrayNumeric.h:19
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
#define DEG2RAD
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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:72
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:41
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019