feature_adjuster.cpp
Go to the documentation of this file.
00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002  //
00003  //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004  //
00005  //  By downloading, copying, installing or using the software you agree to this license.
00006  //  If you do not agree to this license, do not download, install,
00007  //  copy or use the software.
00008  //
00009  //
00010  //                           License Agreement
00011  //                For Open Source Computer Vision Library
00012  //
00013  // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
00014  // Copyright (C) 2009-2010, Willow Garage Inc., all rights reserved.
00015  // Third party copyrights are property of their respective owners.
00016  //
00017  // Redistribution and use in source and binary forms, with or without modification,
00018  // are permitted provided that the following conditions are met:
00019  //
00020  //   * Redistribution's of source code must retain the above copyright notice,
00021  //     this list of conditions and the following disclaimer.
00022  //
00023  //   * Redistribution's in binary form must reproduce the above copyright notice,
00024  //     this list of conditions and the following disclaimer in the documentation
00025  //     and/or other materials provided with the distribution.
00026  //
00027  //   * The name of the copyright holders may not be used to endorse or promote products
00028  //     derived from this software without specific prior written permission.
00029  //
00030  // This software is provided by the copyright holders and contributors "as is" and
00031  // any express or implied warranties, including, but not limited to, the implied
00032  // warranties of merchantability and fitness for a particular purpose are disclaimed.
00033  // In no event shall the Intel Corporation or contributors be liable for any direct,
00034  // indirect, incidental, special, exemplary, or consequential damages
00035  // (including, but not limited to, procurement of substitute goods or services;
00036  // loss of use, data, or profits; or business interruption) however caused
00037  // and on any theory of liability, whether in contract, strict liability,
00038  // or tort (including negligence or otherwise) arising in any way out of
00039  // the use of this software, even if advised of the possibility of such damage.
00040  //
00041  //M*/
00042 
00043 #include "feature_adjuster.h"
00044 //#include "opencv2/features2d/precomp.hpp"
00045 #include "opencv2/features2d/features2d.hpp"
00046 #ifdef CV_NONFREE
00047 #include "opencv2/nonfree/features2d.hpp"
00048 #endif
00049 #include "opencv2/imgproc/imgproc.hpp"
00050 #include "aorb.h"
00051 #include <cassert>
00052 #include <iostream>
00053 #include <algorithm> //for min
00054 //#include <ros/ros.h>
00055 using namespace cv;
00056 
00057 
00058 DetectorAdjuster::DetectorAdjuster(const char* detector_name, double initial_thresh, double min_thresh, double max_thresh, double increase_factor, double decrease_factor ) :
00059     thresh_(initial_thresh), 
00060     min_thresh_(min_thresh), max_thresh_(max_thresh),
00061     increase_factor_(increase_factor), decrease_factor_(decrease_factor),
00062     detector_name_(detector_name)
00063 {
00064 #ifdef CV_NONFREE
00065     if(!(detector_name_ == "SURF" || 
00066          detector_name_ == "SIFT" ||
00067          detector_name_ == "FAST" ||
00068          detector_name_ == "AORB"))
00069     { //None of the above
00070       std::cerr << "Unknown Descriptor";
00071     }
00072 #else
00073     if(detector_name_ == "SURF" || detector_name_ == "SIFT") {
00074         std::cerr << "OpenCV non-free functionality (" << detector_name << ") not built in.";
00075         std::cerr << "To enable non-free functionality build with CV_NONFREE set.";
00076     }
00077     if(!(detector_name_ == "FAST" ||
00078          detector_name_ == "AORB"))
00079     { //None of the above
00080       std::cerr << "Unknown Descriptor";
00081     }
00082 #endif
00083 }
00084 
00085 void DetectorAdjuster::detectImpl(const Mat& image, std::vector<KeyPoint>& keypoints, const Mat& mask) const
00086 {
00087     Ptr<FeatureDetector> detector; 
00088     if(strcmp(detector_name_, "FAST") == 0){
00089       //detector->set("threshold", static_cast<int>(thresh_));
00090       detector = new FastFeatureDetector(thresh_);
00091     }
00092     else if(strcmp(detector_name_, "AORB") == 0){
00093       //Default params except last
00094       detector = new AorbFeatureDetector(10000, 1.2, 8, 15, 0, 2, 0, 31, static_cast<int>(thresh_));
00095       //detector->set("fastThreshold", static_cast<int>(thresh_));//Not threadsafe (parallelized grid)
00096     }
00097 #ifdef CV_NONFREE
00098     else if(strcmp(detector_name_, "SURF") == 0){
00099       //detector->set("hessianThreshold", thresh_);//Not threadsafe (parallelized grid)
00100       detector = new SurfFeatureDetector(thresh_);
00101     }
00102     else if(strcmp(detector_name_, "SIFT") == 0){
00103       //detector->set("contrastThreshold", thresh_);
00104       detector = new SiftFeatureDetector(0 /*max_features*/, 3 /*default lvls/octave*/, thresh_);
00105     }
00106 #else
00107     else if(strcmp(detector_name_, "SIFT") == 0 || strcmp(detector_name_, "SURF") == 0){
00108         std::cerr << "OpenCV non-free functionality (" << detector_name_ << ") not built in.";
00109         std::cerr << "To enable non-free functionality build with CV_NONFREE set.";
00110         std::cerr << "Using ORB.";
00111         detector = new AorbFeatureDetector(10000, 1.2, 8, 15, 0, 2, 0, 31, static_cast<int>(thresh_));
00112     }
00113 #endif
00114     else {
00115       FeatureDetector::create(detector_name_);
00116       std::cerr << "Unknown Descriptor, not setting threshold";
00117     }
00118     //ROS_INFO("Calling Detect with threshold %f", thresh_);
00119     //std::cout << "Performing detection with " << detector_name_ << ". Threshold: " << thresh_ << std::endl;
00120     detector->detect(image, keypoints, mask);
00121 }
00122 
00123 void DetectorAdjuster::setDecreaseFactor(double new_factor){
00124   decrease_factor_ = new_factor;
00125 }
00126 void DetectorAdjuster::setIncreaseFactor(double new_factor){
00127   increase_factor_ = new_factor;
00128 }
00129 
00130 void DetectorAdjuster::tooFew(int, int)
00131 {
00132     thresh_ *= decrease_factor_;
00133     if (thresh_ < min_thresh_)
00134             thresh_ = min_thresh_;
00135 }
00136 
00137 void DetectorAdjuster::tooMany(int, int)
00138 {
00139     thresh_ *= increase_factor_;
00140     if (thresh_ > max_thresh_)
00141             thresh_ = max_thresh_;
00142 }
00143 
00144 //return whether or not the threshhold is beyond
00145 //a useful point
00146 bool DetectorAdjuster::good() const
00147 {
00148     return (thresh_ > min_thresh_) && (thresh_ < max_thresh_);
00149 }
00150 
00151 Ptr<AdjusterAdapter> DetectorAdjuster::clone() const
00152 {
00153     Ptr<AdjusterAdapter> cloned_obj(new DetectorAdjuster(detector_name_, thresh_, min_thresh_, max_thresh_, increase_factor_, decrease_factor_ ));
00154     return cloned_obj;
00155 }
00156 
00159 
00160 
00161  VideoDynamicAdaptedFeatureDetector::VideoDynamicAdaptedFeatureDetector(Ptr<AdjusterAdapter> a,
00162                                          int min_features, int max_features, int max_iters ) :
00163         escape_iters_(max_iters), min_features_(min_features), max_features_(max_features), adjuster_(a)
00164 {}
00165 
00166 cv::Ptr<StatefulFeatureDetector> VideoDynamicAdaptedFeatureDetector::clone() const 
00167 {
00168   StatefulFeatureDetector* fd = new VideoDynamicAdaptedFeatureDetector(adjuster_->clone(), //clone adjuster, so threshold is not shared!
00169                                                                        min_features_, 
00170                                                                        max_features_, 
00171                                                                        escape_iters_);
00172   cv::Ptr<StatefulFeatureDetector> cloned_obj(fd);
00173   return cloned_obj;
00174 }
00175 
00176 bool VideoDynamicAdaptedFeatureDetector::empty() const
00177 {
00178     return !adjuster_ || adjuster_->empty();
00179 }
00180 
00181 bool hasNonZero(const cv::Mat& img){
00182   for (int y = 0; y < img.cols; y++) {
00183     for(int x = 0; x < img.rows; x++) {
00184       if(img.at<uchar>(x,y) != 0) return true;
00185     }
00186   }
00187   return false;
00188 }
00189 void VideoDynamicAdaptedFeatureDetector::detectImpl(const cv::Mat& _image, std::vector<KeyPoint>& keypoints, const cv::Mat& _mask) const
00190 {
00191     //In contraast to the original, no oscillation testing is needed as
00192     //the loop is broken out of anyway, if too many features were found.
00193 
00194     //break if the desired number hasn't been reached.
00195     int iter_count = escape_iters_;
00196     bool checked_for_non_zero_mask = false;
00197 
00198     do { // detect at least once
00199         keypoints.clear();
00200 
00201         //the adjuster takes care of calling the detector with updated parameters
00202         adjuster_->detect(_image, keypoints,_mask);
00203         //ROS_INFO("Detected %zu keypoints", keypoints.size());
00204         int found_keypoints = static_cast<int>(keypoints.size());
00205         if(found_keypoints < min_features_ )
00206         {
00207             adjuster_->tooFew(min_features_, found_keypoints);
00208             //Specific to depth images
00209             if(found_keypoints == 0 && !checked_for_non_zero_mask){
00210               checked_for_non_zero_mask = true;
00211               if(!hasNonZero(_mask)){
00212                 std::cout << ("Breaking detection iterations, because of missing depth");
00213                 break; //does not help to iterate if no points have depth
00214               }
00215             }
00216         }
00217         else if( int(keypoints.size()) > max_features_ )
00218         {
00219             adjuster_->tooMany(max_features_, (int)keypoints.size());
00220             break;//FIXME: Too many is ok as they are clipped anyway?
00221         }
00222         else
00223             break;
00224 
00225         iter_count--;
00226     } while( iter_count > 0 && adjuster_->good() );
00227 
00228 }
00229 
00230 /*
00231  *  VideoGridAdaptedFeatureDetector
00232  */
00233 VideoGridAdaptedFeatureDetector::VideoGridAdaptedFeatureDetector( const cv::Ptr<StatefulFeatureDetector>& _detector, int _maxTotalKeypoints, int _gridRows, int _gridCols, int _edgeThreshold)
00234     : maxTotalKeypoints(_maxTotalKeypoints), gridRows(_gridRows), gridCols(_gridCols), edgeThreshold(_edgeThreshold)
00235 {
00236   detectors.push_back(_detector);//Use original one
00237   while(detectors.size() < gridRows*gridCols){
00238     detectors.push_back(_detector->clone());//clone, so the state is not shared
00239   }
00240 }
00241 
00242 bool VideoGridAdaptedFeatureDetector::empty() const
00243 {
00244     for(std::vector<cv::Ptr<StatefulFeatureDetector> >::const_iterator it = detectors.begin(); it != detectors.end(); ++it){
00245       if((*it)->empty()) return true;
00246     }
00247     return false;
00248 }
00249 
00250 struct ResponseComparator
00251 {
00252     bool operator() (const KeyPoint& a, const KeyPoint& b)
00253     {
00254         return std::abs(a.response) > std::abs(b.response);
00255     }
00256 };
00257 
00258 void keepStrongest( int N, vector<KeyPoint>& keypoints )
00259 {
00260     if( (int)keypoints.size() > N )
00261     {
00262         std::vector<cv::KeyPoint>::iterator nth = keypoints.begin() + N;
00263         std::nth_element( keypoints.begin(), nth, keypoints.end(), ResponseComparator() );
00264         keypoints.erase( nth, keypoints.end() );
00265     }
00266 }
00267 
00270 static void aggregateKeypointsPerGridCell(std::vector<std::vector<cv::KeyPoint> >& sub_keypoint_vectors, //will be modified to global coordinates
00271                                           std::vector<cv::KeyPoint>& keypoints_out, //output
00272                                           cv::Size gridSize, 
00273                                           cv::Size imageSize, 
00274                                           int edgeThreshold) 
00275 {
00276     for(int i = 0; i < gridSize.height; ++i)
00277     {
00278         int rowstart = std::max((i*imageSize.height)/gridSize.height - edgeThreshold, 0);
00279         for( int j = 0; j < gridSize.width; ++j )
00280         {
00281             int colstart = std::max((j*imageSize.width)/gridSize.width - edgeThreshold, 0);
00282 
00283             std::vector<cv::KeyPoint>& cell_keypoints = sub_keypoint_vectors[j+i*gridSize.width];
00284             std::vector<cv::KeyPoint>::iterator it = cell_keypoints.begin(), end = cell_keypoints.end();
00285             for( ; it != end; ++it )
00286             {
00287                 it->pt.x += colstart;
00288                 it->pt.y += rowstart;
00289             }
00290             keypoints_out.insert(keypoints_out.end(), cell_keypoints.begin(), cell_keypoints.end());
00291         }
00292     }
00293 }
00294 
00296 void VideoGridAdaptedFeatureDetector::detectImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask ) const
00297 {
00298     std::vector<std::vector<cv::KeyPoint> > sub_keypoint_vectors(gridCols*gridRows);
00299     keypoints.reserve(maxTotalKeypoints);
00300     int maxPerCell = maxTotalKeypoints / (gridRows * gridCols);
00301 #pragma omp parallel for
00302     for( int i = 0; i < gridRows; ++i )
00303     {
00304         int rowstart = std::max((i*image.rows)/gridRows - edgeThreshold, 0);
00305         int rowend   = std::min(image.rows, ((i+1)*image.rows)/gridRows + edgeThreshold);
00306         cv::Range row_range(rowstart, rowend);
00307 #pragma omp parallel for
00308         for( int j = 0; j < gridCols; ++j )
00309         {
00310             int colstart = std::max((j*image.cols)/gridCols - edgeThreshold, 0);
00311             int colend   = std::min(image.cols, ((j+1)*image.cols)/gridCols + edgeThreshold);
00312             cv::Range col_range(colstart, colend);
00313             cv::Mat sub_image = image(row_range, col_range);
00314             cv::Mat sub_mask;
00315             if( !mask.empty()){
00316                 sub_mask = mask(row_range, col_range);
00317             }
00318 
00319             std::vector<cv::KeyPoint>& sub_keypoints = sub_keypoint_vectors[j+i*gridCols];
00320             detectors[j+i*gridCols]->detect( sub_image, sub_keypoints, sub_mask );
00321             keepStrongest( maxPerCell, sub_keypoints );
00322         }
00323     }
00324     aggregateKeypointsPerGridCell(sub_keypoint_vectors, keypoints, cv::Size(gridCols, gridRows), image.size(), edgeThreshold);
00325 }
00326 
00327 cv::Ptr<StatefulFeatureDetector> VideoGridAdaptedFeatureDetector::clone() const 
00328 {
00329   StatefulFeatureDetector* fd = new VideoGridAdaptedFeatureDetector(detectors[0]->clone(), //clone detector, so threshold is not shared!
00330                                                                     maxTotalKeypoints, 
00331                                                                     gridRows, gridCols, 
00332                                                                     edgeThreshold);
00333   cv::Ptr<StatefulFeatureDetector> cloned_obj(fd);
00334   return cloned_obj;
00335 }
00336 


rgbdslam_v2
Author(s): Felix Endres, Juergen Hess, Nikolas Engelhard
autogenerated on Thu Jun 6 2019 21:49:45