legacy_pose3d.cpp
Go to the documentation of this file.
1
10 /*****************************************************************************
11 ** Preprocessor
12 *****************************************************************************/
13
14 #define ECL_USE_EIGEN3 1
15
16 /*****************************************************************************
17 ** Includes
18 *****************************************************************************/
19
20 #include <gtest/gtest.h>
21 #include <ecl/linear_algebra.hpp>
22 #include <ecl/math/constants.hpp>
23 #include "../../include/ecl/geometry/legacy_pose2d.hpp"
24 #include "../../include/ecl/geometry/legacy_pose3d.hpp"
25
26 /*****************************************************************************
27 ** Using
28 *****************************************************************************/
29
30 using ecl::LegacyPose2D;
31 using ecl::LegacyPose3D;
32 using ecl::linear_algebra::AngleAxis;
33 using ecl::linear_algebra::Matrix3d;
34 using ecl::linear_algebra::Vector3d;
35 using ecl::linear_algebra::Quaternion;
36 using ecl::linear_algebra::Translation3d;
37
38 /*****************************************************************************
39 ** Tests
40 *****************************************************************************/
41
42 TEST(LegacyPose3DTests,constructors) {
43  bool result;
44  Vector3d v; v << 1.0,2.0,3.0;
45  Matrix3d zero_rotation = Matrix3d::Identity();
46  Vector3d zero_translation; zero_translation << 0.0,0.0,0.0;
47  LegacyPose2D<double> pose_2d(0.0,1.0,ecl::pi);
48  LegacyPose3D<double> pose1(zero_rotation,v);
49  EXPECT_TRUE(pose1.rotation().isApprox(zero_rotation,0.1));
50  EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
51  LegacyPose3D<double> pose2(pose_2d);
52  result = pose2.rotation().block<2,2>(0,0).isApprox(pose_2d.rotationMatrix(),0.1);
53  EXPECT_TRUE(result);
54  result = pose2.translation().segment<2>(0).isApprox(pose_2d.translation(),0.1);
55  EXPECT_TRUE(result);
56  LegacyPose3D<double> pose3(Quaternion<double>(pose2.rotation()),v);
57  EXPECT_TRUE(pose3.rotation().isApprox(pose2.rotation(),0.1));
58  EXPECT_TRUE(pose3.translation().isApprox(v,0.1));
59  LegacyPose3D<double> pose4(pose3);
60  EXPECT_TRUE(pose4.rotation().isApprox(pose3.rotation(),0.1));
61  EXPECT_TRUE(pose4.translation().isApprox(pose3.translation(),0.1));
62 }
63
64
65 TEST(LegacyPose3DTests,assignment) {
66  Vector3d v; v << 0.0,1.0,0.0;
67  Matrix3d rot; rot << -1.0, 0.0, 0.0,
68  0.0, -1.0, 0.0,
69  0.0, 0.0, 1.0;;
70  LegacyPose2D<double> pose_2d(0.0,1.0,ecl::pi);
71  LegacyPose3D<double> pose1, pose2;
72  pose1 = pose_2d;
73  EXPECT_TRUE(pose1.rotation().isApprox(rot,0.1));
74  EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
75  pose2 = pose1;
76  EXPECT_TRUE(pose2.rotation().isApprox(rot,0.1));
77  EXPECT_TRUE(pose2.translation().isApprox(v,0.1));
78 }
79
80 TEST(LegacyPose3DTests,eigenStyle) {
81  Vector3d v; v << 0.0,1.0,0.0;
82  Matrix3d rot; rot << -1.0, 0.0, 0.0,
83  0.0, -1.0, 0.0,
84  0.0, 0.0, 1.0;;
86  pose1.rotation(rot);
87  pose1.translation(v);
88  EXPECT_TRUE(pose1.rotation().isApprox(rot,0.1));
89  EXPECT_TRUE(pose1.translation().isApprox(v,0.1));
90 }
91 TEST(LegacyPose3DTests,inverse) {
92  Vector3d z_axis; z_axis << 0.0, 0.0, 1.0;
93  Vector3d zero = Vector3d::Zero();
94  Matrix3d rot = AngleAxis<double>(ecl::pi/2.0,z_axis).toRotationMatrix();
95  Vector3d trans; trans << 1.0, 2.0, 3.0;
96  LegacyPose3D<double> pose(rot, trans);
97  LegacyPose3D<double> inverse = pose.inverse();
98  LegacyPose3D<double> repose = pose*inverse;
99  EXPECT_TRUE(Matrix3d::Identity().isApprox(repose.rotation(),0.1));
100  for ( unsigned int i = 0; i < 3; ++i ) {
101  EXPECT_LT(zero[i],repose.translation()[i]+0.01);
102  EXPECT_GT(zero[i]+0.01,repose.translation()[i]);
103  }
104 }
105
106 TEST(LegacyPose3DTests,operators) {
107  Vector3d z_axis; z_axis << 0.0, 0.0, 1.0;
108  Matrix3d rot = AngleAxis<double>(ecl::pi/2.0,z_axis).toRotationMatrix();
109  Matrix3d rot_pi = AngleAxis<double>(ecl::pi,z_axis).toRotationMatrix();
110  Vector3d trans; trans << 1.0, 2.0, 3.0;
111  Vector3d expected_final_trans; expected_final_trans << -1.0, 3.0, 6.0;
112  LegacyPose3D<double> pose(rot, trans);
113  LegacyPose3D<double> final_pose = pose*pose;
114  EXPECT_TRUE(rot_pi.isApprox(final_pose.rotation()));
115  EXPECT_TRUE(expected_final_trans.isApprox(final_pose.translation()));
116  pose *= pose;
117  EXPECT_TRUE(rot_pi.isApprox(pose.rotation()));
118  EXPECT_TRUE(expected_final_trans.isApprox(pose.translation()));
119 }
120
121 /*****************************************************************************
122 ** Main program
123 *****************************************************************************/
124
125 int main(int argc, char **argv) {