void buildPyramid_templ(
CImagePyramid &obj,
mrpt::utils::CImage &img,
const size_t nOctaves,
const bool smooth_halves,
const bool convert_grayscale)
{
ASSERT_ABOVE_(nOctaves,0)
//TImageSize img_size = img.getSize();
obj.images.resize(nOctaves);
// First octave: Just copy the image:
if (convert_grayscale && img.isColor())
{
// In this case we have to convert to grayscale, so FASTLOAD doesn't really matter:
img.grayscale(obj.images[0]);
}
else
{
// No need to convert to grayscale OR image already is grayscale:
if (FASTLOAD)
obj.images[0].copyFastFrom(img); // Fast copy -> "move", destroying source.
else obj.images[0] = img; // Normal copy
}
// Rest of octaves, if any:
for (size_t o=1;o<nOctaves;o++)
{
if (smooth_halves)
obj.images[o-1].scaleHalfSmooth(obj.images[o]);
else obj.images[o-1].scaleHalf(obj.images[o]);
}
}
/** Stage2 operations:
* - Detect features on each image and on each scale.
*/
void CStereoOdometryEstimator::stage2_detect_features(
CStereoOdometryEstimator::TImagePairData::img_data_t & img_data,
mrpt::utils::CImage & gui_image,
bool update_dyn_thresholds )
{
using namespace mrpt::vision;
m_profiler.enter("_stg2");
// :: Resize output containers:
const size_t nOctaves = img_data.pyr.images.size();
ASSERTDEB_(nOctaves>0)
vector<size_t> nFeatsPassingKLTPerOctave(nOctaves);
img_data.pyr_feats.resize(nOctaves);
img_data.pyr_feats_index.resize(nOctaves);
img_data.pyr_feats_kps.resize(nOctaves);
img_data.pyr_feats_desc.resize(nOctaves);
vector<size_t> kps_to_detect(nOctaves); // number of kps to detect in each octave
kps_to_detect[0] = size_t(params_detect.orb_nfeats*(2*nOctaves)/(std::pow(2,nOctaves)-1));
for( size_t octave = 1; octave < nOctaves; ++octave )
kps_to_detect[octave] = size_t(round(kps_to_detect[0]/std::pow(2,octave)));
// :: For the GUI thread
m_next_gui_info->stats_feats_per_octave.resize(nOctaves); // Reserve size for stats
m_next_gui_info->stats_FAST_thresholds_per_octave.resize(nOctaves);
// :: Detection parameters
// FASTER METHOD --------------------
// - Evaluate the KLT response of all features to discard those in texture-less zones
const unsigned int KLT_win = params_detect.KLT_win;
const double minimum_KLT_response = params_detect.minimum_KLT_response;
// ----------------------------------
// size_t num_feats_this_octave;
// :: Main loop
for( size_t octave = 0; octave < nOctaves; ++octave )
{
// - Image information
Mat input_im = cv::cvarrToMat(img_data.pyr.images[octave].getAs<IplImage>());
const mrpt::utils::TImageSize img_size = img_data.pyr.images[octave].getSize();
// - Profile section name
const std::string sProfileName = mrpt::format("stg2.detect.oct=%u",static_cast<unsigned int>(octave));
// - Auxiliar parameters that will store preliminar extracted information (before NMS)
TKeyPointList feats_vector;
Mat desc_aux;
// ***********************************
// KLT method (use ORB feature vector, no descriptor)
// ***********************************
if( params_detect.detect_method == TDetectParams::dmKLT )
{
m_profiler.enter(sProfileName.c_str());
// detect Shi&Tomasi keypoints
goodFeaturesToTrack(
input_im, // image
feats_vector, // output feature vector
kps_to_detect[octave], // params_detect.orb_nfeats, // number of features to detect
0.01, // quality level
20); // minimum distance
desc_aux = Mat(); // no descriptor
m_profiler.leave(sProfileName.c_str());
}
// ***********************************
// ORB method
// ***********************************
else if( params_detect.detect_method == TDetectParams::dmORB )
{
// ** NOTE ** in this case, nOctaves should be 1 (set in stage1)
const size_t n_feats_to_extract =
params_detect.non_maximal_suppression ?
1.5*params_detect.orb_nfeats :
params_detect.orb_nfeats; // if non-max-sup is ON extract more features to get approx the number of desired output feats.
m_profiler.enter(sProfileName.c_str());
#if CV_MAJOR_VERSION < 3 // OpenCV < 3.0.0
ORB orbDetector(
n_feats_to_extract, // number of ORB features to extract
1.2, // scale difference
params_detect.orb_nlevels, // number of levels
31, // edgeThreshold
0, // firstLevel
2, // WTA_K
ORB::HARRIS_SCORE, // scoreType
31); // patchSize
// detect keypoints and descriptors
orbDetector( input_im, Mat(), feats_vector, desc_aux ); // all the scales in the same call
#else
Ptr<cv::ORB> orbDetector = cv::ORB::create(
n_feats_to_extract, // number of ORB features to extract
1.2, // scale difference
params_detect.orb_nlevels, // number of levels
31, // edgeThreshold
0, // firstLevel
2, // WTA_K
ORB::HARRIS_SCORE, // scoreType
31, // patchSize
m_current_fast_th ); // fast threshold
orbDetector->detectAndCompute( input_im, Mat(), feats_vector, desc_aux ); // all the scales in the same call
#endif
m_profiler.enter(sProfileName.c_str());
}
// ***********************************
// FAST+ORB method
// ***********************************
else if( params_detect.detect_method == TDetectParams::dmFAST_ORB )
{
m_profiler.enter(sProfileName.c_str());
#if CV_MAJOR_VERSION < 3 // OpenCV < 3.0.0
cv::FastFeatureDetector(m_current_fast_th).detect( input_im, feats_vector ); // detect keypoints
MRPT_TODO("Perform non-maximal suppression here -- avoids computing ORB descriptors which are going to be rejected")
ORB().operator()(input_im, Mat(), feats_vector, desc_aux, true ); // extract descriptors
#else
Ptr<cv::FastFeatureDetector> fastDetector = cv::FastFeatureDetector::create( m_current_fast_th );
fastDetector->detect( input_im, feats_vector );
cv::ORB::create()->compute( input_im, feats_vector, desc_aux );
#endif
m_profiler.leave(sProfileName.c_str());
}
// ***********************************
// FASTER method (no descriptor unless specified otherwise)
// ***********************************
else if( params_detect.detect_method == TDetectParams::dmFASTER )
{
// Use a dynamic threshold to maintain a target number of features per square pixel.
if( m_threshold.size() != nOctaves )
m_threshold.assign(nOctaves, params_detect.initial_FAST_threshold);
m_profiler.enter(sProfileName.c_str());
CFeatureExtraction::detectFeatures_SSE2_FASTER12(
img_data.pyr.images[octave],
img_data.pyr_feats[octave],
m_threshold[octave],
false, // don't append to list, overwrite it
octave,
& img_data.pyr_feats_index[octave] ); // row-indexed list of features
const size_t nFeats = img_data.pyr_feats[octave].size();
if( update_dyn_thresholds )
{
// Compute feature density & adjust dynamic threshold:
const double feats_density = nFeats / static_cast<double>(img_size.x * img_size.y);
if( feats_density < 0.8*params_detect.target_feats_per_pixel )
m_threshold[octave] = std::max(1, m_threshold[octave]-1);
else if( feats_density > 1.2*params_detect.target_feats_per_pixel )
m_threshold[octave] = m_threshold[octave]+1;
// Save stats for the GUI:
m_next_gui_info->stats_feats_per_octave[octave] = nFeats;
m_next_gui_info->stats_FAST_thresholds_per_octave[octave] = m_threshold[octave];
}
// compute KLT response
const std::string subSectionName = mrpt::format("stg2.detect.klt.oct=%u",static_cast<unsigned int>(octave));
m_profiler.enter(subSectionName.c_str());
const TImageSize img_size_min( KLT_win+1, KLT_win+1 );
const TImageSize img_size_max( img_size.x-KLT_win-1, img_size.y-KLT_win-1 );
size_t nPassed = 0; // Number of feats in this octave that pass the KLT threshold (for stats only)
for (size_t i=0;i<img_data.pyr_feats[octave].size();i++)
{
TSimpleFeature &f = img_data.pyr_feats[octave][i];
const TPixelCoord pt = f.pt;
if (pt.x>=img_size_min.x && pt.y>=img_size_min.y && pt.x<img_size_max.x && pt.y<img_size_max.y) {
f.response = img_data.pyr.images[octave].KLT_response(pt.x,pt.y,KLT_win);
if (f.response>=minimum_KLT_response) nPassed++;
}
else f.response = 0;
} // end-for
// convert to TKeyPointList (opencv compatible)
m_convert_featureList_to_keypointList( img_data.pyr_feats[octave], feats_vector );
m_profiler.leave(sProfileName.c_str()); // end detect
}
else
THROW_EXCEPTION(" [sVO -- Stg2: Detect] ERROR: Unknown detection method")
// ***********************************
// Non-maximal suppression
// ***********************************
if( params_detect.non_maximal_suppression )
{
if( params_detect.nmsMethod == TDetectParams::nmsmStandard )
{
const size_t imgH = input_im.rows;
const size_t imgW = input_im.cols;
vector<bool> dummy;
m_non_max_sup(
kps_to_detect[octave], // params_detect.orb_nfeats
feats_vector,
desc_aux,
img_data.pyr_feats_kps[octave],
img_data.pyr_feats_desc[octave],
imgH, imgW,
dummy );
}
else if( params_detect.nmsMethod == TDetectParams::nmsmAdaptive )
{
m_adaptive_non_max_sup(
kps_to_detect[octave], // params_detect.orb_nfeats*/
feats_vector,
desc_aux,
img_data.pyr_feats_kps[octave],
img_data.pyr_feats_desc[octave] );
}
else
THROW_EXCEPTION(" [sVO -- Stg2: Detect] Invalid non-maximal-suppression method." );
} // end-if-non-max-sup
else
{
feats_vector.swap(img_data.pyr_feats_kps[octave]);
img_data.pyr_feats_desc[octave] = desc_aux; // this should be fast (just copy the header)
}
// update indexes here
m_update_indexes( img_data, octave, true );
// gui info
m_next_gui_info->stats_feats_per_octave[octave] =
nFeatsPassingKLTPerOctave[octave] = img_data.pyr_feats_kps[octave].size();
} // end-for-octaves
if( params_gui.show_gui && params_gui.draw_all_raw_feats )
{
// (It's almost as efficient to directly draw these small feature marks at this point
// rather than send all the info to the gui thread and then draw there. A quick test shows
// a gain of 75us -> 50us only, so don't optimize unless efficiency pushes really hard).
m_profiler.enter("stg2.draw_feats");
for (size_t octave=0;octave<nOctaves;octave++)
{
const TKeyPointList & f1 = img_data.pyr_feats_kps[octave];
const size_t n1 = f1.size();
const bool org_img_color = gui_image.isColor();
unsigned char* ptr1 = gui_image.get_unsafe(0,0);
const size_t img1_stride = gui_image.getRowStride();
for(size_t i=0;i<n1;++i)
{
const int x=f1[i].pt.x; const int y=f1[i].pt.y;
unsigned char* ptr = ptr1 + img1_stride*y + (org_img_color ? 3*x:x);
if (org_img_color) {
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0xFF;
}
else {
*ptr = 0xFF;
}
} // end-for
} // end-for
m_profiler.leave("stg2.draw_feats");
} // end-if
// for the GUI thread
string sPassKLT = "", sDetect = "";
for( size_t i=0;i<nOctaves;i++ )
{
sPassKLT += mrpt::format( "%u/",static_cast<unsigned int>(nFeatsPassingKLTPerOctave[i]) );
sDetect += mrpt::format( "%u/",static_cast<unsigned int>(img_data.pyr_feats_kps[i].size()) );
}
string aux = mrpt::format( "\n%s feats (%s passed KLT)", sDetect.c_str(), sPassKLT.c_str() );
m_next_gui_info->text_msg_from_detect += aux;
m_profiler.leave("_stg2");
}