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 "rtabmap/core/Features2d.h"
00029 #include "rtabmap/core/util3d.h"
00030 #include "rtabmap/core/util3d_features.h"
00031 #include "rtabmap/core/Stereo.h"
00032 #include "rtabmap/core/util2d.h"
00033 #include "rtabmap/utilite/UStl.h"
00034 #include "rtabmap/utilite/UConversion.h"
00035 #include "rtabmap/utilite/ULogger.h"
00036 #include "rtabmap/utilite/UMath.h"
00037 #include "rtabmap/utilite/ULogger.h"
00038 #include "rtabmap/utilite/UTimer.h"
00039 #include <opencv2/imgproc/imgproc_c.h>
00040 #include <opencv2/core/version.hpp>
00041 #include <opencv2/opencv_modules.hpp>
00042
00043 #if CV_MAJOR_VERSION < 3
00044 #include "opencv/Orb.h"
00045 #ifdef HAVE_OPENCV_GPU
00046 #include <opencv2/gpu/gpu.hpp>
00047 #endif
00048 #else
00049 #include <opencv2/core/cuda.hpp>
00050 #endif
00051
00052 #ifdef HAVE_OPENCV_NONFREE
00053 #if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >=4
00054 #include <opencv2/nonfree/gpu.hpp>
00055 #include <opencv2/nonfree/features2d.hpp>
00056 #endif
00057 #endif
00058 #ifdef HAVE_OPENCV_XFEATURES2D
00059 #include <opencv2/xfeatures2d.hpp>
00060 #include <opencv2/xfeatures2d/nonfree.hpp>
00061 #include <opencv2/xfeatures2d/cuda.hpp>
00062 #endif
00063
00064 namespace rtabmap {
00065
00066 void Feature2D::filterKeypointsByDepth(
00067 std::vector<cv::KeyPoint> & keypoints,
00068 const cv::Mat & depth,
00069 float minDepth,
00070 float maxDepth)
00071 {
00072 cv::Mat descriptors;
00073 filterKeypointsByDepth(keypoints, descriptors, depth, minDepth, maxDepth);
00074 }
00075
00076 void Feature2D::filterKeypointsByDepth(
00077 std::vector<cv::KeyPoint> & keypoints,
00078 cv::Mat & descriptors,
00079 const cv::Mat & depth,
00080 float minDepth,
00081 float maxDepth)
00082 {
00083 UASSERT(minDepth >= 0.0f);
00084 UASSERT(maxDepth <= 0.0f || maxDepth > minDepth);
00085 if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
00086 {
00087 std::vector<cv::KeyPoint> output(keypoints.size());
00088 std::vector<int> indexes(keypoints.size(), 0);
00089 int oi=0;
00090 bool isInMM = depth.type() == CV_16UC1;
00091 for(unsigned int i=0; i<keypoints.size(); ++i)
00092 {
00093 int u = int(keypoints[i].pt.x+0.5f);
00094 int v = int(keypoints[i].pt.y+0.5f);
00095 if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
00096 {
00097 float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
00098 if(uIsFinite(d) && d>minDepth && (maxDepth <= 0.0f || d < maxDepth))
00099 {
00100 output[oi++] = keypoints[i];
00101 indexes[i] = 1;
00102 }
00103 }
00104 }
00105 output.resize(oi);
00106 keypoints = output;
00107
00108 if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
00109 {
00110 if(keypoints.size() == 0)
00111 {
00112 descriptors = cv::Mat();
00113 }
00114 else
00115 {
00116 cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
00117 int di = 0;
00118 for(unsigned int i=0; i<indexes.size(); ++i)
00119 {
00120 if(indexes[i] == 1)
00121 {
00122 if(descriptors.type() == CV_32FC1)
00123 {
00124 memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
00125 }
00126 else
00127 {
00128 memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
00129 }
00130 }
00131 }
00132 descriptors = newDescriptors;
00133 }
00134 }
00135 }
00136 }
00137
00138 void Feature2D::filterKeypointsByDisparity(
00139 std::vector<cv::KeyPoint> & keypoints,
00140 const cv::Mat & disparity,
00141 float minDisparity)
00142 {
00143 cv::Mat descriptors;
00144 filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
00145 }
00146
00147 void Feature2D::filterKeypointsByDisparity(
00148 std::vector<cv::KeyPoint> & keypoints,
00149 cv::Mat & descriptors,
00150 const cv::Mat & disparity,
00151 float minDisparity)
00152 {
00153 if(!disparity.empty() && minDisparity > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
00154 {
00155 std::vector<cv::KeyPoint> output(keypoints.size());
00156 std::vector<int> indexes(keypoints.size(), 0);
00157 int oi=0;
00158 for(unsigned int i=0; i<keypoints.size(); ++i)
00159 {
00160 int u = int(keypoints[i].pt.x+0.5f);
00161 int v = int(keypoints[i].pt.y+0.5f);
00162 if(u >=0 && u<disparity.cols && v >=0 && v<disparity.rows)
00163 {
00164 float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
00165 if(d!=0.0f && uIsFinite(d) && d >= minDisparity)
00166 {
00167 output[oi++] = keypoints[i];
00168 indexes[i] = 1;
00169 }
00170 }
00171 }
00172 output.resize(oi);
00173 keypoints = output;
00174
00175 if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
00176 {
00177 if(keypoints.size() == 0)
00178 {
00179 descriptors = cv::Mat();
00180 }
00181 else
00182 {
00183 cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
00184 int di = 0;
00185 for(unsigned int i=0; i<indexes.size(); ++i)
00186 {
00187 if(indexes[i] == 1)
00188 {
00189 if(descriptors.type() == CV_32FC1)
00190 {
00191 memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
00192 }
00193 else
00194 {
00195 memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
00196 }
00197 }
00198 }
00199 descriptors = newDescriptors;
00200 }
00201 }
00202 }
00203 }
00204
00205 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
00206 {
00207 cv::Mat descriptors;
00208 limitKeypoints(keypoints, descriptors, maxKeypoints);
00209 }
00210
00211 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
00212 {
00213 std::vector<cv::Point3f> keypoints3D;
00214 limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
00215 }
00216
00217 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
00218 {
00219 UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
00220 UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
00221 if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
00222 {
00223 UTimer timer;
00224 ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
00225
00226
00227
00228 std::multimap<float, int> hessianMap;
00229 for(unsigned int i = 0; i <keypoints.size(); ++i)
00230 {
00231
00232 hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
00233 }
00234
00235
00236 int removed = (int)hessianMap.size()-maxKeypoints;
00237 std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
00238 std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
00239 std::vector<cv::Point3f> kpts3DTmp(maxKeypoints);
00240 cv::Mat descriptorsTmp;
00241 if(descriptors.rows)
00242 {
00243 descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
00244 }
00245 for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
00246 {
00247 kptsTmp[k] = keypoints[iter->second];
00248 if(keypoints3D.size())
00249 {
00250 kpts3DTmp[k] = keypoints3D[iter->second];
00251 }
00252 if(descriptors.rows)
00253 {
00254 if(descriptors.type() == CV_32FC1)
00255 {
00256 memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
00257 }
00258 else
00259 {
00260 memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
00261 }
00262 }
00263 }
00264 ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
00265 ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
00266 keypoints = kptsTmp;
00267 keypoints3D = kpts3DTmp;
00268 if(descriptors.rows)
00269 {
00270 descriptors = descriptorsTmp;
00271 }
00272 }
00273 }
00274
00275 void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
00276 {
00277 if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
00278 {
00279 UTimer timer;
00280 ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
00281
00282
00283
00284 std::multimap<float, int> hessianMap;
00285 for(unsigned int i = 0; i <keypoints.size(); ++i)
00286 {
00287
00288 hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
00289 }
00290
00291
00292 int removed = (int)hessianMap.size()-maxKeypoints;
00293 std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
00294 inliers.resize(keypoints.size(), false);
00295 float minimumHessian = 0.0f;
00296 for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
00297 {
00298 inliers[iter->second] = true;
00299 minimumHessian = iter->first;
00300 }
00301 ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
00302 ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
00303 }
00304 else
00305 {
00306 inliers.resize(keypoints.size(), true);
00307 }
00308 }
00309
00310 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
00311 {
00312 return util2d::computeRoi(image, roiRatios);
00313 }
00314
00315 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
00316 {
00317 return util2d::computeRoi(image, roiRatios);
00318 }
00319
00321
00323 Feature2D::Feature2D(const ParametersMap & parameters) :
00324 maxFeatures_(Parameters::defaultKpMaxFeatures()),
00325 _maxDepth(Parameters::defaultKpMaxDepth()),
00326 _minDepth(Parameters::defaultKpMinDepth()),
00327 _roiRatios(std::vector<float>(4, 0.0f)),
00328 _subPixWinSize(Parameters::defaultKpSubPixWinSize()),
00329 _subPixIterations(Parameters::defaultKpSubPixIterations()),
00330 _subPixEps(Parameters::defaultKpSubPixEps()),
00331 gridRows_(Parameters::defaultKpGridRows()),
00332 gridCols_(Parameters::defaultKpGridCols())
00333 {
00334 _stereo = new Stereo(parameters);
00335 this->parseParameters(parameters);
00336 }
00337 Feature2D::~Feature2D()
00338 {
00339 delete _stereo;
00340 }
00341 void Feature2D::parseParameters(const ParametersMap & parameters)
00342 {
00343 uInsert(parameters_, parameters);
00344
00345 Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
00346 Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
00347 Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
00348 Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
00349 Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
00350 Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
00351 Parameters::parse(parameters, Parameters::kKpGridRows(), gridRows_);
00352 Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
00353
00354 UASSERT(gridRows_ >= 1 && gridCols_>=1);
00355 if(maxFeatures_ > 0)
00356 {
00357 maxFeatures_ = maxFeatures_ / (gridRows_ * gridCols_);
00358 }
00359
00360
00361 ParametersMap::const_iterator iter;
00362 if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
00363 {
00364 std::list<std::string> strValues = uSplit(iter->second, ' ');
00365 if(strValues.size() != 4)
00366 {
00367 ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", iter->second.c_str());
00368 }
00369 else
00370 {
00371 std::vector<float> tmpValues(4);
00372 unsigned int i=0;
00373 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00374 {
00375 tmpValues[i] = uStr2Float(*jter);
00376 ++i;
00377 }
00378
00379 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00380 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00381 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00382 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00383 {
00384 _roiRatios = tmpValues;
00385 }
00386 else
00387 {
00388 ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", iter->second.c_str());
00389 }
00390 }
00391 }
00392
00393
00394 UASSERT(_stereo != 0);
00395 if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
00396 {
00397 delete _stereo;
00398 _stereo = Stereo::create(parameters_);
00399 }
00400 else
00401 {
00402 _stereo->parseParameters(parameters);
00403 }
00404 }
00405 Feature2D * Feature2D::create(const ParametersMap & parameters)
00406 {
00407 int type = Parameters::defaultKpDetectorStrategy();
00408 Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
00409 return create((Feature2D::Type)type, parameters);
00410 }
00411 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
00412 {
00413 #ifndef RTABMAP_NONFREE
00414 if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
00415 {
00416 #if CV_MAJOR_VERSION < 3
00417 UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
00418 #else
00419 UWARN("SURF and SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
00420 #endif
00421 type = Feature2D::kFeatureOrb;
00422 }
00423 #if CV_MAJOR_VERSION == 3
00424 if(type == Feature2D::kFeatureFastBrief ||
00425 type == Feature2D::kFeatureFastFreak ||
00426 type == Feature2D::kFeatureGfttBrief ||
00427 type == Feature2D::kFeatureGfttFreak)
00428 {
00429 UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
00430 type = Feature2D::kFeatureOrb;
00431 }
00432 #endif
00433 #endif
00434
00435 #if CV_MAJOR_VERSION < 3
00436 if(type == Feature2D::kFeatureKaze)
00437 {
00438 #ifdef RTABMAP_NONFREE
00439 UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
00440 type = Feature2D::kFeatureSurf;
00441 #else
00442 UWARN("KAZE detector/descriptor can be used only with OpenCV3. ORB is used instead.");
00443 type = Feature2D::kFeatureOrb;
00444 #endif
00445 }
00446 #endif
00447
00448 Feature2D * feature2D = 0;
00449 switch(type)
00450 {
00451 case Feature2D::kFeatureSurf:
00452 feature2D = new SURF(parameters);
00453 break;
00454 case Feature2D::kFeatureSift:
00455 feature2D = new SIFT(parameters);
00456 break;
00457 case Feature2D::kFeatureOrb:
00458 feature2D = new ORB(parameters);
00459 break;
00460 case Feature2D::kFeatureFastBrief:
00461 feature2D = new FAST_BRIEF(parameters);
00462 break;
00463 case Feature2D::kFeatureFastFreak:
00464 feature2D = new FAST_FREAK(parameters);
00465 break;
00466 case Feature2D::kFeatureGfttFreak:
00467 feature2D = new GFTT_FREAK(parameters);
00468 break;
00469 case Feature2D::kFeatureGfttBrief:
00470 feature2D = new GFTT_BRIEF(parameters);
00471 break;
00472 case Feature2D::kFeatureGfttOrb:
00473 feature2D = new GFTT_ORB(parameters);
00474 break;
00475 case Feature2D::kFeatureBrisk:
00476 feature2D = new BRISK(parameters);
00477 break;
00478 case Feature2D::kFeatureKaze:
00479 feature2D = new KAZE(parameters);
00480 break;
00481 #ifdef RTABMAP_NONFREE
00482 default:
00483 feature2D = new SURF(parameters);
00484 type = Feature2D::kFeatureSurf;
00485 break;
00486 #else
00487 default:
00488 feature2D = new ORB(parameters);
00489 type = Feature2D::kFeatureOrb;
00490 break;
00491 #endif
00492
00493 }
00494 return feature2D;
00495 }
00496
00497 std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Mat & maskIn) const
00498 {
00499 UASSERT(!image.empty());
00500 UASSERT(image.type() == CV_8UC1);
00501
00502 cv::Mat mask;
00503 if(!maskIn.empty())
00504 {
00505 if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
00506 {
00507 mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
00508 for(int i=0; i<(int)mask.total(); ++i)
00509 {
00510 float value = 0.0f;
00511 if(maskIn.type()==CV_16UC1)
00512 {
00513 if(((unsigned short*)maskIn.data)[i] > 0 &&
00514 ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
00515 {
00516 value = float(((unsigned short*)maskIn.data)[i])*0.001f;
00517 }
00518 }
00519 else
00520 {
00521 value = ((float*)maskIn.data)[i];
00522 }
00523
00524 if(value>_minDepth &&
00525 (_maxDepth == 0.0f || value <= _maxDepth))
00526 {
00527 ((unsigned char*)mask.data)[i] = 255;
00528 }
00529 }
00530 }
00531 else if(maskIn.type()==CV_8UC1)
00532 {
00533
00534 mask = maskIn;
00535 }
00536 else
00537 {
00538 UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
00539 }
00540 }
00541
00542 UASSERT(mask.empty() || (mask.cols == image.cols && mask.rows == image.rows));
00543
00544 std::vector<cv::KeyPoint> keypoints;
00545 UTimer timer;
00546 cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios);
00547 if(!(globalRoi.width && globalRoi.height))
00548 {
00549 globalRoi = cv::Rect(0,0,image.cols, image.rows);
00550 }
00551
00552
00553 int rowSize = globalRoi.height / gridRows_;
00554 int colSize = globalRoi.width / gridCols_;
00555 for (int i = 0; i<gridRows_; ++i)
00556 {
00557 for (int j = 0; j<gridCols_; ++j)
00558 {
00559 cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
00560 std::vector<cv::KeyPoint> sub_keypoints;
00561 sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
00562 limitKeypoints(sub_keypoints, maxFeatures_);
00563 if(roi.x || roi.y)
00564 {
00565
00566 for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
00567 {
00568 iter->pt.x += roi.x;
00569 iter->pt.y += roi.y;
00570 }
00571 }
00572 keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
00573 }
00574 }
00575 UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
00576
00577 if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
00578 {
00579 std::vector<cv::Point2f> corners;
00580 cv::KeyPoint::convert(keypoints, corners);
00581 cv::cornerSubPix( image, corners,
00582 cv::Size( _subPixWinSize, _subPixWinSize ),
00583 cv::Size( -1, -1 ),
00584 cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
00585
00586 for(unsigned int i=0;i<corners.size(); ++i)
00587 {
00588 keypoints[i].pt = corners[i];
00589 }
00590 UDEBUG("subpixel time = %f s", timer.ticks());
00591 }
00592
00593 return keypoints;
00594 }
00595
00596 cv::Mat Feature2D::generateDescriptors(
00597 const cv::Mat & image,
00598 std::vector<cv::KeyPoint> & keypoints) const
00599 {
00600 cv::Mat descriptors;
00601 if(keypoints.size())
00602 {
00603 UASSERT(!image.empty());
00604 UASSERT(image.type() == CV_8UC1);
00605 descriptors = generateDescriptorsImpl(image, keypoints);
00606 UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
00607 UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
00608 }
00609 return descriptors;
00610 }
00611
00612 std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
00613 const SensorData & data,
00614 const std::vector<cv::KeyPoint> & keypoints) const
00615 {
00616 std::vector<cv::Point3f> keypoints3D;
00617 if(keypoints.size())
00618 {
00619 if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
00620 {
00621
00622 cv::Mat imageMono;
00623
00624 if(data.imageRaw().channels() > 1)
00625 {
00626 cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
00627 }
00628 else
00629 {
00630 imageMono = data.imageRaw();
00631 }
00632
00633 std::vector<cv::Point2f> leftCorners;
00634 cv::KeyPoint::convert(keypoints, leftCorners);
00635 std::vector<unsigned char> status;
00636
00637 std::vector<cv::Point2f> rightCorners;
00638 rightCorners = _stereo->computeCorrespondences(
00639 imageMono,
00640 data.rightRaw(),
00641 leftCorners,
00642 status);
00643
00644 keypoints3D = util3d::generateKeypoints3DStereo(
00645 leftCorners,
00646 rightCorners,
00647 data.stereoCameraModel(),
00648 status,
00649 _minDepth,
00650 _maxDepth);
00651 }
00652 else if(!data.depthRaw().empty() && data.cameraModels().size())
00653 {
00654 keypoints3D = util3d::generateKeypoints3DDepth(
00655 keypoints,
00656 data.depthOrRightRaw(),
00657 data.cameraModels(),
00658 _minDepth,
00659 _maxDepth);
00660 }
00661 }
00662
00663 return keypoints3D;
00664 }
00665
00667
00669 SURF::SURF(const ParametersMap & parameters) :
00670 hessianThreshold_(Parameters::defaultSURFHessianThreshold()),
00671 nOctaves_(Parameters::defaultSURFOctaves()),
00672 nOctaveLayers_(Parameters::defaultSURFOctaveLayers()),
00673 extended_(Parameters::defaultSURFExtended()),
00674 upright_(Parameters::defaultSURFUpright()),
00675 gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
00676 gpuVersion_(Parameters::defaultSURFGpuVersion())
00677 {
00678 parseParameters(parameters);
00679 }
00680
00681 SURF::~SURF()
00682 {
00683 }
00684
00685 void SURF::parseParameters(const ParametersMap & parameters)
00686 {
00687 Feature2D::parseParameters(parameters);
00688
00689 Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
00690 Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
00691 Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
00692 Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves_);
00693 Parameters::parse(parameters, Parameters::kSURFUpright(), upright_);
00694 Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
00695 Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
00696
00697 #ifdef RTABMAP_NONFREE
00698 #if CV_MAJOR_VERSION < 3
00699 if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
00700 {
00701 UWARN("GPU version of SURF not available! Using CPU version instead...");
00702 gpuVersion_ = false;
00703 }
00704 #else
00705 if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
00706 {
00707 UWARN("GPU version of SURF not available! Using CPU version instead...");
00708 gpuVersion_ = false;
00709 }
00710 #endif
00711 if(gpuVersion_)
00712 {
00713 _gpuSurf = cv::Ptr<CV_SURF_GPU>(new CV_SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_));
00714 }
00715 else
00716 {
00717 #if CV_MAJOR_VERSION < 3
00718 _surf = cv::Ptr<CV_SURF>(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_));
00719 #else
00720 _surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
00721 #endif
00722 }
00723 #else
00724 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00725 #endif
00726 }
00727
00728 std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00729 {
00730 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00731 std::vector<cv::KeyPoint> keypoints;
00732
00733 #ifdef RTABMAP_NONFREE
00734 cv::Mat imgRoi(image, roi);
00735 cv::Mat maskRoi;
00736 if(!mask.empty())
00737 {
00738 maskRoi = cv::Mat(mask, roi);
00739 }
00740 if(gpuVersion_)
00741 {
00742 #if CV_MAJOR_VERSION < 3
00743 cv::gpu::GpuMat imgGpu(imgRoi);
00744 cv::gpu::GpuMat maskGpu(maskRoi);
00745 (*_gpuSurf.obj)(imgGpu, maskGpu, keypoints);
00746 #else
00747 cv::cuda::GpuMat imgGpu(imgRoi);
00748 cv::cuda::GpuMat maskGpu(maskRoi);
00749 (*_gpuSurf.get())(imgGpu, maskGpu, keypoints);
00750 #endif
00751 }
00752 else
00753 {
00754 _surf->detect(imgRoi, keypoints, maskRoi);
00755 }
00756 #else
00757 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00758 #endif
00759 return keypoints;
00760 }
00761
00762 cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
00763 {
00764 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00765 cv::Mat descriptors;
00766 #ifdef RTABMAP_NONFREE
00767 if(gpuVersion_)
00768 {
00769 #if CV_MAJOR_VERSION < 3
00770 cv::gpu::GpuMat imgGpu(image);
00771 cv::gpu::GpuMat descriptorsGPU;
00772 (*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
00773 #else
00774 cv::cuda::GpuMat imgGpu(image);
00775 cv::cuda::GpuMat descriptorsGPU;
00776 (*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints, descriptorsGPU, true);
00777 #endif
00778
00779
00780 if (descriptorsGPU.empty())
00781 descriptors = cv::Mat();
00782 else
00783 {
00784 UASSERT(descriptorsGPU.type() == CV_32F);
00785 descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
00786 descriptorsGPU.download(descriptors);
00787 }
00788 }
00789 else
00790 {
00791 _surf->compute(image, keypoints, descriptors);
00792 }
00793 #else
00794 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00795 #endif
00796
00797 return descriptors;
00798 }
00799
00801
00803 SIFT::SIFT(const ParametersMap & parameters) :
00804 nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
00805 contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
00806 edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
00807 sigma_(Parameters::defaultSIFTSigma())
00808 {
00809 parseParameters(parameters);
00810 }
00811
00812 SIFT::~SIFT()
00813 {
00814 }
00815
00816 void SIFT::parseParameters(const ParametersMap & parameters)
00817 {
00818 Feature2D::parseParameters(parameters);
00819
00820 Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
00821 Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
00822 Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
00823 Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
00824
00825 #ifdef RTABMAP_NONFREE
00826 #if CV_MAJOR_VERSION < 3
00827 _sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
00828 #else
00829 _sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
00830 #endif
00831 #else
00832 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00833 #endif
00834 }
00835
00836 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00837 {
00838 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00839 std::vector<cv::KeyPoint> keypoints;
00840 #ifdef RTABMAP_NONFREE
00841 cv::Mat imgRoi(image, roi);
00842 cv::Mat maskRoi;
00843 if(!mask.empty())
00844 {
00845 maskRoi = cv::Mat(mask, roi);
00846 }
00847 _sift->detect(imgRoi, keypoints, maskRoi);
00848 #else
00849 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00850 #endif
00851 return keypoints;
00852 }
00853
00854 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
00855 {
00856 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00857 cv::Mat descriptors;
00858 #ifdef RTABMAP_NONFREE
00859 _sift->compute(image, keypoints, descriptors);
00860 #else
00861 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00862 #endif
00863 return descriptors;
00864 }
00865
00867
00869 ORB::ORB(const ParametersMap & parameters) :
00870 scaleFactor_(Parameters::defaultORBScaleFactor()),
00871 nLevels_(Parameters::defaultORBNLevels()),
00872 edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
00873 firstLevel_(Parameters::defaultORBFirstLevel()),
00874 WTA_K_(Parameters::defaultORBWTA_K()),
00875 scoreType_(Parameters::defaultORBScoreType()),
00876 patchSize_(Parameters::defaultORBPatchSize()),
00877 gpu_(Parameters::defaultORBGpu()),
00878 fastThreshold_(Parameters::defaultFASTThreshold()),
00879 nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression())
00880 {
00881 parseParameters(parameters);
00882 }
00883
00884 ORB::~ORB()
00885 {
00886 }
00887
00888 void ORB::parseParameters(const ParametersMap & parameters)
00889 {
00890 Feature2D::parseParameters(parameters);
00891
00892 Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
00893 Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
00894 Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
00895 Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel_);
00896 Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K_);
00897 Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType_);
00898 Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
00899 Parameters::parse(parameters, Parameters::kORBGpu(), gpu_);
00900
00901 Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
00902 Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);
00903
00904 #if CV_MAJOR_VERSION < 3
00905 #ifdef HAVE_OPENCV_GPU
00906 if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
00907 {
00908 UWARN("GPU version of ORB not available! Using CPU version instead...");
00909 gpu_ = false;
00910 }
00911 #else
00912 if(gpu_)
00913 {
00914 UWARN("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
00915 gpu_ = false;
00916 }
00917 #endif
00918 #else
00919 #ifndef HAVE_OPENCV_CUDAFEATURES2D
00920 if(gpu_)
00921 {
00922 UWARN("GPU version of ORB not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
00923 gpu_ = false;
00924 }
00925 #endif
00926 if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
00927 {
00928 UWARN("GPU version of ORB not available (no GPU found)! Using CPU version instead...");
00929 gpu_ = false;
00930 }
00931 #endif
00932 if(gpu_)
00933 {
00934 #if CV_MAJOR_VERSION < 3
00935 #ifdef HAVE_OPENCV_GPU
00936 _gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
00937 _gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
00938 #else
00939 UFATAL("not supposed to be here");
00940 #endif
00941 #else
00942 #ifdef HAVE_OPENCV_CUDAFEATURES2D
00943 _gpuOrb = CV_ORB_GPU::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
00944 #endif
00945 #endif
00946 }
00947 else
00948 {
00949 #if CV_MAJOR_VERSION < 3
00950 _orb = cv::Ptr<CV_ORB>(new CV_ORB(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, parameters));
00951 #else
00952 _orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
00953 #endif
00954 }
00955 }
00956
00957 std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00958 {
00959 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00960 std::vector<cv::KeyPoint> keypoints;
00961 cv::Mat imgRoi(image, roi);
00962 cv::Mat maskRoi;
00963 if(!mask.empty())
00964 {
00965 maskRoi = cv::Mat(mask, roi);
00966 }
00967
00968 if(gpu_)
00969 {
00970 #if CV_MAJOR_VERSION < 3
00971 #ifdef HAVE_OPENCV_GPU
00972 cv::gpu::GpuMat imgGpu(imgRoi);
00973 cv::gpu::GpuMat maskGpu(maskRoi);
00974 (*_gpuOrb.obj)(imgGpu, maskGpu, keypoints);
00975 #else
00976 UERROR("Cannot use ORBGPU because OpenCV is not built with gpu module.");
00977 #endif
00978 #else
00979 #ifdef HAVE_OPENCV_CUDAFEATURES2D
00980 cv::cuda::GpuMat d_image(imgRoi);
00981 cv::cuda::GpuMat d_mask(maskRoi);
00982 try {
00983 _gpuOrb->detectAndCompute(d_image, d_mask, keypoints, cv::cuda::GpuMat(), false);
00984 } catch (cv::Exception& e) {
00985 const char* err_msg = e.what();
00986 UWARN("OpenCV exception caught: %s", err_msg);
00987 }
00988 #endif
00989 #endif
00990 }
00991 else
00992 {
00993 _orb->detect(imgRoi, keypoints, maskRoi);
00994 }
00995
00996 return keypoints;
00997 }
00998
00999 cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01000 {
01001 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01002 cv::Mat descriptors;
01003 if(image.empty())
01004 {
01005 ULOGGER_ERROR("Image is null ?!?");
01006 return descriptors;
01007 }
01008 if(gpu_)
01009 {
01010 #if CV_MAJOR_VERSION < 3
01011 #ifdef HAVE_OPENCV_GPU
01012 cv::gpu::GpuMat imgGpu(image);
01013 cv::gpu::GpuMat descriptorsGPU;
01014 (*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
01015
01016 if (descriptorsGPU.empty())
01017 descriptors = cv::Mat();
01018 else
01019 {
01020 UASSERT(descriptorsGPU.type() == CV_32F);
01021 descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
01022 descriptorsGPU.download(descriptors);
01023 }
01024 #else
01025 UERROR("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
01026 #endif
01027 #else
01028 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01029 cv::cuda::GpuMat d_image(image);
01030 cv::cuda::GpuMat d_descriptors;
01031 try {
01032 _gpuOrb->detectAndCompute(d_image, cv::cuda::GpuMat(), keypoints, d_descriptors, true);
01033 } catch (cv::Exception& e) {
01034 const char* err_msg = e.what();
01035 UWARN("OpenCV exception caught: %s", err_msg);
01036 }
01037
01038 if (d_descriptors.empty())
01039 descriptors = cv::Mat();
01040 else
01041 {
01042 UASSERT(d_descriptors.type() == CV_32F || d_descriptors.type() == CV_8U);
01043 d_descriptors.download(descriptors);
01044 }
01045 #endif
01046 #endif
01047 }
01048 else
01049 {
01050 _orb->compute(image, keypoints, descriptors);
01051 }
01052
01053 return descriptors;
01054 }
01055
01057
01059 FAST::FAST(const ParametersMap & parameters) :
01060 threshold_(Parameters::defaultFASTThreshold()),
01061 nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
01062 gpu_(Parameters::defaultFASTGpu()),
01063 gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
01064 minThreshold_(Parameters::defaultFASTMinThreshold()),
01065 maxThreshold_(Parameters::defaultFASTMaxThreshold()),
01066 gridRows_(Parameters::defaultFASTGridRows()),
01067 gridCols_(Parameters::defaultFASTGridCols())
01068 {
01069 parseParameters(parameters);
01070 }
01071
01072 FAST::~FAST()
01073 {
01074 }
01075
01076 void FAST::parseParameters(const ParametersMap & parameters)
01077 {
01078 Feature2D::parseParameters(parameters);
01079
01080 Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
01081 Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
01082 Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
01083 Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);
01084
01085 Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
01086 Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
01087 Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
01088 Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
01089
01090 UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
01091 UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
01092
01093 #if CV_MAJOR_VERSION < 3
01094 #ifdef HAVE_OPENCV_GPU
01095 if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
01096 {
01097 UWARN("GPU version of FAST not available! Using CPU version instead...");
01098 gpu_ = false;
01099 }
01100 #else
01101 if(gpu_)
01102 {
01103 UWARN("GPU version of FAST not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
01104 gpu_ = false;
01105 }
01106 #endif
01107 #else
01108 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01109 if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
01110 {
01111 UWARN("GPU version of FAST not available! Using CPU version instead...");
01112 gpu_ = false;
01113 }
01114 #else
01115 if(gpu_)
01116 {
01117 UWARN("GPU version of FAST not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
01118 gpu_ = false;
01119 }
01120 #endif
01121 #endif
01122 if(gpu_)
01123 {
01124 #if CV_MAJOR_VERSION < 3
01125 #ifdef HAVE_OPENCV_GPU
01126 _gpuFast = new CV_FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
01127 #else
01128 UFATAL("not supposed to be here!");
01129 #endif
01130 #else
01131 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01132 UFATAL("not implemented");
01133 #endif
01134 #endif
01135 }
01136 else
01137 {
01138 #if CV_MAJOR_VERSION < 3
01139 if(gridRows_ > 0 && gridCols_ > 0)
01140 {
01141 UDEBUG("grid max features = %d", this->getMaxFeatures());
01142 cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
01143 _fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, this->getMaxFeatures(), gridRows_, gridCols_));
01144 }
01145 else
01146 {
01147 if(gridRows_ > 0)
01148 {
01149 UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
01150 Parameters::kFASTGridRows().c_str(), gridRows_, Parameters::kFASTGridCols().c_str());
01151 }
01152 else if(gridCols_ > 0)
01153 {
01154 UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
01155 Parameters::kFASTGridCols().c_str(), gridCols_, Parameters::kFASTGridRows().c_str());
01156 }
01157 _fast = cv::Ptr<cv::FeatureDetector>(new CV_FAST(threshold_, nonmaxSuppression_));
01158 }
01159 #else
01160 _fast = CV_FAST::create(threshold_, nonmaxSuppression_);
01161 #endif
01162 }
01163 }
01164
01165 std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01166 {
01167 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01168 std::vector<cv::KeyPoint> keypoints;
01169 cv::Mat imgRoi(image, roi);
01170 cv::Mat maskRoi;
01171 if(!mask.empty())
01172 {
01173 maskRoi = cv::Mat(mask, roi);
01174 }
01175 if(gpu_)
01176 {
01177 #if CV_MAJOR_VERSION < 3
01178 #ifdef HAVE_OPENCV_GPU
01179 cv::gpu::GpuMat imgGpu(imgRoi);
01180 cv::gpu::GpuMat maskGpu(maskRoi);
01181 (*_gpuFast.obj)(imgGpu, maskGpu, keypoints);
01182 #else
01183 UERROR("Cannot use FAST GPU because OpenCV is not built with gpu module.");
01184 #endif
01185 #else
01186 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01187 UFATAL("not implemented");
01188 #endif
01189 #endif
01190 }
01191 else
01192 {
01193 _fast->detect(imgRoi, keypoints, maskRoi);
01194 }
01195 return keypoints;
01196 }
01197
01199
01201 FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
01202 FAST(parameters),
01203 bytes_(Parameters::defaultBRIEFBytes())
01204 {
01205 parseParameters(parameters);
01206 }
01207
01208 FAST_BRIEF::~FAST_BRIEF()
01209 {
01210 }
01211
01212 void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
01213 {
01214 FAST::parseParameters(parameters);
01215
01216 Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
01217 #if CV_MAJOR_VERSION < 3
01218 _brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
01219 #else
01220 #ifdef HAVE_OPENCV_XFEATURES2D
01221 _brief = CV_BRIEF::create(bytes_);
01222 #else
01223 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01224 #endif
01225 #endif
01226 }
01227
01228 cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01229 {
01230 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01231 cv::Mat descriptors;
01232 #if CV_MAJOR_VERSION < 3
01233 _brief->compute(image, keypoints, descriptors);
01234 #else
01235 #ifdef HAVE_OPENCV_XFEATURES2D
01236 _brief->compute(image, keypoints, descriptors);
01237 #else
01238 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01239 #endif
01240 #endif
01241 return descriptors;
01242 }
01243
01245
01247 FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
01248 FAST(parameters),
01249 orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
01250 scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
01251 patternScale_(Parameters::defaultFREAKPatternScale()),
01252 nOctaves_(Parameters::defaultFREAKNOctaves())
01253 {
01254 parseParameters(parameters);
01255 }
01256
01257 FAST_FREAK::~FAST_FREAK()
01258 {
01259 }
01260
01261 void FAST_FREAK::parseParameters(const ParametersMap & parameters)
01262 {
01263 FAST::parseParameters(parameters);
01264
01265 Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
01266 Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
01267 Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
01268 Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
01269
01270 #if CV_MAJOR_VERSION < 3
01271 _freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
01272 #else
01273 #ifdef HAVE_OPENCV_XFEATURES2D
01274 _freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
01275 #else
01276 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01277 #endif
01278 #endif
01279 }
01280
01281 cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01282 {
01283 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01284 cv::Mat descriptors;
01285 #if CV_MAJOR_VERSION < 3
01286 _freak->compute(image, keypoints, descriptors);
01287 #else
01288 #ifdef HAVE_OPENCV_XFEATURES2D
01289 _freak->compute(image, keypoints, descriptors);
01290 #else
01291 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01292 #endif
01293 #endif
01294 return descriptors;
01295 }
01296
01298
01300 GFTT::GFTT(const ParametersMap & parameters) :
01301 _qualityLevel(Parameters::defaultGFTTQualityLevel()),
01302 _minDistance(Parameters::defaultGFTTMinDistance()),
01303 _blockSize(Parameters::defaultGFTTBlockSize()),
01304 _useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
01305 _k(Parameters::defaultGFTTK())
01306 {
01307 parseParameters(parameters);
01308 }
01309
01310 GFTT::~GFTT()
01311 {
01312 }
01313
01314 void GFTT::parseParameters(const ParametersMap & parameters)
01315 {
01316 Feature2D::parseParameters(parameters);
01317
01318 Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
01319 Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
01320 Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
01321 Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
01322 Parameters::parse(parameters, Parameters::kGFTTK(), _k);
01323
01324 #if CV_MAJOR_VERSION < 3
01325 _gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
01326 #else
01327 _gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
01328 #endif
01329 }
01330
01331 std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01332 {
01333 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01334 std::vector<cv::KeyPoint> keypoints;
01335 cv::Mat imgRoi(image, roi);
01336 cv::Mat maskRoi;
01337 if(!mask.empty())
01338 {
01339 maskRoi = cv::Mat(mask, roi);
01340 }
01341 _gftt->detect(imgRoi, keypoints, maskRoi);
01342 return keypoints;
01343 }
01344
01346
01348 GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
01349 GFTT(parameters),
01350 bytes_(Parameters::defaultBRIEFBytes())
01351 {
01352 parseParameters(parameters);
01353 }
01354
01355 GFTT_BRIEF::~GFTT_BRIEF()
01356 {
01357 }
01358
01359 void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
01360 {
01361 GFTT::parseParameters(parameters);
01362
01363 Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
01364 #if CV_MAJOR_VERSION < 3
01365 _brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
01366 #else
01367 #ifdef HAVE_OPENCV_XFEATURES2D
01368 _brief = CV_BRIEF::create(bytes_);
01369 #else
01370 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01371 #endif
01372 #endif
01373 }
01374
01375 cv::Mat GFTT_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01376 {
01377 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01378 cv::Mat descriptors;
01379 #if CV_MAJOR_VERSION < 3
01380 _brief->compute(image, keypoints, descriptors);
01381 #else
01382 #ifdef HAVE_OPENCV_XFEATURES2D
01383 _brief->compute(image, keypoints, descriptors);
01384 #else
01385 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01386 #endif
01387 #endif
01388 return descriptors;
01389 }
01390
01392
01394 GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
01395 GFTT(parameters),
01396 orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
01397 scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
01398 patternScale_(Parameters::defaultFREAKPatternScale()),
01399 nOctaves_(Parameters::defaultFREAKNOctaves())
01400 {
01401 parseParameters(parameters);
01402 }
01403
01404 GFTT_FREAK::~GFTT_FREAK()
01405 {
01406 }
01407
01408 void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
01409 {
01410 GFTT::parseParameters(parameters);
01411
01412 Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
01413 Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
01414 Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
01415 Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
01416
01417 #if CV_MAJOR_VERSION < 3
01418 _freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
01419 #else
01420 #ifdef HAVE_OPENCV_XFEATURES2D
01421 _freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
01422 #else
01423 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01424 #endif
01425 #endif
01426 }
01427
01428 cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01429 {
01430 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01431 cv::Mat descriptors;
01432 #if CV_MAJOR_VERSION < 3
01433 _freak->compute(image, keypoints, descriptors);
01434 #else
01435 #ifdef HAVE_OPENCV_XFEATURES2D
01436 _freak->compute(image, keypoints, descriptors);
01437 #else
01438 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01439 #endif
01440 #endif
01441 return descriptors;
01442 }
01443
01445
01447 GFTT_ORB::GFTT_ORB(const ParametersMap & parameters) :
01448 GFTT(parameters),
01449 _orb(parameters)
01450 {
01451 parseParameters(parameters);
01452 }
01453
01454 GFTT_ORB::~GFTT_ORB()
01455 {
01456 }
01457
01458 void GFTT_ORB::parseParameters(const ParametersMap & parameters)
01459 {
01460 GFTT::parseParameters(parameters);
01461 _orb.parseParameters(parameters);
01462 }
01463
01464 cv::Mat GFTT_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01465 {
01466 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01467 return _orb.generateDescriptors(image, keypoints);
01468 }
01469
01471
01473 BRISK::BRISK(const ParametersMap & parameters) :
01474 thresh_(Parameters::defaultBRISKThresh()),
01475 octaves_(Parameters::defaultBRISKOctaves()),
01476 patternScale_(Parameters::defaultBRISKPatternScale())
01477 {
01478 parseParameters(parameters);
01479 }
01480
01481 BRISK::~BRISK()
01482 {
01483 }
01484
01485 void BRISK::parseParameters(const ParametersMap & parameters)
01486 {
01487 Feature2D::parseParameters(parameters);
01488
01489 Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
01490 Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
01491 Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
01492
01493 #if CV_MAJOR_VERSION < 3
01494 brisk_ = cv::Ptr<CV_BRISK>(new CV_BRISK(thresh_, octaves_, patternScale_));
01495 #else
01496 brisk_ = CV_BRISK::create(thresh_, octaves_, patternScale_);
01497 #endif
01498 }
01499
01500 std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01501 {
01502 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01503 std::vector<cv::KeyPoint> keypoints;
01504 cv::Mat imgRoi(image, roi);
01505 cv::Mat maskRoi;
01506 if(!mask.empty())
01507 {
01508 maskRoi = cv::Mat(mask, roi);
01509 }
01510 brisk_->detect(imgRoi, keypoints, maskRoi);
01511 return keypoints;
01512 }
01513
01514 cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01515 {
01516 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01517 cv::Mat descriptors;
01518 brisk_->compute(image, keypoints, descriptors);
01519 return descriptors;
01520 }
01521
01523
01525 KAZE::KAZE(const ParametersMap & parameters) :
01526 extended_(Parameters::defaultKAZEExtended()),
01527 upright_(Parameters::defaultKAZEUpright()),
01528 threshold_(Parameters::defaultKAZEThreshold()),
01529 nOctaves_(Parameters::defaultKAZENOctaves()),
01530 nOctaveLayers_(Parameters::defaultKAZENOctaveLayers()),
01531 diffusivity_(Parameters::defaultKAZEDiffusivity())
01532 {
01533 parseParameters(parameters);
01534 }
01535
01536 KAZE::~KAZE()
01537 {
01538 }
01539
01540 void KAZE::parseParameters(const ParametersMap & parameters)
01541 {
01542 Feature2D::parseParameters(parameters);
01543
01544 Parameters::parse(parameters, Parameters::kKAZEExtended(), extended_);
01545 Parameters::parse(parameters, Parameters::kKAZEUpright(), upright_);
01546 Parameters::parse(parameters, Parameters::kKAZEThreshold(), threshold_);
01547 Parameters::parse(parameters, Parameters::kKAZENOctaves(), nOctaves_);
01548 Parameters::parse(parameters, Parameters::kKAZENOctaveLayers(), nOctaveLayers_);
01549 Parameters::parse(parameters, Parameters::kKAZEDiffusivity(), diffusivity_);
01550
01551 #if CV_MAJOR_VERSION > 2
01552 kaze_ = cv::KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, diffusivity_);
01553 #else
01554 UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
01555 #endif
01556 }
01557
01558 std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01559 {
01560 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01561 std::vector<cv::KeyPoint> keypoints;
01562 #if CV_MAJOR_VERSION > 2
01563 cv::Mat imgRoi(image, roi);
01564 cv::Mat maskRoi;
01565 if (!mask.empty())
01566 {
01567 maskRoi = cv::Mat(mask, roi);
01568 }
01569 kaze_->detect(imgRoi, keypoints, maskRoi);
01570 #else
01571 UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
01572 #endif
01573 return keypoints;
01574 }
01575
01576 cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01577 {
01578 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01579 cv::Mat descriptors;
01580 #if CV_MAJOR_VERSION > 2
01581 kaze_->compute(image, keypoints, descriptors);
01582 #else
01583 UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
01584 #endif
01585 return descriptors;
01586 }
01587
01588 }