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"
34 #include "tf/LinearMath/Matrix3x3.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 
57  StampedTransform t;
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 
93  StampedTransform t;
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 
125  StampedTransform t;
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 
156  StampedTransform t;
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 
208  StampedTransform t;
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 
379  StampedTransform t;
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 
418  StampedTransform t;
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 }
tf::StampedTransform::stamp_
ros::Time stamp_
The timestamp associated with this transform.
Definition: transform_datatypes.h:84
Matrix3x3.h
tf::angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: Quaternion.h:407
tf::TransformStorage
Storage for transforms and their parent.
Definition: time_cache.h:54
tf::TransformStorage::rotation_
tf::Quaternion rotation_
Definition: time_cache.h:83
tf::slerp
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:432
step
unsigned int step
tf::StampedTransform
The Stamped Transform datatype used by tf.
Definition: transform_datatypes.h:81
tf::StampedTransform::frame_id_
std::string frame_id_
The frame_id of the coordinate frame in which this transform is defined.
Definition: transform_datatypes.h:85
main
int main(int argc, char **argv)
Definition: cache_unittest.cpp:438
tf::Quaternion::slerp
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:316
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
Set the rotational element by Quaternion.
Definition: Transform.h:163
tf::TimeCache
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data....
Definition: time_cache.h:97
tf.h
tf::TransformStorage::stamp_
ros::Time stamp_
Definition: time_cache.h:85
tf::TimeCache::getData
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Definition: cache.cpp:192
epsilon
double epsilon
Definition: quaternion.cpp:37
tf::TimeCache::insertData
bool insertData(const TransformStorage &new_data)
Definition: cache.cpp:239
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Set the translational element.
Definition: Transform.h:148
tf::TransformStorage::frame_id_
CompactFrameID frame_id_
Definition: time_cache.h:86
tf::TimeCache::clearList
void clearList()
Definition: cache.cpp:264
TEST
TEST(TimeCache, Repeatability)
Definition: cache_unittest.cpp:48
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
ros::Time
values
std::vector< double > values
Vector3.h
tf::Transform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: Transform.h:170
tf::TransformStorage::translation_
tf::Vector3 translation_
Definition: time_cache.h:84
tf::Quaternion::angle
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:213
tf::Quaternion::setEuler
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:76
tf
Definition: exceptions.h:38
seed_rand
void seed_rand()
Definition: cache_unittest.cpp:37
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30

tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07