knnvalidate.cpp
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 #include "nabo/nabo.h"
00033 #include "helpers.h"
00034 //#include "experimental/nabo_experimental.h"
00035 #include <iostream>
00036 #include <fstream>
00037 #include <stdexcept>
00038 
00039 using namespace std;
00040 using namespace Nabo;
00041 
00042 template<typename T, typename CloudType>
00043 struct Loader
00044 {
00045         void loadMatrix(const char *fileName)
00046         {
00047                 data = load<T>(fileName);
00048         }
00049         CloudType getValue() const
00050         {
00051                 return data;
00052         }
00053 private:
00054         CloudType data;
00055 };
00056 
00057 template<typename T>
00058 struct Loader<T, Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned> >
00059 {
00060         void loadMatrix(const char *fileName)
00061         {
00062                 data = load<T>(fileName);
00063         }
00064         Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned> getValue() const
00065         {
00066                 return Eigen::Map<const Eigen::Matrix<T, 3, Eigen::Dynamic>, Eigen::Aligned>(data.data(), 3, data.cols());
00067         }
00068 
00069 private:
00070         Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> data;
00071 };
00072 
00073 template<typename T, typename CloudType>
00074 void validate(const char *fileName, const int K, const int dim, const int method, const T maxRadius)
00075 {
00076         typedef Nabo::NearestNeighbourSearch<T, CloudType> NNS;
00077         typedef vector<NNS*> NNSV;
00078         typedef typename NNS::Matrix Matrix;
00079         typedef typename NNS::Vector Vector;
00080         typedef typename NNS::IndexMatrix IndexMatrix;
00081         
00082         Loader<T, CloudType> loader;
00083         loader.loadMatrix(fileName);
00084 
00085         // check if file is ok
00086         const CloudType d = loader.getValue();
00087         if (d.rows() != dim)
00088         {
00089                 cerr << "Provided data has " << d.rows() << " dimensions, but the requested dimensions were " << dim << endl;
00090                 exit(2);
00091         }
00092         if (K >= d.cols())
00093         {
00094                 cerr << "Requested more nearest neighbour than points in the data set" << endl;
00095                 exit(2);
00096         }
00097         
00098         // create different methods
00099         NNSV nnss;
00100         unsigned searchTypeCount(NNS::SEARCH_TYPE_COUNT);
00101         #ifndef HAVE_OPENCL
00102         searchTypeCount -= 3;
00103         #endif // HAVE_OPENCL
00104         for (unsigned i = 0; i < searchTypeCount; ++i)
00105                 nnss.push_back(NNS::create(d, d.rows(), typename NNS::SearchType(i)));
00106         //nnss.push_back(new KDTreeBalancedPtInLeavesStack<T>(d, false));
00107         
00108         
00109         // check methods together
00110         const int itCount(method != -1 ? method : d.cols() * 2);
00111         
00112         /*
00113         // element-by-element search
00114         typedef typename NNS::IndexVector IndexVector;
00115         for (int i = 0; i < itCount; ++i)
00116         {
00117                 const Vector q(createQuery<T>(d, *nnss[0], i, method));
00118                 const IndexVector indexes_bf(nnss[0]->knn(q, K, 0, NNS::SORT_RESULTS));
00119                 for (size_t j = 1; j < nnss.size(); ++j)
00120                 {
00121                         const IndexVector indexes_kdtree(nnss[j]->knn(q, K, 0, NNS::SORT_RESULTS));
00122                         if (indexes_bf.size() != K)
00123                         {
00124                                 cerr << "Different number of points found between brute force and request" << endl;
00125                                 exit(3);
00126                         }
00127                         if (indexes_bf.size() != indexes_kdtree.size())
00128                         {
00129                                 cerr << "Different number of points found between brute force and NNS type "<< j  << endl;
00130                                 exit(3);
00131                         }
00132                         for (size_t k = 0; k < size_t(K); ++k)
00133                         {
00134                                 Vector pbf(d.col(indexes_bf[k]));
00135                                 //cerr << indexes_kdtree[k] << endl;
00136                                 Vector pkdtree(d.col(indexes_kdtree[k]));
00137                                 if (fabsf((pbf-q).squaredNorm() - (pkdtree-q).squaredNorm()) >= numeric_limits<float>::epsilon())
00138                                 {
00139                                         cerr << "Method " << j << ", cloud point " << i << ", neighbour " << k << " of " << K << " is different between bf and kdtree (dist " << (pbf-pkdtree).norm() << ")\n";
00140                                         cerr << "* query:\n";
00141                                         cerr << q << "\n";
00142                                         cerr << "* indexes " << indexes_bf[k] << " (bf) vs " <<  indexes_kdtree[k] << " (kdtree)\n";
00143                                         cerr << "* coordinates:\n";
00144                                         cerr << "bf: (dist " << (pbf-q).norm() << ")\n";
00145                                         cerr << pbf << "\n";
00146                                         cerr << "kdtree (dist " << (pkdtree-q).norm() << ")\n";
00147                                         cerr << pkdtree << endl;
00148                                         exit(4);
00149                                 }
00150                         }
00151                 }
00152         }
00153         */
00154         // create big query
00155         // check all-in-one query
00156         Matrix q(createQuery<T>(d, itCount, method));
00157         IndexMatrix indexes_bf(K, q.cols());
00158         Matrix dists2_bf(K, q.cols());
00159         nnss[0]->knn(q, indexes_bf, dists2_bf, K, 0, NNS::SORT_RESULTS, maxRadius);
00160         assert(indexes_bf.cols() == q.cols());
00161         for (size_t j = 1; j < nnss.size(); ++j)
00162         {
00163                 IndexMatrix indexes_kdtree(K, q.cols());
00164                 Matrix dists2_kdtree(K, q.cols());
00165                 nnss[j]->knn(q, indexes_kdtree, dists2_kdtree, K, 0, NNS::SORT_RESULTS, maxRadius);
00166                 if (indexes_bf.rows() != K)
00167                 {
00168                         cerr << "Different number of points found between brute force and request" << endl;
00169                         exit(3);
00170                 }
00171                 if (indexes_bf.cols() != indexes_kdtree.cols())
00172                 {
00173                         cerr << "Different number of points found between brute force and NNS type "<< j  << endl;
00174                         exit(3);
00175                 }
00176                 
00177                 for (int i = 0; i < q.cols(); ++i)
00178                 {
00179                         for (size_t k = 0; k < size_t(K); ++k)
00180                         {
00181                                 if (dists2_bf(k,i) == numeric_limits<float>::infinity())
00182                                         continue;
00183                                 const int pbfi(indexes_bf(k,i));
00184                                 const Vector pbf(d.col(pbfi));
00185                                 const int pkdt(indexes_kdtree(k,i));
00186                                 if (pkdt < 0 || pkdt >= d.cols())
00187                                 {
00188                                         cerr << "Method " << j << ", query point " << i << ", neighbour " << k << " of " << K << " has invalid index " << pkdt << " out of range [0:" << d.cols() << "[" << endl;
00189                                         exit(4);
00190                                 }
00191                                 const Vector pkdtree(d.col(pkdt));
00192                                 const Vector pq(q.col(i));
00193                                 const float distDiff(fabsf((pbf-pq).squaredNorm() - (pkdtree-pq).squaredNorm()));
00194                                 if (distDiff > numeric_limits<float>::epsilon())
00195                                 {
00196                                         cerr << "Method " << j << ", query point " << i << ", neighbour " << k << " of " << K << " is different between bf and kdtree (dist2 " << distDiff << ")\n";
00197                                         cerr << "* query point:\n";
00198                                         cerr << pq << "\n";
00199                                         cerr << "* indexes " << pbfi << " (bf) vs " << pkdt << " (kdtree)\n";
00200                                         cerr << "* coordinates:\n";
00201                                         cerr << "bf: (dist " << (pbf-pq).norm() << ")\n";
00202                                         cerr << pbf << "\n";
00203                                         cerr << "kdtree (dist " << (pkdtree-pq).norm() << ")\n";
00204                                         cerr << pkdtree << endl;
00205                                         cerr << "* bf neighbours:\n";
00206                                         for (int l = 0; l < K; ++l)
00207                                                 cerr << indexes_bf(l,i) << " (dist " << (d.col(indexes_bf(l,i))-pq).norm() << ")\n";
00208                                         cerr << "* kdtree neighbours:\n";
00209                                         for (int l = 0; l < K; ++l)
00210                                                 cerr << indexes_kdtree(l,i) << " (dist " << (d.col(indexes_kdtree(l,i))-pq).norm() << ")\n";
00211                                         exit(4);
00212                                 }
00213                         }
00214                 }
00215         }
00216         
00217 //      cout << "\tstats kdtree: "
00218 //              << kdt.getStatistics().totalVisitCount << " on "
00219 //              << (long long)(itCount) * (long long)(d.cols()) << " ("
00220 //              << (100. * double(kdt.getStatistics().totalVisitCount)) /  (double(itCount) * double(d.cols())) << " %"
00221 //              << ")\n" << endl;
00222         
00223         // delete searches
00224         for (typename NNSV::iterator it(nnss.begin()); it != nnss.end(); ++it)
00225                 delete (*it);
00226 }
00227 
00228 int main(int argc, char* argv[])
00229 {
00230         if (argc < 4)
00231         {
00232                 cerr << "Usage " << argv[0] << " DATA K DIM METHOD [MAX_RADIUS]" << endl;
00233                 return 1;
00234         }
00235         
00236         const int K(atoi(argv[2]));
00237         const int dim(atoi(argv[3]));
00238         const int method(atoi(argv[4]));
00239         const float maxRadius(argc >= 6 ? float(atof(argv[5])) : numeric_limits<float>::infinity());
00240         
00241         if (dim == 3)
00242         {
00243                 validate<float, Eigen::MatrixXf>(argv[1], K, dim, method, maxRadius);
00244                 validate<float, Eigen::Matrix3Xf>(argv[1], K, dim, method, maxRadius);
00245                 validate<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >(argv[1], K, dim, method, maxRadius);
00246         } else
00247         {
00248                 validate<float, Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> >(argv[1], K, dim, method, maxRadius);
00249         }
00250         //validate<double>(argv[1], K, method);
00251         
00252         return 0;
00253 }


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