MRPT  2.0.4
CPose3DInterpolator_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>
12 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/system/datetime.h>
15 #include <Eigen/Dense>
16 
17 template class mrpt::CTraitsTest<mrpt::poses::CPose3DInterpolator>;
18 
19 TEST(CPose3DInterpolator, interp)
20 {
21  using namespace mrpt::poses;
22  using namespace mrpt; // for 0.0_deg
23  using mrpt::DEG2RAD;
25  using mrpt::math::TPose3D;
26 
27  auto t0 = mrpt::Clock::now();
28  mrpt::Clock::duration dt(std::chrono::milliseconds(100));
29 
30  CPose3DInterpolator pose_path;
31 
32  pose_path.insert(t0, TPose3D(1., 2., 3., 30.0_deg, .0_deg, .0_deg));
33  pose_path.insert(
34  t0 + 2 * dt,
35  TPose3D(
36  1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0), .0_deg, .0_deg));
37 
38  TPose3D interp;
39  bool valid;
40  pose_path.interpolate(t0 + dt, interp, valid);
41 
42  EXPECT_TRUE(valid);
43  const TPose3D interp_good(
44  1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), .0_deg, .0_deg);
46  .0,
47  (CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
48  CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
49  .array()
50  .abs()
51  .sum(),
52  2e-4);
53 }
std::chrono::duration< rep, period > duration
Definition: Clock.h:24
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
TEST(CPose3DInterpolator, 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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
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)
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:368



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020