index_testing.h
Go to the documentation of this file.
00001 /***********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
00005  * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
00006  *
00007  * THE BSD LICENSE
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * 1. Redistributions of source code must retain the above copyright
00014  *    notice, this list of conditions and the following disclaimer.
00015  * 2. Redistributions in binary form must reproduce the above copyright
00016  *    notice, this list of conditions and the following disclaimer in the
00017  *    documentation and/or other materials provided with the distribution.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
00020  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
00021  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
00022  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
00023  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
00024  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00025  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00026  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00028  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *************************************************************************/
00030 
00031 #ifndef RTABMAP_FLANN_INDEX_TESTING_H_
00032 #define RTABMAP_FLANN_INDEX_TESTING_H_
00033 
00034 #include <cstring>
00035 #include <cassert>
00036 #include <cmath>
00037 
00038 #include "rtflann/util/matrix.h"
00039 #include "rtflann/algorithms/nn_index.h"
00040 #include "rtflann/util/result_set.h"
00041 #include "rtflann/util/logger.h"
00042 #include "rtflann/util/timer.h"
00043 
00044 
00045 namespace rtflann
00046 {
00047 
00048 inline int countCorrectMatches(size_t* neighbors, size_t* groundTruth, int n)
00049 {
00050     int count = 0;
00051     for (int i=0; i<n; ++i) {
00052         for (int k=0; k<n; ++k) {
00053             if (neighbors[i]==groundTruth[k]) {
00054                 count++;
00055                 break;
00056             }
00057         }
00058     }
00059     return count;
00060 }
00061 
00062 
00063 template <typename Distance>
00064 typename Distance::ResultType computeDistanceRaport(const Matrix<typename Distance::ElementType>& inputData, typename Distance::ElementType* target,
00065                 size_t* neighbors, size_t* groundTruth, int veclen, int n, const Distance& distance)
00066 {
00067     typedef typename Distance::ResultType DistanceType;
00068 
00069     DistanceType ret = 0;
00070     for (int i=0; i<n; ++i) {
00071         DistanceType den = distance(inputData[groundTruth[i]], target, veclen);
00072         DistanceType num = distance(inputData[neighbors[i]], target, veclen);
00073 
00074         if ((den==0)&&(num==0)) {
00075             ret += 1;
00076         }
00077         else {
00078             ret += num/den;
00079         }
00080     }
00081 
00082     return ret;
00083 }
00084 
00085 template <typename Index, typename Distance>
00086 float search_with_ground_truth(Index& index, const Matrix<typename Distance::ElementType>& inputData,
00087                                const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches, int nn, int checks,
00088                                float& time, typename Distance::ResultType& dist, const Distance& distance, int skipMatches)
00089 {
00090     typedef typename Distance::ElementType ElementType;
00091     typedef typename Distance::ResultType DistanceType;
00092 
00093     if (matches.cols<size_t(nn)) {
00094         Logger::info("matches.cols=%d, nn=%d\n",matches.cols,nn);
00095         throw FLANNException("Ground truth is not computed for as many neighbors as requested");
00096     }
00097 
00098     SearchParams searchParams(checks);
00099 
00100     size_t* indices = new size_t[nn+skipMatches];
00101     DistanceType* dists = new DistanceType[nn+skipMatches];
00102     
00103     Matrix<size_t> indices_mat(indices, 1, nn+skipMatches);
00104     Matrix<DistanceType> dists_mat(dists, 1, nn+skipMatches);
00105         
00106     size_t* neighbors = indices + skipMatches;
00107 
00108     int correct = 0;
00109     DistanceType distR = 0;
00110     StartStopTimer t;
00111     int repeats = 0;
00112     while (t.value<0.2) {
00113         repeats++;
00114         t.start();
00115         correct = 0;
00116         distR = 0;        
00117         for (size_t i = 0; i < testData.rows; i++) {
00118             index.knnSearch(Matrix<ElementType>(testData[i], 1, testData.cols), indices_mat, dists_mat, nn+skipMatches, searchParams);
00119 
00120             correct += countCorrectMatches(neighbors,matches[i], nn);
00121             distR += computeDistanceRaport<Distance>(inputData, testData[i], neighbors, matches[i], testData.cols, nn, distance);
00122         }
00123         t.stop();
00124     }
00125     time = float(t.value/repeats);
00126 
00127     delete[] indices;
00128     delete[] dists;
00129 
00130     float precicion = (float)correct/(nn*testData.rows);
00131 
00132     dist = distR/(testData.rows*nn);
00133 
00134     Logger::info("%8d %10.4g %10.5g %10.5g %10.5g\n",
00135                  checks, precicion, time, 1000.0 * time / testData.rows, dist);
00136 
00137     return precicion;
00138 }
00139 
00140 
00141 template <typename Index, typename Distance>
00142 float test_index_checks(Index& index, const Matrix<typename Distance::ElementType>& inputData,
00143                         const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
00144                         int checks, float& precision, const Distance& distance, int nn = 1, int skipMatches = 0)
00145 {
00146     typedef typename Distance::ResultType DistanceType;
00147 
00148     Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
00149     Logger::info("---------------------------------------------------------\n");
00150 
00151     float time = 0;
00152     DistanceType dist = 0;
00153     precision = search_with_ground_truth(index, inputData, testData, matches, nn, checks, time, dist, distance, skipMatches);
00154 
00155     return time;
00156 }
00157 
00158 template <typename Index, typename Distance>
00159 float test_index_precision(Index& index, const Matrix<typename Distance::ElementType>& inputData,
00160                            const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
00161                            float precision, int& checks, const Distance& distance, int nn = 1, int skipMatches = 0)
00162 {
00163     typedef typename Distance::ResultType DistanceType;
00164     const float SEARCH_EPS = 0.001f;
00165 
00166     Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
00167     Logger::info("---------------------------------------------------------\n");
00168 
00169     int c2 = 1;
00170     float p2;
00171     int c1 = 1;
00172 //     float p1;
00173     float time;
00174     DistanceType dist;
00175 
00176     p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
00177 
00178     if (p2>precision) {
00179         Logger::info("Got as close as I can\n");
00180         checks = c2;
00181         return time;
00182     }
00183 
00184     while (p2<precision) {
00185         c1 = c2;
00186 //         p1 = p2;
00187         c2 *=2;
00188         p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
00189     }
00190 
00191     int cx;
00192     float realPrecision;
00193     if (fabs(p2-precision)>SEARCH_EPS) {
00194         Logger::info("Start linear estimation\n");
00195         // after we got to values in the vecinity of the desired precision
00196         // use linear approximation get a better estimation
00197 
00198         cx = (c1+c2)/2;
00199         realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
00200         while (fabs(realPrecision-precision)>SEARCH_EPS) {
00201 
00202             if (realPrecision<precision) {
00203                 c1 = cx;
00204             }
00205             else {
00206                 c2 = cx;
00207             }
00208             cx = (c1+c2)/2;
00209             if (cx==c1) {
00210                 Logger::info("Got as close as I can\n");
00211                 break;
00212             }
00213             realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
00214         }
00215 
00216         c2 = cx;
00217         p2 = realPrecision;
00218 
00219     }
00220     else {
00221         Logger::info("No need for linear estimation\n");
00222         cx = c2;
00223         realPrecision = p2;
00224     }
00225 
00226     checks = cx;
00227     return time;
00228 }
00229 
00230 
00231 template <typename Index, typename Distance>
00232 void test_index_precisions(Index& index, const Matrix<typename Distance::ElementType>& inputData,
00233                            const Matrix<typename Distance::ElementType>& testData, const Matrix<int>& matches,
00234                            float* precisions, int precisions_length, const Distance& distance, int nn = 1, int skipMatches = 0, float maxTime = 0)
00235 {
00236     typedef typename Distance::ResultType DistanceType;
00237 
00238     const float SEARCH_EPS = 0.001;
00239 
00240     // make sure precisions array is sorted
00241     std::sort(precisions, precisions+precisions_length);
00242 
00243     int pindex = 0;
00244     float precision = precisions[pindex];
00245 
00246     Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
00247     Logger::info("---------------------------------------------------------\n");
00248 
00249     int c2 = 1;
00250     float p2;
00251 
00252     int c1 = 1;
00253     float p1;
00254 
00255     float time;
00256     DistanceType dist;
00257 
00258     p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
00259 
00260     // if precision for 1 run down the tree is already
00261     // better then some of the requested precisions, then
00262     // skip those
00263     while (precisions[pindex]<p2 && pindex<precisions_length) {
00264         pindex++;
00265     }
00266 
00267     if (pindex==precisions_length) {
00268         Logger::info("Got as close as I can\n");
00269         return;
00270     }
00271 
00272     for (int i=pindex; i<precisions_length; ++i) {
00273 
00274         precision = precisions[i];
00275         while (p2<precision) {
00276             c1 = c2;
00277             p1 = p2;
00278             c2 *=2;
00279             p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
00280             if ((maxTime> 0)&&(time > maxTime)&&(p2<precision)) return;
00281         }
00282 
00283         int cx;
00284         float realPrecision;
00285         if (fabs(p2-precision)>SEARCH_EPS) {
00286             Logger::info("Start linear estimation\n");
00287             // after we got to values in the vecinity of the desired precision
00288             // use linear approximation get a better estimation
00289 
00290             cx = (c1+c2)/2;
00291             realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
00292             while (fabs(realPrecision-precision)>SEARCH_EPS) {
00293 
00294                 if (realPrecision<precision) {
00295                     c1 = cx;
00296                 }
00297                 else {
00298                     c2 = cx;
00299                 }
00300                 cx = (c1+c2)/2;
00301                 if (cx==c1) {
00302                     Logger::info("Got as close as I can\n");
00303                     break;
00304                 }
00305                 realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
00306             }
00307 
00308             c2 = cx;
00309             p2 = realPrecision;
00310 
00311         }
00312         else {
00313             Logger::info("No need for linear estimation\n");
00314             cx = c2;
00315             realPrecision = p2;
00316         }
00317 
00318     }
00319 }
00320 
00321 }
00322 
00323 #endif //FLANN_INDEX_TESTING_H_


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20