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 UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
00214 if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
00215 {
00216 UTimer timer;
00217 ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
00218
00219
00220
00221 std::multimap<float, int> hessianMap;
00222 for(unsigned int i = 0; i <keypoints.size(); ++i)
00223 {
00224
00225 hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
00226 }
00227
00228
00229 int removed = (int)hessianMap.size()-maxKeypoints;
00230 std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
00231 std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
00232 cv::Mat descriptorsTmp;
00233 if(descriptors.rows)
00234 {
00235 descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
00236 }
00237 for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
00238 {
00239 kptsTmp[k] = keypoints[iter->second];
00240 if(descriptors.rows)
00241 {
00242 if(descriptors.type() == CV_32FC1)
00243 {
00244 memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
00245 }
00246 else
00247 {
00248 memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
00249 }
00250 }
00251 }
00252 ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
00253 ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
00254 keypoints = kptsTmp;
00255 if(descriptors.rows)
00256 {
00257 descriptors = descriptorsTmp;
00258 }
00259 }
00260 }
00261
00262 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
00263 {
00264 std::list<std::string> strValues = uSplit(roiRatios, ' ');
00265 if(strValues.size() != 4)
00266 {
00267 UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
00268 }
00269 else
00270 {
00271 std::vector<float> values(4);
00272 unsigned int i=0;
00273 for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
00274 {
00275 values[i] = uStr2Float(*iter);
00276 ++i;
00277 }
00278
00279 if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
00280 values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
00281 values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
00282 values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
00283 {
00284 return computeRoi(image, values);
00285 }
00286 else
00287 {
00288 UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
00289 }
00290 }
00291 return cv::Rect();
00292 }
00293
00294 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
00295 {
00296 if(!image.empty() && roiRatios.size() == 4)
00297 {
00298 float width = image.cols;
00299 float height = image.rows;
00300 cv::Rect roi(0, 0, width, height);
00301 UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
00302 UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
00303
00304
00305 if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
00306 {
00307 roi.x = width * roiRatios[0];
00308 }
00309
00310
00311 if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
00312 {
00313 roi.width -= width * roiRatios[1] + width * roiRatios[0];
00314 }
00315
00316
00317 if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
00318 {
00319 roi.y = height * roiRatios[2];
00320 }
00321
00322
00323 if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
00324 {
00325 roi.height -= height * roiRatios[3] + height * roiRatios[2];
00326 }
00327 UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
00328
00329 return roi;
00330 }
00331 else
00332 {
00333 UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
00334 return cv::Rect();
00335 }
00336 }
00337
00339
00341 Feature2D::Feature2D(const ParametersMap & parameters) :
00342 maxFeatures_(Parameters::defaultKpMaxFeatures()),
00343 _maxDepth(Parameters::defaultKpMaxDepth()),
00344 _minDepth(Parameters::defaultKpMinDepth()),
00345 _roiRatios(std::vector<float>(4, 0.0f)),
00346 _subPixWinSize(Parameters::defaultKpSubPixWinSize()),
00347 _subPixIterations(Parameters::defaultKpSubPixIterations()),
00348 _subPixEps(Parameters::defaultKpSubPixEps())
00349 {
00350 _stereo = new Stereo(parameters);
00351 this->parseParameters(parameters);
00352 }
00353 Feature2D::~Feature2D()
00354 {
00355 delete _stereo;
00356 }
00357 void Feature2D::parseParameters(const ParametersMap & parameters)
00358 {
00359 uInsert(parameters_, parameters);
00360
00361 Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
00362 Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
00363 Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
00364 Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
00365 Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
00366 Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
00367
00368
00369 ParametersMap::const_iterator iter;
00370 if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
00371 {
00372 std::list<std::string> strValues = uSplit(iter->second, ' ');
00373 if(strValues.size() != 4)
00374 {
00375 ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", iter->second.c_str());
00376 }
00377 else
00378 {
00379 std::vector<float> tmpValues(4);
00380 unsigned int i=0;
00381 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00382 {
00383 tmpValues[i] = uStr2Float(*jter);
00384 ++i;
00385 }
00386
00387 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00388 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00389 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00390 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00391 {
00392 _roiRatios = tmpValues;
00393 }
00394 else
00395 {
00396 ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", iter->second.c_str());
00397 }
00398 }
00399 }
00400
00401
00402 UASSERT(_stereo != 0);
00403 if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
00404 {
00405 delete _stereo;
00406 _stereo = Stereo::create(parameters_);
00407 }
00408 else
00409 {
00410 _stereo->parseParameters(parameters);
00411 }
00412 }
00413 Feature2D * Feature2D::create(const ParametersMap & parameters)
00414 {
00415 int type = Parameters::defaultKpDetectorStrategy();
00416 Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
00417 return create((Feature2D::Type)type, parameters);
00418 }
00419 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
00420 {
00421 #ifndef RTABMAP_NONFREE
00422 if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
00423 {
00424 #if CV_MAJOR_VERSION < 3
00425 UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
00426 #else
00427 UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
00428 #endif
00429 type = Feature2D::kFeatureOrb;
00430 }
00431 #if CV_MAJOR_VERSION == 3
00432 if(type == Feature2D::kFeatureFastBrief ||
00433 type == Feature2D::kFeatureFastFreak ||
00434 type == Feature2D::kFeatureGfttBrief ||
00435 type == Feature2D::kFeatureGfttFreak)
00436 {
00437 UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
00438 type = Feature2D::kFeatureOrb;
00439 }
00440 #endif
00441 #endif
00442
00443 Feature2D * feature2D = 0;
00444 switch(type)
00445 {
00446 case Feature2D::kFeatureSurf:
00447 feature2D = new SURF(parameters);
00448 break;
00449 case Feature2D::kFeatureSift:
00450 feature2D = new SIFT(parameters);
00451 break;
00452 case Feature2D::kFeatureOrb:
00453 feature2D = new ORB(parameters);
00454 break;
00455 case Feature2D::kFeatureFastBrief:
00456 feature2D = new FAST_BRIEF(parameters);
00457 break;
00458 case Feature2D::kFeatureFastFreak:
00459 feature2D = new FAST_FREAK(parameters);
00460 break;
00461 case Feature2D::kFeatureGfttFreak:
00462 feature2D = new GFTT_FREAK(parameters);
00463 break;
00464 case Feature2D::kFeatureGfttBrief:
00465 feature2D = new GFTT_BRIEF(parameters);
00466 break;
00467 case Feature2D::kFeatureGfttOrb:
00468 feature2D = new GFTT_ORB(parameters);
00469 break;
00470 case Feature2D::kFeatureBrisk:
00471 feature2D = new BRISK(parameters);
00472 break;
00473 #ifdef RTABMAP_NONFREE
00474 default:
00475 feature2D = new SURF(parameters);
00476 type = Feature2D::kFeatureSurf;
00477 break;
00478 #else
00479 default:
00480 feature2D = new ORB(parameters);
00481 type = Feature2D::kFeatureOrb;
00482 break;
00483 #endif
00484
00485 }
00486 return feature2D;
00487 }
00488
00489 std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Mat & maskIn) const
00490 {
00491 UASSERT(!image.empty());
00492 UASSERT(image.type() == CV_8UC1);
00493
00494 cv::Mat mask;
00495 if(!maskIn.empty())
00496 {
00497 if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
00498 {
00499 mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
00500 for(int i=0; i<(int)mask.total(); ++i)
00501 {
00502 float value = 0.0f;
00503 if(maskIn.type()==CV_16UC1)
00504 {
00505 if(((unsigned short*)maskIn.data)[i] > 0 &&
00506 ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
00507 {
00508 value = float(((unsigned short*)maskIn.data)[i])*0.001f;
00509 }
00510 }
00511 else
00512 {
00513 value = ((float*)maskIn.data)[i];
00514 }
00515
00516 if(value>_minDepth &&
00517 (_maxDepth == 0.0f || value <= _maxDepth))
00518 {
00519 ((unsigned char*)mask.data)[i] = 255;
00520 }
00521 }
00522 }
00523 else if(maskIn.type()==CV_8UC1)
00524 {
00525
00526 mask = maskIn;
00527 }
00528 else
00529 {
00530 UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
00531 }
00532 }
00533
00534 UASSERT(mask.empty() || (mask.cols == image.cols && mask.rows == image.rows));
00535
00536 std::vector<cv::KeyPoint> keypoints;
00537 UTimer timer;
00538
00539
00540 cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
00541 keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
00542 UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
00543
00544 limitKeypoints(keypoints, maxFeatures_);
00545
00546 if(roi.x || roi.y)
00547 {
00548
00549 for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
00550 {
00551 iter->pt.x += roi.x;
00552 iter->pt.y += roi.y;
00553 }
00554 }
00555
00556 if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
00557 {
00558 std::vector<cv::Point2f> corners;
00559 cv::KeyPoint::convert(keypoints, corners);
00560 cv::cornerSubPix( image, corners,
00561 cv::Size( _subPixWinSize, _subPixWinSize ),
00562 cv::Size( -1, -1 ),
00563 cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
00564
00565 for(unsigned int i=0;i<corners.size(); ++i)
00566 {
00567 keypoints[i].pt = corners[i];
00568 }
00569 UDEBUG("subpixel time = %f s", timer.ticks());
00570 }
00571
00572 return keypoints;
00573 }
00574
00575 cv::Mat Feature2D::generateDescriptors(
00576 const cv::Mat & image,
00577 std::vector<cv::KeyPoint> & keypoints) const
00578 {
00579 UASSERT(!image.empty());
00580 UASSERT(image.type() == CV_8UC1);
00581 cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
00582 UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
00583 UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
00584 return descriptors;
00585 }
00586
00587 std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
00588 const SensorData & data,
00589 const std::vector<cv::KeyPoint> & keypoints) const
00590 {
00591 std::vector<cv::Point3f> keypoints3D;
00592 if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
00593 {
00594
00595 cv::Mat imageMono;
00596
00597 if(data.imageRaw().channels() > 1)
00598 {
00599 cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
00600 }
00601 else
00602 {
00603 imageMono = data.imageRaw();
00604 }
00605
00606 std::vector<cv::Point2f> leftCorners;
00607 cv::KeyPoint::convert(keypoints, leftCorners);
00608 std::vector<unsigned char> status;
00609
00610 std::vector<cv::Point2f> rightCorners;
00611 rightCorners = _stereo->computeCorrespondences(
00612 imageMono,
00613 data.rightRaw(),
00614 leftCorners,
00615 status);
00616
00617 keypoints3D = util3d::generateKeypoints3DStereo(
00618 leftCorners,
00619 rightCorners,
00620 data.stereoCameraModel(),
00621 status,
00622 _minDepth,
00623 _maxDepth);
00624 }
00625 else if(!data.depthRaw().empty() && data.cameraModels().size())
00626 {
00627 keypoints3D = util3d::generateKeypoints3DDepth(
00628 keypoints,
00629 data.depthOrRightRaw(),
00630 data.cameraModels(),
00631 _minDepth,
00632 _maxDepth);
00633 }
00634
00635 return keypoints3D;
00636 }
00637
00639
00641 SURF::SURF(const ParametersMap & parameters) :
00642 hessianThreshold_(Parameters::defaultSURFHessianThreshold()),
00643 nOctaves_(Parameters::defaultSURFOctaves()),
00644 nOctaveLayers_(Parameters::defaultSURFOctaveLayers()),
00645 extended_(Parameters::defaultSURFExtended()),
00646 upright_(Parameters::defaultSURFUpright()),
00647 gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
00648 gpuVersion_(Parameters::defaultSURFGpuVersion())
00649 {
00650 parseParameters(parameters);
00651 }
00652
00653 SURF::~SURF()
00654 {
00655 }
00656
00657 void SURF::parseParameters(const ParametersMap & parameters)
00658 {
00659 Feature2D::parseParameters(parameters);
00660
00661 Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
00662 Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
00663 Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
00664 Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves_);
00665 Parameters::parse(parameters, Parameters::kSURFUpright(), upright_);
00666 Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
00667 Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
00668
00669 #ifdef RTABMAP_NONFREE
00670 #if CV_MAJOR_VERSION < 3
00671 if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
00672 {
00673 UWARN("GPU version of SURF not available! Using CPU version instead...");
00674 gpuVersion_ = false;
00675 }
00676 #else
00677 if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
00678 {
00679 UWARN("GPU version of SURF not available! Using CPU version instead...");
00680 gpuVersion_ = false;
00681 }
00682 #endif
00683 if(gpuVersion_)
00684 {
00685 _gpuSurf = cv::Ptr<CV_SURF_GPU>(new CV_SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_));
00686 }
00687 else
00688 {
00689 #if CV_MAJOR_VERSION < 3
00690 _surf = cv::Ptr<CV_SURF>(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_));
00691 #else
00692 _surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
00693 #endif
00694 }
00695 #else
00696 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00697 #endif
00698 }
00699
00700 std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00701 {
00702 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00703 std::vector<cv::KeyPoint> keypoints;
00704
00705 #ifdef RTABMAP_NONFREE
00706 cv::Mat imgRoi(image, roi);
00707 cv::Mat maskRoi;
00708 if(!mask.empty())
00709 {
00710 maskRoi = cv::Mat(mask, roi);
00711 }
00712 if(gpuVersion_)
00713 {
00714 #if CV_MAJOR_VERSION < 3
00715 cv::gpu::GpuMat imgGpu(imgRoi);
00716 cv::gpu::GpuMat maskGpu(maskRoi);
00717 (*_gpuSurf.obj)(imgGpu, maskGpu, keypoints);
00718 #else
00719 cv::cuda::GpuMat imgGpu(imgRoi);
00720 cv::cuda::GpuMat maskGpu(maskRoi);
00721 (*_gpuSurf.get())(imgGpu, maskGpu, keypoints);
00722 #endif
00723 }
00724 else
00725 {
00726 _surf->detect(imgRoi, keypoints, maskRoi);
00727 }
00728 #else
00729 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00730 #endif
00731 return keypoints;
00732 }
00733
00734 cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
00735 {
00736 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00737 cv::Mat descriptors;
00738 #ifdef RTABMAP_NONFREE
00739 if(gpuVersion_)
00740 {
00741 #if CV_MAJOR_VERSION < 3
00742 cv::gpu::GpuMat imgGpu(image);
00743 cv::gpu::GpuMat descriptorsGPU;
00744 (*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
00745 #else
00746 cv::cuda::GpuMat imgGpu(image);
00747 cv::cuda::GpuMat descriptorsGPU;
00748 (*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints, descriptorsGPU, true);
00749 #endif
00750
00751
00752 if (descriptorsGPU.empty())
00753 descriptors = cv::Mat();
00754 else
00755 {
00756 UASSERT(descriptorsGPU.type() == CV_32F);
00757 descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
00758 descriptorsGPU.download(descriptors);
00759 }
00760 }
00761 else
00762 {
00763 _surf->compute(image, keypoints, descriptors);
00764 }
00765 #else
00766 UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
00767 #endif
00768
00769 return descriptors;
00770 }
00771
00773
00775 SIFT::SIFT(const ParametersMap & parameters) :
00776 nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
00777 contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
00778 edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
00779 sigma_(Parameters::defaultSIFTSigma())
00780 {
00781 parseParameters(parameters);
00782 }
00783
00784 SIFT::~SIFT()
00785 {
00786 }
00787
00788 void SIFT::parseParameters(const ParametersMap & parameters)
00789 {
00790 Feature2D::parseParameters(parameters);
00791
00792 Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
00793 Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
00794 Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
00795 Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
00796
00797 #ifdef RTABMAP_NONFREE
00798 #if CV_MAJOR_VERSION < 3
00799 _sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
00800 #else
00801 _sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
00802 #endif
00803 #else
00804 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00805 #endif
00806 }
00807
00808 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00809 {
00810 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00811 std::vector<cv::KeyPoint> keypoints;
00812 #ifdef RTABMAP_NONFREE
00813 cv::Mat imgRoi(image, roi);
00814 cv::Mat maskRoi;
00815 if(!mask.empty())
00816 {
00817 maskRoi = cv::Mat(mask, roi);
00818 }
00819 _sift->detect(imgRoi, keypoints, maskRoi);
00820 #else
00821 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00822 #endif
00823 return keypoints;
00824 }
00825
00826 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
00827 {
00828 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00829 cv::Mat descriptors;
00830 #ifdef RTABMAP_NONFREE
00831 _sift->compute(image, keypoints, descriptors);
00832 #else
00833 UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
00834 #endif
00835 return descriptors;
00836 }
00837
00839
00841 ORB::ORB(const ParametersMap & parameters) :
00842 scaleFactor_(Parameters::defaultORBScaleFactor()),
00843 nLevels_(Parameters::defaultORBNLevels()),
00844 edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
00845 firstLevel_(Parameters::defaultORBFirstLevel()),
00846 WTA_K_(Parameters::defaultORBWTA_K()),
00847 scoreType_(Parameters::defaultORBScoreType()),
00848 patchSize_(Parameters::defaultORBPatchSize()),
00849 gpu_(Parameters::defaultORBGpu()),
00850 fastThreshold_(Parameters::defaultFASTThreshold()),
00851 nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression())
00852 {
00853 parseParameters(parameters);
00854 }
00855
00856 ORB::~ORB()
00857 {
00858 }
00859
00860 void ORB::parseParameters(const ParametersMap & parameters)
00861 {
00862 Feature2D::parseParameters(parameters);
00863
00864 Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
00865 Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
00866 Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
00867 Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel_);
00868 Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K_);
00869 Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType_);
00870 Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
00871 Parameters::parse(parameters, Parameters::kORBGpu(), gpu_);
00872
00873 Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
00874 Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);
00875
00876 #if CV_MAJOR_VERSION < 3
00877 #ifdef HAVE_OPENCV_GPU
00878 if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
00879 {
00880 UWARN("GPU version of ORB not available! Using CPU version instead...");
00881 gpu_ = false;
00882 }
00883 #else
00884 if(gpu_)
00885 {
00886 UWARN("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
00887 gpu_ = false;
00888 }
00889 #endif
00890 #else
00891 #ifndef HAVE_OPENCV_CUDAFEATURES2D
00892 if(gpu_)
00893 {
00894 UWARN("GPU version of ORB not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
00895 gpu_ = false;
00896 }
00897 #endif
00898 if(gpu_)
00899 {
00900 UWARN("GPU version of ORB available but not implemented yet! Using CPU version instead...");
00901 }
00902 gpu_ = false;
00903 #endif
00904 if(gpu_)
00905 {
00906 #if CV_MAJOR_VERSION < 3
00907 #ifdef HAVE_OPENCV_GPU
00908 _gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
00909 _gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
00910 #else
00911 UFATAL("not supposed to be here");
00912 #endif
00913 #else
00914 #ifdef HAVE_OPENCV_CUDAFEATURES2D
00915 UFATAL("not implemented");
00916 #endif
00917 #endif
00918 }
00919 else
00920 {
00921 #if CV_MAJOR_VERSION < 3
00922 _orb = cv::Ptr<CV_ORB>(new CV_ORB(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, parameters));
00923 #else
00924 _orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
00925 #endif
00926 }
00927 }
00928
00929 std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
00930 {
00931 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00932 std::vector<cv::KeyPoint> keypoints;
00933 cv::Mat imgRoi(image, roi);
00934 cv::Mat maskRoi;
00935 if(!mask.empty())
00936 {
00937 maskRoi = cv::Mat(mask, roi);
00938 }
00939
00940 if(gpu_)
00941 {
00942 #if CV_MAJOR_VERSION < 3
00943 #ifdef HAVE_OPENCV_GPU
00944 cv::gpu::GpuMat imgGpu(imgRoi);
00945 cv::gpu::GpuMat maskGpu(maskRoi);
00946 (*_gpuOrb.obj)(imgGpu, maskGpu, keypoints);
00947 #else
00948 UERROR("Cannot use ORBGPU because OpenCV is not built with gpu module.");
00949 #endif
00950 #else
00951 #ifdef HAVE_OPENCV_CUDAFEATURES2D
00952 UFATAL("not implemented");
00953 #endif
00954 #endif
00955 }
00956 else
00957 {
00958 _orb->detect(imgRoi, keypoints, maskRoi);
00959 }
00960
00961 return keypoints;
00962 }
00963
00964 cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
00965 {
00966 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
00967 cv::Mat descriptors;
00968 if(image.empty())
00969 {
00970 ULOGGER_ERROR("Image is null ?!?");
00971 return descriptors;
00972 }
00973 if(gpu_)
00974 {
00975 #if CV_MAJOR_VERSION < 3
00976 #ifdef HAVE_OPENCV_GPU
00977 cv::gpu::GpuMat imgGpu(image);
00978 cv::gpu::GpuMat descriptorsGPU;
00979 (*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
00980
00981 if (descriptorsGPU.empty())
00982 descriptors = cv::Mat();
00983 else
00984 {
00985 UASSERT(descriptorsGPU.type() == CV_32F);
00986 descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
00987 descriptorsGPU.download(descriptors);
00988 }
00989 #else
00990 UERROR("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
00991 #endif
00992 #else
00993 cv::cuda::GpuMat imgGpu(image);
00994 cv::cuda::GpuMat descriptorsGPU;
00995 #ifdef HAVE_OPENCV_CUDAFEATURES2D
00996 UFATAL("not implemented");
00997 #endif
00998 #endif
00999 }
01000 else
01001 {
01002 _orb->compute(image, keypoints, descriptors);
01003 }
01004
01005 return descriptors;
01006 }
01007
01009
01011 FAST::FAST(const ParametersMap & parameters) :
01012 threshold_(Parameters::defaultFASTThreshold()),
01013 nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
01014 gpu_(Parameters::defaultFASTGpu()),
01015 gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
01016 minThreshold_(Parameters::defaultFASTMinThreshold()),
01017 maxThreshold_(Parameters::defaultFASTMaxThreshold()),
01018 gridRows_(Parameters::defaultFASTGridRows()),
01019 gridCols_(Parameters::defaultFASTGridCols())
01020 {
01021 parseParameters(parameters);
01022 }
01023
01024 FAST::~FAST()
01025 {
01026 }
01027
01028 void FAST::parseParameters(const ParametersMap & parameters)
01029 {
01030 Feature2D::parseParameters(parameters);
01031
01032 Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
01033 Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
01034 Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
01035 Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);
01036
01037 Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
01038 Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
01039 Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
01040 Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
01041
01042 UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
01043 UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
01044
01045 #if CV_MAJOR_VERSION < 3
01046 #ifdef HAVE_OPENCV_GPU
01047 if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
01048 {
01049 UWARN("GPU version of FAST not available! Using CPU version instead...");
01050 gpu_ = false;
01051 }
01052 #else
01053 if(gpu_)
01054 {
01055 UWARN("GPU version of FAST not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
01056 gpu_ = false;
01057 }
01058 #endif
01059 #else
01060 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01061 if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
01062 {
01063 UWARN("GPU version of FAST not available! Using CPU version instead...");
01064 gpu_ = false;
01065 }
01066 #else
01067 if(gpu_)
01068 {
01069 UWARN("GPU version of FAST not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
01070 gpu_ = false;
01071 }
01072 #endif
01073 #endif
01074 if(gpu_)
01075 {
01076 #if CV_MAJOR_VERSION < 3
01077 #ifdef HAVE_OPENCV_GPU
01078 _gpuFast = new CV_FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
01079 #else
01080 UFATAL("not supposed to be here!");
01081 #endif
01082 #else
01083 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01084 UFATAL("not implemented");
01085 #endif
01086 #endif
01087 }
01088 else
01089 {
01090 #if CV_MAJOR_VERSION < 3
01091 if(gridRows_ > 0 && gridCols_ > 0)
01092 {
01093 UDEBUG("grid max features = %d", this->getMaxFeatures());
01094 cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
01095 _fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, this->getMaxFeatures(), gridRows_, gridCols_));
01096 }
01097 else
01098 {
01099 if(gridRows_ > 0)
01100 {
01101 UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
01102 Parameters::kFASTGridRows().c_str(), gridRows_, Parameters::kFASTGridCols().c_str());
01103 }
01104 else if(gridCols_ > 0)
01105 {
01106 UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
01107 Parameters::kFASTGridCols().c_str(), gridCols_, Parameters::kFASTGridRows().c_str());
01108 }
01109 _fast = cv::Ptr<cv::FeatureDetector>(new CV_FAST(threshold_, nonmaxSuppression_));
01110 }
01111 #else
01112 _fast = CV_FAST::create(threshold_, nonmaxSuppression_);
01113 #endif
01114 }
01115 }
01116
01117 std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01118 {
01119 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01120 std::vector<cv::KeyPoint> keypoints;
01121 cv::Mat imgRoi(image, roi);
01122 cv::Mat maskRoi;
01123 if(!mask.empty())
01124 {
01125 maskRoi = cv::Mat(mask, roi);
01126 }
01127 if(gpu_)
01128 {
01129 #if CV_MAJOR_VERSION < 3
01130 #ifdef HAVE_OPENCV_GPU
01131 cv::gpu::GpuMat imgGpu(imgRoi);
01132 cv::gpu::GpuMat maskGpu(maskRoi);
01133 (*_gpuFast.obj)(imgGpu, maskGpu, keypoints);
01134 #else
01135 UERROR("Cannot use FAST GPU because OpenCV is not built with gpu module.");
01136 #endif
01137 #else
01138 #ifdef HAVE_OPENCV_CUDAFEATURES2D
01139 UFATAL("not implemented");
01140 #endif
01141 #endif
01142 }
01143 else
01144 {
01145 _fast->detect(imgRoi, keypoints, maskRoi);
01146 }
01147 return keypoints;
01148 }
01149
01151
01153 FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
01154 FAST(parameters),
01155 bytes_(Parameters::defaultBRIEFBytes())
01156 {
01157 parseParameters(parameters);
01158 }
01159
01160 FAST_BRIEF::~FAST_BRIEF()
01161 {
01162 }
01163
01164 void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
01165 {
01166 FAST::parseParameters(parameters);
01167
01168 Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
01169 #if CV_MAJOR_VERSION < 3
01170 _brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
01171 #else
01172 #ifdef HAVE_OPENCV_XFEATURES2D
01173 _brief = CV_BRIEF::create(bytes_);
01174 #else
01175 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01176 #endif
01177 #endif
01178 }
01179
01180 cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01181 {
01182 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01183 cv::Mat descriptors;
01184 #if CV_MAJOR_VERSION < 3
01185 _brief->compute(image, keypoints, descriptors);
01186 #else
01187 #ifdef HAVE_OPENCV_XFEATURES2D
01188 _brief->compute(image, keypoints, descriptors);
01189 #else
01190 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01191 #endif
01192 #endif
01193 return descriptors;
01194 }
01195
01197
01199 FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
01200 FAST(parameters),
01201 orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
01202 scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
01203 patternScale_(Parameters::defaultFREAKPatternScale()),
01204 nOctaves_(Parameters::defaultFREAKNOctaves())
01205 {
01206 parseParameters(parameters);
01207 }
01208
01209 FAST_FREAK::~FAST_FREAK()
01210 {
01211 }
01212
01213 void FAST_FREAK::parseParameters(const ParametersMap & parameters)
01214 {
01215 FAST::parseParameters(parameters);
01216
01217 Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
01218 Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
01219 Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
01220 Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
01221
01222 #if CV_MAJOR_VERSION < 3
01223 _freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
01224 #else
01225 #ifdef HAVE_OPENCV_XFEATURES2D
01226 _freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
01227 #else
01228 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01229 #endif
01230 #endif
01231 }
01232
01233 cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01234 {
01235 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01236 cv::Mat descriptors;
01237 #if CV_MAJOR_VERSION < 3
01238 _freak->compute(image, keypoints, descriptors);
01239 #else
01240 #ifdef HAVE_OPENCV_XFEATURES2D
01241 _freak->compute(image, keypoints, descriptors);
01242 #else
01243 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01244 #endif
01245 #endif
01246 return descriptors;
01247 }
01248
01250
01252 GFTT::GFTT(const ParametersMap & parameters) :
01253 _qualityLevel(Parameters::defaultGFTTQualityLevel()),
01254 _minDistance(Parameters::defaultGFTTMinDistance()),
01255 _blockSize(Parameters::defaultGFTTBlockSize()),
01256 _useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
01257 _k(Parameters::defaultGFTTK())
01258 {
01259 parseParameters(parameters);
01260 }
01261
01262 GFTT::~GFTT()
01263 {
01264 }
01265
01266 void GFTT::parseParameters(const ParametersMap & parameters)
01267 {
01268 Feature2D::parseParameters(parameters);
01269
01270 Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
01271 Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
01272 Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
01273 Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
01274 Parameters::parse(parameters, Parameters::kGFTTK(), _k);
01275
01276 #if CV_MAJOR_VERSION < 3
01277 _gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
01278 #else
01279 _gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
01280 #endif
01281 }
01282
01283 std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01284 {
01285 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01286 std::vector<cv::KeyPoint> keypoints;
01287 cv::Mat imgRoi(image, roi);
01288 cv::Mat maskRoi;
01289 if(!mask.empty())
01290 {
01291 maskRoi = cv::Mat(mask, roi);
01292 }
01293 _gftt->detect(imgRoi, keypoints, maskRoi);
01294 return keypoints;
01295 }
01296
01298
01300 GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
01301 GFTT(parameters),
01302 bytes_(Parameters::defaultBRIEFBytes())
01303 {
01304 parseParameters(parameters);
01305 }
01306
01307 GFTT_BRIEF::~GFTT_BRIEF()
01308 {
01309 }
01310
01311 void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
01312 {
01313 GFTT::parseParameters(parameters);
01314
01315 Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
01316 #if CV_MAJOR_VERSION < 3
01317 _brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
01318 #else
01319 #ifdef HAVE_OPENCV_XFEATURES2D
01320 _brief = CV_BRIEF::create(bytes_);
01321 #else
01322 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01323 #endif
01324 #endif
01325 }
01326
01327 cv::Mat GFTT_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01328 {
01329 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01330 cv::Mat descriptors;
01331 #if CV_MAJOR_VERSION < 3
01332 _brief->compute(image, keypoints, descriptors);
01333 #else
01334 #ifdef HAVE_OPENCV_XFEATURES2D
01335 _brief->compute(image, keypoints, descriptors);
01336 #else
01337 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
01338 #endif
01339 #endif
01340 return descriptors;
01341 }
01342
01344
01346 GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
01347 GFTT(parameters),
01348 orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
01349 scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
01350 patternScale_(Parameters::defaultFREAKPatternScale()),
01351 nOctaves_(Parameters::defaultFREAKNOctaves())
01352 {
01353 parseParameters(parameters);
01354 }
01355
01356 GFTT_FREAK::~GFTT_FREAK()
01357 {
01358 }
01359
01360 void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
01361 {
01362 GFTT::parseParameters(parameters);
01363
01364 Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
01365 Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
01366 Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
01367 Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
01368
01369 #if CV_MAJOR_VERSION < 3
01370 _freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
01371 #else
01372 #ifdef HAVE_OPENCV_XFEATURES2D
01373 _freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
01374 #else
01375 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01376 #endif
01377 #endif
01378 }
01379
01380 cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01381 {
01382 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01383 cv::Mat descriptors;
01384 #if CV_MAJOR_VERSION < 3
01385 _freak->compute(image, keypoints, descriptors);
01386 #else
01387 #ifdef HAVE_OPENCV_XFEATURES2D
01388 _freak->compute(image, keypoints, descriptors);
01389 #else
01390 UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
01391 #endif
01392 #endif
01393 return descriptors;
01394 }
01395
01397
01399 GFTT_ORB::GFTT_ORB(const ParametersMap & parameters) :
01400 GFTT(parameters),
01401 _orb(parameters)
01402 {
01403 parseParameters(parameters);
01404 }
01405
01406 GFTT_ORB::~GFTT_ORB()
01407 {
01408 }
01409
01410 void GFTT_ORB::parseParameters(const ParametersMap & parameters)
01411 {
01412 GFTT::parseParameters(parameters);
01413 _orb.parseParameters(parameters);
01414 }
01415
01416 cv::Mat GFTT_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01417 {
01418 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01419 return _orb.generateDescriptors(image, keypoints);
01420 }
01421
01423
01425 BRISK::BRISK(const ParametersMap & parameters) :
01426 thresh_(Parameters::defaultBRISKThresh()),
01427 octaves_(Parameters::defaultBRISKOctaves()),
01428 patternScale_(Parameters::defaultBRISKPatternScale())
01429 {
01430 parseParameters(parameters);
01431 }
01432
01433 BRISK::~BRISK()
01434 {
01435 }
01436
01437 void BRISK::parseParameters(const ParametersMap & parameters)
01438 {
01439 Feature2D::parseParameters(parameters);
01440
01441 Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
01442 Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
01443 Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
01444
01445 #if CV_MAJOR_VERSION < 3
01446 brisk_ = cv::Ptr<CV_BRISK>(new CV_BRISK(thresh_, octaves_, patternScale_));
01447 #else
01448 brisk_ = CV_BRISK::create(thresh_, octaves_, patternScale_);
01449 #endif
01450 }
01451
01452 std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
01453 {
01454 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01455 std::vector<cv::KeyPoint> keypoints;
01456 cv::Mat imgRoi(image, roi);
01457 cv::Mat maskRoi;
01458 if(!mask.empty())
01459 {
01460 maskRoi = cv::Mat(mask, roi);
01461 }
01462 brisk_->detect(imgRoi, keypoints, maskRoi);
01463 return keypoints;
01464 }
01465
01466 cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
01467 {
01468 UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
01469 cv::Mat descriptors;
01470 brisk_->compute(image, keypoints, descriptors);
01471 return descriptors;
01472 }
01473
01474 }