cache_unittest.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 #include "tf/LinearMath/Vector3.h"
00034 #include "tf/LinearMath/Matrix3x3.h"
00035 
00036 
00037 void seed_rand()
00038 {
00039   //Seed random number generator with current microseond count
00040   timeval temp_time_struct;
00041   gettimeofday(&temp_time_struct,NULL);
00042   srand(temp_time_struct.tv_usec);
00043 };
00044 
00045 using namespace tf;
00046 
00047 
00048 TEST(TimeCache, Repeatability)
00049 {
00050   unsigned int runs = 100;
00051 
00052   seed_rand();
00053   
00054   tf::TimeCache  cache;
00055   std::vector<double> values(runs);
00056 
00057   StampedTransform t;
00058   t.setIdentity();
00059   
00060   for ( uint64_t i = 1; i < runs ; i++ )
00061   {
00062     values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00063     std::stringstream ss;
00064     ss << values[i];
00065     t.frame_id_ = ss.str();
00066     t.stamp_ = ros::Time().fromNSec(i);
00067     
00068     TransformStorage stor(t, i, 0);
00069 
00070     cache.insertData(stor);
00071   }
00072 
00073   TransformStorage out;
00074   for ( uint64_t i = 1; i < runs ; i++ )
00075   {
00076     cache.getData(ros::Time().fromNSec(i), out);
00077     EXPECT_EQ(out.frame_id_, i);
00078     EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00079   }
00080   
00081 }
00082 
00083 
00084 TEST(TimeCache, RepeatabilityReverseInsertOrder)
00085 {
00086   unsigned int runs = 100;
00087 
00088   seed_rand();
00089   
00090   tf::TimeCache  cache;
00091   std::vector<double> values(runs);
00092 
00093   StampedTransform t;
00094   t.setIdentity();
00095   
00096   for ( int i = runs -1; i >= 0 ; i-- )
00097   {
00098     values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00099     t.stamp_ = ros::Time().fromNSec(i);
00100 
00101     TransformStorage stor(t, i, 0);
00102 
00103     cache.insertData(stor);
00104   }
00105 
00106   TransformStorage out;
00107   for ( uint64_t i = 1; i < runs ; i++ )
00108   {
00109     cache.getData(ros::Time().fromNSec(i), out);
00110     EXPECT_EQ(out.frame_id_, i);
00111     EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00112   }
00113   
00114 }
00115 
00116 TEST(TimeCache, RepeatabilityRandomInsertOrder)
00117 {
00118   seed_rand();
00119   
00120   tf::TimeCache  cache;
00121   double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
00122   std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); 
00123   unsigned int runs = values.size();
00124 
00125   StampedTransform t;
00126   t.setIdentity();
00127 
00128   for ( uint64_t i = 0; i <runs ; i++ )
00129   {
00130     values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00131     t.stamp_ = ros::Time().fromNSec(i);
00132 
00133     TransformStorage stor(t, i, 0);
00134     
00135     cache.insertData(stor);
00136   }
00137 
00138   TransformStorage out;
00139   for ( uint64_t i = 1; i < runs ; i++ )
00140   {
00141     cache.getData(ros::Time().fromNSec(i), out);
00142     EXPECT_EQ(out.frame_id_, i);
00143     EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00144   }
00145 }
00146 
00147 TEST(TimeCache, ZeroAtFront)
00148 {
00149   uint64_t runs = 100;
00150 
00151   seed_rand();
00152   
00153   tf::TimeCache  cache;
00154   std::vector<double> values(runs);
00155 
00156   StampedTransform t;
00157   t.setIdentity();
00158   
00159   for ( uint64_t i = 0; i <runs ; i++ )
00160   {
00161     values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00162     t.stamp_ = ros::Time().fromNSec(i);
00163 
00164     TransformStorage stor(t, i, 0);
00165     
00166     cache.insertData(stor);
00167   }
00168 
00169   t.stamp_ = ros::Time().fromNSec(runs);
00170   TransformStorage stor(t, runs, 0);
00171   cache.insertData(stor);
00172   
00173   for ( uint64_t i = 1; i < runs ; i++ )
00174   {
00175     cache.getData(ros::Time().fromNSec(i), stor);
00176     EXPECT_EQ(stor.frame_id_, i);
00177     EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00178   }
00179 
00180   cache.getData(ros::Time(), stor);
00181   EXPECT_EQ(stor.frame_id_, runs);
00182   EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
00183 
00184 
00185   t.stamp_ = ros::Time().fromNSec(runs+1);
00186   TransformStorage stor2(t, runs+1, 0);
00187   cache.insertData(stor2);
00188 
00189   //Make sure we get a different value now that a new values is added at the front
00190   cache.getData(ros::Time(), stor);
00191   EXPECT_EQ(stor.frame_id_, runs+1);
00192   EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
00193 }
00194 
00195 TEST(TimeCache, CartesianInterpolation)
00196 {
00197   uint64_t runs = 100;
00198   double epsilon = 1e-6;
00199   seed_rand();
00200   
00201   tf::TimeCache  cache;
00202   std::vector<double> xvalues(2);
00203   std::vector<double> yvalues(2);
00204   std::vector<double> zvalues(2);
00205 
00206   uint64_t offset = 200;
00207 
00208   StampedTransform t;
00209   t.setIdentity();
00210   
00211   for ( uint64_t i = 1; i < runs ; i++ )
00212   {
00213     for (uint64_t step = 0; step < 2 ; step++)
00214     {
00215       xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00216       yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00217       zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00218     
00219       t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
00220       t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00221       TransformStorage stor(t, 2, 0);
00222       cache.insertData(stor);
00223     }
00224     
00225     TransformStorage out;
00226     for (int pos = 0; pos < 100 ; pos++)
00227     {
00228       cache.getData(ros::Time().fromNSec(offset + pos), out);
00229       double x_out = out.translation_.x();
00230       double y_out = out.translation_.y();
00231       double z_out = out.translation_.z();
00232       EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
00233       EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
00234       EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
00235     }
00236     
00237 
00238     cache.clearList();
00239   }
00240 }
00241 
00242 
00243 /* TEST IS BROKEN, NEED TO PREVENT THIS
00245 TEST(TimeCache, ReparentingInterpolationProtection)
00246 {
00247   double epsilon = 1e-6;
00248   uint64_t offset = 555;
00249 
00250   tf::TimeCache cache;
00251   std::vector<double> xvalues(2);
00252   std::vector<double> yvalues(2);
00253   std::vector<double> zvalues(2);
00254 
00255   StampedTransform t;
00256   t.setIdentity();
00257 
00258   for (uint64_t step = 0; step < 2 ; step++)
00259   {
00260     xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00261     yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00262     zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00263     
00264     t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
00265     t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00266     TransformStorage stor(t, step + 4, 0);
00267     cache.insertData(stor);
00268   }
00269   
00270   TransformStorage out;
00271   for (int pos = 0; pos < 100 ; pos ++)
00272   {
00273     EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out));
00274     double x_out = out.translation_.x();
00275     double y_out = out.translation_.y();
00276     double z_out = out.translation_.z();
00277     EXPECT_NEAR(xvalues[0], x_out, epsilon);
00278     EXPECT_NEAR(yvalues[0], y_out, epsilon);
00279     EXPECT_NEAR(zvalues[0], z_out, epsilon);
00280   }
00281   
00282   for (int pos = 100; pos < 120 ; pos ++)
00283   {
00284     EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out));
00285     double x_out = out.translation_.x();
00286     double y_out = out.translation_.y();
00287     double z_out = out.translation_.z();
00288     EXPECT_NEAR(xvalues[1], x_out, epsilon);
00289     EXPECT_NEAR(yvalues[1], y_out, epsilon);
00290     EXPECT_NEAR(zvalues[1], z_out, epsilon);
00291   }
00292 }
00293 
00294 // EXTRAPOLATION DISABLED
00295 TEST(TimeCache, CartesianExtrapolation)
00296 {
00297   uint64_t runs = 100;
00298   double epsilon = 1e-5;
00299   seed_rand();
00300   
00301   tf::TimeCache  cache;
00302   std::vector<double> xvalues(2);
00303   std::vector<double> yvalues(2);
00304   std::vector<double> zvalues(2);
00305 
00306   uint64_t offset = 555;
00307 
00308   StampedTransform t;
00309   t.setIdentity();
00310   
00311   for ( uint64_t i = 1; i < runs ; i++ )
00312   {
00313     for (uint64_t step = 0; step < 2 ; step++)
00314     {
00315       xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00316       yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00317       zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00318     
00319       t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
00320       t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00321       TransformStorage stor(t, 2, 0);
00322       cache.insertData(stor);
00323     }
00324     
00325     TransformStorage out;
00326     for (int pos = -200; pos < 300 ; pos ++)
00327     {
00328       cache.getData(ros::Time().fromNSec(offset + pos), out);
00329       double x_out = out.translation_.x();
00330       double y_out = out.translation_.y();
00331       double z_out = out.translation_.z();
00332       EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
00333       EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
00334       EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
00335     }
00336     
00337     cache.clearList();
00338   }
00339 }
00340 */
00341 
00342 TEST(Bullet, Slerp)
00343 {
00344   uint64_t runs = 100;
00345   seed_rand();
00346 
00347   tf::Quaternion q1, q2;
00348   q1.setEuler(0,0,0);
00349   
00350   for (uint64_t i = 0 ; i < runs ; i++)
00351   {
00352     q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00353                 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00354                 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX);
00355     
00356     
00357     tf::Quaternion q3 = slerp(q1,q2,0.5);
00358     
00359     EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
00360   }
00361 
00362 }
00363 
00364 
00365 TEST(TimeCache, AngularInterpolation)
00366 {
00367   uint64_t runs = 100;
00368   double epsilon = 1e-6;
00369   seed_rand();
00370   
00371   tf::TimeCache  cache;
00372   std::vector<double> yawvalues(2);
00373   std::vector<double> pitchvalues(2);
00374   std::vector<double> rollvalues(2);
00375   uint64_t offset = 200;
00376 
00377   std::vector<tf::Quaternion> quats(2);
00378 
00379   StampedTransform t;
00380   t.setIdentity();
00381   
00382   for ( uint64_t i = 1; i < runs ; i++ )
00383   {
00384     for (uint64_t step = 0; step < 2 ; step++)
00385     {
00386       yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0;
00387       pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00388       rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00389       quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
00390       t.setRotation(quats[step]);
00391       t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
00392       TransformStorage stor(t, 3, 0);
00393       cache.insertData(stor);
00394     }
00395     
00396     TransformStorage out;
00397     for (int pos = 0; pos < 100 ; pos ++)
00398     {
00399       cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position
00400       tf::Quaternion quat = out.rotation_; //get the quaternion out of the transform
00401 
00402       //Generate a ground truth quaternion directly calling slerp
00403       tf::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
00404       
00405       //Make sure the transformed one and the direct call match
00406       EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
00407     }
00408     
00409     cache.clearList();
00410   }
00411 }
00412 
00413 TEST(TimeCache, DuplicateEntries)
00414 {
00415 
00416   TimeCache cache;
00417 
00418   StampedTransform t;
00419   t.setIdentity();
00420   t.stamp_ = ros::Time().fromNSec(1);
00421   TransformStorage stor(t, 3, 0);
00422   cache.insertData(stor);
00423   cache.insertData(stor);
00424 
00425 
00426   cache.getData(ros::Time().fromNSec(1), stor);
00427   
00428   EXPECT_TRUE(!std::isnan(stor.translation_.x()));
00429   EXPECT_TRUE(!std::isnan(stor.translation_.y()));
00430   EXPECT_TRUE(!std::isnan(stor.translation_.z()));
00431   EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
00432   EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
00433   EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
00434   EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
00435 }
00436 
00437 
00438 int main(int argc, char **argv){
00439   testing::InitGoogleTest(&argc, argv);
00440   return RUN_ALL_TESTS();
00441 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:02:09