tf_benchmark.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 
34 #include "tf/LinearMath/Vector3.h"
35 
36 void seed_rand()
37 {
38  //Seed random number generator with current microseond count
39  timeval temp_time_struct;
40  gettimeofday(&temp_time_struct,NULL);
41  srand(temp_time_struct.tv_usec);
42 };
43 
44 void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
45 {
46  seed_rand();
47  for ( uint64_t i = 0; i < runs ; i++ )
48  {
49  xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
50  yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
51  zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
52  }
53 }
54 
55 
56 using namespace tf;
57 
58 
59 TEST(tf_benchmark, canTransformCacheLength0)
60 {
61  tf::Transformer mTR(true);
62 
63  uint64_t runs = 100000;
64  ros::WallTime start_time = ros::WallTime::now();
65 
66 
67  start_time = ros::WallTime::now();
68  for (uint64_t i = 0 ; i < runs; i++)
69  {
70  EXPECT_FALSE(mTR.canTransform("target","source_frame", ros::Time::now()));
71  }
72  ros::WallDuration run_duration = ros::WallTime::now() - start_time;
73  double frequency = (double)runs / run_duration.toSec() ;
74  printf("can frequency %.2f KHz\n", frequency / 1000.0);
75  EXPECT_GT( frequency, 10000.0);
76 }
77 
78 
79 TEST(tf_benchmark, canTransformCacheLength10000)
80 {
81  tf::Transformer mTR(true);
82 
83  unsigned int cache_length = 10000;
84  for (unsigned int i = 0; i < cache_length; i++)
85  {
86  StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child");
87  mTR.setTransform(tranStamped);
88  }
89 
90  uint64_t runs = 100000;
91  ros::WallTime start_time = ros::WallTime::now();
92 
93 
94  //Worst case
95  start_time = ros::WallTime::now();
96  for (uint64_t i = 0 ; i < runs; i++)
97  {
98  EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10)));
99  }
100  ros::WallDuration run_duration = ros::WallTime::now() - start_time;
101  double frequency = (double)runs / run_duration.toSec() ;
102  printf("Worst Case Frequency %.2f KHz\n", frequency / 1000.0);
103  EXPECT_GT( frequency, 10000.0);
104 
105  //Worst case
106  start_time = ros::WallTime::now();
107  for (uint64_t i = 0 ; i < runs; i++)
108  {
109  EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2)));
110  }
111  run_duration = ros::WallTime::now() - start_time;
112  frequency = (double)runs / run_duration.toSec() ;
113  printf("Intermediate Case Frequency %.2f KHz\n", frequency / 1000.0);
114  EXPECT_GT( frequency, 10000.0);
115 
116  //Best case
117  start_time = ros::WallTime::now();
118  for (uint64_t i = 0 ; i < runs; i++)
119  {
120  EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1)));
121  }
122  run_duration = ros::WallTime::now() - start_time;
123  frequency = (double)runs / run_duration.toSec() ;
124  printf("Best Case Frequency %.2f KHz\n", frequency / 1000.0);
125  EXPECT_GT( frequency, 10000.0);
126 }
127 
128 TEST(tf_benchmark, lookupTransformCacheLength10000)
129 {
130  tf::Transformer mTR(true);
131 
132  unsigned int cache_length = 10000;
133  for (unsigned int i = 0; i < cache_length; i++)
134  {
135  StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child");
136  mTR.setTransform(tranStamped);
137  }
138 
139  uint64_t runs = 100000;
140  ros::WallTime start_time = ros::WallTime::now();
141 
142  StampedTransform rv;
143  //Worst case
144  start_time = ros::WallTime::now();
145  for (uint64_t i = 0 ; i < runs; i++)
146  {
147  mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10), rv);
148  }
149  ros::WallDuration run_duration = ros::WallTime::now() - start_time;
150  double frequency = (double)runs / run_duration.toSec() ;
151  printf("Worst Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
152  EXPECT_GT( frequency, 10000.0);
153 
154  //Worst case
155  start_time = ros::WallTime::now();
156  for (uint64_t i = 0 ; i < runs; i++)
157  {
158  mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2), rv);
159  }
160  run_duration = ros::WallTime::now() - start_time;
161  frequency = (double)runs / run_duration.toSec() ;
162  printf("Intermediate Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
163  EXPECT_GT( frequency, 10000.0);
164 
165  //Best case
166  start_time = ros::WallTime::now();
167  for (uint64_t i = 0 ; i < runs; i++)
168  {
169  mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1), rv);
170  }
171  run_duration = ros::WallTime::now() - start_time;
172  frequency = (double)runs / run_duration.toSec() ;
173  printf("Best Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
174  EXPECT_GT( frequency, 10000.0);
175 }
176 
177 TEST(tf_benchmark, benchmarkExhaustiveSearch)
178 {
179  uint64_t runs = 40000;
180  double epsilon = 1e-6;
181  seed_rand();
182 
183  tf::Transformer mTR(true);
184  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
185  for ( uint64_t i = 0; i < runs ; i++ )
186  {
187  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
188  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
189  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
190 
191  StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10.0 + i), "my_parent", "child");
192  mTR.setTransform(tranStamped);
193  }
194 
195  ros::WallTime start_time = ros::WallTime::now();
196  for ( uint64_t i = 0; i < runs ; i++ )
197 
198  {
199  Stamped<tf::Transform> inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child");
200 
201  try{
202  Stamped<Pose> outpose;
203  outpose.setIdentity(); //to make sure things are getting mutated
204  mTR.transformPose("my_parent",inpose, outpose);
205  EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
206  EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
207  EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
208  }
209  catch (tf::TransformException & ex)
210  {
211  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
212  bool exception_improperly_thrown = true;
213  EXPECT_FALSE(exception_improperly_thrown);
214  }
215  }
216 
217  Stamped<Pose> inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child");
218  Stamped<Pose> outpose;
219  outpose.setIdentity(); //to make sure things are getting mutated
220  mTR.transformPose("child",inpose, outpose);
221  EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
222  EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
223  EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
224 
225  ros::WallDuration run_duration = ros::WallTime::now() - start_time;
226  double frequency = (double)runs / run_duration.toSec() ;
227  printf("exhaustive search frequency %.2f KHz\n", frequency / 1000.0);
228  EXPECT_GT( frequency, 500.0);
229 
230 }
231 
232 int main(int argc, char **argv){
233  ros::Time::init();
234  testing::InitGoogleTest(&argc, argv);
235  return RUN_ALL_TESTS();
236 }
double epsilon
Definition: quaternion.cpp:37
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
Time & fromNSec(uint64_t t)
void seed_rand()
int main(int argc, char **argv)
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
Add transform information to the tf data structure.
Definition: tf.cpp:229
Definition: exceptions.h:38
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Definition: tf.cpp:358
TEST(tf_benchmark, canTransformCacheLength0)
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
static void init()
void transformPose(const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as...
Definition: tf.cpp:494
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:238
static WallTime now()
void generate_rand_vectors(double scale, uint64_t runs, std::vector< double > &xvalues, std::vector< double > &yvalues, std::vector< double > &zvalues)
static Time now()
The Stamped Transform datatype used by tf.
A Class which provides coordinate transforms between any two frames in a system.
Definition: tf.h:90
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 Mon Jun 10 2019 12:25:26