tf_benchmark.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <gtest/gtest.h>
00031 #include <tf/tf.h>
00032 #include <sys/time.h>
00033 
00034 #include "tf/LinearMath/Vector3.h"
00035 
00036 void seed_rand()
00037 {
00038   //Seed random number generator with current microseond count
00039   timeval temp_time_struct;
00040   gettimeofday(&temp_time_struct,NULL);
00041   srand(temp_time_struct.tv_usec);
00042 };
00043 
00044 void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
00045 {
00046   seed_rand();
00047   for ( uint64_t i = 0; i < runs ; i++ )
00048   {
00049     xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00050     yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00051     zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00052   }
00053 }
00054 
00055 
00056 using namespace tf;
00057 
00058 
00059 TEST(tf_benchmark, canTransformCacheLength0)
00060 {
00061   tf::Transformer mTR(true);
00062 
00063   uint64_t runs = 100000;
00064   ros::WallTime start_time = ros::WallTime::now();
00065 
00066 
00067   start_time = ros::WallTime::now();
00068   for (uint64_t i = 0 ; i < runs; i++)
00069   {
00070     EXPECT_FALSE(mTR.canTransform("target","source_frame", ros::Time::now()));
00071   }
00072   ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00073   double frequency = (double)runs / run_duration.toSec() ;
00074   printf("can frequency %.2f KHz\n", frequency / 1000.0);
00075   EXPECT_GT( frequency, 10000.0);
00076 }
00077 
00078 
00079 TEST(tf_benchmark, canTransformCacheLength10000)
00080 {
00081   tf::Transformer mTR(true);
00082 
00083   unsigned int cache_length = 10000;
00084   for (unsigned int i = 0; i < cache_length; i++)
00085   {
00086     StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child");
00087     mTR.setTransform(tranStamped);
00088   }
00089 
00090   uint64_t runs = 100000;
00091   ros::WallTime start_time = ros::WallTime::now();
00092 
00093 
00094   //Worst case
00095   start_time = ros::WallTime::now();
00096   for (uint64_t i = 0 ; i < runs; i++)
00097   {
00098     EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10)));
00099   }
00100   ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00101   double frequency = (double)runs / run_duration.toSec() ;
00102   printf("Worst Case Frequency %.2f KHz\n", frequency / 1000.0);
00103   EXPECT_GT( frequency, 10000.0);
00104 
00105   //Worst case
00106   start_time = ros::WallTime::now();
00107   for (uint64_t i = 0 ; i < runs; i++)
00108   {
00109     EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2)));
00110   }
00111   run_duration = ros::WallTime::now() - start_time;
00112   frequency = (double)runs / run_duration.toSec() ;
00113   printf("Intermediate Case Frequency %.2f KHz\n", frequency / 1000.0);
00114   EXPECT_GT( frequency, 10000.0);
00115 
00116   //Best case
00117   start_time = ros::WallTime::now();
00118   for (uint64_t i = 0 ; i < runs; i++)
00119   {
00120     EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1)));
00121   }
00122   run_duration = ros::WallTime::now() - start_time;
00123   frequency = (double)runs / run_duration.toSec() ;
00124   printf("Best Case Frequency %.2f KHz\n", frequency / 1000.0);
00125   EXPECT_GT( frequency, 10000.0);
00126 }
00127 
00128 TEST(tf_benchmark, lookupTransformCacheLength10000)
00129 {
00130   tf::Transformer mTR(true);
00131 
00132   unsigned int cache_length = 10000;
00133   for (unsigned int i = 0; i < cache_length; i++)
00134   {
00135     StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child");
00136     mTR.setTransform(tranStamped);
00137   }
00138 
00139   uint64_t runs = 100000;
00140   ros::WallTime start_time = ros::WallTime::now();
00141 
00142   StampedTransform rv;
00143   //Worst case
00144   start_time = ros::WallTime::now();
00145   for (uint64_t i = 0 ; i < runs; i++)
00146   {
00147     mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10), rv);
00148   }
00149   ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00150   double frequency = (double)runs / run_duration.toSec() ;
00151   printf("Worst Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
00152   EXPECT_GT( frequency, 10000.0);
00153 
00154   //Worst case
00155   start_time = ros::WallTime::now();
00156   for (uint64_t i = 0 ; i < runs; i++)
00157   {
00158     mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2), rv);
00159   }
00160   run_duration = ros::WallTime::now() - start_time;
00161   frequency = (double)runs / run_duration.toSec() ;
00162   printf("Intermediate Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
00163   EXPECT_GT( frequency, 10000.0);
00164 
00165   //Best case
00166   start_time = ros::WallTime::now();
00167   for (uint64_t i = 0 ; i < runs; i++)
00168   {
00169     mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1), rv);
00170   }
00171   run_duration = ros::WallTime::now() - start_time;
00172   frequency = (double)runs / run_duration.toSec() ;
00173   printf("Best Case Lookup Frequency %.2f KHz\n", frequency / 1000.0);
00174   EXPECT_GT( frequency, 10000.0);
00175 }
00176 
00177 TEST(tf_benchmark, benchmarkExhaustiveSearch)
00178 {
00179   uint64_t runs = 40000;
00180   double epsilon = 1e-6;
00181   seed_rand();
00182   
00183   tf::Transformer mTR(true);
00184   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00185   for ( uint64_t i = 0; i < runs ; i++ )
00186   {
00187     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00188     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00189     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00190 
00191     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");
00192     mTR.setTransform(tranStamped);
00193   }
00194 
00195   ros::WallTime start_time = ros::WallTime::now();
00196   for ( uint64_t i = 0; i < runs ; i++ )
00197 
00198   {
00199     Stamped<tf::Transform> inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child");
00200 
00201     try{
00202     Stamped<Pose> outpose;
00203     outpose.setIdentity(); //to make sure things are getting mutated
00204     mTR.transformPose("my_parent",inpose, outpose);
00205     EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
00206     EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
00207     EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
00208     }
00209     catch (tf::TransformException & ex)
00210     {
00211       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00212       bool exception_improperly_thrown = true;
00213       EXPECT_FALSE(exception_improperly_thrown);
00214     }
00215   }
00216   
00217   Stamped<Pose> inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child");
00218   Stamped<Pose> outpose;
00219   outpose.setIdentity(); //to make sure things are getting mutated
00220   mTR.transformPose("child",inpose, outpose);
00221   EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00222   EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00223   EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00224   
00225   ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00226   double frequency = (double)runs / run_duration.toSec() ;
00227   printf("exhaustive search frequency %.2f KHz\n", frequency / 1000.0);
00228   EXPECT_GT( frequency, 500.0);
00229   
00230 }
00231 
00232 int main(int argc, char **argv){
00233         ros::Time::init();
00234   testing::InitGoogleTest(&argc, argv);
00235   return RUN_ALL_TESTS();
00236 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:04