CFeatureExtraction_harris_KLT.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/vision/CFeatureExtraction.h>

// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

using namespace mrpt;
using namespace mrpt::img;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace std;

/************************************************************************************************
*                           extractFeaturesKLT
************************************************************************************************/
void CFeatureExtraction::extractFeaturesKLT(
    const mrpt::img::CImage& inImg, CFeatureList& feats, unsigned int init_ID,
    unsigned int nDesiredFeatures, const TImageROI& ROI) const
{
//#define VERBOSE_TIMING

#ifdef VERBOSE_TIMING
    CTicTac tictac;
#endif
    MRPT_START

#if MRPT_HAS_OPENCV
    const unsigned int MAX_COUNT = 300;

    // -----------------------------------------------------------------
    // Create OpenCV Local Variables
    // -----------------------------------------------------------------
    int count = 0;
    int nPts;

#ifdef VERBOSE_TIMING
    tictac.Tic();
#endif
    const cv::Mat img(cv::cvarrToMat(inImg.getAs<IplImage>()));

#ifdef VERBOSE_TIMING
    cout << "[KLT] Attach: " << tictac.Tac() * 1000.0f << endl;
#endif
    const CImage inImg_gray(inImg, FAST_REF_OR_CONVERT_TO_GRAY);
    const cv::Mat cGrey(cv::cvarrToMat(inImg_gray.getAs<IplImage>()));

    nDesiredFeatures <= 0 ? nPts = MAX_COUNT : nPts = nDesiredFeatures;

#ifdef VERBOSE_TIMING
    tictac.Tic();
#endif

#ifdef VERBOSE_TIMING
    cout << "[KLT] Create: " << tictac.Tac() * 1000.0f << endl;
#endif
    count = nPts;  // Number of points to find

    // -----------------------------------------------------------------
    // Select good features with subpixel accuracy (USING HARRIS OR KLT)
    // -----------------------------------------------------------------
    const bool use_harris = (options.featsType == featHarris);

#ifdef VERBOSE_TIMING
    tictac.Tic();
#endif
    std::vector<cv::Point2f> points;
    cv::goodFeaturesToTrack(
        cGrey, points, nPts,
        (double)options.harrisOptions.threshold,  // for rejecting weak local
        // maxima ( with min_eig <
        // threshold*max(eig_image) )
        (double)options.harrisOptions
            .min_distance,  // minimum distance between features
        cv::noArray(),  // mask
        3,  // blocksize
        use_harris, /* harris */
        options.harrisOptions.k);
#ifdef VERBOSE_TIMING
    cout << "[KLT] Find feats: " << tictac.Tac() * 1000.0f << endl;
#endif

    if (nDesiredFeatures > 0 && count < nPts)
        cout << "\n[WARNING][selectGoodFeaturesKLT]: Only " << count << " of "
             << nDesiredFeatures << " points could be extracted in the image."
             << endl;

    if (options.FIND_SUBPIXEL)
    {
#ifdef VERBOSE_TIMING
        tictac.Tic();
#endif
        // Subpixel interpolation
        cv::cornerSubPix(
            cGrey, points, cv::Size(3, 3), cv::Size(-1, -1),
            cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.05));

#ifdef VERBOSE_TIMING
        cout << "[KLT] subpixel: " << tictac.Tac() * 1000.0f << endl;
#endif
    }

// -----------------------------------------------------------------
// Fill output structure
// -----------------------------------------------------------------
#ifdef VERBOSE_TIMING
    tictac.Tic();
#endif

    feats.clear();
    unsigned int borderFeats = 0;
    unsigned int nCFeats = init_ID;
    int i = 0;
    const int limit = min(nPts, count);
    int offset = (int)this->options.patchSize / 2 + 1;
    unsigned int imgH = inImg.getHeight();
    unsigned int imgW = inImg.getWidth();

    while (i < limit)
    {
        const int xBorderInf = (int)floor(points[i].x - options.patchSize / 2);
        const int xBorderSup = (int)floor(points[i].x + options.patchSize / 2);
        const int yBorderInf = (int)floor(points[i].y - options.patchSize / 2);
        const int yBorderSup = (int)floor(points[i].y + options.patchSize / 2);

        if (options.patchSize == 0 ||
            ((xBorderSup < (int)imgW) && (xBorderInf > 0) &&
             (yBorderSup < (int)imgH) && (yBorderInf > 0)))
        {
            CFeature::Ptr ft = mrpt::make_aligned_shared<CFeature>();

            ft->type = featKLT;
            ft->x = points[i].x;  // X position
            ft->y = points[i].y;  // Y position
            ft->track_status = status_TRACKED;  // Feature Status
            ft->response = 0.0;  // A value proportional to the quality of the
            // feature (unused yet)
            ft->ID = nCFeats++;  // Feature ID into extraction
            ft->patchSize = options.patchSize;  // The size of the feature patch

            if (options.patchSize > 0)
            {
                inImg.extract_patch(
                    ft->patch, round(ft->x) - offset, round(ft->y) - offset,
                    options.patchSize,
                    options.patchSize);  // Image patch surronding the feature
            }

            feats.push_back(ft);

        }  // end if
        else
            borderFeats++;

        i++;
    }  // end while

#ifdef VERBOSE_TIMING
    cout << "[KLT] Create output: " << tictac.Tac() * 1000.0f << endl;
#endif

#else
    THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif

    MRPT_END

}  // end of function

