test/odometry_helper.cpp
Go to the documentation of this file.
1 
11 /*****************************************************************************
12 ** Includes
13 *****************************************************************************/
14 
15 #include <ecl/config/macros.hpp>
16 #include <gtest/gtest.h>
17 #include "../../include/ecl/geometry/odometry_helper.hpp"
18 
19 /*****************************************************************************
20 ** Using
21 *****************************************************************************/
22 
23 using namespace ecl::odometry;
24 
25 /*****************************************************************************
26 ** Tests
27 *****************************************************************************/
28 
29 #if defined(ECL_CXX11_FOUND)
30 
31  TEST(OdometryTests,distances) {
32  Trajectory2D trajectory(3, 5);
33 
40  trajectory << -2.0, 2.0, 2.0, -2.0, 0.0,
41  -2.0, -2.0, 2.0, 2.0, 0.0,
42  0.3, -2.3, 0.0, 4.0, -4.0;
43 
44  Pose2D pose;
45  double distance_pose = 0.0;
46  double distance_position = 0.0;
47 
48  pose = Pose2D(0.0, -3.0, 4.0);
49  distance_pose = distance(pose, trajectory);
50 
51  EXPECT_EQ(1.0, distance_pose);
52  distance_position = distance(getPosition(pose), trajectory);
53  EXPECT_EQ(distance_pose, distance_position);
54 
55  pose = Pose2D(3.0, 1.0, 0.1);
56  distance_pose = distance(pose, trajectory);
57  EXPECT_EQ(1.0, distance_pose);
58  distance_position = distance(getPosition(pose), trajectory);
59  EXPECT_EQ(distance_pose, distance_position);
60 
61  pose = Pose2D(-3.0, 1.0, -0.1);
62  distance_pose = distance(pose, trajectory);
63  // diagnoal -> sqrt(2)
64  EXPECT_GT(1.4143, distance_pose);
65  EXPECT_LT(1.4142, distance_pose);
66  distance_position = distance(getPosition(pose), trajectory);
67  EXPECT_EQ(distance_pose, distance_position);
68 
69  pose = Pose2D(1.0, 3.0, -3.2);
70  distance_pose = distance(pose, trajectory);
71  EXPECT_EQ(1.0, distance_pose);
72  distance_position = distance(getPosition(pose), trajectory);
73  EXPECT_EQ(distance_pose, distance_position);
74 
75  pose = Pose2D(1.0, -3.0, 4.1);
76  distance_pose = distance(pose, trajectory);
77  EXPECT_EQ(1.0, distance_pose);
78  distance_position = distance(getPosition(pose), trajectory);
79  EXPECT_EQ(distance_pose, distance_position);
80 
81  pose = Pose2D(-2.0, -4.0, 0.1);
82  distance_pose = distance(pose, trajectory);
83  EXPECT_EQ(2.0, distance_pose);
84  distance_position = distance(getPosition(pose), trajectory);
85  EXPECT_EQ(distance_pose, distance_position);
86 
87  pose = Pose2D(-2.0, -2.0, 0.1);
88  distance_pose = distance(pose, trajectory);
89  EXPECT_EQ(0.0, distance_pose);
90  distance_position = distance(getPosition(pose), trajectory);
91  EXPECT_EQ(distance_pose, distance_position);
92 
93  pose = Pose2D(1.0, 0.0, -8.0);
94  distance_pose = distance(pose, trajectory);
95  EXPECT_EQ(1.0, distance_pose);
96  distance_position = distance(getPosition(pose), trajectory);
97  EXPECT_EQ(distance_pose, distance_position);
98  }
99 
100  TEST(OdometryTests,trajectories) {
101  Trajectory2DPtr trajectory;
102  EXPECT_EQ(true, empty(trajectory));
103 
104  Odom2DTrajectoryPtr odom_traj;
105  EXPECT_EQ(true, empty(odom_traj));
106 
107  trajectory = std::make_shared<Trajectory2D>();
108  EXPECT_EQ(true, empty(trajectory));
109  EXPECT_EQ(0, size(trajectory));
110  EXPECT_EQ(0, size(*trajectory));
111 
112  odom_traj = std::make_shared<Odom2DTrajectory>();
113  EXPECT_EQ(true, empty(odom_traj));
114  EXPECT_EQ(0, size(odom_traj));
115  EXPECT_EQ(0, size(*odom_traj));
116 
117  trajectory = std::make_shared<Trajectory2D>(3, 1);
118  *trajectory << 0.0, 0.0, 0.0;
119  EXPECT_EQ(false, empty(trajectory));
120  EXPECT_EQ(1, size(trajectory));
121  EXPECT_EQ(1, size(*trajectory));
122 
123  odom_traj = std::make_shared<Odom2DTrajectory>(6, 1);
124  *odom_traj << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
125  EXPECT_EQ(false, empty(odom_traj));
126  EXPECT_EQ(1, size(odom_traj));
127  EXPECT_EQ(1, size(*odom_traj));
128 
129  resize(*trajectory, 4);
130  EXPECT_EQ(false, empty(trajectory));
131  EXPECT_EQ(4, size(trajectory));
132  EXPECT_EQ(4, size(*trajectory));
133 
134  resize(*odom_traj, 7);
135  EXPECT_EQ(false, empty(odom_traj));
136  EXPECT_EQ(7, size(odom_traj));
137  EXPECT_EQ(7, size(*odom_traj));
138 
139  resize(*trajectory, 0);
140  EXPECT_EQ(true, empty(trajectory));
141  EXPECT_EQ(0, size(trajectory));
142  EXPECT_EQ(0, size(*trajectory));
143 
144  resize(*odom_traj, 0);
145  EXPECT_EQ(true, empty(odom_traj));
146  EXPECT_EQ(0, size(odom_traj));
147  EXPECT_EQ(0, size(*odom_traj));
148 
149  resize(*odom_traj, 3);
150  addAtEnd(*trajectory, getPoses(*odom_traj));
151  EXPECT_EQ(false, empty(trajectory));
152  EXPECT_EQ(3, size(trajectory));
153  EXPECT_EQ(3, size(*trajectory));
154 
155  EXPECT_EQ(getAt(*trajectory, 0), getPose(getAt(*odom_traj, 0)));
156  EXPECT_EQ(getAt(*trajectory, 2), getAt(getPoses(*odom_traj), 2));
157 
158  EXPECT_EQ(getAt(*trajectory, 0), getFront(*trajectory));
159  EXPECT_EQ(getAt(*odom_traj, 0), getFront(*odom_traj));
160 
161  EXPECT_EQ(getAt(*trajectory, size(trajectory) - 1), getBack(*trajectory));
162  EXPECT_EQ(getAt(*odom_traj, 2), getBack(*odom_traj));
163 
164  setAt(*trajectory, 2, getFront(*trajectory));
165  EXPECT_EQ(getAt(*trajectory, 2), getFront(*trajectory));
166 
167  setAt(*odom_traj, 1, getFront(*odom_traj));
168  EXPECT_EQ(getAt(*odom_traj, 1), getFront(*odom_traj));
169  }
170 #endif
171 
172 /*****************************************************************************
173 ** Main program
174 *****************************************************************************/
175 
176 int main(int argc, char **argv) {
177 
178  testing::InitGoogleTest(&argc,argv);
179  return RUN_ALL_TESTS();
180 }
181 
182 
ecl::odometry::Pose2D
Eigen::Vector3f Pose2D
Float representation of a pose in 2D (x, y, heading).
Definition: odometry_typedefs.hpp:66
ecl::odometry::size
ecl_geometry_PUBLIC int size(const Trajectory2D &trajectory)
Get the size of the trajectory.
Definition: lib/odometry_helper.cpp:76
TEST
TEST(AngleTests, setters)
Definition: angles.cpp:31
ecl::odometry::setAt
ecl_geometry_PUBLIC void setAt(Trajectory2D &trajectory, const int &index, const Pose2D &pose)
Set element at index of trajectory.
Definition: lib/odometry_helper.cpp:169
ecl::odometry::getFront
ecl_geometry_PUBLIC Pose2D getFront(const Trajectory2D &trajectory)
Get front (first) element of trajectory.
Definition: lib/odometry_helper.cpp:189
ecl::odometry::getAt
ecl_geometry_PUBLIC Pose2D getAt(const Trajectory2D &trajectory, const int &index)
Get element of trajectory.
Definition: lib/odometry_helper.cpp:179
ecl::odometry::distance
ecl_geometry_PUBLIC double distance(const Pose2D &pose, const Trajectory2D &trajectory)
Shortest distance between a pose and a trajectory.
Definition: lib/odometry_helper.cpp:31
ecl::odometry::Trajectory2D
Eigen::Matrix3Xf Trajectory2D
Float representation of a trajectory in 2D (poses in 2D).
Definition: odometry_typedefs.hpp:74
main
int main(int argc, char **argv)
Definition: test/odometry_helper.cpp:176
ecl::odometry::getPosition
ecl_geometry_PUBLIC Position2D getPosition(const Odom2D &odom)
Extract position from odometry.
Definition: lib/odometry_helper.cpp:332
ecl::odometry::resize
ecl_geometry_PUBLIC void resize(Trajectory2D &trajectory, const int &size)
Resizes trajectory appending uninitialised values if needed.
Definition: lib/odometry_helper.cpp:159
ecl::odometry::getPose
ecl_geometry_PUBLIC Pose2D getPose(const Odom2D &odom)
Extract pose from odometry.
Definition: lib/odometry_helper.cpp:337
ecl::odometry
Definition: odometry_helper.hpp:30
ecl::odometry::getBack
ecl_geometry_PUBLIC Pose2D getBack(const Trajectory2D &trajectory)
Get back (last) element of trajectory.
Definition: lib/odometry_helper.cpp:194
ecl::odometry::getPoses
ecl_geometry_PUBLIC Trajectory2D getPoses(const Odom2DTrajectory &trajectory)
Extract poses of odom trajectory.
Definition: lib/odometry_helper.cpp:209
macros.hpp
ecl::odometry::addAtEnd
ecl_geometry_PUBLIC void addAtEnd(Trajectory2D &target, const Trajectory2D &addition)
Concat two odometry trajectories.
Definition: lib/odometry_helper.cpp:111
ecl::odometry::empty
ecl_geometry_PUBLIC bool empty(const Trajectory2D &trajectory)
Check if trajectory ptr is empty (ptr not set or has no poses)
Definition: lib/odometry_helper.cpp:66

ecl_geometry
Author(s): Daniel Stonier
autogenerated on Wed Mar 2 2022 00:16:39