tf_unittest_future.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <tf/tf.h>
3 #include <sys/time.h>
4 
6 
7 using namespace tf;
8 
9 void seed_rand()
10 {
11  //Seed random number generator with current microseond count
12  timeval temp_time_struct;
13  gettimeofday(&temp_time_struct,NULL);
14  srand(temp_time_struct.tv_usec);
15 };
16 
17 
18 
19 TEST(tf, SignFlipExtrapolate)
20 {
21  double epsilon = 1e-3;
22 
23  double truex, truey, trueyaw1, trueyaw2;
24 
25  truex = 5.220;
26  truey = 1.193;
27  trueyaw1 = 2.094;
28  trueyaw2 = 2.199;
29  ros::Time ts0;
30  ts0.fromSec(46.6);
31  ros::Time ts1;
32  ts1.fromSec(46.7);
33  ros::Time ts2;
34  ts2.fromSec(46.8);
35 
36  TransformStorage tout;
37  double yaw, pitch, roll;
38 
40  (tf::Transform(tf::Quaternion(0.000, 0.000, -0.8386707128751809, 0.5446388118427071),
41  tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)),
42  ts0, "odom", "other0"), 3);
44  (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8660255375641606, -0.4999997682866531),
45  tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)),
46  ts1, "odom", "other1"), 3);
48  (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919),
49  tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)),
50  ts2, "odom", "other2"), 3);
51 
52  tf::TimeCache tc;
53  tf::Transform res;
54 
55  tc.interpolate(t0, t1, ts1, tout);
56  res = tout.inverse();
57  res.getBasis().getEulerZYX(yaw,pitch,roll);
58 
59  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
60  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
61  EXPECT_NEAR(yaw, trueyaw1, epsilon);
62 
63  tc.interpolate(t0, t1, ts2, tout);
64  res = tout.inverse();
65  res.getBasis().getEulerZYX(yaw,pitch,roll);
66 
67  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
68  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
69  EXPECT_NEAR(yaw, trueyaw2, epsilon);
70 
71  tc.interpolate(t1, t2, ts2, tout);
72  res = tout.inverse();
73  res.getBasis().getEulerZYX(yaw,pitch,roll);
74 
75  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
76  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
77  EXPECT_NEAR(yaw, trueyaw2, epsilon);
78 }
79 
80 
81 
84 int main(int argc, char **argv){
85  testing::InitGoogleTest(&argc, argv);
86  return RUN_ALL_TESTS();
87 }
88 
double epsilon
Definition: quaternion.cpp:37
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:170
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Definition: time_cache.h:97
Definition: exceptions.h:38
Time & fromSec(double t)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
TEST(tf, SignFlipExtrapolate)
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
void seed_rand()
Storage for transforms and their parent.
Definition: time_cache.h:54
int main(int argc, char **argv)
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28