16 #include <gtest/gtest.h> 17 #include "../../include/ecl/geometry/odometry_helper.hpp" 29 #if defined(ECL_CXX11_FOUND) 31 TEST(OdometryTests,distances) {
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;
45 double distance_pose = 0.0;
46 double distance_position = 0.0;
48 pose =
Pose2D(0.0, -3.0, 4.0);
49 distance_pose =
distance(pose, trajectory);
51 EXPECT_EQ(1.0, distance_pose);
53 EXPECT_EQ(distance_pose, distance_position);
55 pose =
Pose2D(3.0, 1.0, 0.1);
56 distance_pose =
distance(pose, trajectory);
57 EXPECT_EQ(1.0, distance_pose);
59 EXPECT_EQ(distance_pose, distance_position);
61 pose =
Pose2D(-3.0, 1.0, -0.1);
62 distance_pose =
distance(pose, trajectory);
64 EXPECT_GT(1.4143, distance_pose);
65 EXPECT_LT(1.4142, distance_pose);
67 EXPECT_EQ(distance_pose, distance_position);
69 pose =
Pose2D(1.0, 3.0, -3.2);
70 distance_pose =
distance(pose, trajectory);
71 EXPECT_EQ(1.0, distance_pose);
73 EXPECT_EQ(distance_pose, distance_position);
75 pose =
Pose2D(1.0, -3.0, 4.1);
76 distance_pose =
distance(pose, trajectory);
77 EXPECT_EQ(1.0, distance_pose);
79 EXPECT_EQ(distance_pose, distance_position);
81 pose =
Pose2D(-2.0, -4.0, 0.1);
82 distance_pose =
distance(pose, trajectory);
83 EXPECT_EQ(2.0, distance_pose);
85 EXPECT_EQ(distance_pose, distance_position);
87 pose =
Pose2D(-2.0, -2.0, 0.1);
88 distance_pose =
distance(pose, trajectory);
89 EXPECT_EQ(0.0, distance_pose);
91 EXPECT_EQ(distance_pose, distance_position);
93 pose =
Pose2D(1.0, 0.0, -8.0);
94 distance_pose =
distance(pose, trajectory);
95 EXPECT_EQ(1.0, distance_pose);
97 EXPECT_EQ(distance_pose, distance_position);
100 TEST(OdometryTests,trajectories) {
101 Trajectory2DPtr trajectory;
102 EXPECT_EQ(
true,
empty(trajectory));
104 Odom2DTrajectoryPtr odom_traj;
105 EXPECT_EQ(
true,
empty(odom_traj));
107 trajectory = std::make_shared<Trajectory2D>();
108 EXPECT_EQ(
true,
empty(trajectory));
109 EXPECT_EQ(0,
size(trajectory));
110 EXPECT_EQ(0,
size(*trajectory));
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));
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));
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));
130 EXPECT_EQ(
false,
empty(trajectory));
131 EXPECT_EQ(4,
size(trajectory));
132 EXPECT_EQ(4,
size(*trajectory));
135 EXPECT_EQ(
false,
empty(odom_traj));
136 EXPECT_EQ(7,
size(odom_traj));
137 EXPECT_EQ(7,
size(*odom_traj));
140 EXPECT_EQ(
true,
empty(trajectory));
141 EXPECT_EQ(0,
size(trajectory));
142 EXPECT_EQ(0,
size(*trajectory));
145 EXPECT_EQ(
true,
empty(odom_traj));
146 EXPECT_EQ(0,
size(odom_traj));
147 EXPECT_EQ(0,
size(*odom_traj));
151 EXPECT_EQ(
false,
empty(trajectory));
152 EXPECT_EQ(3,
size(trajectory));
153 EXPECT_EQ(3,
size(*trajectory));
176 int main(
int argc,
char **argv) {
178 testing::InitGoogleTest(&argc,argv);
179 return RUN_ALL_TESTS();
ecl_geometry_PUBLIC Trajectory2D getPoses(const Odom2DTrajectory &trajectory)
Extract poses of odom trajectory.
int main(int argc, char **argv)
ecl_geometry_PUBLIC Position2D getPosition(const Odom2D &odom)
Extract position from odometry.
ecl_geometry_PUBLIC void resize(Trajectory2D &trajectory, const int &size)
Resizes trajectory appending uninitialised values if needed.
Eigen::Matrix3Xf Trajectory2D
Float representation of a trajectory in 2D (poses in 2D).
ecl_geometry_PUBLIC Pose2D getPose(const Odom2D &odom)
Extract pose from odometry.
ecl_geometry_PUBLIC bool empty(const Trajectory2D &trajectory)
Check if trajectory ptr is empty (ptr not set or has no poses)
ecl_geometry_PUBLIC Pose2D getBack(const Trajectory2D &trajectory)
Get back (last) element of trajectory.
Eigen::Vector3f Pose2D
Float representation of a pose in 2D (x, y, heading).
ecl_geometry_PUBLIC int size(const Trajectory2D &trajectory)
Get the size of the trajectory.
ecl_geometry_PUBLIC void addAtEnd(Trajectory2D &target, const Trajectory2D &addition)
Concat two odometry trajectories.
ecl_geometry_PUBLIC Pose2D getFront(const Trajectory2D &trajectory)
Get front (first) element of trajectory.
ecl_geometry_PUBLIC Pose2D getAt(const Trajectory2D &trajectory, const int &index)
Get element of trajectory.
TEST(AngleTests, setters)
ecl_geometry_PUBLIC void setAt(Trajectory2D &trajectory, const int &index, const Pose2D &pose)
Set element at index of trajectory.
ecl_geometry_PUBLIC double distance(const Pose2D &pose, const Trajectory2D &trajectory)
Shortest distance between a pose and a trajectory.