MRPT  2.0.1
CPose2DInterpolator_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
13 #include <mrpt/system/datetime.h>
14 
15 template class mrpt::CTraitsTest<mrpt::poses::CPose2DInterpolator>;
16 
17 TEST(CPose2DInterpolator, interp)
18 {
19  using namespace mrpt::poses;
20  using namespace mrpt; // for 0.0_deg
21  using mrpt::DEG2RAD;
22  using mrpt::math::TPose2D;
23 
24  auto t0 = mrpt::Clock::now();
25  using namespace std::chrono_literals;
26  auto dt = 100ms;
27 
28  CPose2DInterpolator pose_path;
29 
30  pose_path.insert(t0, TPose2D(1., 2., 30.0_deg));
31  pose_path.insert(
32  t0 + 2 * dt, TPose2D(1. + 3., 2. + 4., DEG2RAD(30.0 + 20.0)));
33 
34  TPose2D interp;
35  bool valid;
36  pose_path.interpolate(t0 + dt, interp, valid);
37 
38  EXPECT_TRUE(valid);
39 
40  const TPose2D interp_good(1. + 1.5, 2. + 2.0, DEG2RAD(30.0 + 10.0));
41  for (unsigned int i = 0; i < interp_good.size(); i++)
42  {
43  EXPECT_NEAR(interp_good[i], interp[i], 1e-4);
44  }
45 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
Definition: Clock.cpp:94
constexpr double DEG2RAD(const double x)
Degrees to radians.
TEST(CPose2DInterpolator, interp)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
Lightweight 2D pose.
Definition: TPose2D.h:22
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020