odometry_helper.cpp
Go to the documentation of this file.
00001 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <ecl/config/macros.hpp>
00016 #include <gtest/gtest.h>
00017 #include "../../include/ecl/geometry/odometry_helper.hpp"
00018 
00019 /*****************************************************************************
00020 ** Using
00021 *****************************************************************************/
00022 
00023 using namespace ecl::odometry;
00024 
00025 /*****************************************************************************
00026 ** Tests
00027 *****************************************************************************/
00028 
00029 #if defined(ECL_CXX11_FOUND)
00030 
00031   TEST(OdometryTests,distances) {
00032     Trajectory2D trajectory(3, 5);
00033 
00040     trajectory << -2.0, 2.0, 2.0, -2.0, 0.0,
00041                   -2.0, -2.0, 2.0, 2.0, 0.0,
00042                   0.3, -2.3, 0.0, 4.0, -4.0;
00043 
00044     Pose2D pose;
00045     double distance_pose = 0.0;
00046     double distance_position = 0.0;
00047 
00048     pose = Pose2D(0.0, -3.0, 4.0);
00049     distance_pose = distance(pose, trajectory);
00050 
00051     EXPECT_EQ(1.0, distance_pose);
00052     distance_position = distance(getPosition(pose), trajectory);
00053     EXPECT_EQ(distance_pose, distance_position);
00054 
00055     pose = Pose2D(3.0, 1.0, 0.1);
00056     distance_pose = distance(pose, trajectory);
00057     EXPECT_EQ(1.0, distance_pose);
00058     distance_position = distance(getPosition(pose), trajectory);
00059     EXPECT_EQ(distance_pose, distance_position);
00060 
00061     pose = Pose2D(-3.0, 1.0, -0.1);
00062     distance_pose = distance(pose, trajectory);
00063     // diagnoal -> sqrt(2)
00064     EXPECT_GT(1.4143, distance_pose);
00065     EXPECT_LT(1.4142, distance_pose);
00066     distance_position = distance(getPosition(pose), trajectory);
00067     EXPECT_EQ(distance_pose, distance_position);
00068 
00069     pose = Pose2D(1.0, 3.0, -3.2);
00070     distance_pose = distance(pose, trajectory);
00071     EXPECT_EQ(1.0, distance_pose);
00072     distance_position = distance(getPosition(pose), trajectory);
00073     EXPECT_EQ(distance_pose, distance_position);
00074 
00075     pose = Pose2D(1.0, -3.0, 4.1);
00076     distance_pose = distance(pose, trajectory);
00077     EXPECT_EQ(1.0, distance_pose);
00078     distance_position = distance(getPosition(pose), trajectory);
00079     EXPECT_EQ(distance_pose, distance_position);
00080 
00081     pose = Pose2D(-2.0, -4.0, 0.1);
00082     distance_pose = distance(pose, trajectory);
00083     EXPECT_EQ(2.0, distance_pose);
00084     distance_position = distance(getPosition(pose), trajectory);
00085     EXPECT_EQ(distance_pose, distance_position);
00086 
00087     pose = Pose2D(-2.0, -2.0, 0.1);
00088     distance_pose = distance(pose, trajectory);
00089     EXPECT_EQ(0.0, distance_pose);
00090     distance_position = distance(getPosition(pose), trajectory);
00091     EXPECT_EQ(distance_pose, distance_position);
00092 
00093     pose = Pose2D(1.0, 0.0, -8.0);
00094     distance_pose = distance(pose, trajectory);
00095     EXPECT_EQ(1.0, distance_pose);
00096     distance_position = distance(getPosition(pose), trajectory);
00097     EXPECT_EQ(distance_pose, distance_position);
00098   }
00099 
00100   TEST(OdometryTests,trajectories) {
00101     Trajectory2DPtr trajectory;
00102     EXPECT_EQ(true, empty(trajectory));
00103 
00104     Odom2DTrajectoryPtr odom_traj;
00105     EXPECT_EQ(true, empty(odom_traj));
00106 
00107     trajectory = std::make_shared<Trajectory2D>();
00108     EXPECT_EQ(true, empty(trajectory));
00109     EXPECT_EQ(0, size(trajectory));
00110     EXPECT_EQ(0, size(*trajectory));
00111 
00112     odom_traj = std::make_shared<Odom2DTrajectory>();
00113     EXPECT_EQ(true, empty(odom_traj));
00114     EXPECT_EQ(0, size(odom_traj));
00115     EXPECT_EQ(0, size(*odom_traj));
00116 
00117     trajectory = std::make_shared<Trajectory2D>(3, 1);
00118     *trajectory << 0.0, 0.0, 0.0;
00119     EXPECT_EQ(false, empty(trajectory));
00120     EXPECT_EQ(1, size(trajectory));
00121     EXPECT_EQ(1, size(*trajectory));
00122 
00123     odom_traj = std::make_shared<Odom2DTrajectory>(6, 1);
00124     *odom_traj << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
00125     EXPECT_EQ(false, empty(odom_traj));
00126     EXPECT_EQ(1, size(odom_traj));
00127     EXPECT_EQ(1, size(*odom_traj));
00128 
00129     resize(*trajectory, 4);
00130     EXPECT_EQ(false, empty(trajectory));
00131     EXPECT_EQ(4, size(trajectory));
00132     EXPECT_EQ(4, size(*trajectory));
00133 
00134     resize(*odom_traj, 7);
00135     EXPECT_EQ(false, empty(odom_traj));
00136     EXPECT_EQ(7, size(odom_traj));
00137     EXPECT_EQ(7, size(*odom_traj));
00138 
00139     resize(*trajectory, 0);
00140     EXPECT_EQ(true, empty(trajectory));
00141     EXPECT_EQ(0, size(trajectory));
00142     EXPECT_EQ(0, size(*trajectory));
00143 
00144     resize(*odom_traj, 0);
00145     EXPECT_EQ(true, empty(odom_traj));
00146     EXPECT_EQ(0, size(odom_traj));
00147     EXPECT_EQ(0, size(*odom_traj));
00148 
00149     resize(*odom_traj, 3);
00150     addAtEnd(*trajectory, getPoses(*odom_traj));
00151     EXPECT_EQ(false, empty(trajectory));
00152     EXPECT_EQ(3, size(trajectory));
00153     EXPECT_EQ(3, size(*trajectory));
00154 
00155     EXPECT_EQ(getAt(*trajectory, 0), getPose(getAt(*odom_traj, 0)));
00156     EXPECT_EQ(getAt(*trajectory, 2), getAt(getPoses(*odom_traj), 2));
00157 
00158     EXPECT_EQ(getAt(*trajectory, 0), getFront(*trajectory));
00159     EXPECT_EQ(getAt(*odom_traj, 0), getFront(*odom_traj));
00160 
00161     EXPECT_EQ(getAt(*trajectory, size(trajectory) - 1), getBack(*trajectory));
00162     EXPECT_EQ(getAt(*odom_traj, 2), getBack(*odom_traj));
00163 
00164     setAt(*trajectory, 2, getFront(*trajectory));
00165     EXPECT_EQ(getAt(*trajectory, 2), getFront(*trajectory));
00166 
00167     setAt(*odom_traj, 1, getFront(*odom_traj));
00168     EXPECT_EQ(getAt(*odom_traj, 1), getFront(*odom_traj));
00169   }
00170 #endif
00171 
00172 /*****************************************************************************
00173 ** Main program
00174 *****************************************************************************/
00175 
00176 int main(int argc, char **argv) {
00177 
00178     testing::InitGoogleTest(&argc,argv);
00179     return RUN_ALL_TESTS();
00180 }
00181 
00182 


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Thu Jun 6 2019 21:17:52