Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DInterpolator_unittest.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/system/datetime.h>
13 #include <gtest/gtest.h>
14 
15 
16 TEST(CPose3DInterpolator,interp)
17 {
18  using namespace mrpt::poses;
19  using mrpt::math::TPose3D;
21 
24 
25  CPose3DInterpolator pose_path;
26 
27  pose_path.insert( t0, TPose3D(1.,2.,3., DEG2RAD(30.0), DEG2RAD(.0),DEG2RAD(.0) ) );
28  pose_path.insert( t0+2*dt, TPose3D(1.+3.,2.+4.,3.+5., DEG2RAD(30.0+20.0), DEG2RAD(.0),DEG2RAD(.0) ) );
29 
30  TPose3D interp;
31  bool valid;
32  pose_path.interpolate(t0+dt,interp,valid);
33 
34  EXPECT_TRUE(valid);
35  const TPose3D interp_good(1.+1.5,2.+2.0,3.+2.5, DEG2RAD(30.0+10.0), DEG2RAD(.0),DEG2RAD(.0) );
36  EXPECT_NEAR(.0, (CPose3D(interp_good).getHomogeneousMatrixVal() - CPose3D(interp).getHomogeneousMatrixVal()).array().abs().sum(), 1e-4);
37 }
38 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
double DEG2RAD(const double x)
Degrees to radians.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
pose_t & interpolate(mrpt::system::TTimeStamp 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...
void insert(mrpt::system::TTimeStamp t, const pose_t &p)
Inserts a new pose in the sequence.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
TEST(CPose3DInterpolator, interp)
#define DEG2RAD
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
GLuint interp
Definition: glext.h:6197



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019