main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <opencv2/core/core.hpp>
00029 #include <opencv2/core/types_c.h>
00030 #include <opencv2/highgui/highgui_c.h>
00031 #include <opencv2/imgproc/imgproc_c.h>
00032 #include <opencv2/flann/miniflann.hpp>
00033 #include <opencv2/features2d/features2d.hpp>
00034 #include <opencv2/nonfree/features2d.hpp>
00035 #include <rtabmap/utilite/ULogger.h>
00036 #include <rtabmap/utilite/UTimer.h>
00037 #include <rtabmap/utilite/UConversion.h>
00038 #include <rtabmap/utilite/UStl.h>
00039 #include <rtabmap/utilite/UMath.h>
00040 #include <fstream>
00041 #include <vector>
00042 #include <list>
00043 #include <string>
00044 #include <iostream>
00045 
00046 void showUsage()
00047 {
00048         printf("Usage:\n"
00049                         "vocabularyComparison.exe \"dictionary/path\"\n"
00050                         "  Dictionary path example: \"data/Dictionary49k.txt\""
00051                         "  Note that 400 first descriptors in the file are used as queries.\n");
00052         exit(1);
00053 }
00054 
00055 int main(int argc, char * argv[])
00056 {
00057         if(argc < 2)
00058         {
00059                 showUsage();
00060         }
00061         ULogger::setType(ULogger::kTypeConsole);
00062         ULogger::setLevel(ULogger::kDebug);
00063 
00064         std::string dictionaryPath = argv[argc-1];
00065         std::list<std::vector<float> > objectDescriptors;
00066         //std::list<std::vector<float> > descriptors;
00067         std::map<int, std::vector<float> > descriptors;
00068         int dimension  = 0;
00069         UTimer timer;
00070         int objectDescriptorsSize= 400;
00071 
00072         std::ifstream file;
00073         if(!dictionaryPath.empty())
00074         {
00075                 file.open(dictionaryPath.c_str(), std::ifstream::in);
00076         }
00077         if(file.good())
00078         {
00079                 UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str());
00080 
00081                 // first line is the header
00082                 std::string str;
00083                 std::list<std::string> strList;
00084                 std::getline(file, str);
00085                 strList = uSplitNumChar(str);
00086                 for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
00087                 {
00088                         if(uIsDigit(iter->at(0)))
00089                         {
00090                                 dimension = std::atoi(iter->c_str());
00091                                 break;
00092                         }
00093                 }
00094 
00095                 if(dimension == 0 || dimension > 1000)
00096                 {
00097                         UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
00098                 }
00099                 else
00100                 {
00101                         int descriptorsLoaded = 0;
00102                         // Process all words
00103                         while(file.good())
00104                         {
00105                                 std::getline(file, str);
00106                                 strList = uSplit(str);
00107                                 if((int)strList.size() == dimension+1)
00108                                 {
00109                                         //first one is the visual word id
00110                                         std::list<std::string>::iterator iter = strList.begin();
00111                                         int id = atoi(iter->c_str());
00112                                         ++iter;
00113 
00114                                         std::vector<float> descriptor(dimension);
00115                                         int i=0;
00116 
00117                                         //get descriptor
00118                                         for(;i<dimension && iter != strList.end(); ++i, ++iter)
00119                                         {
00120                                                 descriptor[i] = uStr2Float(*iter);
00121                                         }
00122                                         if(i != dimension)
00123                                         {
00124                                                 UERROR("");
00125                                         }
00126 
00127                                         if(++descriptorsLoaded<=objectDescriptorsSize)
00128                                         {
00129                                                 objectDescriptors.push_back(descriptor);
00130                                         }
00131                                         else
00132                                         {
00133                                                 //descriptors.push_back(descriptor);
00134                                                 descriptors.insert(std::make_pair(id, descriptor));
00135                                         }
00136                                 }
00137                                 else if(str.size())
00138                                 {
00139                                         UWARN("Cannot parse line \"%s\"", str.c_str());
00140                                 }
00141                         }
00142                 }
00143 
00144                 UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension);
00145         }
00146         else
00147         {
00148                 UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
00149         }
00150         file.close();
00151 
00152         if(descriptors.size() && objectDescriptors.size() && dimension)
00153         {
00154                 cv::Mat dataTree;
00155                 cv::Mat queries;
00156 
00157                 UDEBUG("Creating data structures...");
00158                 // Create the data structure
00159                 dataTree = cv::Mat((int)descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
00160                 {//scope
00161                         //std::list<std::vector<float> >::const_iterator iter = descriptors.begin();
00162                         std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin();
00163                         for(unsigned int i=0; i < descriptors.size(); ++i, ++iter)
00164                         {
00165                                 UTimer tim;
00166                                 //memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float));
00167                                 memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float));
00168                                 //if(i%100==0)
00169                                 //      UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks());
00170                         }
00171                 }
00172 
00173                 queries = cv::Mat((int)objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
00174                 {//scope
00175                         std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin();
00176                         for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter)
00177                         {
00178                                 UTimer tim;
00179                                 memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float));
00180                                 //if(i%100==0)
00181                                 //      UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks());
00182                         }
00183                 }
00184 
00185                 UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks());
00186 
00187                 UDEBUG("Creating indexes...");
00188                 cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams());
00189                 UDEBUG("Time to create linearIndex = %f s", timer.ticks());
00190 
00191                 cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1));
00192                 UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks());
00193 
00194                 cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4));
00195                 UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks());
00196 
00197                 cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams());
00198                 UDEBUG("Time to create kMeansIndex = %f s", timer.ticks());
00199 
00200                 cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams());
00201                 UDEBUG("Time to create compositeIndex = %f s", timer.ticks());
00202 
00203                 //cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams());
00204                 //UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks());
00205 
00206 
00207                 UDEBUG("Search indexes...");
00208                 int k=2; // 2 nearest neighbors
00209                 cv::Mat results(queries.rows, k, CV_32SC1); // results index
00210                 cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1
00211 
00212                 linearIndex->knnSearch(queries, results, dists, k);
00213                 //std::cout << results.t() << std::endl;
00214                 cv::Mat transposedLinear = dists.t();
00215                 UDEBUG("Time to search linearIndex = %f s", timer.ticks());
00216 
00217                 kdTreeIndex1->knnSearch(queries, results, dists, k);
00218                 //std::cout << results.t() << std::endl;
00219                 cv::Mat transposed = dists.t();
00220                 UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
00221                                 timer.ticks(),
00222                                 transposed.cols,
00223                                 uMeanSquaredError(  (float*)transposed.data,
00224                                         transposed.cols,
00225                                         (float*)transposedLinear.data,
00226                                         transposedLinear.cols),
00227                                 uMeanSquaredError(  &transposed.at<float>(1,0),
00228                                         transposed.cols,
00229                                         &transposedLinear.at<float>(1,0),
00230                                         transposedLinear.cols));
00231 
00232                 kdTreeIndex4->knnSearch(queries, results, dists, k);
00233                 //std::cout << results.t() << std::endl;
00234                 transposed = dists.t();
00235                 UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
00236                                 timer.ticks(),
00237                                 transposed.cols,
00238                                 uMeanSquaredError(  (float*)transposed.data,
00239                                         transposed.cols,
00240                                         (float*)transposedLinear.data,
00241                                         transposedLinear.cols),
00242                                 uMeanSquaredError(  &transposed.at<float>(1,0),
00243                                         transposed.cols,
00244                                         &transposedLinear.at<float>(1,0),
00245                                         transposedLinear.cols));
00246 
00247                 kMeansIndex->knnSearch(queries, results, dists, k);
00248                 //std::cout << results.t() << std::endl;
00249                 transposed = dists.t();
00250                 UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
00251                                 timer.ticks(),
00252                                 transposed.cols,
00253                                 uMeanSquaredError(  (float*)transposed.data,
00254                                         transposed.cols,
00255                                         (float*)transposedLinear.data,
00256                                         transposedLinear.cols),
00257                                 uMeanSquaredError(  &transposed.at<float>(1,0),
00258                                         transposed.cols,
00259                                         &transposedLinear.at<float>(1,0),
00260                                         transposedLinear.cols));
00261 
00262                 compositeIndex->knnSearch(queries, results, dists, k);
00263                 //std::cout << results.t() << std::endl;
00264                 transposed = dists.t();
00265                 UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
00266                                 timer.ticks(),
00267                                 transposed.cols,
00268                                 uMeanSquaredError(  (float*)transposed.data,
00269                                         transposed.cols,
00270                                         (float*)transposedLinear.data,
00271                                         transposedLinear.cols),
00272                                 uMeanSquaredError(  &transposed.at<float>(1,0),
00273                                         transposed.cols,
00274                                         &transposedLinear.at<float>(1,0),
00275                                         transposedLinear.cols));
00276 
00277                 //autoTunedIndex->knnSearch(queries, results, dists, k);
00278                 //UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks());
00279 
00280                 delete linearIndex;
00281                 delete kdTreeIndex1;
00282                 delete kdTreeIndex4;
00283                 delete kMeansIndex;
00284                 delete compositeIndex;
00285                 //delete autoTunedIndex;
00286         }
00287 
00288     return 0;
00289 }


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