helpers.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
00004 You can contact the author at <stephane at magnenat dot net>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the <organization> nor the
00016       names of its contributors may be used to endorse or promote products
00017       derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef __NABE_TEST_HELPERS_H
00033 #define __NABE_TEST_HELPERS_H
00034 
00035 #include "nabo/nabo.h"
00036 #include <limits>
00037 #include <iostream>
00038 #include <fstream>
00039 
00040 #ifdef BOOST_STDINT
00041         #include <boost/cstdint.hpp>
00042         using boost::uint64_t;
00043 #else // BOOST_STDINT
00044         #include <stdint.h>
00045 #endif // BOOST_STDINT
00046 
00047 using namespace std;
00048 using namespace Nabo;
00049 
00050 template<typename T>
00051 typename NearestNeighbourSearch<T>::Matrix load(const char *fileName)
00052 {
00053         typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00054         
00055         ifstream ifs(fileName);
00056         if (!ifs.good())
00057         {
00058                 cerr << "Cannot open file "<< fileName << endl;
00059                 exit(1);
00060         }
00061         
00062         vector<T> data;
00063         int dim(0);
00064         bool firstLine(true);
00065         
00066         while (!ifs.eof())
00067         {
00068                 char line[1024];
00069                 ifs.getline(line, sizeof(line));
00070                 line[sizeof(line)-1] = 0;
00071                 
00072                 char *token = strtok(line, " \t,;");
00073                 while (token)
00074                 {
00075                         if (firstLine)
00076                                 ++dim;
00077                         data.push_back(atof(token));
00078                         token = strtok(NULL, " \t,;"); // FIXME: non reentrant, use strtok_r
00079                 }
00080                 firstLine = false;
00081         }
00082         
00083         return Matrix::Map(&data[0], dim, data.size() / dim);
00084 }
00085 
00086 template<typename T>
00087 typename NearestNeighbourSearch<T>::Vector createQuery(const typename NearestNeighbourSearch<T>::Matrix& d, const NearestNeighbourSearch<T>& kdt, const int i, const int method)
00088 {
00089         typedef typename NearestNeighbourSearch<T>::Vector VectorT;
00090         if (method < 0)
00091         {
00092                 VectorT q = d.col(i % d.cols());
00093                 T absBound = 0;
00094                 for (int j = 0; j < q.size(); ++j)
00095                         absBound += kdt.maxBound(j) - kdt.minBound(j);
00096                 absBound /= (-method); // divided by -method
00097 #ifdef EIGEN3_API
00098                 if (i < d.cols())
00099                         q.array() += absBound;
00100                 else
00101                         q.array() -= absBound;
00102 #else // EIGEN3_API
00103                 if (i < d.cols())
00104                         q.cwise() += absBound;
00105                 else
00106                         q.cwise() -= absBound;
00107 #endif // EIGEN3_API
00108                 return q;
00109         }
00110         else
00111         {
00112                 VectorT q(kdt.minBound.size());
00113                 for (int j = 0; j < q.size(); ++j)
00114                         q(j) = kdt.minBound(j) + T(rand()) * (kdt.maxBound(j) - kdt.minBound(j)) / T(RAND_MAX);
00115                 return q;
00116         }
00117 }
00118 
00119 template<typename T>
00120 typename NearestNeighbourSearch<T>::Matrix createQuery(const typename NearestNeighbourSearch<T>::Matrix& d, const int itCount, const int method)
00121 {
00122         typedef typename NearestNeighbourSearch<T>::Matrix MatrixT;
00123         typedef Nabo::NearestNeighbourSearch<T> NNS;
00124         NNS* nns = NNS::create(d, d.rows(), typename NNS::SearchType(0));
00125         MatrixT q(d.rows(), itCount);
00126         for (int i = 0; i < itCount; ++i)
00127                 q.col(i) = createQuery<T>(d, *nns, i, method);
00128         delete nns;
00129         return q;
00130 }
00131 
00132 #ifdef __MACH__
00133 #include <mach/clock.h>
00134 #include <mach/mach.h>
00135 #else
00136 #include <time.h>
00137 #endif
00138 
00139 #ifdef _POSIX_TIMERS 
00140 namespace boost
00141 {
00142         /*
00143                 High-precision timer class, using gettimeofday().
00144                 The interface is a subset of the one boost::timer provides,
00145                 but the implementation is much more precise
00146                 on systems where clock() has low precision, such as glibc.
00147         */
00148         struct timer
00149         {
00150                 typedef uint64_t Time;
00151                 
00152                 timer():_start_time(curTime()){ } 
00153                 void restart() { _start_time = curTime(); }
00154                 double elapsed() const                  // return elapsed time in seconds
00155                 { return  double(curTime() - _start_time) / double(1000000000); }
00156 
00157         private:
00158                 Time curTime() const {
00159                         #ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
00160                         clock_serv_t cclock;
00161                         mach_timespec_t ts;
00162                         host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
00163                         clock_get_time(cclock, &ts);
00164                         mach_port_deallocate(mach_task_self(), cclock);
00165                         #else
00166                         struct timespec ts;
00167                         #ifdef CLOCK_PROCESS_CPUTIME_ID
00168                         clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts);
00169                         #else // BSD and old Linux
00170                         clock_gettime(CLOCK_PROF, &ts);
00171                         #endif
00172                         #endif
00173                         return Time(ts.tv_sec) * Time(1000000000) + Time(ts.tv_nsec);
00174                 }
00175                 Time _start_time;
00176         };
00177 }
00178 #else // _POSIX_TIMERS
00179         #include <boost/timer.hpp>
00180 #endif // _POSIX_TIMERS
00181 
00182 #endif // __NABE_TEST_HELPERS_H
00183 


libnabo
Author(s): Stéphane Magnenat
autogenerated on Thu Sep 10 2015 10:54:55