MRPT_TODO("Delete? Refactor / join to mrpt::vision::CGenericFeatureTracker?")
#if 0
/*------------------------------------------------------------
                    findMoreFeatures
-------------------------------------------------------------*/
void  CFeatureExtraction::findMoreFeatures( const mrpt::img::CImage &img,
                                            const CFeatureList &inList,
                                            CFeatureList &outList,
                                            unsigned int nDesiredFeats) const
{
#if MRPT_HAS_OPENCV
    MRPT_START

    if( options.featsType == featHarris || options.featsType == featKLT )
    {
        // Mask the points already stored in the list
        cv::Mat mask = cv::Mat::ones(img.getHeight(), img.getWidth(), CV_8UC1 ); // Set the whole mask to 1 initially

        CFeatureList::const_iterator itKLT;

        TFeatureID mxID = 0;

        for( itKLT = inList.begin(); itKLT != inList.end(); itKLT++ )
        {
            int cx = (int)(*itKLT)->x;
            int cy = (int)(*itKLT)->y;

            // Mask surronding pixels
            size_t xxI = max( 0, cx - 15 );
            size_t xxE = min( cx + 15, mask.cols()-1 );
            size_t yyI = max( 0, cy - 15 );
            size_t yyE = min( cy + 15, (int)mask.rows()-1 );

            for(size_t yy = yyI; yy < yyE; yy++)
                for(size_t xx = xxI; xx < xxE; xx++)
                    cvSet2D( mask, yy, xx, zero );

            if( (*itKLT)->ID > mxID )
                mxID = (*itKLT)->ID;

        }
        selectGoodFeaturesKLT( img, outList, mxID + 1, nDesiredFeats, &mask );
    }
    else if( options.featsType == featFAST )
    {
        TFeatureID mxID = inList.getMaxID();
        extractFeaturesFAST( img, outList, mxID+1 );        // Find all the possible FAST features

        // Delete all that are too close to the current ones
        CFeatureList::iterator          itOutList;
        CFeatureList::const_iterator    itInList;
        for( itInList = inList.begin(); itInList != inList.end(); ++itInList )
        {
            for( itOutList = outList.begin(); itOutList != outList.end(); )
            {
                if( fabs((*itOutList)->x-(*itInList)->x) < 15 ||  fabs((*itOutList)->y-(*itInList)->y) < 15 )
                    itOutList = outList.erase( itOutList );
                else
                    ++itOutList;
            } // end-for
        } // end-for

        if( nDesiredFeats != 0 && outList.size() > nDesiredFeats )
            outList.resize( nDesiredFeats );
    }
    MRPT_END
#else
    THROW_EXCEPTION("MRPT was compiled without OpenCV");
#endif
} // end findMoreFeatures
#endif

MRPT_TODO("Delete? Is not this a duplicate of extractFeaturesKLT ()???")
#if 0
/************************************************************************************************
*                               selectGoodFeaturesKLT                                               *
************************************************************************************************/
void  CFeatureExtraction::selectGoodFeaturesKLT(
        const mrpt::img::CImage         &img,
        CFeatureList            &feats,
        unsigned int            init_ID,
        unsigned int            nDesiredFeatures) const
{
#if MRPT_HAS_OPENCV
    // Get image size
    size_t imgW = img.getWidth();
    size_t imgH = img.getHeight();

    // Check image size and ROI limits
    ASSERT_( imgH > 0 && imgW > 0 );                                                    // Proper Image Size
    ASSERT_( ROI.xMin >= 0 && ROI.yMin >= 0 && ROI.xMax < imgW && ROI.yMax < imgH );    // Proper TImageROI Limits

    // Mask
    CvMatrix mask( imgH, imgW, CV_8UC1 );
    CvScalar zero = cvRealScalar( 0.0 );
    CvScalar one = cvRealScalar( 1.0 );

    if( ROI.xMin == 0 && ROI.xMax == 0 && ROI.yMin == 0 && ROI.yMax == 0 )      // Use the whole image (except the borders)
    {
        if( options.patchSize == 0 )
            selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, nullptr );
        else
        {
            // We mask the borders of the image in order to get an appropiate patch (according to the patch size)
            // ---------------------------------------------------------------------
            // 00000000000000000000000000000000000
            // 00000000000000000000000000000000000
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00111111111111111111111111111111100
            // 00000000000000000000000000000000000
            // 00000000000000000000000000000000000

            cvSet( mask, one );                                             // Set the whole mask to 1 initially

            size_t border = (options.patchSize-1)/2 + 1;
            size_t xx,yy;                                                   // Put outside of the "for"s to avoid errors in VC6
            for( xx = 0; xx < imgW; xx++ )
                for( yy = 0; yy < border; yy++ )
                    cvSet2D( mask, yy, xx, zero );                          // Set to 0 the pixels not to be processed

            for( xx = 0; xx < imgW; xx++ )
                for( yy = imgH - border; yy < imgH; yy++ )
                    cvSet2D( mask, yy, xx, zero );                          // Set to 0 the pixels not to be processed

            for( xx = 0; xx < border; xx++ )
                for( yy = border; yy < imgH - border; yy++ )
                    cvSet2D( mask, yy, xx, zero ) ;                         // Set to 0 the pixels not to be processed

            for( xx = imgW - border; xx < imgW; xx++ )
                for( yy = border; yy < imgH - border; yy++ )
                    cvSet2D( mask, yy, xx, zero ) ;                         // Set to 0 the pixels not to be processed

            selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
        } // end if-else patchSize == 0
    } // end if ROI does not exist
    else
    {
        cvSet( mask, zero );                                                // Set the whole mask to 0 initially
        for( unsigned int xx = ROI.xMin; xx < ROI.xMax; xx++ )
            for( unsigned int yy = ROI.yMin; yy < ROI.yMax; yy++ )
                cvSet2D( mask, yy, xx, one) ;                               // Set to 1 the pixels to be processed

        selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
    } // end if-else there exists a ROI
#else
    THROW_EXCEPTION("MRPT was compiled without OpenCV");
#endif
}

#endif