cache_unittest.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 #include <tf/tf.h>
32 #include <sys/time.h>
33 #include "tf/LinearMath/Vector3.h"
35 
36 
37 void seed_rand()
38 {
39  //Seed random number generator with current microseond count
40  timeval temp_time_struct;
41  gettimeofday(&temp_time_struct,NULL);
42  srand(temp_time_struct.tv_usec);
43 };
44 
45 using namespace tf;
46 
47 
48 TEST(TimeCache, Repeatability)
49 {
50  unsigned int runs = 100;
51 
52  seed_rand();
53 
54  tf::TimeCache cache;
55  std::vector<double> values(runs);
56 
58  t.setIdentity();
59 
60  for ( uint64_t i = 1; i < runs ; i++ )
61  {
62  values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
63  std::stringstream ss;
64  ss << values[i];
65  t.frame_id_ = ss.str();
66  t.stamp_ = ros::Time().fromNSec(i);
67 
68  TransformStorage stor(t, i, 0);
69 
70  cache.insertData(stor);
71  }
72 
73  TransformStorage out;
74  for ( uint64_t i = 1; i < runs ; i++ )
75  {
76  cache.getData(ros::Time().fromNSec(i), out);
77  EXPECT_EQ(out.frame_id_, i);
78  EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
79  }
80 
81 }
82 
83 
84 TEST(TimeCache, RepeatabilityReverseInsertOrder)
85 {
86  unsigned int runs = 100;
87 
88  seed_rand();
89 
90  tf::TimeCache cache;
91  std::vector<double> values(runs);
92 
94  t.setIdentity();
95 
96  for ( int i = runs -1; i >= 0 ; i-- )
97  {
98  values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
99  t.stamp_ = ros::Time().fromNSec(i);
100 
101  TransformStorage stor(t, i, 0);
102 
103  cache.insertData(stor);
104  }
105 
106  TransformStorage out;
107  for ( uint64_t i = 1; i < runs ; i++ )
108  {
109  cache.getData(ros::Time().fromNSec(i), out);
110  EXPECT_EQ(out.frame_id_, i);
111  EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
112  }
113 
114 }
115 
116 TEST(TimeCache, RepeatabilityRandomInsertOrder)
117 {
118  seed_rand();
119 
120  tf::TimeCache cache;
121  double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
122  std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double));
123  unsigned int runs = values.size();
124 
126  t.setIdentity();
127 
128  for ( uint64_t i = 0; i <runs ; i++ )
129  {
130  values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
131  t.stamp_ = ros::Time().fromNSec(i);
132 
133  TransformStorage stor(t, i, 0);
134 
135  cache.insertData(stor);
136  }
137 
138  TransformStorage out;
139  for ( uint64_t i = 1; i < runs ; i++ )
140  {
141  cache.getData(ros::Time().fromNSec(i), out);
142  EXPECT_EQ(out.frame_id_, i);
143  EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
144  }
145 }
146 
147 TEST(TimeCache, ZeroAtFront)
148 {
149  uint64_t runs = 100;
150 
151  seed_rand();
152 
153  tf::TimeCache cache;
154  std::vector<double> values(runs);
155 
157  t.setIdentity();
158 
159  for ( uint64_t i = 0; i <runs ; i++ )
160  {
161  values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
162  t.stamp_ = ros::Time().fromNSec(i);
163 
164  TransformStorage stor(t, i, 0);
165 
166  cache.insertData(stor);
167  }
168 
169  t.stamp_ = ros::Time().fromNSec(runs);
170  TransformStorage stor(t, runs, 0);
171  cache.insertData(stor);
172 
173  for ( uint64_t i = 1; i < runs ; i++ )
174  {
175  cache.getData(ros::Time().fromNSec(i), stor);
176  EXPECT_EQ(stor.frame_id_, i);
177  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
178  }
179 
180  cache.getData(ros::Time(), stor);
181  EXPECT_EQ(stor.frame_id_, runs);
182  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
183 
184 
185  t.stamp_ = ros::Time().fromNSec(runs+1);
186  TransformStorage stor2(t, runs+1, 0);
187  cache.insertData(stor2);
188 
189  //Make sure we get a different value now that a new values is added at the front
190  cache.getData(ros::Time(), stor);
191  EXPECT_EQ(stor.frame_id_, runs+1);
192  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
193 }
194 
195 TEST(TimeCache, CartesianInterpolation)
196 {
197  uint64_t runs = 100;
198  double epsilon = 1e-6;
199  seed_rand();
200 
201  tf::TimeCache cache;
202  std::vector<double> xvalues(2);
203  std::vector<double> yvalues(2);
204  std::vector<double> zvalues(2);
205 
206  uint64_t offset = 200;
207 
209  t.setIdentity();
210 
211  for ( uint64_t i = 1; i < runs ; i++ )
212  {
213  for (uint64_t step = 0; step < 2 ; step++)
214  {
215  xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
216  yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
217  zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
218 
219  t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
220  t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
221  TransformStorage stor(t, 2, 0);
222  cache.insertData(stor);
223  }
224 
225  TransformStorage out;
226  for (int pos = 0; pos < 100 ; pos++)
227  {
228  cache.getData(ros::Time().fromNSec(offset + pos), out);
229  double x_out = out.translation_.x();
230  double y_out = out.translation_.y();
231  double z_out = out.translation_.z();
232  EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
233  EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
234  EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
235  }
236 
237 
238  cache.clearList();
239  }
240 }
241 
242 
243 /* TEST IS BROKEN, NEED TO PREVENT THIS
245 TEST(TimeCache, ReparentingInterpolationProtection)
246 {
247  double epsilon = 1e-6;
248  uint64_t offset = 555;
249 
250  tf::TimeCache cache;
251  std::vector<double> xvalues(2);
252  std::vector<double> yvalues(2);
253  std::vector<double> zvalues(2);
254 
255  StampedTransform t;
256  t.setIdentity();
257 
258  for (uint64_t step = 0; step < 2 ; step++)
259  {
260  xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
261  yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
262  zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
263 
264  t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
265  t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
266  TransformStorage stor(t, step + 4, 0);
267  cache.insertData(stor);
268  }
269 
270  TransformStorage out;
271  for (int pos = 0; pos < 100 ; pos ++)
272  {
273  EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out));
274  double x_out = out.translation_.x();
275  double y_out = out.translation_.y();
276  double z_out = out.translation_.z();
277  EXPECT_NEAR(xvalues[0], x_out, epsilon);
278  EXPECT_NEAR(yvalues[0], y_out, epsilon);
279  EXPECT_NEAR(zvalues[0], z_out, epsilon);
280  }
281 
282  for (int pos = 100; pos < 120 ; pos ++)
283  {
284  EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out));
285  double x_out = out.translation_.x();
286  double y_out = out.translation_.y();
287  double z_out = out.translation_.z();
288  EXPECT_NEAR(xvalues[1], x_out, epsilon);
289  EXPECT_NEAR(yvalues[1], y_out, epsilon);
290  EXPECT_NEAR(zvalues[1], z_out, epsilon);
291  }
292 }
293 
294 // EXTRAPOLATION DISABLED
295 TEST(TimeCache, CartesianExtrapolation)
296 {
297  uint64_t runs = 100;
298  double epsilon = 1e-5;
299  seed_rand();
300 
301  tf::TimeCache cache;
302  std::vector<double> xvalues(2);
303  std::vector<double> yvalues(2);
304  std::vector<double> zvalues(2);
305 
306  uint64_t offset = 555;
307 
308  StampedTransform t;
309  t.setIdentity();
310 
311  for ( uint64_t i = 1; i < runs ; i++ )
312  {
313  for (uint64_t step = 0; step < 2 ; step++)
314  {
315  xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
316  yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
317  zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
318 
319  t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
320  t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
321  TransformStorage stor(t, 2, 0);
322  cache.insertData(stor);
323  }
324 
325  TransformStorage out;
326  for (int pos = -200; pos < 300 ; pos ++)
327  {
328  cache.getData(ros::Time().fromNSec(offset + pos), out);
329  double x_out = out.translation_.x();
330  double y_out = out.translation_.y();
331  double z_out = out.translation_.z();
332  EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
333  EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
334  EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
335  }
336 
337  cache.clearList();
338  }
339 }
340 */
341 
342 TEST(Bullet, Slerp)
343 {
344  uint64_t runs = 100;
345  seed_rand();
346 
347  tf::Quaternion q1, q2;
348  q1.setEuler(0,0,0);
349 
350  for (uint64_t i = 0 ; i < runs ; i++)
351  {
352  q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
353  1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
354  1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX);
355 
356 
357  tf::Quaternion q3 = slerp(q1,q2,0.5);
358 
359  EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
360  }
361 
362 }
363 
364 
365 TEST(TimeCache, AngularInterpolation)
366 {
367  uint64_t runs = 100;
368  double epsilon = 1e-6;
369  seed_rand();
370 
371  tf::TimeCache cache;
372  std::vector<double> yawvalues(2);
373  std::vector<double> pitchvalues(2);
374  std::vector<double> rollvalues(2);
375  uint64_t offset = 200;
376 
377  std::vector<tf::Quaternion> quats(2);
378 
380  t.setIdentity();
381 
382  for ( uint64_t i = 1; i < runs ; i++ )
383  {
384  for (uint64_t step = 0; step < 2 ; step++)
385  {
386  yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0;
387  pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
388  rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
389  quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
390  t.setRotation(quats[step]);
391  t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
392  TransformStorage stor(t, 3, 0);
393  cache.insertData(stor);
394  }
395 
396  TransformStorage out;
397  for (int pos = 0; pos < 100 ; pos ++)
398  {
399  cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position
400  tf::Quaternion quat = out.rotation_; //get the quaternion out of the transform
401 
402  //Generate a ground truth quaternion directly calling slerp
403  tf::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
404 
405  //Make sure the transformed one and the direct call match
406  EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
407  }
408 
409  cache.clearList();
410  }
411 }
412 
413 TEST(TimeCache, DuplicateEntries)
414 {
415 
416  TimeCache cache;
417 
419  t.setIdentity();
420  t.stamp_ = ros::Time().fromNSec(1);
421  TransformStorage stor(t, 3, 0);
422  cache.insertData(stor);
423  cache.insertData(stor);
424 
425 
426  cache.getData(ros::Time().fromNSec(1), stor);
427 
428  EXPECT_TRUE(!std::isnan(stor.translation_.x()));
429  EXPECT_TRUE(!std::isnan(stor.translation_.y()));
430  EXPECT_TRUE(!std::isnan(stor.translation_.z()));
431  EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
432  EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
433  EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
434  EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
435 }
436 
437 
438 int main(int argc, char **argv){
439  testing::InitGoogleTest(&argc, argv);
440  return RUN_ALL_TESTS();
441 }
double epsilon
Definition: quaternion.cpp:37
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
Set the rotational element by Quaternion.
Definition: Transform.h:163
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
TEST(TimeCache, Repeatability)
Time & fromNSec(uint64_t t)
std::vector< double > values
CompactFrameID frame_id_
Definition: time_cache.h:86
void clearList()
Definition: cache.cpp:264
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
ros::Time stamp_
The timestamp associated with this transform.
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: Quaternion.h:314
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: Quaternion.h:430
tf::Vector3 translation_
Definition: time_cache.h:84
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Return the ***half*** angle between two quaternions.
Definition: Quaternion.h:405
void setIdentity()
Set this transformation to the identity.
Definition: Transform.h:170
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
tfScalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Definition: Quaternion.h:211
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
bool insertData(const TransformStorage &new_data)
Definition: cache.cpp:239
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:74
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Definition: cache.cpp:192
unsigned int step
Storage for transforms and their parent.
Definition: time_cache.h:54
tf::Quaternion rotation_
Definition: time_cache.h:83
void seed_rand()
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Set the translational element.
Definition: Transform.h:148
std::string frame_id_
The frame_id of the coordinate frame in which this transform is defined.
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