00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "find_object/Settings.h"
00029
00030 #include "find_object/utilite/ULogger.h"
00031 #include "Vocabulary.h"
00032 #include <QtCore/QVector>
00033 #include <stdio.h>
00034 #include <opencv2/gpu/gpu.hpp>
00035
00036 namespace find_object {
00037
00038 Vocabulary::Vocabulary()
00039 {
00040 }
00041
00042 Vocabulary::~Vocabulary()
00043 {
00044 }
00045
00046 void Vocabulary::clear()
00047 {
00048 indexedDescriptors_ = cv::Mat();
00049 notIndexedDescriptors_ = cv::Mat();
00050 wordToObjects_.clear();
00051 notIndexedWordIds_.clear();
00052 }
00053
00054 void Vocabulary::save(QDataStream & streamPtr) const
00055 {
00056 if(!indexedDescriptors_.empty() && !wordToObjects_.empty())
00057 {
00058 UASSERT(notIndexedDescriptors_.empty() && notIndexedWordIds_.empty());
00059
00060
00061 streamPtr << wordToObjects_;
00062
00063
00064 qint64 dataSize = indexedDescriptors_.elemSize()*indexedDescriptors_.cols*indexedDescriptors_.rows;
00065 streamPtr << indexedDescriptors_.rows <<
00066 indexedDescriptors_.cols <<
00067 indexedDescriptors_.type() <<
00068 dataSize;
00069 streamPtr << QByteArray((char*)indexedDescriptors_.data, dataSize);
00070 }
00071 }
00072
00073 void Vocabulary::load(QDataStream & streamPtr)
00074 {
00075
00076 streamPtr >> wordToObjects_;
00077
00078
00079 int rows,cols,type;
00080 qint64 dataSize;
00081 streamPtr >> rows >> cols >> type >> dataSize;
00082 QByteArray data;
00083 streamPtr >> data;
00084 indexedDescriptors_ = cv::Mat(rows, cols, type, data.data()).clone();
00085
00086 update();
00087 }
00088
00089 QMultiMap<int, int> Vocabulary::addWords(const cv::Mat & descriptors, int objectId, bool incremental)
00090 {
00091 QMultiMap<int, int> words;
00092 if (descriptors.empty())
00093 {
00094 return words;
00095 }
00096
00097 if(incremental)
00098 {
00099 int k = 2;
00100 cv::Mat results;
00101 cv::Mat dists;
00102
00103 bool globalSearch = false;
00104 if(!indexedDescriptors_.empty() && indexedDescriptors_.rows >= (int)k)
00105 {
00106 UASSERT(indexedDescriptors_.type() == descriptors.type() && indexedDescriptors_.cols == descriptors.cols);
00107 this->search(descriptors, results, dists, k);
00108
00109 if( dists.type() == CV_32S )
00110 {
00111 cv::Mat temp;
00112 dists.convertTo(temp, CV_32F);
00113 dists = temp;
00114 }
00115
00116 globalSearch = true;
00117 }
00118
00119 notIndexedWordIds_.reserve(notIndexedWordIds_.size() + descriptors.rows);
00120 notIndexedDescriptors_.reserve(notIndexedDescriptors_.rows + descriptors.rows);
00121 int matches = 0;
00122 for(int i = 0; i < descriptors.rows; ++i)
00123 {
00124 QMultiMap<float, int> fullResults;
00125 if(notIndexedDescriptors_.rows)
00126 {
00127 UASSERT(notIndexedDescriptors_.type() == descriptors.type() && notIndexedDescriptors_.cols == descriptors.cols);
00128
00129
00130
00131 cv::Mat tmpResults;
00132 cv::Mat tmpDists;
00133 if(descriptors.type()==CV_8U)
00134 {
00135
00136
00137
00138
00139 int normType = cv::NORM_HAMMING;
00140 if(Settings::currentDescriptorType().compare("ORB") &&
00141 (Settings::getFeature2D_ORB_WTA_K()==3 || Settings::getFeature2D_ORB_WTA_K()==4))
00142 {
00143 normType = cv::NORM_HAMMING2;
00144 }
00145
00146 cv::batchDistance( descriptors.row(i),
00147 notIndexedDescriptors_,
00148 tmpDists,
00149 CV_32S,
00150 tmpResults,
00151 normType,
00152 notIndexedDescriptors_.rows>=k?k:1,
00153 cv::Mat(),
00154 0,
00155 false);
00156 }
00157 else
00158 {
00159 cv::flann::Index tmpIndex;
00160 tmpIndex.build(notIndexedDescriptors_, cv::flann::LinearIndexParams(), Settings::getFlannDistanceType());
00161 tmpIndex.knnSearch(descriptors.row(i), tmpResults, tmpDists, notIndexedDescriptors_.rows>1?k:1, Settings::getFlannSearchParams());
00162 }
00163
00164 if( tmpDists.type() == CV_32S )
00165 {
00166 cv::Mat temp;
00167 tmpDists.convertTo(temp, CV_32F);
00168 tmpDists = temp;
00169 }
00170
00171 for(int j = 0; j < tmpResults.cols; ++j)
00172 {
00173 if(tmpResults.at<int>(0,j) >= 0)
00174 {
00175
00176 fullResults.insert(tmpDists.at<float>(0,j), notIndexedWordIds_.at(tmpResults.at<int>(0,j)));
00177 }
00178 }
00179 }
00180
00181 if(globalSearch)
00182 {
00183 for(int j=0; j<k; ++j)
00184 {
00185 if(results.at<int>(i,j) >= 0)
00186 {
00187
00188 fullResults.insert(dists.at<float>(i,j), results.at<int>(i,j));
00189 }
00190 }
00191 }
00192
00193 bool match = false;
00194
00195 if(fullResults.size() >= 2 &&
00196 fullResults.begin().key() <= Settings::getNearestNeighbor_4nndrRatio() * (++fullResults.begin()).key())
00197 {
00198 match = true;
00199 }
00200
00201 if(match)
00202 {
00203 words.insert(fullResults.begin().value(), i);
00204 wordToObjects_.insert(fullResults.begin().value(), objectId);
00205 ++matches;
00206 }
00207 else
00208 {
00209
00210 notIndexedWordIds_.push_back(indexedDescriptors_.rows + notIndexedDescriptors_.rows);
00211 notIndexedDescriptors_.push_back(descriptors.row(i));
00212 words.insert(notIndexedWordIds_.back(), i);
00213 wordToObjects_.insert(notIndexedWordIds_.back(), objectId);
00214 }
00215 }
00216 }
00217 else
00218 {
00219 for(int i = 0; i < descriptors.rows; ++i)
00220 {
00221 wordToObjects_.insert(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i, objectId);
00222 words.insert(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i, i);
00223 notIndexedWordIds_.push_back(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i);
00224 }
00225
00226
00227 notIndexedDescriptors_.push_back(descriptors);
00228 }
00229
00230 return words;
00231 }
00232
00233 void Vocabulary::update()
00234 {
00235 if(!notIndexedDescriptors_.empty())
00236 {
00237 if(!indexedDescriptors_.empty())
00238 {
00239 UASSERT(indexedDescriptors_.cols == notIndexedDescriptors_.cols &&
00240 indexedDescriptors_.type() == notIndexedDescriptors_.type() );
00241 }
00242
00243
00244 indexedDescriptors_.push_back(notIndexedDescriptors_);
00245
00246 notIndexedDescriptors_ = cv::Mat();
00247 notIndexedWordIds_.clear();
00248 }
00249
00250 if(!indexedDescriptors_.empty() && !Settings::isBruteForceNearestNeighbor())
00251 {
00252 cv::flann::IndexParams * params = Settings::createFlannIndexParams();
00253 flannIndex_.build(indexedDescriptors_, *params, Settings::getFlannDistanceType());
00254 delete params;
00255 }
00256 }
00257
00258 void Vocabulary::search(const cv::Mat & descriptors, cv::Mat & results, cv::Mat & dists, int k)
00259 {
00260 if(!indexedDescriptors_.empty())
00261 {
00262 UASSERT(descriptors.type() == indexedDescriptors_.type() && descriptors.cols == indexedDescriptors_.cols);
00263
00264 if(Settings::isBruteForceNearestNeighbor())
00265 {
00266 std::vector<std::vector<cv::DMatch> > matches;
00267 if(Settings::getNearestNeighbor_BruteForce_gpu() && cv::gpu::getCudaEnabledDeviceCount())
00268 {
00269 cv::gpu::GpuMat newDescriptorsGpu(descriptors);
00270 cv::gpu::GpuMat lastDescriptorsGpu(indexedDescriptors_);
00271 if(indexedDescriptors_.type()==CV_8U)
00272 {
00273 cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
00274 gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
00275 }
00276 else
00277 {
00278 cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
00279 gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
00280 }
00281 }
00282 else
00283 {
00284 cv::BFMatcher matcher(indexedDescriptors_.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
00285 matcher.knnMatch(descriptors, indexedDescriptors_, matches, k);
00286 }
00287
00288
00289 results = cv::Mat(matches.size(), k, CV_32SC1);
00290 dists = cv::Mat(matches.size(), k, CV_32FC1);
00291 for(unsigned int i=0; i<matches.size(); ++i)
00292 {
00293 for(int j=0; j<k; ++j)
00294 {
00295 results.at<int>(i, j) = matches[i].at(j).trainIdx;
00296 dists.at<float>(i, j) = matches[i].at(j).distance;
00297 }
00298 }
00299 }
00300 else
00301 {
00302 flannIndex_.knnSearch(descriptors, results, dists, k, Settings::getFlannSearchParams());
00303 }
00304
00305 if( dists.type() == CV_32S )
00306 {
00307 cv::Mat temp;
00308 dists.convertTo(temp, CV_32F);
00309 dists = temp;
00310 }
00311 }
00312 }
00313
00314 }